Commit 07ceb45d authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[py] Add angular momentum task to tsid-biped, which improves the behavior during walking

parent 819353ad
Pipeline #13502 passed with stage
in 32 minutes and 20 seconds
......@@ -57,6 +57,7 @@ T_post = 1.5 # simulation time after walking
w_com = 1.0 # weight of center of mass task
w_cop = 0.0 # weight of center of pressure task
w_am = 1e-4 # weight of angular momentum task
w_foot = 1e0 # weight of the foot motion task
w_contact = 1e2 # weight of the foot in contact
w_posture = 1e-4 # weight of joint posture task
......@@ -70,6 +71,7 @@ v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_am = 10.0 # proportional gain of angular momentum task
kp_posture = 1.0 # proportional gain of joint posture task
gain_vector = kp_posture*np.ones(nv-6)
masks_posture = np.ones(nv-6)
......
......@@ -84,6 +84,7 @@ for i in range(-N_pre, N+N_post):
tsid.add_contact_RF()
tsid.remove_contact_LF()
if i<0:
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
elif i<N:
......
......@@ -30,6 +30,7 @@ contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contac
w_com = 1.0 # weight of center of mass task
w_cop = 0.0 # weight of center of pressure task
w_am = 0.0 # weight of angular momentum 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
......@@ -43,6 +44,7 @@ v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_am = 10.0 # proportional gain of angular momentum task
kp_posture = 1.0 # proportional gain of joint posture task
gain_vector = kp_posture*np.ones(nv-6)
masks_posture = np.ones(nv-6)
......
......@@ -88,7 +88,7 @@ for i in range(-N_pre, N+N_post):
print("Starting to walk (remove contact left foot)")
tsid.remove_contact_LF()
# activate CoP task only when robot starts walking
tsid.formulation.updateTaskWeight(tsid.copTask.name, 1e-3)
tsid.formulation.updateTaskWeight(tsid.copTask.name, 1e-4)
elif i>0 and i<N-1:
if contact_phase[i] != contact_phase[i-1]:
print("Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i]))
......@@ -98,6 +98,12 @@ for i in range(-N_pre, N+N_post):
else:
tsid.add_contact_RF()
tsid.remove_contact_LF()
elif i==N:
# switch to double support at the end
if contact_phase[i-1] == 'left':
tsid.add_contact_RF()
else:
tsid.add_contact_LF()
if i<0:
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
......
......@@ -71,6 +71,14 @@ class TsidBiped:
copTask = tsid.TaskCopEquality("task-cop", robot)
formulation.addForceTask(copTask, conf.w_cop, 1, 0.0)
amTask = tsid.TaskAMEquality("task-am", robot)
amTask.setKp(conf.kp_am * np.array([1., 1., 0.]))
amTask.setKd(2.0 * np.sqrt(conf.kp_am * np.array([1., 1., 0.])))
formulation.addMotionTask(amTask, conf.w_am, 1, 0.)
sampleAM = tsid.TrajectorySample(3)
amTask.setReference(sampleAM)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector)
......@@ -127,6 +135,7 @@ class TsidBiped:
self.comTask = comTask
self.copTask = copTask
self.amTask = amTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
......
Markdown is supported
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