def compute_stopping_cs(self, move_to_support_polygon=True):
"""
Compute a Contact Sequence with centroidal trajectories to bring the current last_phase
to a stop without contact changes
:param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon
:return:
"""
phase_stop = ContactPhase(self.get_last_phase())
tools.setInitialFromFinalValues(phase_stop, phase_stop)
phase_stop.timeInitial = phase_stop.timeFinal
phase_stop.duration = DURATION_0_STEP # FIXME !!
# try 0-step:
success, phase = zeroStepCapturability(phase_stop, self.cfg)
if success:
cs_ref = ContactSequence(0)
cs_ref.append(phase)
# TEST : add another phase to go back in the center of the support polygon
if move_to_support_polygon:
phase_projected = ContactPhase()
phase_projected.timeInitial = phase.timeFinal
phase_projected.duration = DURATION_0_STEP
tools.copyContactPlacement(phase, phase_projected)
tools.setInitialFromFinalValues(phase, phase_projected)
phase_projected.c_final = tools.computeCenterOfSupportPolygonFromPhase(
phase_stop, self.fullBody.DEFAULT_COM_HEIGHT)
#FIXME 'default height'
tools.connectPhaseTrajToFinalState(phase_projected)
cs_ref.append(phase_projected)
else:
# TODO try 1 step :
raise RuntimeError("One step capturability not implemented yet !")
tools.computeRootTrajFromContacts(self.fullBody, cs_ref)
self.last_phase = cs_ref.contactPhases[-1].copy()
# define the final root position, translation from the CoM position and rotation from the feet rotation
q_final = np.zeros(7)
q_final[:3] = self.last_phase.c_final[::]
placement_rot_root, _ = tools.rootOrientationFromFeetPlacement(self.cfg.Robot, None, self.last_phase, None)
quat_root = Quaternion(placement_rot_root.rotation)
q_final[3:7] = [quat_root.x, quat_root.y, quat_root.z, quat_root.w]
self.last_phase.q_final = q_final
self.last_phase_flag.value = False
self.last_phase_pickled = Array(c_ubyte, MAX_PICKLE_SIZE) # reset currently stored whole body last phase
return cs_ref
def createPhaseFromConfig(fb, q, limbsInContact, t_init=-1):
phase = ContactPhase()
phase.q_init = np.array(q)
fb.setCurrentConfig(q)
com = np.array(fb.getCenterOfMass())
if t_init > 0:
phase.timeInitial = 0.
phase.c_init = com.copy()
phase.c_final = com.copy()
if fb.client.robot.getDimensionExtraConfigSpace() >= 6 and len(
q) == fb.getConfigSize():
# add dc and ddc values from extraDOF
phase.dc_init = np.array(q[-6:-3])
phase.dc_final = np.array(q[-6:-3])
phase.ddc_init = np.array(q[-3:])
phase.ddc_final = np.array(q[-3:])
for limb in limbsInContact:
eeName = fb.dict_limb_joint[limb]
q_j = fb.getJointPosition(eeName)
placement = SE3FromConfig(q_j)
patch = ContactPatch(
placement) # TODO set friction / other parameters here
phase.addContact(eeName, patch)
return phase