Commit 91d114c1 authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Guilhem Saurel
Browse files

tsid_biped and talos_conf: postural task use a weight vector

parent 5266921f
......@@ -24,11 +24,49 @@ contactNormal = np.array([0., 0., 1.]) # direction of the normal to the con
w_com = 1.0 # weight of center of mass task
w_foot = 1e-1 # weight of the foot motion task
w_contact = -1.0 # weight of foot in contact (negative means infinite weight)
w_posture = 1e-4 # weight of joint posture task
w_posture = 1e-1 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_torque_bounds = 1.0 # weight of the torque bounds
w_joint_bounds = 0.0
gain_vector = np.array( # gain vector for postural task
[
10.,
5.,
5.,
1.,
1.,
10., # lleg #low gain on axis along y and knee
10.,
5.,
5.,
1.,
1.,
10., #rleg
5000.,
5000., #chest
500.,
1000.,
10.,
10.,
10.,
10.,
100.,
50., #larm
50.,
100.,
10.,
10.,
10.,
10.,
100.,
50., #rarm
100.,
100.
] #head
)
masks_posture = np.ones(32)
tau_max_scaling = 1.45 # scaling factor of torque bounds
v_max_scaling = 0.8
......
......@@ -72,8 +72,9 @@ class TsidBiped:
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * np.ones(robot.nv-6))
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * np.ones(robot.nv-6))
postureTask.setKp(conf.kp_posture * conf.gain_vector)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture * conf.gain_vector))
postureTask.mask(conf.masks_posture)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment