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 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()
0.4,
-0.005, #Right Arm
0.0,
0.0
] + [0] * 6
q_ref[0:2] = [-3., -1.7575]
v(q_ref) # move root position
q_ref[2] -= 0.015
fb.setReferenceConfig(q_ref)
fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6)
# create hand contacts :
pRH = SE3.Identity()
pRH = rotatePlacement(pRH, 'y', -0.8115781021773631)
n = computeContactNormal(pRH).tolist()
state = State(fb, q=q_ref, limbsIncontact=[fb.rLegId, fb.lLegId])
pLHand = [-2.75, -1.235, 1.02]
pRHand = [-2.75, -2.28, 1.02]
"""
from hpp.corbaserver.rbprm.tools.display_tools import *
createSphere('s',v)
moveSphere('s',v,pLHand)
"""
state, success = StateHelper.addNewContact(state,
fb.rArmId,
pRHand,
n,
lockOtherJoints=True)
def zeroStepCapturability(phase, cfg):
"""
Try to compute a centroidal trajectory that bring the given phase to a complete stop without contact changes.
I take the initial centroidal state and the duration from the state. As long as the contacts.
If successfull, it fill the final centroidal state in the phase and the centroidal trajectories
:param phase: the current ContactPhase, must have contacts and initial centroidal values defined
:return: [success, phase]
"""
# initialize the equilibrium lib
eq = Equilibrium("equilibrium", cfg.MASS, 4, SolverLP.SOLVER_LP_QPOASES, True, 10, True)
# set the contacts from the phase:
for eeName in phase.effectorsInContact():
placement = phase.contactPatch(eeName).placement
size_feet = cfg.IK_eff_size[eeName][::]
size_feet[0] -= 2. * MARGIN
size_feet[1] -= 2. * MARGIN
points = buildRectangularContactPoints(size_feet,cfg.Robot.dict_offset[eeName])
positions = np.zeros([4,3])
for i in range(4):
positions[i,:] = (placement.rotation @ points[:,i]).T + placement.translation
normals = array([computeContactNormal(placement)] * 4)
# one position or normal per line of the matrix ! (opposite convention to the mlp package)
eq.setNewContacts(positions, normals, cfg.MU, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
# try to solve the 0 step capturability :
# The boolean set if we use the angular moment or not, -1 is used to set the continuous mode
if USE_AM:
L0 = phase.L_init
else:
L0 = np.zeros(3)
print("Initial com : ", phase.c_init)
print("Initial AM : ", phase.L_init)
#print("phase duration used :", phase.duration )
res = com_traj.zeroStepCapturability(eq, phase.c_init, phase.dc_init, L0, USE_AM, phase.duration, -1)
if not res.success:
return False, phase
#print("zero step, res.x : ", res.x)
print("zero step, com final : ", res.c_of_t(res.c_of_t.max()))
# save the results inside the phase
# update the definition interval of the bezier curve :
wp = res.c_of_t.waypoints()
phase.c_t = bezier(wp, phase.timeInitial, phase.timeFinal)
phase.dc_t = phase.c_t.compute_derivate(1)
phase.ddc_t = phase.c_t.compute_derivate(2)
wp = res.dL_of_t.waypoints()
# see https://github.com/humanoid-path-planner/hpp-bezier-com-traj/blob/v4.8.0/src/solve_0_step.cpp#L208
# the wp must be devided by the duration to get the same results
wp /= phase.duration
phase.dL_t = bezier(wp, phase.timeInitial, phase.timeFinal)
wp = phase.dL_t.compute_primitive(1).waypoints()
wp *= phase.duration
for i in range(wp.shape[1]):
wp[:,i] += phase.L_init
phase.L_t = bezier(wp, phase.timeInitial, phase.timeFinal)
# set the final centroidal values from the trajectory:
tmax = phase.timeFinal
phase.c_final = phase.c_t(tmax)
phase.dc_final = phase.dc_t(tmax)
phase.ddc_final = phase.ddc_t(tmax)
phase.L_final = phase.L_t(tmax)
phase.dL_final = phase.dL_t(tmax)
return True, phase