def rootOrientationFromFeetPlacement(phase, phase_next):
#FIXME : extract only the yaw rotation
qr = Quaternion(phase.RF_patch.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
ql = Quaternion(phase.LF_patch.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
q_rot = qr.slerp(0.5, ql)
placement_init = SE3.Identity()
placement_init.rotation = q_rot.matrix()
if phase_next:
if not isContactActive(phase, cfg.Robot.rfoot) and isContactActive(
phase_next, cfg.Robot.rfoot):
qr = Quaternion(phase_next.RF_patch.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if not isContactActive(phase, cfg.Robot.lfoot) and isContactActive(
phase_next, cfg.Robot.lfoot):
ql = Quaternion(phase_next.LF_patch.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
q_rot = qr.slerp(0.5, ql)
placement_end = SE3.Identity()
placement_end.rotation = q_rot.matrix()
return placement_init, placement_end
def rootOrientationFromFeetPlacement(Robot, phase_prev, phase, phase_next):
"""
Compute an initial and final root orientation for the ContactPhase
The initial orientation is a mean between both feet contact position in the current (or previous) phase
the final orientation is considering the new contact position of the moving feet
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param phase_prev: the previous contact phase
:param phase: the current contact phase
:param phase_next: the next contact phase
:return: a list of SE3 objects: [initial placement, final placement]
"""
#FIXME : extract only the yaw rotation
qr = None
ql = None
patchR = None
patchL = None
if phase.isEffectorInContact(Robot.rfoot):
patchR = phase.contactPatch(Robot.rfoot)
elif phase_prev and phase_prev.isEffectorInContact(Robot.rfoot):
patchR = phase_prev.contactPatch(Robot.rfoot)
if patchR:
qr = Quaternion(patchR.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if phase.isEffectorInContact(Robot.lfoot):
patchL = phase.contactPatch(Robot.lfoot)
elif phase_prev and phase_prev.isEffectorInContact(Robot.lfoot):
patchL = phase_prev.contactPatch(Robot.lfoot)
if patchL:
ql = Quaternion(patchL.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
if ql is not None and qr is not None:
q_rot = qr.slerp(0.5, ql)
elif qr is not None:
q_rot = qr
elif ql is not None:
q_rot = ql
else:
raise RuntimeError("In rootOrientationFromFeetPlacement, cannot deduce feet initial contacts positions.")
placement_init = SE3.Identity()
placement_init.rotation = q_rot.matrix()
# compute the final orientation :
if phase_next:
if not phase.isEffectorInContact(Robot.rfoot) and phase_next.isEffectorInContact(Robot.rfoot):
qr = Quaternion(phase_next.contactPatch(Robot.rfoot).placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if not phase.isEffectorInContact(Robot.lfoot) and phase_next.isEffectorInContact(Robot.lfoot):
ql = Quaternion(phase_next.contactPatch(Robot.lfoot).placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
if ql is not None and qr is not None:
q_rot = qr.slerp(0.5, ql)
elif qr is not None:
q_rot = qr
elif ql is not None:
q_rot = ql
else:
raise RuntimeError("In rootOrientationFromFeetPlacement, cannot deduce feet initial contacts positions.")
placement_end = SE3.Identity()
placement_end.rotation = q_rot.matrix()
return placement_init, placement_end
