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