コード例 #1
0
 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. ])
コード例 #2
0
ファイル: icub.py プロジェクト: mitkof6/arboris-python
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'],
    }
コード例 #3
0
ファイル: joints.py プロジェクト: efim/arboris-python
 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])
コード例 #4
0
ファイル: joints.py プロジェクト: mitkof6/arboris-python
 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])
コード例 #5
0
ファイル: 05_force_control.py プロジェクト: mickey79/LQPctrl
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)
コード例 #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'])]
コード例 #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}
コード例 #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)]],
    }
コード例 #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'],
    }
コード例 #10
0
#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.}
コード例 #11
0
ファイル: icub.py プロジェクト: mitkof6/arboris-python
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)]],
    }
コード例 #12
0
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 """===============================================