def testSubFrames(self):
     body = Body()
     body.update_dynamic(self.alea_pose, self.alea_jac, self.alea_djac,
                         self.alea_twist)
     sframe = SubFrame(body, Hg.rotz(3.14/3.), 'Brand New Frame')
     self.assertListsAlmostEqual(sframe.bpose,
     array([ [ 0.50045969, -0.86575984,  0.        ,  0.        ],
             [ 0.86575984,  0.50045969,  0.        ,  0.        ],
             [ 0.        ,  0.        ,  1.        ,  0.        ],
             [ 0.        ,  0.        ,  0.        ,  1.        ]]))
     self.assertListsAlmostEqual(sframe.pose,
         [ [ 4.3287992 ,  2.50229845, 0., 0.  ],
           [ 2.16695503, -1.75051589, 0., 4.2 ],
           [ 0.        ,  0.        , 0., 0.  ],
           [ 7.79183856,  4.5041372 , 0., 3.  ] ])
     self.assertListsAlmostEqual(sframe.jacobian,
         [ [ 2.10193069,  4.00367751,  0.50045969],
           [-3.63619133, -6.92607872, -0.86575984],
           [ 0.        ,  7.        ,  0.        ],
           [ 1.30119519,  0.        ,  0.43287992],
           [-2.25097558,  0.        ,  0.25022984],
           [ 8.8       ,  0.        ,  2.        ] ])
     self.assertListsAlmostEqual(sframe.djacobian,
         [ [  0.50045969,  36.76228101,   0.        ],
           [ -0.86575984,  20.32669907,   0.        ],
           [  0.        ,   4.2       ,   2.6       ],
           [  0.50045969,   0.86575984,   0.        ],
           [ -0.86575984,   0.50045969,   0.        ],
           [  7.1       ,   6.        ,   0.        ] ])
     self.assertListsAlmostEqual(sframe.twist,
         [ 0., 0., 7.1, 36.86237295, 20.1535471, 0. ])
Exemple #2
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'],
    }
Exemple #3
0
 def pose(self):
     """
     >>> j = RzJoint(gpos = 3.14/2., gvel = 1.)
     >>> j.pose
     array([[  7.96326711e-04,  -9.99999683e-01,   0.00000000e+00,
               0.00000000e+00],
            [  9.99999683e-01,   7.96326711e-04,   0.00000000e+00,
               0.00000000e+00],
            [  0.00000000e+00,   0.00000000e+00,   1.00000000e+00,
               0.00000000e+00],
            [  0.00000000e+00,   0.00000000e+00,   0.00000000e+00,
               1.00000000e+00]])
     """
     return Hg.rotz(self.gpos[0])
Exemple #4
0
 def pose(self):
     """
     >>> j = RzJoint(gpos = 3.14/2., gvel = 1.)
     >>> j.pose
     array([[  7.96326711e-04,  -9.99999683e-01,   0.00000000e+00,
               0.00000000e+00],
            [  9.99999683e-01,   7.96326711e-04,   0.00000000e+00,
               0.00000000e+00],
            [  0.00000000e+00,   0.00000000e+00,   1.00000000e+00,
               0.00000000e+00],
            [  0.00000000e+00,   0.00000000e+00,   0.00000000e+00,
               1.00000000e+00]])
     """
     return Hg.rotz(self.gpos[0])
Exemple #5
0
const  = w.getconstraints()



#########################################
#                                       #
# CREATE TASKS, EVENTS & LQP controller #
#                                       #
#########################################
## TASKS
from LQPctrl.task import ForceTask, FrameTask
from LQPctrl.task_ctrl import ValueCtrl, KpCtrl
from arboris.homogeneousmatrix import rotz, transl
from numpy import pi
tasks = []
goal = rotz(pi/8)
goal[0:3,3] = [-0.4, .5, 0]
goal = transl(-.8,-.5,0)
tasks.append(ForceTask( const["const"], ValueCtrl([0, 0.001,.01]) , [], 1., 0, True))



## EVENTS
events = []


## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder":10,"Elbow":5,"Wrist":2}

lqpc = LQPcontroller(gforcemax, tasks=tasks)
Exemple #6
0
shapes = w.getshapes()

#w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01))

#########################################
#                                       #
# CREATE TASKS, EVENTS & LQP controller #
#                                       #
#########################################
## TASKS
from LQPctrl.task import FrameTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
from numpy import pi
tasks = []
goal = rotz(3 * pi / 4)
goal[0:3, 3] = [-.75, -.0, 0]
tasks.append(FrameTask(frames["EndEffector"], KpCtrl(goal, 2), [], 1., 0,
                       True))

## EVENTS
events = []

## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2}

data = {}
data = {
    'collision shapes': [(shapes['ee'], shapes['obstacle']),
                         (shapes['ee'], shapes['obstacle2'])]
Exemple #7
0
# CREATE TASKS, EVENTS & LQP controller #
#                                       #
#########################################
## TASKS
from LQPctrl.task import FrameTask
from LQPctrl.task_ctrl import KpTrajCtrl
from arboris.homogeneousmatrix import rotz
from numpy import pi, tile, cos, sin, zeros, arange
tasks = []

amp, T, phi = .5, 4., pi/2
omega = 2*pi/T
t = arange(0, 1.5*T, dt)
x, vx, ax = amp*cos(t*omega + phi), -omega*amp*sin(t*omega + phi), -(omega**2)*amp*cos(t*omega + phi)
y, vy, ay = amp*sin(t*omega + phi),  omega*amp*cos(t*omega + phi), -(omega**2)*amp*sin(t*omega + phi)
pos, vel, acc = tile(rotz(pi/8), (len(t),1,1)), tile(zeros(6), (len(t),1)), tile(zeros(6), (len(t),1))
pos[:,0,3], pos[:,1,3] = x , y
vel[:,3], vel[:,4]     = vx, vy
acc[:,3], acc[:,4]     = ax, ay
goal = [pos, vel, acc]
tasks.append(FrameTask(frames["EndEffector"], KpTrajCtrl(goal, 20), [], 1., 0, True))


## EVENTS
events = []


## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder":10,"Elbow":5,"Wrist":2}
Exemple #8
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)]],
    }
Exemple #9
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'],
    }
#w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01))



#########################################
#                                       #
# CREATE TASKS, EVENTS & LQP controller #
#                                       #
#########################################
## TASKS
from LQPctrl.task import FrameTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
from numpy import pi
tasks = []
goal = rotz(3*pi/4)
goal[0:3,3] = [-.75, -.0, 0]
tasks.append(FrameTask(frames["EndEffector"], KpCtrl(goal, 2), [], 1., 0, True))


## EVENTS
events = []


## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder":10,"Elbow":5,"Wrist":2}

data={}
data = {'collision shapes':[(shapes['ee'], shapes['obstacle']), (shapes['ee'], shapes['obstacle2'])]}
opt = {'avoidance horizon': 1.}
Exemple #11
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)]],
    }
from common import create_3r_and_init, RecordFramePosition

from arboris.core import simulate
from arboris.homogeneousmatrix import rotz
from arboris.observers import PerfMonitor

from LQPctrl.task import FrameTask
from LQPctrl.task_ctrl import KpCtrl
from LQPctrl.LQPctrl import LQPcontroller

from numpy import pi, arange, array, zeros

import pylab as pl

############### INIT ###################
goal = rotz(pi/8)
goal[0:3,3] = [-0.4, .5, 0]
gforcemax = {"Shoulder":10,"Elbow":5,"Wrist":2}

options = []
for cost in ['normal', 'wrench consistent']:
    for norm in ['normal', 'inv(lambda)']:
        for form in ['dgvel chi', 'chi']:
            options.append(cost+";"+norm+";"+form)


############### SIMULATIONS ###################
results = {}
traj = {}
for opt in options:
    print """===============================================