forked from DAInamite/programming-humanoid-robot-in-python
-
Notifications
You must be signed in to change notification settings - Fork 0
/
forward_kinematics.py
134 lines (115 loc) · 5.28 KB
/
forward_kinematics.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
'''In this exercise you need to implement forward kinematics for NAO robot
* Tasks:
1. complete the kinematics chain definition (self.chains in class ForwardKinematicsAgent)
The documentation from Aldebaran is here:
http://doc.aldebaran.com/2-1/family/robots/bodyparts.html#effector-chain
2. implement the calculation of local transformation for one joint in function
ForwardKinematicsAgent.local_trans. The necessary documentation are:
http://doc.aldebaran.com/2-1/family/nao_h21/joints_h21.html
http://doc.aldebaran.com/2-1/family/nao_h21/links_h21.html
3. complete function ForwardKinematicsAgent.forward_kinematics, save the transforms of all body parts in torso
coordinate into self.transforms of class ForwardKinematicsAgent
* Hints:
the local_trans has to consider different joint axes and link parameters for different joints
'''
# add PYTHONPATH
import os
import sys
sys.path.append(os.path.join(os.path.abspath(os.path.dirname(__file__)), '..', 'joint_control'))
from numpy.matlib import matrix, identity, array, dot
from math import sin, cos
from angle_interpolation import AngleInterpolationAgent
class ForwardKinematicsAgent(AngleInterpolationAgent):
def __init__(self, simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
super(ForwardKinematicsAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode)
self.transforms = {n: identity(4) for n in self.joint_names}
# chains defines the name of chain and joints of the chain
self.chains = {'Head': ['HeadYaw', 'HeadPitch'],
# YOUR CODE HERE
'RArm': ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll'],
'LArm': ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll'],
'RLeg': ['RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'],
'LLeg': ['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll']
}
self.joint_length = { # Head
'HeadYaw': (0, 0, 126.5),
'HeadPitch': (0, 0, 0), # Head Chain
# Right arm
'RShoulderPitch': (0, -98, 100),
'RShoulderRoll': (0, 0, 0),
'RElbowYaw': (105, -15, 0),
'RElbowRoll': (0, 0, 0),
'RWristYaw': (55.95, 0, 0),
# Left arm
'LShoulderPitch': (0, 98, 100),
'LShoulderRoll': (0, 0, 0),
'LElbowYaw': (105, 15, 0),
'LElbowRoll': (0, 0, 0),
'LWristYaw': (55.95, 0, 0),
# Right leg
'RHipYawPitch': (0, -50, -85),
'RHipRoll': (0, 0, 0),
'RHipPitch': (0, 0, 0),
'RKneePitch': (0, 0, -100),
'RAnklePitch': (0, 0, -102.9),
'RAnkleRoll': (0, 0, 0),
# Left leg
'LHipYawPitch': (0, 50, -85),
'LHipRoll': (0, 0, 0),
'LHipPitch': (0, 0, 0),
'LKneePitch': (0, 0, -100),
'LAnklePitch': (0, 0, -102.9),
'LAnkleRoll': (0, 0, 0)}
def think(self, perception):
self.forward_kinematics(perception.joint)
return super(ForwardKinematicsAgent, self).think(perception)
def local_trans(self, joint_name, joint_angle):
'''calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
'''
T = identity(4)
# YOUR CODE HERE
s = sin(joint_angle)
c = cos(joint_angle)
# X
if 'Roll' in joint_name:
T = array([[ 1, 0, 0, 0]
,[ 0, c,-s, 0]
,[ 0, s, c, 0]
,[ 0, 0, 0, 1]])
# Y
if 'Pitch' in joint_name:
T = array([[ c, 0, s, 0]
,[ 0, 1, 0, 0]
,[-s, 0, c, 0]
,[ 0, 0, 0, 1]])
# Z
if 'Yaw' in joint_name:
T = array([[ c, s, 0, 0]
,[-s, c, 0, 0]
,[ 0, 0, 1, 0]
,[ 0, 0, 0, 1]])
T[0:3, 3] = self.joint_length[joint_name]
return T
def forward_kinematics(self, joints):
'''forward kinematics
:param joints: {joint_name: joint_angle}
'''
for chain_joints in self.chains.values():
T = identity(4)
for joint in chain_joints:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
# YOUR CODE HERE
T = dot(T, Tl)
self.transforms[joint] = T
if __name__ == '__main__':
agent = ForwardKinematicsAgent()
agent.run()