Commit 466ee04c authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[contact sequence] add an option to ignore rotation of feet (for debug)

parent cd454cff
...@@ -98,10 +98,11 @@ def generateContactSequence(fb,configs,beginId,endId,viewer=None, curves_initGue ...@@ -98,10 +98,11 @@ def generateContactSequence(fb,configs,beginId,endId,viewer=None, curves_initGue
MLF = SE3.Identity() MLF = SE3.Identity()
MRF.translation = np.matrix(q_rl[0:3]) MRF.translation = np.matrix(q_rl[0:3])
MLF.translation = np.matrix(q_ll[0:3]) MLF.translation = np.matrix(q_ll[0:3])
rot_rl = Quaternion(q_rl[3],q_rl[4],q_rl[5],q_rl[6]) if not FORCE_STRAIGHT_LINE :
rot_ll = Quaternion(q_ll[3],q_ll[4],q_ll[5],q_ll[6]) rot_rl = Quaternion(q_rl[3],q_rl[4],q_rl[5],q_rl[6])
MRF.rotation = rot_rl.matrix() rot_ll = Quaternion(q_ll[3],q_ll[4],q_ll[5],q_ll[6])
MLF.rotation = rot_ll.matrix() MRF.rotation = rot_rl.matrix()
MLF.rotation = rot_ll.matrix()
# apply the transform ankle -> center of contact # apply the transform ankle -> center of contact
MRF *= MRsole_offset MRF *= MRsole_offset
...@@ -319,10 +320,11 @@ def generateContactSequence(fb,configs,beginId,endId,viewer=None, curves_initGue ...@@ -319,10 +320,11 @@ def generateContactSequence(fb,configs,beginId,endId,viewer=None, curves_initGue
MLF = SE3.Identity() MLF = SE3.Identity()
MRF.translation = np.matrix(q_rl[0:3]) MRF.translation = np.matrix(q_rl[0:3])
MLF.translation = np.matrix(q_ll[0:3]) MLF.translation = np.matrix(q_ll[0:3])
rot_rl = Quaternion(q_rl[3],q_rl[4],q_rl[5],q_rl[6]) if not FORCE_STRAIGHT_LINE :
rot_ll = Quaternion(q_ll[3],q_ll[4],q_ll[5],q_ll[6]) rot_rl = Quaternion(q_rl[3],q_rl[4],q_rl[5],q_rl[6])
MRF.rotation = rot_rl.matrix() rot_ll = Quaternion(q_ll[3],q_ll[4],q_ll[5],q_ll[6])
MLF.rotation = rot_ll.matrix() MRF.rotation = rot_rl.matrix()
MLF.rotation = rot_ll.matrix()
# apply the transform ankle -> center of contact # apply the transform ankle -> center of contact
MRF *= MRsole_offset MRF *= MRsole_offset
MLF *= MLsole_offset MLF *= MLsole_offset
......
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