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, 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 display_stones_lock(self):
"""
Wait for the lock to access to gepetto-gui and then display the stepping stones for the current contact_sequence
"""
logger.info("Waiting lock to display stepping stones ...")
self.viewer_lock.acquire()
logger.info("Display stepping stones ...")
displaySteppingStones(self.cs, self.gui, "world",
self.cfg.Robot) # FIXME: change world if changed in pinocchio ...
logger.info("Display done.")
self.viewer_lock.release()
def loadMotionFromFiles(gui, path, npzFilename, csFilename):
# load cs from file :
cs = ContactSequenceHumanoid(0)
cs.loadFromBinary(path + csFilename)
displaySteppingStones(cs, gui, sceneName, Robot)
colors = [[0., 0., 1., 1.], [0., 1., 0., 1.]]
displayCOMTrajectory(cs, gui, sceneName, colors)
#extract data from npz archive :
res = wb_res.loadFromNPZ(path + npzFilename)
displayFeetTrajFromResult(gui, sceneName, res, Robot)
plot.plotALLFromWB(cs, res)
return res, cs
def run_contact_generation(self):
print("### MLP : contact sequence ###")
generate_contact_sequence, ContactGenerationOutputs = self.cfg.get_contact_generation_method()
self.cs, self.fullBody, self.viewer = generate_contact_sequence(self.cfg)
ContactGenerationOutputs.assertRequirements(self.cs)
self.gui = self.viewer.client.gui
if self.cfg.WRITE_STATUS:
with open(self.cfg.STATUS_FILENAME, "a") as f:
f = open(self.cfg.STATUS_FILENAME, "a")
f.write("gen_cs_success: True\n")
if self.cfg.DISPLAY_CS_STONES:
display_tools.displaySteppingStones(self.cs, self.gui, self.viewer.sceneName, self.cfg.Robot)
if self.cfg.DISPLAY_CS:
if display_tools.DisplayContactSequenceRequirements.checkAndFillRequirements(self.cs, self.cfg, self.fullBody):
input("Press Enter to display the contact sequence ...")
display_tools.displayContactSequence(self.viewer, self.cs)
from mlp.utils.cs_tools import addPhaseFromConfig
import multicontact_api
from multicontact_api import ContactSequence
import mlp.viewer.display_tools as display_tools
from pinocchio import SE3
from numpy import array
from mlp.utils.cs_tools import walk
from solo_rbprm.solo import Robot # change robot here
multicontact_api.switchToNumpyArray()
ENV_NAME = "multicontact/ground"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
gui = v.client.gui
sceneName = v.sceneName
cs = ContactSequence(0)
limbs = fb.limbs_names
#Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
addPhaseFromConfig(fb, cs, q_ref, limbs)
walk(fb, cs, 0.5, 0.1, limbs, first_half_step=False)
display_tools.displaySteppingStones(cs, gui, sceneName, fb)
filename = "solo_flatGround.cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
cs, fullBody, viewer = generateContactSequence()
if cfg.WRITE_STATUS:
f = open(cfg.STATUS_FILENAME, "a")
f.write("gen_cs_success: True\n")
f.close()
if cfg.DISPLAY_CS:
raw_input("Press Enter to display the contact sequence ...")
display_tools.displayContactSequence(viewer, cs)
if cfg.SAVE_CS:
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + ".cs"
print "Write contact sequence binary file : ", filename
cs.saveAsBinary(filename)
if cfg.DISPLAY_CS_STONES:
display_tools.displaySteppingStones(cs, viewer.client.gui,
viewer.sceneName, cfg.Robot)
print "------------------------------"
print "### MLP : centroidal, initial Guess ###"
from mlp.centroidal import generateCentroidalInitGuess
cs_initGuess = generateCentroidalInitGuess(cs,
fullBody=fullBody,
viewer=viewer)
if cfg.DISPLAY_INIT_GUESS_TRAJ and cs_initGuess:
colors = [viewer.color.red, viewer.color.yellow]
display_tools.displayCOMTrajectory(cs_initGuess, viewer.client.gui,
viewer.sceneName, colors, "_init")
print "------------------------------"
print "### MLP : centroidal ###"
def generateContactSequence():
RF, root_init, pb, coms, footpos, allfeetpos, res = runLPScript()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, False)
q_init = fb.referenceConfig[::] + [0] * 6
q_init[0:7] = root_init
#q_init[2] += fb.referenceConfig[2] - 0.98 # 0.98 is in the _path script
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 = ContactSequenceHumanoid(0)
addPhaseFromConfig(fb, v, 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.contact_phases[-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] += 0.005 # FIXME it shouldn't be required !!
# compute desired foot rotation :
if USE_ORIENTATION:
if pId < len(pb["phaseData"]) - 1:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.LF_patch.placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.RF_patch.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 = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.matrix(pos).T
placement.rotation = rot.matrix()
moveEffectorToPlacement(fb,
v,
cs,
movingID,
placement,
initStateCenterSupportPolygon=True)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = fb.referenceConfig[::] + [0] * 6
p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
for i in range(3):
q_end[i] += p_end[i]
setFinalState(cs, q=q_end)
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
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
