Esempio n. 1
0
 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)))
Esempio n. 2
0
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),
         }
Esempio n. 4
0
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)))
        ],
    }
Esempio n. 5
0
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'],
    }
Esempio n. 6
0
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()
Esempio n. 7
0
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()
Esempio n. 8
0
#                                       #
#########################################
## 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,}
Esempio n. 9
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)]],
    }
Esempio n. 10
0
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'],
    }
Esempio n. 11
0
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"))
Esempio n. 12
0
 def ipose(self):
     return Hg.rotx(-self.gpos[0])
Esempio n. 13
0
 def pose(self):
     return Hg.rotx(self.gpos[0])
Esempio n. 14
0
 def ipose(self):
     return Hg.rotx(-self.gpos[0])
Esempio n. 15
0
 def pose(self):
     return Hg.rotx(self.gpos[0])
Esempio n. 16
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)]],
    }