def testRxJoint(self):
j = RxJoint()
self.assertListsAlmostEqual(j.pose, rotx(j.gpos[0]))
self.assertListsAlmostEqual(j.ipose, rotx(-j.gpos[0]))
self.assertListsAlmostEqual(j.jacobian, (
[[1.], [0.], [0.], [0.], [0.], [0.]]))
self.assertListsAlmostEqual(j.djacobian, zeros((6,1)))
def get_contact_data():
""" somes extras: give some other important frames
Each subframe is described as follows:
subframe name : [body frame, dims, H_body_subframe]
they can be used for contact point for example
"""
return {
'lh1' : ['l_hand', (.012,), transl(.012, .0205, 0.)],
'lh2' : ['l_hand', (.012,), transl(.012, -.0205, 0.)],
'lh3' : ['l_hand', (.012,), transl(.057, .0205, 0.)],
'lh4' : ['l_hand', (.012,), transl(.057, -.0205, 0.)],
'l_hand_tip' : ['l_hand', (.012,), transl(.069, 0, 0)],
'l_hand_palm' : ['l_hand', (.012,), transl(.0345, 0, 0)],
'rh1' : ['r_hand', (.012,), transl(-.012, .0205, 0.)],
'rh2' : ['r_hand', (.012,), transl(-.012, -.0205, 0.)],
'rh3' : ['r_hand', (.012,), transl(-.057, .0205, 0.)],
'rh4' : ['r_hand', (.012,), transl(-.057, -.0205, 0.)],
'r_hand_tip' : ['r_hand', (.012,), transl(-.069, 0, 0)],
'r_hand_palm' : ['r_hand', (.012,), transl(-.0345, 0, 0)],
'lf1' : ['l_foot', (.002,), transl(-.039,-.027,-.031)],
'lf2' : ['l_foot', (.002,), transl(-.039, .027,-.031)],
'lf3' : ['l_foot', (.002,), transl(-.039, .027, .099)],
'lf4' : ['l_foot', (.002,), transl(-.039,-.027, .099)],
'l_sole': ['l_foot', (), dot(transl(-.041, .0 , .034),
dot(roty(-pi/2), rotx(-pi/2) ) )],
'rf1' : ['r_foot', (.002,), transl(-.039,-.027, .031)],
'rf2' : ['r_foot', (.002,), transl(-.039, .027, .031)],
'rf3' : ['r_foot', (.002,), transl(-.039, .027,-.099)],
'rf4' : ['r_foot', (.002,), transl(-.039,-.027,-.099)],
'r_sole': ['r_foot', (), dot(transl(-.041, .0 ,-.034),
dot(roty(pi/2), rotx(pi/2) ) )],
}
def get_meshes_positions():
return {
"head_mesh" : dot(transl(0,0,-0.055), rotzyx(pi/2., 0, 0)),
"torso_mesh" : dot(transl(0,0.024,-0.144), rotzyx(-pi/2., pi, 0)),
"pelvis_mesh" : dot(transl(0.031,0.04,0), rotzyx(pi/2.,pi,pi/2.)),
"l_upperleg_mesh": dot(transl(0,0,-.05), rotzyx( pi/2., 0, 0)),
"r_upperleg_mesh": dot(transl(0,0, .05), rotzyx(-pi/2., pi, 0)),
"l_lowerleg_mesh": rotzyx(pi,0, pi/2.),
"r_lowerleg_mesh": rotzyx(pi,0, pi/2.),
"l_foot_mesh" : dot(transl(-0.041,0,-0.03), rotzyx(0, pi/2.,0)),
"r_foot_mesh" : dot(transl(-0.041,0, 0.03), rotzyx(pi,-pi/2.,0)),
"l_upperarm_mesh": dot(transl(0,0.01, 0.14), rotzyx(-pi/2.,0,-pi/2.)),
"r_upperarm_mesh": dot(transl(0,0.01,-0.14), rotzyx(-pi/2.,0,-pi/2.)),
"l_forearm_mesh" : dot(transl(0,0,-0.004), rotzyx(pi,0,-pi/2.)),
"r_forearm_mesh" : dot(transl(0,0, 0.004), rotzyx(0,0,-pi/2.)),
## simple meshes
"waist_mesh" : transl(.006, 0,-.1),
"chest_cylinder_mesh": transl(0, 0, -.0447),
"neck_1_mesh" : eye(4),
"neck_2_mesh" : eye(4),
"head_sphere_mesh": transl(0, 0, .0825),
"l_shoulder_1_mesh": transl(0, 0, .07224),
"l_shoulder_2_mesh": eye(4),
"l_arm_cyl_mesh": transl(0, 0, .078),
"l_elbow_1_mesh": eye(4),
"l_forearm_cyl_mesh": transl(0, 0, .07),
"l_wrist_1_mesh": eye(4),
"l_hand_mesh": transl(.0345, 0, 0),
"r_shoulder_1_mesh": transl(0, 0, -.07224),
"r_shoulder_2_mesh": eye(4),
"r_arm_cyl_mesh": transl(0, 0, -.078),
"r_elbow_1_mesh": eye(4),
"r_forearm_cyl_mesh":transl(0, 0, -.07),
"r_wrist_1_mesh": eye(4),
"r_hand_mesh": transl(-.0345, 0, 0),
"l_hip_1_mesh": transl(0, 0, .0375),
"l_hip_2_mesh": eye(4),
"l_thigh_cyl1_mesh": transl(0, 0, -.112),
"l_thigh_cyl2_mesh": dot(transl(0, 0, -.224), roty(pi/2)),
"l_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)),
"l_ankle_1_mesh": eye(4),
"l_foot_cyl_mesh": transl(0,0,.0125),
"r_hip_1_mesh": transl(0, 0, -.0375),
"r_hip_2_mesh": eye(4),
"r_thigh_cyl1_mesh": transl(0, 0, .112),
"r_thigh_cyl2_mesh": dot(transl(0, 0, .224), roty(pi/2)),
"r_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)),
"r_ankle_1_mesh": eye(4),
"r_foot_cyl_mesh": transl(0,0,-.0125),
}
def get_contact_data():
""" somes extras: give some other important frames
Each subframe is described as follows:
subframe name : [body frame, dims, H_body_subframe]
they can be used for contact point for example
"""
return {
'lh1': ['l_hand', (.012, ),
transl(.012, .0205, 0.)],
'lh2': ['l_hand', (.012, ),
transl(.012, -.0205, 0.)],
'lh3': ['l_hand', (.012, ),
transl(.057, .0205, 0.)],
'lh4': ['l_hand', (.012, ),
transl(.057, -.0205, 0.)],
'l_hand_tip': ['l_hand', (.012, ),
transl(.069, 0, 0)],
'l_hand_palm': ['l_hand', (.012, ),
transl(.0345, 0, 0)],
'rh1': ['r_hand', (.012, ),
transl(-.012, .0205, 0.)],
'rh2': ['r_hand', (.012, ),
transl(-.012, -.0205, 0.)],
'rh3': ['r_hand', (.012, ),
transl(-.057, .0205, 0.)],
'rh4': ['r_hand', (.012, ),
transl(-.057, -.0205, 0.)],
'r_hand_tip': ['r_hand', (.012, ),
transl(-.069, 0, 0)],
'r_hand_palm': ['r_hand', (.012, ),
transl(-.0345, 0, 0)],
'lf1': ['l_foot', (.002, ),
transl(-.039, -.027, -.031)],
'lf2': ['l_foot', (.002, ),
transl(-.039, .027, -.031)],
'lf3': ['l_foot', (.002, ),
transl(-.039, .027, .099)],
'lf4': ['l_foot', (.002, ),
transl(-.039, -.027, .099)],
'l_sole': [
'l_foot', (),
dot(transl(-.041, .0, .034), dot(roty(-pi / 2), rotx(-pi / 2)))
],
'rf1': ['r_foot', (.002, ),
transl(-.039, -.027, .031)],
'rf2': ['r_foot', (.002, ),
transl(-.039, .027, .031)],
'rf3': ['r_foot', (.002, ),
transl(-.039, .027, -.099)],
'rf4': ['r_foot', (.002, ),
transl(-.039, -.027, -.099)],
'r_sole': [
'r_foot', (),
dot(transl(-.041, .0, -.034), dot(roty(pi / 2), rotx(pi / 2)))
],
}
def get_joints_data():
""" This dictionnary describes the joint of the robot
Each joint has 3 arguments
name : [parent_body, H_parentbody_jointframe, child_body]
parent_body: the name parent body (a string)
H_parentbody_jointframe: give the transformation matrix from the
parent_body frame to the joint frame. The joint is
always a RzJoint, a rotation around z axis.
chid_body: the child body frame (a string) which is linked with
the joint to the parent body.
"""
return {
'torso_pitch': ['waist' , dot(rotz(pi/2), roty(-pi/2)), 'lap_belt_1'],
'torso_roll': ['lap_belt_1', T_DH(0, 0, .032, pi/2) , 'lap_belt_2'],
'torso_yaw': ['lap_belt_2', T_DH(0, -pi/2, 0, pi/2) , 'chest'],
'head_pitch': ['chest' , T_DH(-.1933, -pi/2, .00231, -pi/2), 'neck_1'],
'head_roll': ['neck_1', T_DH(0, pi/2, .033, pi/2) , 'neck_2'],
'head_yaw': ['neck_2', T_DH(.001, -pi/2, 0, -pi/2) , 'head'],
'l_shoulder_pitch': ['chest' , T_DH(-.1433, 105*pi/180, .0233647,-pi/2), 'l_shoulder_1'],
'l_shoulder_roll': ['l_shoulder_1', T_DH(.10774, pi/2, 0, -pi/2) , 'l_shoulder_2'],
'l_shoulder_yaw': ['l_shoulder_2', T_DH(0, -pi/2, 0, pi/2) , 'l_arm'],
'l_elbow_pitch': ['l_arm' , T_DH(.15228, 75*pi/180, 0, -pi/2) , 'l_elbow_1'],
'l_elbow_yaw': ['l_elbow_1' , T_DH(0, 0, -.015, pi/2) , 'l_forearm'],
'l_wrist_roll': ['l_forearm' , T_DH(.1373, -pi/2, 0, pi/2) , 'l_wrist_1'],
'l_wrist_pitch': ['l_wrist_1' , T_DH(0, pi/2, 0, pi/2) , 'l_hand'],
'r_shoulder_pitch': ['chest' , T_DH(-.1433,-105*pi/180,-.0233647, pi/2), 'r_shoulder_1'],
'r_shoulder_roll': ['r_shoulder_1', T_DH(-.10774, -pi/2, 0, pi/2) , 'r_shoulder_2'],
'r_shoulder_yaw': ['r_shoulder_2', T_DH(0, -pi/2, 0, -pi/2) , 'r_arm'],
'r_elbow_pitch': ['r_arm' , T_DH(-.15228, -105*pi/180, 0, -pi/2) , 'r_elbow_1'],
'r_elbow_yaw': ['r_elbow_1' , T_DH(0, 0, .015, pi/2) , 'r_forearm'],
'r_wrist_roll': ['r_forearm' , T_DH(-.1373, -pi/2, 0, pi/2) , 'r_wrist_1'],
'r_wrist_pitch': ['r_wrist_1' , T_DH(0, pi/2, 0, pi/2) , 'r_hand'],
'l_hip_pitch': ['waist' , dot(transl(0,-.0681, -.1199), rotx(-pi/2)), 'l_hip_1'],
'l_hip_roll': ['l_hip_1' , T_DH(0, pi/2, 0, -pi/2) , 'l_hip_2'],
'l_hip_yaw': ['l_hip_2' , T_DH(0, pi/2, 0, -pi/2) , 'l_thigh'],
'l_knee': ['l_thigh' , T_DH(-.2236 , -pi/2, 0, pi/2) , 'l_shank'],
'l_ankle_pitch': ['l_shank' , T_DH(0 , pi/2, -.213 , pi) , 'l_ankle_1'],
'l_ankle_roll': ['l_ankle_1', T_DH(0, 0, 0, -pi/2) , 'l_foot'],
'r_hip_pitch': ['waist' , dot(transl(0, .0681, -.1199), rotx(-pi/2)), 'r_hip_1'],
'r_hip_roll': ['r_hip_1' , T_DH(0, pi/2, 0, pi/2) , 'r_hip_2'],
'r_hip_yaw': ['r_hip_2' , T_DH(0, pi/2, 0, pi/2) , 'r_thigh'],
'r_knee': ['r_thigh' , T_DH(.2236 , -pi/2, 0, -pi/2) , 'r_shank'],
'r_ankle_pitch': ['r_shank' , T_DH(0 , pi/2, -.213 , pi) , 'r_ankle_1'],
'r_ankle_roll': ['r_ankle_1', T_DH(0, 0, 0, pi/2) , 'r_foot'],
}
def add_snake(w,
nbody,
lengths=None,
masses=None,
gpos=None,
gvel=None,
is_fixed=True):
"""Add a snake-shape robot to the world.
**Example:**
>>> w = World()
>>> add_snake(w, 3, lengths=[1.5, 1., 5.])
"""
assert isinstance(w, World)
if lengths is None:
lengths = [.5] * nbody
assert nbody == len(lengths)
if masses is None:
masses = [2.] * nbody
assert nbody == len(masses)
if gpos is None:
gpos = [0.] * nbody
assert nbody == len(gpos)
if gvel is None:
gvel = [0.] * nbody
assert nbody == len(gvel)
if is_fixed:
frame = w.ground
else:
L = lengths[0] / 2.
body = Body(mass=box([L, L, L], masses[0]))
w.add_link(w.ground, FreeJoint(), body)
frame = body
for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
radius = length / 10. #TODO use 5 instead of 10
#angle = pi/2.
angle = 0. #TODO: remove, it is here for testing
body = Body(
mass=transport(cylinder(length, radius, mass),
dot(rotx(angle), transl(0., -length / 2., 0.))))
joint = RzJoint(gpos=q, gvel=dq)
w.add_link(frame, joint, body)
frame = SubFrame(body, transl(0., length, 0.))
w.register(frame)
w.init()
def add_snake(w, nbody, lengths=None, masses=None, gpos=None, gvel=None,
is_fixed=True):
"""Add a snake-shape robot to the world.
**Example:**
>>> w = World()
>>> add_snake(w, 3, lengths=[1.5, 1., 5.])
"""
assert isinstance(w, World)
if lengths is None:
lengths = [.5] * nbody
assert nbody == len(lengths)
if masses is None:
masses = [2.] * nbody
assert nbody == len(masses)
if gpos is None:
gpos = [0.] * nbody
assert nbody == len(gpos)
if gvel is None:
gvel = [0.] * nbody
assert nbody == len(gvel)
if is_fixed:
frame = w.ground
else:
L = lengths[0]/2.
body = Body(mass=box([L, L, L], masses[0]))
w.add_link(w.ground, FreeJoint(), body)
frame = body
for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
radius = length/10. #TODO use 5 instead of 10
#angle = pi/2.
angle = 0. #TODO: remove, it is here for testing
body = Body(mass=transport(cylinder(length, radius, mass),
dot(rotx(angle),
transl(0., -length/2., 0.))))
joint = RzJoint(gpos=q, gvel=dq)
w.add_link(frame, joint, body)
frame = SubFrame(body, transl(0., length, 0.))
w.register(frame)
w.init()
# #
#########################################
## TASKS
from LQPctrl.task import MultiJointTask, JointTask
from LQPctrl.task_ctrl import KpCtrl
from LQPctrl.walk import WalkingCtrl, WalkingTask
tasks = []
tasks.append(MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 1e-2 , 0, True, "standing_pose"))
goal = {"action": "goto", "pos": [-0.2,0]}
zmp = {'QonR':1e-6, 'horizon':1.7, 'dt':dt, 'cdof':[0,1]}
feet = {"Kp":150, "Kd":None, "weight":1e+1,
"l_frame":frames['l_sole'], "r_frame":frames['r_sole'], "R0":rotx(pi/2.), "l_const":l_const, "r_const":r_const}
#step={"length":.1, "side":.05, "height":.01, "time": .8, "ratio":.7, "start": "right"}
step={"length":.05, "side":.05, "height":.01, "time": .8, "ratio":.7, "start": "left"}
wctrl = WalkingCtrl(goal, zmp, feet, step)
tasks.append(WalkingTask(icub_bodies, wctrl, [], 1., 0, True, "walk"))
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = icub.get_torque_limits()
#opt = {'base weights' : (1e-8, 1e-8, 1e-8)} #, 'formalism':'chi'
#sopt = {'show_progress':False,
# 'abstol' : 1e-11,
# 'reltol' : 1e-10,
# 'feastol' : 1e-11,}
def get_bodies_data():
""" This dictionnary describes the bodies of the robot
Each body is a list of shape
name : [ [(dims1) | mass1 | H1],
[(dims2) | mass2 | H2] ]
about dims (in meter):
if dims has 1 element: it is a sphere; get (radius,)
if dims has 2 elements: it is a cylinder with z-axis; get (length, radius)
if dims has 3 elements: it is a box; get half-lengths along (x,y,z)
masses are in kg
about transformation matrix H:
it represents the transformation from the body frame to the center of
the shape.
transl for translation along (x,y,z) (meter)
rotx,roty, rotz for rotation around x,y,z axis
dot for matrices multiplication
"""
return {
### WARNING: pb between shape and inertia in the c++ file of the simulator
#'waist' : [[(0.002, 0.065, 0.027), 0.20297, dot(transl(0, 0, -0.065), rotx(pi/2))]],
'waist': [[(.032, .0235, .057215), .20297,
transl(.006, 0, -.1)]],
'lap_belt_1': [[(.0315, .0635, .088), 3.623,
dot(transl(-.0383, -.0173, 0), rotz(-pi / 4))]],
'lap_belt_2': [[(.097, .031), .91179,
transl(0, 0, .008)]],
'chest': [[(.0274, .04), .45165,
transl(0, 0, -.0447)],
[(.036, .0545, .059), 1.8388,
dot(transl(-.03, 0, -.1174), rotz(pi / 12))],
[(.036, .0545, .059), 1.8388,
dot(transl(.03, 0, -.1174), rotz(-pi / 12))]],
'neck_1': [[(.077, .015), .28252, eye(4)]],
'neck_2': [[(.033, .015), .1, eye(4)]],
'head': [[(.08, ), .78, transl(0, 0, .0825)]],
'l_shoulder_1': [[(.011, .031), .48278,
transl(0, 0, .07224)]],
'l_shoulder_2': [[(.059, .03), .20779, eye(4)]],
'l_arm': [[(.156, .026), 1.1584,
transl(0, 0, .078)]],
'l_elbow_1': [[(.01, ), .050798, eye(4)]],
'l_forearm': [[(.14, .02), .48774,
transl(0, 0, .07)]],
'l_wrist_1': [[(.04, .01), .05, eye(4)]],
'l_hand': [[(.0345, .0325, .012), .19099,
transl(.0345, 0, 0)]],
'r_shoulder_1': [[(.011, .031), .48278,
transl(0, 0, -.07224)]],
'r_shoulder_2': [[(.059, .03), .20779, eye(4)]],
'r_arm': [[(.156, .026), 1.1584,
transl(0, 0, -.078)]],
'r_elbow_1': [[(.01, ), .050798, eye(4)]],
'r_forearm': [[(.14, .02), .48774,
transl(0, 0, -.07)]],
'r_wrist_1': [[(.04, .01), .05, eye(4)]],
'r_hand': [[(.0345, .0325, .012), .19099,
transl(-.0345, 0, 0)]],
'l_hip_1': [[(.013, .038), .32708,
transl(0, 0, .0375)]],
'l_hip_2': [[(.075, .031), 1.5304, eye(4)]],
'l_thigh': [[(.224, .034), 1.5304,
transl(0, 0, -.112)],
[(.077, .0315), .79206,
dot(transl(0, 0, -.224), roty(pi / 2))]],
'l_shank': [[(.213, .0315), .95262,
dot(transl(0, -.1065, 0), rotx(pi / 2))]],
'l_ankle_1': [[(.063, .0245), .14801, eye(4)]],
'l_foot': [[(.095, .027), .59285,
transl(0, 0, .0125)],
[(.002, .027, .065), .08185,
transl(-.039, 0, .034)]],
'r_hip_1': [[(.013, .038), .32708,
transl(0, 0, -.0375)]],
'r_hip_2': [[(.075, .031), 1.5304, eye(4)]],
'r_thigh': [[(.224, .034), 1.5304,
transl(0, 0, .112)],
[(.077, .0315), .79206,
dot(transl(0, 0, .224), roty(pi / 2))]],
'r_shank': [[(.213, .0315), .95262,
dot(transl(0, -.1065, 0), rotx(pi / 2))]],
'r_ankle_1': [[(.063, .0245), .14801, eye(4)]],
'r_foot': [[(.095, .027), .59285,
transl(0, 0, -.0125)],
[(.002, .027, .065), .08185,
transl(-.039, 0, -.034)]],
}
def get_joints_data():
""" This dictionnary describes the joint of the robot
Each joint has 3 arguments
name : [parent_body, H_parentbody_jointframe, child_body]
parent_body: the name parent body (a string)
H_parentbody_jointframe: give the transformation matrix from the
parent_body frame to the joint frame. The joint is
always a RzJoint, a rotation around z axis.
chid_body: the child body frame (a string) which is linked with
the joint to the parent body.
"""
return {
'torso_pitch':
['waist', dot(rotz(pi / 2), roty(-pi / 2)), 'lap_belt_1'],
'torso_roll': ['lap_belt_1',
T_DH(0, 0, .032, pi / 2), 'lap_belt_2'],
'torso_yaw': ['lap_belt_2',
T_DH(0, -pi / 2, 0, pi / 2), 'chest'],
'head_pitch':
['chest', T_DH(-.1933, -pi / 2, .00231, -pi / 2), 'neck_1'],
'head_roll': ['neck_1',
T_DH(0, pi / 2, .033, pi / 2), 'neck_2'],
'head_yaw': ['neck_2',
T_DH(.001, -pi / 2, 0, -pi / 2), 'head'],
'l_shoulder_pitch': [
'chest',
T_DH(-.1433, 105 * pi / 180, .0233647, -pi / 2), 'l_shoulder_1'
],
'l_shoulder_roll':
['l_shoulder_1',
T_DH(.10774, pi / 2, 0, -pi / 2), 'l_shoulder_2'],
'l_shoulder_yaw':
['l_shoulder_2', T_DH(0, -pi / 2, 0, pi / 2), 'l_arm'],
'l_elbow_pitch':
['l_arm',
T_DH(.15228, 75 * pi / 180, 0, -pi / 2), 'l_elbow_1'],
'l_elbow_yaw': ['l_elbow_1',
T_DH(0, 0, -.015, pi / 2), 'l_forearm'],
'l_wrist_roll':
['l_forearm',
T_DH(.1373, -pi / 2, 0, pi / 2), 'l_wrist_1'],
'l_wrist_pitch': ['l_wrist_1',
T_DH(0, pi / 2, 0, pi / 2), 'l_hand'],
'r_shoulder_pitch': [
'chest',
T_DH(-.1433, -105 * pi / 180, -.0233647, pi / 2), 'r_shoulder_1'
],
'r_shoulder_roll':
['r_shoulder_1',
T_DH(-.10774, -pi / 2, 0, pi / 2), 'r_shoulder_2'],
'r_shoulder_yaw':
['r_shoulder_2', T_DH(0, -pi / 2, 0, -pi / 2), 'r_arm'],
'r_elbow_pitch':
['r_arm',
T_DH(-.15228, -105 * pi / 180, 0, -pi / 2), 'r_elbow_1'],
'r_elbow_yaw': ['r_elbow_1',
T_DH(0, 0, .015, pi / 2), 'r_forearm'],
'r_wrist_roll':
['r_forearm',
T_DH(-.1373, -pi / 2, 0, pi / 2), 'r_wrist_1'],
'r_wrist_pitch': ['r_wrist_1',
T_DH(0, pi / 2, 0, pi / 2), 'r_hand'],
'l_hip_pitch':
['waist',
dot(transl(0, -.0681, -.1199),
rotx(-pi / 2)), 'l_hip_1'],
'l_hip_roll': ['l_hip_1',
T_DH(0, pi / 2, 0, -pi / 2), 'l_hip_2'],
'l_hip_yaw': ['l_hip_2',
T_DH(0, pi / 2, 0, -pi / 2), 'l_thigh'],
'l_knee': ['l_thigh',
T_DH(-.2236, -pi / 2, 0, pi / 2), 'l_shank'],
'l_ankle_pitch': ['l_shank',
T_DH(0, pi / 2, -.213, pi), 'l_ankle_1'],
'l_ankle_roll': ['l_ankle_1',
T_DH(0, 0, 0, -pi / 2), 'l_foot'],
'r_hip_pitch': [
'waist',
dot(transl(0, .0681, -.1199), rotx(-pi / 2)), 'r_hip_1'
],
'r_hip_roll': ['r_hip_1',
T_DH(0, pi / 2, 0, pi / 2), 'r_hip_2'],
'r_hip_yaw': ['r_hip_2',
T_DH(0, pi / 2, 0, pi / 2), 'r_thigh'],
'r_knee': ['r_thigh',
T_DH(.2236, -pi / 2, 0, -pi / 2), 'r_shank'],
'r_ankle_pitch': ['r_shank',
T_DH(0, pi / 2, -.213, pi), 'r_ankle_1'],
'r_ankle_roll': ['r_ankle_1',
T_DH(0, 0, 0, pi / 2), 'r_foot'],
}
from LQPctrl.walk import WalkingCtrl, WalkingTask
tasks = []
tasks.append(
MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 1e-2, 0, True,
"standing_pose"))
goal = {"action": "goto", "pos": [-0.2, 0]}
zmp = {'QonR': 1e-6, 'horizon': 1.7, 'dt': dt, 'cdof': [0, 1]}
feet = {
"Kp": 150,
"Kd": None,
"weight": 1e+1,
"l_frame": frames['l_sole'],
"r_frame": frames['r_sole'],
"R0": rotx(pi / 2.),
"l_const": l_const,
"r_const": r_const
}
#step={"length":.1, "side":.05, "height":.01, "time": .8, "ratio":.7, "start": "right"}
step = {
"length": .05,
"side": .05,
"height": .01,
"time": .8,
"ratio": .7,
"start": "left"
}
wctrl = WalkingCtrl(goal, zmp, feet, step)
tasks.append(WalkingTask(icub_bodies, wctrl, [], 1., 0, True, "walk"))
def ipose(self):
return Hg.rotx(-self.gpos[0])
def pose(self):
return Hg.rotx(self.gpos[0])
def ipose(self):
return Hg.rotx(-self.gpos[0])
def pose(self):
return Hg.rotx(self.gpos[0])
def get_bodies_data():
""" This dictionnary describes the bodies of the robot
Each body is a list of shape
name : [ [(dims1) | mass1 | H1],
[(dims2) | mass2 | H2] ]
about dims (in meter):
if dims has 1 element: it is a sphere; get (radius,)
if dims has 2 elements: it is a cylinder with z-axis; get (length, radius)
if dims has 3 elements: it is a box; get half-lengths along (x,y,z)
masses are in kg
about transformation matrix H:
it represents the transformation from the body frame to the center of
the shape.
transl for translation along (x,y,z) (meter)
rotx,roty, rotz for rotation around x,y,z axis
dot for matrices multiplication
"""
return {
### WARNING: pb between shape and inertia in the c++ file of the simulator
#'waist' : [[(0.002, 0.065, 0.027), 0.20297, dot(transl(0, 0, -0.065), rotx(pi/2))]],
'waist' : [[(.032, .0235, .057215), .20297, transl(.006, 0,-.1)]],
'lap_belt_1': [[(.0315, .0635, .088) , 3.623 , dot(transl(-.0383, -.0173, 0), rotz(-pi/4))]],
'lap_belt_2': [[(.097, .031) , .91179, transl(0, 0, .008)]],
'chest': [[(.0274, .04) , .45165, transl(0, 0, -.0447)],
[(.036, .0545, .059) , 1.8388, dot(transl(-.03, 0,-.1174), rotz( pi/12))],
[(.036, .0545, .059) , 1.8388, dot(transl( .03, 0,-.1174), rotz(-pi/12))]],
'neck_1': [[(.077, .015), .28252, eye(4)]],
'neck_2': [[(.033, .015), .1 , eye(4)]],
'head': [[(.08,) , .78 , transl(0, 0, .0825)]],
'l_shoulder_1': [[(.011, .031) , .48278 , transl(0, 0, .07224)]],
'l_shoulder_2': [[(.059, .03) , .20779 , eye(4)]],
'l_arm': [[(.156, .026) , 1.1584 , transl(0, 0, .078)]],
'l_elbow_1': [[(.01,) , .050798, eye(4)]],
'l_forearm': [[(.14, .02) , .48774 , transl(0, 0, .07)]],
'l_wrist_1': [[(.04, .01) , .05 , eye(4)]],
'l_hand': [[(.0345, .0325, .012), .19099 , transl(.0345, 0, 0)]],
'r_shoulder_1': [[(.011, .031) , .48278 , transl(0, 0, -.07224)]],
'r_shoulder_2': [[(.059, .03) , .20779 , eye(4)]],
'r_arm': [[(.156, .026) , 1.1584 , transl(0, 0, -.078)]],
'r_elbow_1': [[(.01,) , .050798, eye(4)]],
'r_forearm': [[(.14, .02) , .48774 , transl(0, 0, -.07)]],
'r_wrist_1': [[(.04, .01) , .05 , eye(4)]],
'r_hand': [[(.0345, .0325, .012), .19099 , transl(-.0345, 0, 0)]],
'l_hip_1': [[(.013, .038) , .32708, transl(0, 0, .0375)]],
'l_hip_2': [[(.075, .031) , 1.5304, eye(4)]],
'l_thigh': [[(.224, .034) , 1.5304, transl(0, 0, -.112)],
[(.077,.0315) , .79206, dot(transl(0, 0, -.224), roty(pi/2))]],
'l_shank': [[(.213, .0315) , .95262, dot(transl(0, -.1065, 0), rotx(pi/2))]],
'l_ankle_1': [[(.063, .0245) , .14801, eye(4)]],
'l_foot': [[(.095, .027) , .59285, transl(0,0,.0125)],
[(.002, .027, .065), .08185, transl(-.039, 0, .034)]],
'r_hip_1': [[(.013, .038) , .32708, transl(0, 0, -.0375)]],
'r_hip_2': [[(.075, .031) , 1.5304, eye(4)]],
'r_thigh': [[(.224, .034) , 1.5304, transl(0, 0, .112)],
[(.077,.0315) , .79206, dot(transl(0, 0, .224), roty(pi/2))]],
'r_shank': [[(.213, .0315) , .95262, dot(transl(0, -.1065, 0), rotx(pi/2))]],
'r_ankle_1': [[(.063, .0245) , .14801, eye(4)]],
'r_foot': [[(.095, .027) , .59285, transl(0,0,-.0125)],
[(.002, .027, .065), .08185, transl(-.039, 0, -.034)]],
}