def generate_contact_sequence_sl1m(cfg):
#RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript(cfg)
RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(cfg)
multicontact_api.switchToNumpyArray()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, True)
q_init = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
q_init[0:7] = root_init
if cfg.SL1M_USE_MIP:
# if MIP is used, allfeetpos contains a list of all position and not just the new position
feet_height_init = allfeetpos[0][0][2]
else:
feet_height_init = allfeetpos[0][2]
logger.info("feet height initial = %s", feet_height_init)
#q_init[2] = feet_height_init + cfg.IK_REFERENCE_CONFIG[2]
#q_init[2] += EPS_Z
#q_init[2] = fb.referenceConfig[2] # 0.98 is in the _path script
if v:
v(q_init)
cs = build_cs_from_sl1m(cfg.SL1M_USE_MIP, fb, cfg.IK_REFERENCE_CONFIG, root_end, pb, RF, allfeetpos,
cfg.SL1M_USE_ORIENTATION, cfg.SL1M_USE_INTERPOLATED_ORIENTATION, q_init=q_init)
if cfg.DISPLAY_CS_STONES:
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
def addPhaseFromConfig(fb, cs, q, limbsInContact, t_init = -1):
"""
Add a new 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 cs: the current contact sequence
: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
"""
multicontact_api.switchToNumpyArray() # FIXME : why is it required to add it again here ?
cs.append(createPhaseFromConfig(fb, q, limbsInContact, t_init))
import numpy as np
import mlp.config as cfg
import multicontact_api
from multicontact_api import ContactPhase, ContactSequence
from mlp.utils.util import createFullbodyStatesFromCS
from mlp.utils.cs_tools import connectPhaseTrajToFinalState, setInitialFromFinalValues, copyPhaseInitToFinal
from mlp.utils.requirements import Requirements
multicontact_api.switchToNumpyArray()
class Inputs(Requirements):
timings = True
consistentContacts = True
configurationValues = True
class Outputs(Inputs):
COMtrajectories = True
COMvalues = True
## Produce a centroidal trajectory where the CoM only move when the contacts are fixed.
## It is then fixed during the swing phase where one contact is repositionned.
## The position reached by the CoM are given with 2-PAC
## The trajectory of the CoM is a quintic spline with initial and final velocity/acceleration constrained to 0
def getTargetCOMPosition(fullBody, id_state):
print("call 2-pac for ids : "+str(id_state)+ " ; "+str(id_state+1))
tab = fullBody.isReachableFromState(id_state, id_state + 1, True, False)
print("tab results : ",tab)
success = tab[0]
assert success, "2-pac failed for state id : " + str(id_state)
if len(tab) == 7:
def addPhaseFromConfig(fb, cs, q, limbsInContact, t_init=-1):
multicontact_api.switchToNumpyArray(
) # FIXME : why is it required to add it again here ?
cs.append(createPhaseFromConfig(fb, q, limbsInContact, t_init))
def generateContactSequence():
#RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript()
RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(
)
multicontact_api.switchToNumpyArray()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, True)
q_init = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
q_init[0:7] = root_init
feet_height_init = allfeetpos[0][2]
print("feet height initial = ", feet_height_init)
q_init[2] = feet_height_init + cfg.IK_REFERENCE_CONFIG[2]
q_init[2] += EPS_Z
#q_init[2] = fb.referenceConfig[2] # 0.98 is in the _path script
if v:
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if cfg.SL1M_USE_ORIENTATION:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if pId < len(pb["phaseData"]) - 1:
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
else:
quat1 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if cfg.SL1M_USE_INTERPOLATED_ORIENTATION:
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.contactPatch(
fb.lfoot).placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.contactPatch(
fb.rfoot).placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = quat0 # rotation of the root, from the guide
else:
rot = quat0 # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
print("feet height final = ", feet_height_end)
q_end[2] = feet_height_end + cfg.IK_REFERENCE_CONFIG[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
if cfg.DISPLAY_CS_STONES:
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
