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. ])
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 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])
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])
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)
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'])]
# 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}
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'],
}
#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.}
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 """===============================================