def computeRootTrajFromConfigurations(cs):
assert cs.haveConfigurationsValues(
), "computeRootTrajFromConfigurations require haveConfigurationsValues"
for phase in cs.contactPhases:
p_init = SE3FromConfig(phase.q_init)
p_final = SE3FromConfig(phase.q_final)
phase.root_t = SE3Curve(p_init, p_final, phase.timeInitial,
phase.timeFinal)
def computeRootTrajFromConfigurations(cs):
"""
Add root_t trajectories to each contact phases in the sequences. This trajecories are linear interpolation in SE3
from the initial root positon in q_init to it's final position in q_final, with the same duration as the phases.
:param cs: the ContactSequence to modify
"""
assert cs.haveConfigurationsValues(), "computeRootTrajFromConfigurations require haveConfigurationsValues"
for phase in cs.contactPhases:
p_init = SE3FromConfig(phase.q_init)
p_final = SE3FromConfig(phase.q_final)
phase.root_t = SE3Curve(p_init, p_final, phase.timeInitial, phase.timeFinal)
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 addPhaseFromConfig(fb, v, cs, q, limbsInContact):
phase = ContactPhaseHumanoid()
phase.reference_configurations.append(np.matrix((q)).T)
if v:
v(q)
fb.setCurrentConfig(q)
state = np.matrix(np.zeros(9)).T
state[0:3] = np.matrix(fb.getCenterOfMass()).T
phase.init_state = state.copy()
phase.final_state = state.copy()
# set Identity to each contact placement (otherwise it's uninitialized)
phase.RF_patch.placement = SE3.Identity()
phase.LF_patch.placement = SE3.Identity()
phase.RH_patch.placement = SE3.Identity()
phase.LH_patch.placement = SE3.Identity()
for limb in limbsInContact:
eeName = fb.dict_limb_joint[limb]
q_j = fb.getJointPosition(eeName)
patch = contactPatchForEffector(phase, eeName, fb)
placement = SE3FromConfig(q_j)
patch.placement = placement.act(fb.dict_offset[eeName])
patch.active = True
cs.contact_phases.append(phase)
if v:
display_tools.displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
def contactPlacementFromConfig(fullBody, q, eeName):
fullBody.setCurrentConfig(q)
q = fullBody.getJointPosition(eeName)
p = SE3FromConfig(q)
# transform to contact position (from joint position)
# p*=cfg.Robot.dict_offset[eeName] # test ??
tr = p.translation + cfg.Robot.dict_offset[eeName].translation
p.translation = tr
return p
def contactPlacementFromRBPRMState(fb, stateId, eeName):
# get limbName from effector name :
fb.setCurrentConfig(fb.getConfigAtState(stateId))
placement = SE3FromConfig(fb.getJointPosition(eeName))
if fb.cType == "_3_DOF":
limbId = list(fb.dict_limb_joint.keys())[list(fb.dict_limb_joint.values()).index(eeName)]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId)
normal = np.array(n)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal)
placement.rotation = quat.matrix()
return placement
def effectorPlacementFromPhaseConfig(phase, eeName, fullBody):
if fullBody is None:
raise RuntimeError(
"Cannot compute the effector placement from the configuration without initialized fullBody object."
)
if not phase.q_init.any():
raise RuntimeError(
"Cannot compute the effector placement as the initial configuration is not initialized in the ContactPhase."
)
fullBody.setCurrentConfig(phase.q_init.tolist())
return SE3FromConfig(fullBody.getJointPosition(eeName))
def effectorPlacementFromPhaseConfig(phase, eeName, fullBody):
"""
Compute the effector placement from the initial wholebody configuration stored in the contact phase
:param phase: a ContactPhase, must have a .q_init member initialized
:param eeName: the effector name
:param fullBody: an rbprm.FullBody object
:return: a pinocchio.SE3 placement of the given effector name
"""
if fullBody is None :
raise RuntimeError("Cannot compute the effector placement from the configuration without initialized fullBody object.")
if not phase.q_init.any():
raise RuntimeError("Cannot compute the effector placement as the initial configuration is not initialized in the ContactPhase.")
fullBody.setCurrentConfig(phase.q_init.tolist())
return SE3FromConfig(fullBody.getJointPosition(eeName))
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
effectorsInContact = phase.effectorsInContact()
contacts = [
] # contacts should contains the limb names, not the effector names
list_effector = list(fb.dict_limb_joint.values())
for eeName in effectorsInContact:
contacts += [
list(fb.dict_limb_joint.keys())[list_effector.index(eeName)]
]
#q = phase.q_init.tolist() # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(
phase, fb.DEFAULT_COM_HEIGHT).tolist()
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(phase.root_t.evaluateAsSE3(phase.timeInitial).rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = phase.contactPatch(eeName).placement
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = phase.contactPatch(eeName).placement
p = placement.translation.tolist()
n = computeContactNormal(placement).tolist()
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print(
"Cannot project the configuration to contact, for effector : ",
eeName)
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print(
"cannot project com to the middle of the support polygon."
)
phase.q_init = np.array(state.q())
return state.q()
def contactPlacementFromRBPRMState(fb, stateId, eeName):
"""
Compute the placement of eeName for the given stateId
:param fb: an instance of rbprm.FullBody
:param stateId: the Id of the state
:param eeName: the name of the effector
:return: the placement represented as a pinocchio.SE3 object
"""
# get limbName from effector name :
fb.setCurrentConfig(fb.getConfigAtState(stateId))
placement = SE3FromConfig(fb.getJointPosition(eeName))
if fb.cType == "_3_DOF":
limbId = list(fb.dict_limb_joint.keys())[list(
fb.dict_limb_joint.values()).index(eeName)]
[p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(
stateId, False, limbId)
normal = np.array(n)
quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]),
normal)
placement.rotation = quat.matrix()
return placement
def generateConfigFromPhase(fb, phase, projectCOM=False):
fb.usePosturalTaskContactCreation(False)
contacts = getActiveContactLimbs(phase, fb)
#q = phase.reference_configurations[0].T.tolist()[0] # should be the correct config for the previous phase, if used only from high level helper methods
q = fb.referenceConfig[::] + [0] * 6 # FIXME : more generic !
root = computeCenterOfSupportPolygonFromPhase(phase, fb).T.tolist()[0]
q[0:2] = root[0:2]
q[2] += root[2] - fb.DEFAULT_COM_HEIGHT
quat = Quaternion(
rootOrientationFromFeetPlacement(phase, None)[0].rotation)
q[3:7] = [quat.x, quat.y, quat.z, quat.w]
# create state in fullBody :
state = State(fb, q=q, limbsIncontact=contacts)
# check if q is consistent with the contact placement in the phase :
fb.setCurrentConfig(q)
for limbId in contacts:
eeName = fb.dict_limb_joint[limbId]
placement_fb = SE3FromConfig(fb.getJointPosition(eeName))
placement_phase = JointPlacementForEffector(phase, eeName, fb)
if placement_fb != placement_phase: # add a threshold instead of 0 ? how ?
# need to project the new contact :
placement = getContactPlacement(phase, eeName, fb)
p = placement.translation.T.tolist()[0]
n = computeContactNormal(placement).T.tolist()[0]
state, success = StateHelper.addNewContact(state, limbId, p, n,
1000)
if not success:
print "Cannot project the configuration to contact, for effector : ", eeName
return state.q()
if projectCOM:
success = projectCoMInSupportPolygon(state)
if not success:
print "cannot project com to the middle of the support polygon."
phase.reference_configurations[0] = np.matrix(state.q()).T
return state.q()
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