def createPhaseFromConfig(fb, q, limbsInContact, t_init = -1): """ Build a contact phase from a wholebody configuration and a list of active contacts. Set the q_init, c_init and c_final from the joint positions, and add ContactPatch for each limb in contact, with the current placement corresponding to the wholebody configuration :param fb: an rbprm.Fullbody object :param q: a list of joint position :param limbsInContact: a list of limbs currently in contact :param t_init: if provided, the new COntactPhase will begin at this time :return: a new ContactPhase """ phase = ContactPhase() phase.q_init = np.array(q) fb.setCurrentConfig(q) com = np.array(fb.getCenterOfMass()) if t_init >= 0: phase.timeInitial = t_init 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) if fb.cType == '_3_DOF': placement.rotation = np.identity(3) # FIXME: use contact normal instead of identity, but it's unknown here patch = ContactPatch(placement) # TODO set friction / other parameters here phase.addContact(eeName, patch) return phase
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