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 create_stairs_cs():
ENV_NAME = "multicontact/bauzil_stairs"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
cs = ContactSequence(0)
# Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
q_ref[0:2] = [0.07, 1.2]
addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId])
step_height = 0.1
step_width = 0.3
displacement = SE3.Identity()
displacement.translation = array([step_width, 0, step_height])
cs.moveEffectorOf(fb.rfoot, displacement)
cs.moveEffectorOf(fb.lfoot, displacement)
q_end = q_ref[::]
q_end[0] += step_width
q_end[2] += step_height
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, array(com), q=q_end)
return cs
def generate_contact_sequence_load(cfg):
fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
cs = ContactSequence(0)
print("Import contact sequence binary file : ", cfg.CS_FILENAME)
cs.loadFromBinary(cfg.CS_FILENAME)
display_tools.displayWBconfig(v, cs.contactPhases[0].q_init)
return cs, fb, v
def generateContactSequence():
fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
cs = ContactSequenceHumanoid(0)
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + ".cs"
print "Import contact sequence binary file : ", filename
cs.loadFromBinary(filename)
display_tools.displayWBconfig(
v, cs.contact_phases[0].reference_configurations[0])
return cs, fb, v
from mlp.utils.cs_tools import addPhaseFromConfig, setFinalState import multicontact_api from multicontact_api import ContactSequence, ContactPatch import mlp.viewer.display_tools as display_tools from pinocchio import SE3 from numpy import array from talos_rbprm.talos import Robot # change robot here multicontact_api.switchToNumpyArray() ENV_NAME = "multicontact/ground" # Build the robot object and the viewer fb, v = display_tools.initScene(Robot, ENV_NAME, False) gui = v.client.gui sceneName = v.sceneName # display the origin and x,y,z axis in the viewer : v.addLandmark(sceneName, 1) #initialize an empty contact sequence cs = ContactSequence(0) # start from the reference configuration of the robot : q_ref = fb.referenceConfig[::] + [0] * 6 #q_ref[0:2] = [ , ] # change the x,y position of the base as needed # q_ref[2] += # the z position of the base already correspond to a floor at z=0, change it accordingly to the environment # q_ref[3:7] = # quaternion (x,y,z) of the base # display the configuration v(q_ref) com = array(fb.getCenterOfMass())
def generateContactSequence():
fb, v = initScene(cfg.Robot, cfg.ENV_NAME)
return None, fb, v
def __init__(self, cfg):
# Disable most of the automatic display that could not be updated automatically when motion is replanned
cfg.DISPLAY_CS = False
cfg.DISPLAY_CS_STONES = True
cfg.DISPLAY_SL1M_SURFACES = False
cfg.DISPLAY_INIT_GUESS_TRAJ = False
cfg.DISPLAY_WP_COST = False
cfg.DISPLAY_COM_TRAJ = False
cfg.DISPLAY_FEET_TRAJ = False
cfg.DISPLAY_ALL_FEET_TRAJ = False
cfg.DISPLAY_WB_MOTION = False
cfg.IK_SHOW_PROGRESS = False
cfg.centroidal_initGuess_method = "none" # not supported by this class yet
cfg.ITER_DYNAMIC_FILTER = 0 # not supported by this class yet
cfg.CHECK_FINAL_MOTION = False
# Disable computation of additionnal data to speed up the wholebody computation time
cfg.IK_store_centroidal = False # c,dc,ddc,L,dL (of the computed wholebody motion)
cfg.IK_store_zmp = False
cfg.IK_store_effector = False
cfg.IK_store_contact_forces = False
cfg.IK_store_joints_derivatives = True
cfg.IK_store_joints_torque = False
cfg.EFF_CHECK_COLLISION = False # WIP: disable limb-rrt
cfg.contact_generation_method = "sl1m" # Reactive planning class is specific to SL1M for now
# Store the specific settings for connecting the initial/goal points as they may be changed
cfg.Robot.DEFAULT_COM_HEIGHT += cfg.COM_SHIFT_Z
self.previous_com_shift_z = cfg.COM_SHIFT_Z
self.previous_time_shift_com = cfg.TIME_SHIFT_COM
cfg.COM_SHIFT_Z = 0.
cfg.TIME_SHIFT_COM = 0.
self.previous_connect_goal = cfg.DURATION_CONNECT_GOAL
cfg.DURATION_CONNECT_GOAL = 0.
self.TIMEOPT_CONFIG_FILE = cfg.TIMEOPT_CONFIG_FILE
super().__init__(cfg)
# Get the centroidal and wholebody methods selected in the configuraton file
self.generate_centroidal, self.CentroidalInputs, self.CentroidalOutputs = self.cfg.get_centroidal_method()
self.generate_effector_trajectories, self.EffectorInputs, self.EffectorOutputs = \
self.cfg.get_effector_initguess_method()
self.generate_wholebody, self.WholebodyInputs, self.WholebodyOutputs = self.cfg.get_wholebody_method()
# define members that will stores processes and queues
self.process_compute_cs = None
self.process_centroidal = None
self.process_wholebody = None
self.process_viewer = None
self.process_gepetto_gui = None
self.pipe_cs_in = None
self.pipe_cs_out = None
self.pipe_cs_com_in = None
self.pipe_cs_com_out = None
self.queue_qt = None
self.viewer_lock = Lock() # lock to access gepetto-gui API
self.loop_viewer_lock = Lock() # lock to guarantee that only one loop_viewer is executed at any given time
self.last_phase_lock = Lock() # lock to read/write last_phase data
self.last_phase_pickled = Array(c_ubyte,
MAX_PICKLE_SIZE) # will contain the last contact phase send to the viewer
self.last_phase = None
self.stop_motion_flag = Value(c_bool) # true if a stop is requested
self.last_phase_flag = Value(c_bool) # true if the last phase have changed
self.cfg.Robot.minDist = 0.7 # See bezier-com-traj doc,
# minimal distance between the CoM and the contact points along the Z axis
# initialize the guide planner class:
self.client_hpp = None
self.robot = None
self.gui = None
self.pyb_sim = None
self.guide_planner = self.init_guide_planner()
# initialize a fullBody rbprm object
self.fullBody, _ = initScene(cfg.Robot, cfg.ENV_NAME, context="fullbody")
self.fullBody.setCurrentConfig(cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6)
self.current_root_goal = []
self.current_guide_id = 0
# Set up gepetto gui and a pinocchio robotWrapper with display
self.init_viewer()
# Set up a pybullet environment:
if USE_PYB:
self.init_pybullet()
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
