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