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