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 ...@@ -57,6 +57,7 @@ T_post = 1.5 # simulation time after walking
w_com = 1.0 # weight of center of mass task w_com = 1.0 # weight of center of mass task
w_cop = 0.0 # weight of center of pressure 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_foot = 1e0 # weight of the foot motion task
w_contact = 1e2 # weight of the foot in contact w_contact = 1e2 # weight of the foot in contact
w_posture = 1e-4 # weight of joint posture task w_posture = 1e-4 # weight of joint posture task
...@@ -70,6 +71,7 @@ v_max_scaling = 0.8 ...@@ -70,6 +71,7 @@ v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 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_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 kp_posture = 1.0 # proportional gain of joint posture task
gain_vector = kp_posture*np.ones(nv-6) gain_vector = kp_posture*np.ones(nv-6)
masks_posture = np.ones(nv-6) masks_posture = np.ones(nv-6)
......
...@@ -84,6 +84,7 @@ for i in range(-N_pre, N+N_post): ...@@ -84,6 +84,7 @@ for i in range(-N_pre, N+N_post):
tsid.add_contact_RF() tsid.add_contact_RF()
tsid.remove_contact_LF() tsid.remove_contact_LF()
if i<0: if i<0:
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0]) tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
elif i<N: elif i<N:
......
...@@ -30,6 +30,7 @@ contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contac ...@@ -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_com = 1.0 # weight of center of mass task
w_cop = 0.0 # weight of center of pressure 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_foot = 1e-1 # weight of the foot motion task
w_contact = -1.0 # weight of foot in contact (negative means infinite weight) 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-4 # weight of joint posture task
...@@ -43,6 +44,7 @@ v_max_scaling = 0.8 ...@@ -43,6 +44,7 @@ v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 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_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 kp_posture = 1.0 # proportional gain of joint posture task
gain_vector = kp_posture*np.ones(nv-6) gain_vector = kp_posture*np.ones(nv-6)
masks_posture = np.ones(nv-6) masks_posture = np.ones(nv-6)
......
...@@ -88,7 +88,7 @@ for i in range(-N_pre, N+N_post): ...@@ -88,7 +88,7 @@ for i in range(-N_pre, N+N_post):
print("Starting to walk (remove contact left foot)") print("Starting to walk (remove contact left foot)")
tsid.remove_contact_LF() tsid.remove_contact_LF()
# activate CoP task only when robot starts walking # 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: elif i>0 and i<N-1:
if contact_phase[i] != contact_phase[i-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])) 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): ...@@ -98,6 +98,12 @@ for i in range(-N_pre, N+N_post):
else: else:
tsid.add_contact_RF() tsid.add_contact_RF()
tsid.remove_contact_LF() 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: if i<0:
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,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: ...@@ -71,6 +71,14 @@ class TsidBiped:
copTask = tsid.TaskCopEquality("task-cop", robot) copTask = tsid.TaskCopEquality("task-cop", robot)
formulation.addForceTask(copTask, conf.w_cop, 1, 0.0) 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 = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector) postureTask.setKp(conf.kp_posture * conf.gain_vector)
...@@ -127,6 +135,7 @@ class TsidBiped: ...@@ -127,6 +135,7 @@ class TsidBiped:
self.comTask = comTask self.comTask = comTask
self.copTask = copTask self.copTask = copTask
self.amTask = amTask
self.postureTask = postureTask self.postureTask = postureTask
self.contactRF = contactRF self.contactRF = contactRF
self.contactLF = contactLF 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