def compute_from_cs(self): """ Split the complete ContactSequence stored in self.cs and send each subsequence in the pipe_cs """ pid_centroidal = 0 last_iter_centroidal = False logger.info("## Compute from cs, size = %d", self.cs.size()) last_phase = self.get_last_phase() if last_phase: tools.setFinalFromInitialValues(last_phase, self.cs.contactPhases[0]) while pid_centroidal + 5 < self.cs.size(): logger.debug("## Current pid = %d", pid_centroidal) if pid_centroidal + 7 >= self.cs.size(): logger.debug("## Last centroidal iter") # last iter, take all the remaining phases num_phase = self.cs.size() - pid_centroidal last_iter_centroidal = True else: num_phase = 5 logger.debug("## Num phase = %d", num_phase) # Extract the phases [pid_centroidal; pid_centroidal +num_phases] from cs_full cs_iter = ContactSequence(0) for i in range(pid_centroidal, pid_centroidal + num_phase): logger.debug("-- Add phase : %d", i) cs_iter.append(self.cs.contactPhases[i]) self.pipe_cs_in.send([cs_iter, last_iter_centroidal]) # This call may be blocking if the pipe is full pid_centroidal += 2 self.pipe_cs_in.close()
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_effector_trajectories_for_sequence_load(cfg, cs, fullBody=None): cs_ref = ContactSequence() filename = cfg.REF_FILENAME print("Load contact sequence with end effector trajectories from : ", filename) cs_ref.loadFromBinary(filename) return cs_ref
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 generate_effector_trajectories_for_sequence(cfg, cs, generate_end_effector_traj, fullBody = None): """ Generate an effector trajectory for each effectors which are going to be in contact in the next phase :param cfg: an instance of the configuration class :param cs: the contact sequence :param generate_end_effector_traj: a pointer to the method used to generate an end effector trajectory for one phase :param fullBody: an instance of rbprm FullBody :return: a new contact sequence, containing the same data as the one given as input plus the effector trajectories for each swing phases """ cs_res = ContactSequence(cs) effectors = cs_res.getAllEffectorsInContact() previous_phase = None for pid in range(cs_res.size()-1): # -1 as last phase never have effector trajectories phase = cs_res.contactPhases[pid] next_phase = cs_res.contactPhases[pid+1] if pid > 0 : previous_phase = cs_res.contactPhases[pid-1] for eeName in effectors: if not phase.isEffectorInContact(eeName) and next_phase.isEffectorInContact(eeName): # eeName will be in compute in the next phase, a trajectory should be added in the current phase placement_end = next_phase.contactPatch(eeName).placement time_interval = [phase.timeInitial, phase.timeFinal] if previous_phase is not None and previous_phase.isEffectorInContact(eeName): placement_init = previous_phase.contactPatch(eeName).placement else: placement_init = effectorPlacementFromPhaseConfig(phase,eeName,fullBody) # build the trajectory : traj = generate_end_effector_traj(cfg, time_interval,placement_init,placement_end) phase.addEffectorTrajectory(eeName,traj) return cs_res
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None): cs_res = ContactSequence(0) cs_res.loadFromBinary(cfg.COM_FILENAME) print("Import contact sequence binary file : ", cfg.COM_FILENAME) return cs_res
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None): rp = RosPack() urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' logger.info("load robot : %s", urdf) robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) logger.info("robot loaded.") cs_wb = ContactSequence() logger.warning("Load wholebody contact sequence from file : %s", cfg.WB_FILENAME) cs_wb.loadFromBinary(cfg.WB_FILENAME) return cs_wb, robot
def compute_wholebody(self, robot, cs_com, last_q=None, last_v=None, last_iter=False): """ Compute the wholebody motion for the given ContactSequence :param robot: a TSID RobotWrapper instance :param cs_com: a ContactSequence with centroidal trajectories :param last_q: the last wholebody configuration (used as Initial configuration for this iteration) :param last_v: the last joint velocities vector (used as initial joint velocities for this iteration) :param last_iter: if True, the complete ContactSequence is used, if False only the first 2 phases are used :return: a ContactSequence with wholebody data, the last wholebody configuration, the last joint velocity, the last phase, the TSID RobotWrapper used """ if not self.EffectorInputs.checkAndFillRequirements(cs_com, self.cfg, self.fullBody): raise RuntimeError( "The current contact sequence cannot be given as input to the end effector method selected.") # Generate end effector trajectories for the contactSequence cs_ref_full = self.generate_effector_trajectories(self.cfg, cs_com, self.fullBody) # EffectorOutputs.assertRequirements(cs_ref_full) # Split contactSequence if it is not the last iteration if last_iter: cs_ref = cs_ref_full last_phase = cs_com.contactPhases[-1] else: cs_cut = ContactSequence() for i in range(2): cs_cut.append(cs_ref_full.contactPhases[i]) cs_ref = cs_cut # last_phase should be a double support phase, it should contains all the contact data: last_phase = cs_com.contactPhases[2] tools.setFinalFromInitialValues(last_phase, last_phase) if last_q is not None: cs_ref.contactPhases[0].q_init = last_q if last_v is not None: t_init = cs_ref.contactPhases[0].timeInitial cs_ref.contactPhases[0].dq_t = polynomial(last_v.reshape(-1, 1), t_init, t_init) ### Generate the wholebody trajectory: update_root_traj_timings(cs_ref) if not self.WholebodyInputs.checkAndFillRequirements(cs_ref, self.cfg, self.fullBody): raise RuntimeError( "The current contact sequence cannot be given as input to the wholeBody method selected.") cs_wb, robot = self.generate_wholebody(self.cfg, cs_ref, robot=robot) logger.info("-- compute whole body END") # WholebodyOutputs.assertRequirements(cs_wb) # Retrieve the last phase, q, and v from this outputs: last_phase_wb = cs_wb.contactPhases[-1] last_q = last_phase_wb.q_t(last_phase_wb.timeFinal) last_v = last_phase_wb.dq_t(last_phase_wb.timeFinal) tools.deletePhaseCentroidalTrajectories(last_phase) # Remove unnecessary data to reduce serialized size last_phase.q_final = last_q last_phase.dq_t = polynomial(last_v.reshape(-1, 1), last_phase.timeFinal, last_phase.timeFinal) #last_phase.c_final = last_phase_wb.c_final #last_phase.dc_final = last_phase_wb.dc_final #last_phase.L_final = last_phase_wb.L_final return cs_wb, last_q, last_v, last_phase, robot
def contactSequenceFromRBPRMConfigs(fb, beginId, endId): """ Build a multicontact_api ContactSequence from rbprm states list :param fb: an instance of rbprm.FullBody :param beginId: the first state Id :param endId: the last state Id :return: a multicontact_api ContactSequence """ logger.warning("generate contact sequence from planning : ") n_states = endId - beginId + 1 # There could be either contact break, creation or repositionning between each adjacent states. # But there should be only contacts break or creation between each adjacent contactPhases cs = ContactSequence(0) prev_phase = None phaseId = 0 # create initial ContactPhase cs.append(createPhaseFromRBPRMState(fb, beginId)) for stateId in range(beginId + 1, endId + 1): # from the second state to the last one logger.info("current state id = %d", stateId) previous_phase = cs.contactPhases[-1] eeName, variationType = getContactVariationBetweenStates( fb, stateId - 1, stateId) if eeName is not None: if variationType == VariationType.REPOSITIONNED: # in case of repositionning, the centroidal motion will happend in the next intermediate phase, # and thus the previous_phase will not move : copyPhaseInitToFinal(previous_phase) else: # set the final values of the previous phase to be the current one : setPhaseFinalValues(fb, stateId, previous_phase) if variationType == VariationType.BROKEN: # remove the given contact : cs.breakContact(eeName) else: # get placement of the new contact from the planning : contact_placement = contactPlacementFromRBPRMState( fb, stateId, eeName) if variationType == VariationType.CREATED: # create a new contact : cs.createContact(eeName, ContactPatch(contact_placement)) elif variationType == VariationType.REPOSITIONNED: # move existing contact to new placement : cs.moveEffectorToPlacement(eeName, contact_placement) # set the initial values for the current phase from planning : setPhaseInitialValues(fb, stateId, cs.contactPhases[-1]) # set the same values as the final ones for the intermediate state created : setPhaseFinalValues(fb, stateId, cs.contactPhases[-2]) # else : no contact changes, ignore this state setPhaseFinalValues(fb, endId, cs.contactPhases[-1]) return cs
def generate_centroidal_load(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter=True): cs_res = ContactSequence(0) cs_res.loadFromBinary(cfg.COM_FILENAME) logger.warning("Import contact sequence binary file : %s", cfg.COM_FILENAME) return cs_res
def test_com_motion_above_feet_COM(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs")) self.assertEqual(cs.size(), 1) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) checkCS(self, cs)
def test_step_in_place(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "step_in_place.cs")) self.assertEqual(cs.size(), 9) self.assertTrue(cs.haveConsistentContacts()) self.assertFalse(cs.haveFriction()) self.assertFalse(cs.haveContactModelDefined())
def test_walk_20cm_quasistatic(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs")) self.assertEqual(cs.size(), 23) self.assertFalse(cs.haveFriction()) self.assertFalse(cs.haveContactModelDefined()) self.assertTrue(cs.haveConsistentContacts())
def generateCentroidalTrajectory(cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter=True): """ Generate straight line trajectories from the center of the support polygon of one phase to the center in the next phase. Compute trajectories as Quintic splines, starting and ending with null acceleration and velocity at each phase change Add a null angular momentum. :param cs: :param cs_initGuess: :param fullBody: :param viewer: :return: """ if cs_initGuess: print("WARNING : in centroidal.geometric, initial guess is ignored.") if not first_iter: print( "WARNING : in centroidal.geometric, it is useless to iterate several times." ) if cs.haveCOMvalues(): # do not overwrite com values in input sequence cs_result = ContactSequence(cs) computePhasesCOMValues(cs_result, cfg.Robot.DEFAULT_COM_HEIGHT, overwrite=True) else: # add them in output sequence if not present computePhasesCOMValues(cs, cfg.Robot.DEFAULT_COM_HEIGHT) cs_result = ContactSequence(cs) for pid, phase in enumerate(cs_result.contactPhases): genCOMTrajFromPhaseStates(phase) genAMTrajFromPhaseStates(phase) return cs_result
def compute_centroidal(self, cs, previous_phase, last_iter=False): """ Solve the centroidal problem for the given ContactSequence :param cs: the ContactSequence used :param previous_phase: If provided, copy the final data of this phase as initial data for the given ContactSequence :param last_iter: If True, return a ContactSequence corresponding to the complete contactSequence given as input If False, the result is splitted and only the first 3 phases are returned :return: The ContactSequence with centroidal trajectories, and the last phase """ # update the initial state with the data from the previous intermediate state: if previous_phase: tools.setInitialFromFinalValues(previous_phase, cs.contactPhases[0]) self.cfg.COM_SHIFT_Z = 0. self.cfg.TIME_SHIFT_COM = 0. #else: # self.cfg.COM_SHIFT_Z = self.previous_com_shift_z # self.cfg.TIME_SHIFT_COM = self.previous_time_shift_com if last_iter: # Set settings specific to the last iteration that need to connect exactly to the final goal position self.cfg.DURATION_CONNECT_GOAL = self.previous_connect_goal self.cfg.TIMEOPT_CONFIG_FILE = self.TIMEOPT_CONFIG_FILE else: # Set settings for the middle of the sequence: do not need to connect exactly to the goal self.cfg.DURATION_CONNECT_GOAL = 0. self.cfg.TIMEOPT_CONFIG_FILE = self.TIMEOPT_CONFIG_FILE.rstrip(".yaml") + "_lowgoal.yaml" if not self.CentroidalInputs.checkAndFillRequirements(cs, self.cfg, None): raise RuntimeError( "The current contact sequence cannot be given as input to the centroidal method selected.") cs_full = self.generate_centroidal(self.cfg, cs, None, None) # CentroidalOutputs.assertRequirements(cs_full) if last_iter: return cs_full, None else: cs_cut = ContactSequence(0) for i in range(3): cs_cut.append(cs_full.contactPhases[i]) return cs_cut, cs_cut.contactPhases[1]
def generate_centroidal_quasistatic(cfg, cs, cs_initGuess=None, fullBody=None, viewer=None, first_iter=True): if cs_initGuess: logger.warning("Initial guess is ignored.") if not fullBody: raise ValueError("quasiStatic called without fullBody object.") if not first_iter: logger.warning("It is useless to iterate several times.") beginId, endId = createFullbodyStatesFromCS(cs, fullBody) logger.info("beginid = %d", beginId) logger.info("endId = %d", endId) cs_result = ContactSequence(cs) # for each phase in the cs, create a corresponding FullBody State and call 2-PAC, # then fill the cs struct with a quintic spline connecting the two points found id_phase = 0 for id_state in range(beginId, endId + 1): logger.debug("id_state = %d", id_state) logger.debug("id_phase = %d", id_phase) phase_fixed = cs_result.contactPhases[ id_phase] # phase where the CoM move and the contacts are fixed # set initial state to be the final one of the previous phase : if id_phase > 1: setInitialFromFinalValues(phase_swing, phase_fixed) # compute 'optimal' position of the COM to go before switching phase: if id_state == endId: c = phase_fixed.c_final else: c = getTargetCOMPosition(fullBody, id_state, cfg.COM_SHIFT_Z) # set 'c' the final position of current phase : phase_fixed.c_final = c phase_fixed.dc_final = np.zeros(3) phase_fixed.ddc_final = np.zeros(3) connectPhaseTrajToFinalState(phase_fixed) id_phase += 1 if id_state < endId: phase_swing = cs_result.contactPhases[ id_phase] # phase where the CoM is fixed and an effector move # in swing phase, com do not move : setInitialFromFinalValues(phase_fixed, phase_swing) copyPhaseInitToFinal(phase_swing) connectPhaseTrajToFinalState(phase_swing) id_phase += 1 return cs_result
def generateEffectorTrajectoriesForSequence(cs, fullBody=None): """ Generate an effector trajectory for each effectors which are going to be in contact in the next phase :param cs: :return: """ cs_res = ContactSequence(cs) effectors = cs_res.getAllEffectorsInContact() previous_phase = None for pid in range( cs_res.size() - 1): # -1 as last phase never have effector trajectories phase = cs_res.contactPhases[pid] next_phase = cs_res.contactPhases[pid + 1] if pid > 0: previous_phase = cs_res.contactPhases[pid - 1] for eeName in effectors: if not phase.isEffectorInContact( eeName) and next_phase.isEffectorInContact(eeName): # eeName will be in compute in the next phase, a trajectory should be added in the current phase placement_end = next_phase.contactPatch(eeName).placement time_interval = [phase.timeInitial, phase.timeFinal] if previous_phase is not None and previous_phase.isEffectorInContact( eeName): placement_init = previous_phase.contactPatch( eeName).placement else: placement_init = effectorPlacementFromPhaseConfig( phase, eeName, fullBody) # build the trajectory : traj = generateEndEffectorTraj(time_interval, placement_init, placement_end) phase.addEffectorTrajectory(eeName, traj) return cs_res
def copyEffectorTrajectories(cs_eff, cs): """ Copy all the effector trajectories contained in cs_eff in a copy of cs :param cs_eff: the contact sequence containing the effector trajectories :param cs: the contact sequence to copy :return: A new ContactSequence, which is a copy of cs with the addition of the trajectories from cs_eff Return None if the phases do not have the same duration in cs_eff and cs """ if cs_eff.size() != cs.size(): return None cs_res = ContactSequence(cs) for pid, phase_eff in enumerate(cs_eff.contactPhases): if len(phase_eff.effectorsWithTrajectory()) > 0: phase = cs_res.contactPhases[pid] if phase.timeInitial != phase_eff.timeInitial or phase.timeFinal != phase_eff.timeFinal: print("Unable to copy effector trajectories, the phase duration have changed") return None for eeName, traj in phase_eff.effectorTrajectories().items(): phase.addEffectorTrajectory(eeName, traj) return cs_res
def store_mcapi_traj(self, traj_name): if not mcapi_import: print( 'multicontact_api package import has failed, check your install' ) return self.set_data_lst_as_arrays() # trajectory with only one ContactPhase (simpler to read/write) # when feet not in contact, the force is exactly zero, that's the only diff cs = ContactSequence() cp = ContactPhase() # assign trajectories : t_arr = self.data_log['t'] cp.timeInitial = t_arr[0] cp.timeFinal = t_arr[-1] cp.duration = t_arr[-1] - t_arr[0] # col number of trajectories should be the time traj size hence the transpose cp.q_t = piecewise.FromPointsList(self.data_log['q'].T, t_arr) cp.dq_t = piecewise.FromPointsList(self.data_log['v'].T, t_arr) cp.ddq_t = piecewise.FromPointsList(self.data_log['dv'].T, t_arr) cp.tau_t = piecewise.FromPointsList(self.data_log['tau'].T, t_arr) cp.c_t = piecewise.FromPointsList(self.data_log['c'].T, t_arr) cp.dc_t = piecewise.FromPointsList(self.data_log['dc'].T, t_arr) cp.L_t = piecewise.FromPointsList(self.data_log['Lc'].T, t_arr) # contact force trajectories for i_foot, frame_name in enumerate(self.contact_names): cp.addContact(frame_name, ContactPatch( pin.SE3(), 0.5)) # dummy placement and friction coeff cp.addContactForceTrajectory( frame_name, piecewise.FromPointsList(self.data_log['f{}'.format(i_foot)].T, t_arr)) cs.append(cp) # only one contact phase savepath = os.path.join(self.directory, traj_name + '.cs') cs.saveAsBinary(savepath) print('Saved ' + savepath)
def compute_stopping_cs(self, move_to_support_polygon=True): """ Compute a Contact Sequence with centroidal trajectories to bring the current last_phase to a stop without contact changes :param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon :return: """ phase_stop = ContactPhase(self.get_last_phase()) tools.setInitialFromFinalValues(phase_stop, phase_stop) phase_stop.timeInitial = phase_stop.timeFinal phase_stop.duration = DURATION_0_STEP # FIXME !! # try 0-step: success, phase = zeroStepCapturability(phase_stop, self.cfg) if success: cs_ref = ContactSequence(0) cs_ref.append(phase) # TEST : add another phase to go back in the center of the support polygon if move_to_support_polygon: phase_projected = ContactPhase() phase_projected.timeInitial = phase.timeFinal phase_projected.duration = DURATION_0_STEP tools.copyContactPlacement(phase, phase_projected) tools.setInitialFromFinalValues(phase, phase_projected) phase_projected.c_final = tools.computeCenterOfSupportPolygonFromPhase( phase_stop, self.fullBody.DEFAULT_COM_HEIGHT) #FIXME 'default height' tools.connectPhaseTrajToFinalState(phase_projected) cs_ref.append(phase_projected) else: # TODO try 1 step : raise RuntimeError("One step capturability not implemented yet !") tools.computeRootTrajFromContacts(self.fullBody, cs_ref) self.last_phase = cs_ref.contactPhases[-1].copy() # define the final root position, translation from the CoM position and rotation from the feet rotation q_final = np.zeros(7) q_final[:3] = self.last_phase.c_final[::] placement_rot_root, _ = tools.rootOrientationFromFeetPlacement(self.cfg.Robot, None, self.last_phase, None) quat_root = Quaternion(placement_rot_root.rotation) q_final[3:7] = [quat_root.x, quat_root.y, quat_root.z, quat_root.w] self.last_phase.q_final = q_final self.last_phase_flag.value = False self.last_phase_pickled = Array(c_ubyte, MAX_PICKLE_SIZE) # reset currently stored whole body last phase return cs_ref
def generate_centroidal_croc(cfg, cs, cs_initGuess=None, fb=None, viewer=None, first_iter=True): if cs_initGuess: logger.warning("Initial guess is ignored.") if not fb: raise ValueError("CROC called without fullBody object.") if not first_iter: logger.warning("It is useless to iterate several times.") beginId, endId = createFullbodyStatesFromCS(cs, fb) logger.info("beginid = %d", beginId) logger.info("endId = %d", endId) cs_result = ContactSequence(cs) resetCOMtrajectories(cs_result) # erase all pre existing CoM trajectories if (2 * (endId - beginId) + 1) != cs.size(): raise NotImplemented( "Current implementation of CROC require to have 2 contact phases per States in fullBody" ) # for each phase in the cs, create a corresponding FullBody State and call CROC, # then fill the cs struct id_phase = 0 current_t = cs.contactPhases[0].timeInitial for id_state in range(beginId, endId): logger.info("id_state = %d", id_state) logger.info("id_phase = %d", id_phase) # First, compute the bezier curves between the states : pid = fb.isDynamicallyReachableFromState(id_state, id_state + 1, True, numPointsPerPhases=0) if len(pid) != 4: logger.error("# Cannot compute CROC between states %d, %d", id_state, id_state + 1) return cs c1 = fb.getPathAsBezier(int(pid[1])) # curve before contact break c2 = fb.getPathAsBezier(int(pid[2])) # curve for swing phase c3 = fb.getPathAsBezier(int(pid[3])) # curve after contact creation # NOTE : this bezier are defined in [0,tmax] ! We need to switch the time intervals # fill the phases with the curves from CROC : # c1 : phase = cs_result.contactPhases[id_phase] setCOMfromCurve(phase, c1) current_t = phase.timeFinal # c2 id_phase += 1 phase = cs_result.contactPhases[id_phase] phase.timeInitial = current_t setCOMfromCurve(phase, c2) current_t = phase.timeFinal # c3 id_phase += 1 phase = cs_result.contactPhases[id_phase] phase.timeInitial = current_t setCOMfromCurve(phase, c3) # pid should not increase here, as the next c1 trajectory will be added to the same phase as the current c3 one. return cs_result
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)
def CSfromMomentumopt(planner_setting, cs, init_state, dyn_states, t_init=0, connect_goal=True): """ Create a ContactSequence and fill it with the results from momentumopt :param planner_setting: :param cs: a multicontact_api ContactSequence from which the contacts are copied :param init_state: initial state used by momentumopt :param dyn_states: the results of momentumopt :return: a multicontact_api ContactSequence with centroidal trajectories """ logger.info("Start to convert result to mc-api ...") cs_com = ContactSequence(cs) MASS = planner_setting.get(mopt.PlannerDoubleParam_RobotMass) p_id = 0 # phase id in cs # dyn_states[0] is at t == dt , not t == 0 ! use init_state for t == 0 p0 = cs_com.contactPhases[0] c_init = init_state.com p0.timeInitial = t_init # init arrays to store the discrete points. one value per columns c_t = c_init.reshape(3, 1) dc_t = p0.dc_init.reshape(3, 1) ddc_t = p0.ddc_init.reshape(3, 1) L_t = p0.L_init.reshape(3, 1) dL_t = p0.dL_init.reshape(3, 1) times = array(t_init) current_t = t_init # loop for all dynamicsStates for k, ds in enumerate(dyn_states): #extract states values from ds : if k == 0: ddc = (ds.lmom / MASS) / ds.dt # acceleration dL = ds.amom / ds.dt # angular momentum variation else: ddc = ((ds.lmom / MASS) - (dyn_states[k - 1].lmom / MASS)) / ds.dt dL = (ds.amom - dyn_states[k - 1].amom) / ds.dt c = ds.com # position dc = ds.lmom / MASS # velocity L = ds.amom # angular momentum # stack the values in the arrays: c_t = append(c_t, c.reshape(3, 1), axis=1) dc_t = append(dc_t, dc.reshape(3, 1), axis=1) ddc_t = append(ddc_t, ddc.reshape(3, 1), axis=1) L_t = append(L_t, L.reshape(3, 1), axis=1) dL_t = append(dL_t, dL.reshape(3, 1), axis=1) current_t += ds.dt times = append(times, current_t) if k > 0 and isNewPhase(dyn_states[k - 1], ds) and p_id < cs_com.size() - 1: #last k of current phase, first k of next one (same trajectories and time) # set the trajectories for the current phase from the arrays : phase = cs_com.contactPhases[p_id] setCOMtrajectoryFromPoints(phase, c_t, dc_t, ddc_t, times, overwriteInit=(p_id > 0)) setAMtrajectoryFromPoints(phase, L_t, dL_t, times, overwriteInit=(p_id > 0)) # set final time : phase.timeFinal = times[-1] # Start new phase : p_id += 1 phase = cs_com.contactPhases[p_id] # set initial time : phase.timeInitial = times[-1] # reset arrays of values to only the last point : c_t = c_t[:, -1].reshape(3, 1) dc_t = dc_t[:, -1].reshape(3, 1) ddc_t = ddc_t[:, -1].reshape(3, 1) L_t = L_t[:, -1].reshape(3, 1) dL_t = dL_t[:, -1].reshape(3, 1) times = times[-1] # set final phase : phase = cs_com.contactPhases[-1] setCOMtrajectoryFromPoints(phase, c_t, dc_t, ddc_t, times, overwriteFinal=not connect_goal) setAMtrajectoryFromPoints(phase, L_t, dL_t, times, overwriteFinal=not connect_goal) # set final time : phase.timeFinal = times[-1] logger.info("Converting results to mc-api done.") return cs_com
from pinocchio import SE3 from numpy import array import multicontact_api from multicontact_api import ContactSequence import mlp.viewer.display_tools as display_tools from talos_rbprm.talos import Robot # change robot here multicontact_api.switchToNumpyArray() ENV_NAME = "multicontact/bauzil_stairs" fb, v = display_tools.initScene(Robot, ENV_NAME, False) gui = v.client.gui sceneName = v.sceneName 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]) num_steps = 6 step_height = 0.1 step_width = 0.3 displacement = SE3.Identity() displacement.translation = array([step_width, 0, step_height]) for i in range(num_steps): cs.moveEffectorOf(fb.rfoot, displacement) cs.moveEffectorOf(fb.lfoot, displacement)
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()) # create a first contact phase corresponding to this configuration limbsInContact = [fb.rLegId, fb.lLegId ] # define the limbs in contact for this first phase addPhaseFromConfig(fb, cs, q_ref, limbsInContact)
# examples_dir = '' # file_name = 'sinY_nomove.cs' # file_name = 'sinY_trunk.cs' # file_name = 'sinY_notrunk.cs' file_name = 'talos_sin_traj_R3SO3.cs' # file_name = 'talos_contact_switch_R3SO3.cs' # INTTYPE = 'preint' INTTYPE = 'direct' # INTTYPE = 'directSE3' # INTTYPE = 'pinocchio' PATH = 'figs_'+INTTYPE+'/' cs = ContactSequence() print('Loadding cs file: ', examples_dir + file_name) cs.loadFromBinary(examples_dir + file_name) q_traj = cs.concatenateQtrajectories() dq_traj = cs.concatenateDQtrajectories() ddq_traj = cs.concatenateDDQtrajectories() c_traj = cs.concatenateCtrajectories() dc_traj = cs.concatenateDCtrajectories() Lc_traj = cs.concatenateLtrajectories() contact_frames = cs.getAllEffectorsInContact() f_traj_lst = [cs.concatenateContactForceTrajectories(l) for l in contact_frames] print(contact_frames) min_ts = q_traj.min() max_ts = q_traj.max()
import mlp.viewer.display_tools as display_tools from pinocchio import SE3 from mlp.utils.util import rotatePlacement, computeContactNormal from talos_rbprm.talos import Robot # change robot here from numpy import array from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper multicontact_api.switchToNumpyArray() ENV_NAME = "multicontact/stairsAirbus_noRamp" fb, v = display_tools.initScene(Robot, ENV_NAME, False) gui = v.client.gui sceneName = v.sceneName cs = ContactSequence(0) #Create an initial contact phase : q_ref = [ 0, 0, 0.9832773, 0, 0.0, 0.0, 1.0, #Free flyer 1.45, 0.0, -0.611354, 1.059395, -0.448041,
def generate_wholebody_tsid(cfg, cs_ref, fullBody=None, viewer=None, robot=None, queue_qt = None): """ Generate the whole body motion corresponding to the given contactSequence :param cs: Contact sequence containing the references, it will only be modified if the end effector trajectories are not valid. New references will be generated and added to the cs :param fullBody: Required to compute collision free end effector trajectories :param viewer: If provided, and the settings are enabled, display the end effector trajectories and the last step computed :param robot: a tsid.RobotWrapper instance. If None, a new one is created from the urdf files defined in cfg :param queue_qt: If not None, the joint trajectories are send to this multiprocessing.Queue during computation The queue take a tuple: [q_t (a Curve object), ContactPhase (may be None), Bool (True mean that this is the last trajectory of the motion)] :return: a new ContactSequence object, containing the wholebody trajectories, and the other trajectories computed from the wholebody motion request with cfg.IK_STORE_* and a robotWrapper instance """ # define nested functions used in control loop # def appendJointsValues(first_iter_for_phase = False): """ Append the current q value to the current phase.q_t trajectory :param first_iter_for_phase: if True, set the current phase.q_init value and initialize a new Curve for phase.q_t """ if first_iter_for_phase: phase.q_init = q phase.q_t = piecewise(polynomial(q.reshape(-1,1), t, t)) #phase.root_t = piecewise_SE3(constantSE3curve(SE3FromConfig(q) ,t)) else: phase.q_t.append(q, t) #phase.root_t.append(SE3FromConfig(q), t) if queue_qt: queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves()-1), phase.dq_t.curve_at_index(phase.dq_t.num_curves()-1), None, False]) def appendJointsDerivatives(first_iter_for_phase=False): """ Append the current v and dv value to the current phase.dq_t and phase.ddq_t trajectory :param first_iter_for_phase: if True, initialize a new Curve for phase.dq_t and phase.ddq_t """ if first_iter_for_phase: phase.dq_t = piecewise(polynomial(v.reshape(-1, 1), t, t)) phase.ddq_t = piecewise(polynomial(dv.reshape(-1, 1), t, t)) else: phase.dq_t.append(v, t) phase.ddq_t.append(dv, t) def appendTorques(first_iter_for_phase = False): """ Append the current tau value to the current phase.tau_t trajectory :param first_iter_for_phase: if True, initialize a new Curve for phase.tau_t """ tau = invdyn.getActuatorForces(sol) if first_iter_for_phase: phase.tau_t = piecewise(polynomial(tau.reshape(-1,1), t, t)) else: phase.tau_t.append(tau, t) def appendCentroidal(first_iter_for_phase = False): """ Compute the values of the CoM position, velocity, acceleration, the anuglar momentum and it's derivative from the wholebody data and append them to the current phase trajectories :param first_iter_for_phase: if True, set the initial values for the current phase and initialize the centroidal trajectories """ pcom, vcom, acom = pinRobot.com(q, v, dv) L = pinRobot.centroidalMomentum(q, v).angular dL = pin.computeCentroidalMomentumTimeVariation(pinRobot.model, pinRobot.data, q, v, dv).angular if first_iter_for_phase: phase.c_init = pcom phase.dc_init = vcom phase.ddc_init = acom phase.L_init = L phase.dL_init = dL phase.c_t = piecewise(polynomial(pcom.reshape(-1,1), t , t)) phase.dc_t = piecewise(polynomial(vcom.reshape(-1,1), t, t)) phase.ddc_t = piecewise(polynomial(acom.reshape(-1,1), t, t)) phase.L_t = piecewise(polynomial(L.reshape(-1,1), t, t)) phase.dL_t = piecewise(polynomial(dL.reshape(-1,1), t, t)) else: phase.c_t.append(pcom, t) phase.dc_t.append(vcom, t) phase.ddc_t.append(acom, t) phase.L_t.append(L, t) phase.dL_t.append(dL, t) def appendZMP(first_iter_for_phase = False): """ Compute the zmp from the current wholebody data and append it to the current phase :param first_iter_for_phase: if True, initialize a new Curve for phase.zmp_t """ tau = pin.rnea(pinRobot.model, pinRobot.data, q, v, dv) # tau without external forces, only used for the 6 first # res.tau_t[:6,k_t] = tau[:6] phi0 = pinRobot.data.oMi[1].act(Force(tau[:6])) wrench = phi0.vector zmp = shiftZMPtoFloorAltitude(cs, t, phi0, cfg.Robot) if first_iter_for_phase: phase.zmp_t = piecewise(polynomial(zmp.reshape(-1,1), t, t)) phase.wrench_t = piecewise(polynomial(wrench.reshape(-1,1), t, t)) else: phase.zmp_t.append(zmp, t) phase.wrench_t.append(wrench, t) def appendEffectorsTraj(first_iter_for_phase = False): """ Append the current position of the effectors not in contact to the current phase trajectories :param first_iter_for_phase: if True, initialize a new Curve for the phase effector trajectories """ if first_iter_for_phase and phase_prev: for eeName in phase_prev.effectorsWithTrajectory(): if t > phase_prev.effectorTrajectory(eeName).max(): placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName) phase_prev.effectorTrajectory(eeName).append(placement, t) for eeName in phase.effectorsWithTrajectory(): placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName) if first_iter_for_phase: phase.addEffectorTrajectory(eeName, piecewise_SE3(constantSE3curve(placement, t))) else: phase.effectorTrajectory(eeName).append(placement, t) def appendContactForcesTrajs(first_iter_for_phase = False): """ Append the current contact force value to the current phase, for all the effectors in contact :param first_iter_for_phase: if True, initialize a new Curve for the phase contact trajectories """ for eeName in phase.effectorsInContact(): contact = dic_contacts[eeName] if invdyn.checkContact(contact.name, sol): contact_forces = invdyn.getContactForce(contact.name, sol) contact_normal_force = np.array(contact.getNormalForce(contact_forces)) else: logger.warning("invdyn check contact returned false while the reference contact is active !") contact_normal_force = np.zeros(1) if cfg.Robot.cType == "_3_DOF": contact_forces = np.zeros(3) else: contact_forces = np.zeros(12) if first_iter_for_phase: phase.addContactForceTrajectory(eeName, piecewise(polynomial(contact_forces.reshape(-1,1), t, t))) phase.addContactNormalForceTrajectory(eeName, piecewise(polynomial(contact_normal_force.reshape(1,1), t, t))) else: phase.contactForce(eeName).append(contact_forces, t) phase.contactNormalForce(eeName).append(contact_normal_force.reshape(1), t) def storeData(first_iter_for_phase = False): """ Append all the required data (selected in the configuration file) to the current ContactPhase :param first_iter_for_phase: if True, set the initial values for the current phase and correctly initiliaze empty trajectories for this phase """ if cfg.IK_store_joints_derivatives: appendJointsDerivatives(first_iter_for_phase) appendJointsValues(first_iter_for_phase) if cfg.IK_store_joints_torque: appendTorques(first_iter_for_phase) if cfg.IK_store_centroidal: appendCentroidal(first_iter_for_phase) if cfg.IK_store_zmp: appendZMP(first_iter_for_phase) if cfg.IK_store_effector: appendEffectorsTraj(first_iter_for_phase) if cfg.IK_store_contact_forces: appendContactForcesTrajs(first_iter_for_phase) def printIntermediate(): """ Print the current state: active contacts, tracking errors, computed joint acceleration and velocity :return: """ if logger.isEnabledFor(logging.INFO): print("Time %.3f" % (t)) for eeName, contact in dic_contacts.items(): if invdyn.checkContact(contact.name, sol): f = invdyn.getContactForce(contact.name, sol) print("\tnormal force %s: %.1f" % (contact.name.ljust(20, '.'), contact.getNormalForce(f))) print("\ttracking err %s: %.3f" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2))) for eeName in phase.effectorsWithTrajectory(): task = dic_effectors_tasks[eeName] error = task.position_error if cfg.Robot.cType == "_3_DOF": error = error[0:3] print("\ttracking err %s: %.3f" % (task.name.ljust(20, '.'), norm(error, 2))) print("\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv))) def checkDiverge(): """ Check if either the joint velocity or acceleration is over a treshold or is NaN, and raise an error """ if norm(dv) > 1e6 or norm(v) > 1e6: logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") logger.error("/!\ ABORT : controler unstable at t = %f /!\ ", t) logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") raise ValueError("ABORT : controler unstable at t = " + str(t)) if math.isnan(norm(dv)) or math.isnan(norm(v)): logger.error("!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") logger.error("/!\ ABORT : nan at t = %f /!\ ", t) logger.error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") raise ValueError("ABORT : controler unstable at t = " + str(t)) def stopHere(): """ Set the current data as final values for the current phase, resize the contact sequence if needed and return :return: [The current ContactSequence, the RobotWrapper] """ setPreviousFinalValues(phase_prev, phase, cfg) if cfg.WB_ABORT_WHEN_INVALID: # cut the sequence up to the last phase cs.resize(pid) return cs, robot elif cfg.WB_RETURN_INVALID: # cut the sequence up to the current phase cs.resize(pid+1) return cs, robot ### End of nested functions definitions ### if not viewer: logger.warning("No viewer linked, cannot display end_effector trajectories.") logger.warning("Start TSID ... ") # copy the given contact sequence to keep it as reference : cs = ContactSequence(cs_ref) # delete all the 'reference' trajectories from result (to leave room for the real trajectories stored) deleteAllTrajectories(cs) # Create a robot wrapper if robot is None or cfg.IK_store_centroidal or cfg.IK_store_zmp: rp = RosPack() package_name = cfg.Robot.packageName.split("/")[0] package_path = rp.get_path(package_name) urdf = package_path + cfg.Robot.packageName.lstrip( package_name) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' logger.info("load robot : %s", urdf) if robot is None: logger.info("load robot : %s", urdf) robot = tsid.RobotWrapper(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) else: logger.info("Use given robot in tsid.") logger.info("robot loaded in tsid.") if cfg.IK_store_centroidal or cfg.IK_store_zmp: logger.info("load pinocchio robot ...") # FIXME : tsid robotWrapper don't have all the required methods, only pinocchio have them pinRobot = pin.RobotWrapper.BuildFromURDF(urdf, package_path, pin.JointModelFreeFlyer()) logger.info("pinocchio robot loaded.") # get the selected end effector trajectory generation method effectorMethod, effectorCanRetry = cfg.get_effector_method() # get the selected simulator method Simulator = cfg.get_simulator_class() simulator = Simulator(cfg.IK_dt, robot.model()) ### Define initial state of the robot ### phase0 = cs.contactPhases[0] q = phase0.q_init[:robot.nq].copy() if not q.any(): raise RuntimeError("The contact sequence doesn't contain an initial whole body configuration") t = phase0.timeInitial if cs_ref.contactPhases[0].dq_t and cs_ref.contactPhases[0].dq_t.min() <= t <= cs_ref.contactPhases[0].dq_t.max(): v = cs_ref.contactPhases[0].dq_t(t) else: v = np.zeros(robot.nv) logger.debug("V_init used in tsid : %s", v) invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False) invdyn.computeProblemData(t, q, v) simulator.init(q, v) # add initial contacts : dic_contacts = {} for eeName in cs.contactPhases[0].effectorsInContact(): # replace the initial contact patch placements if needed to match exactly the current position in the problem: updateContactPlacement(cs, 0, eeName, getCurrentEffectorPosition(robot, invdyn.data(), eeName), cfg.Robot.cType == "_6_DOF") # create the contacts : contact = createContactForEffector(cfg, invdyn, robot, eeName, phase0.contactPatch(eeName), False) dic_contacts.update({eeName: contact}) if cfg.EFF_CHECK_COLLISION: # initialise object needed to check the motion from mlp.utils import check_path validator = check_path.PathChecker(fullBody, cfg.CHECK_DT, logger.isEnabledFor(logging.INFO)) ### Initialize all task used ### logger.info("initialize tasks : ") if cfg.w_com > 0. : comTask = tsid.TaskComEquality("task-com", robot) comTask.setKp(cfg.kp_com * np.ones(3)) comTask.setKd(2.0 * np.sqrt(cfg.kp_com) * np.ones(3)) invdyn.addMotionTask(comTask, cfg.w_com, cfg.level_com, 0.0) else: comTask = None if cfg.w_am > 0.: amTask = tsid.TaskAMEquality("task-am", robot) amTask.setKp(cfg.kp_am * np.array([1., 1., 0.])) amTask.setKd(2.0 * np.sqrt(cfg.kp_am * np.array([1., 1., 0.]))) invdyn.addMotionTask(amTask, cfg.w_am, cfg.level_am, 0.) else: amTask = None if cfg.w_posture > 0.: postureTask = tsid.TaskJointPosture("task-joint-posture", robot) postureTask.setKp(cfg.kp_posture * cfg.gain_vector) postureTask.setKd(2.0 * np.sqrt(cfg.kp_posture * cfg.gain_vector)) postureTask.setMask(cfg.masks_posture) invdyn.addMotionTask(postureTask, cfg.w_posture, cfg.level_posture, 0.0) q_ref = cfg.IK_REFERENCE_CONFIG samplePosture = tsid.TrajectorySample(q_ref.shape[0] - 7) samplePosture.pos(q_ref[7:]) # -7 because we remove the freeflyer part else : postureTask = None if cfg.w_rootOrientation > 0. : orientationRootTask = tsid.TaskSE3Equality("task-orientation-root", robot, 'root_joint') mask = np.ones(6) mask[0:3] = 0 mask[5] = cfg.YAW_ROT_GAIN orientationRootTask.setMask(mask) orientationRootTask.setKp(cfg.kp_rootOrientation * mask) orientationRootTask.setKd(2.0 * np.sqrt(cfg.kp_rootOrientation * mask)) invdyn.addMotionTask(orientationRootTask, cfg.w_rootOrientation, cfg.level_rootOrientation, 0.0) else: orientationRootTask = None # init effector task objects : usedEffectors = cs.getAllEffectorsInContact() dic_effectors_tasks = createEffectorTasksDic(cfg, usedEffectors, robot) # Add bounds tasks if required: if cfg.w_torque_bounds > 0.: tau_max = cfg.scaling_torque_bounds*robot.model().effortLimit[-robot.na:] tau_min = -tau_max actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot) actuationBoundsTask.setBounds(tau_min, tau_max) invdyn.addActuationTask(actuationBoundsTask, cfg.w_torque_bounds, 0, 0.0) if cfg.w_joint_bounds > 0.: jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, cfg.IK_dt) v_max = cfg.scaling_vel_bounds * robot.model().velocityLimit[-robot.na:] v_min = -v_max print("v_max : ", v_max) jointBoundsTask.setVelocityBounds(v_min, v_max) invdyn.addMotionTask(jointBoundsTask, cfg.w_joint_bounds, 0, 0.0) solver = tsid.SolverHQuadProg("qp solver") solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) # time check dt = cfg.IK_dt logger.info("dt : %f", dt) logger.info("tsid initialized, start control loop") #raw_input("Enter to start the motion (motion displayed as it's computed, may be slower than real-time)") time_start = time.time() # For each phases, create the necessary task and references trajectories : for pid in range(cs.size()): logger.info("## for phase : %d", pid) logger.info("t = %f", t) # phase_ref contains the reference trajectories and should not be modified exept for new effector trajectories # when the first ones was not collision free phase_ref = cs_ref.contactPhases[pid] # phase de not contains trajectories (exept for the end effectors) and should be modified with the new values computed phase = cs.contactPhases[pid] if pid > 0: phase_prev = cs.contactPhases[pid - 1] else: phase_prev = None if pid < cs.size() - 1: phase_next = cs.contactPhases[pid + 1] else: phase_next = None time_interval = [phase_ref.timeInitial, phase_ref.timeFinal] logger.info("time_interval %s", time_interval) # take CoM and AM trajectory from the phase, with their derivatives com_traj = [phase_ref.c_t, phase_ref.dc_t, phase_ref.ddc_t] am_traj = [phase_ref.L_t, phase_ref.L_t, phase_ref.dL_t] # add root's orientation ref from reference config : root_traj = phase_ref.root_t # add se3 tasks for end effector when required for eeName in phase.effectorsWithTrajectory(): logger.info("add se3 task for %s", eeName) task = dic_effectors_tasks[eeName] invdyn.addMotionTask(task, cfg.w_eff, cfg.level_eff, 0.) adjustEndEffectorTrajectoryIfNeeded(cfg, phase_ref, robot, invdyn.data(), eeName, effectorMethod) logger.info("t interval : %s", time_interval) # add newly created contacts : new_contacts_names = [] # will store the names of the contact tasks created at this phase for eeName in usedEffectors: if phase_prev and phase_ref.isEffectorInContact(eeName) and not phase_prev.isEffectorInContact(eeName): invdyn.removeTask(dic_effectors_tasks[eeName].name, 0.0) # remove pin task for this contact logger.info("remove se3 effector task : %s", dic_effectors_tasks[eeName].name) if logger.isEnabledFor(logging.DEBUG): current_placement = getCurrentEffectorPosition(robot, invdyn.data(), eeName) logger.debug("Current effector placement : %s", current_placement) logger.debug("Reference effector placement : %s", cs.contactPhases[pid].contactPatch(eeName).placement) updateContactPlacement(cs, pid, eeName, getCurrentEffectorPosition(robot, invdyn.data(), eeName), cfg.Robot.cType == "_6_DOF") contact = createContactForEffector(cfg, invdyn, robot, eeName, phase.contactPatch(eeName)) new_contacts_names += [contact.name] dic_contacts.update({eeName: contact}) logger.info("Create contact for : %s", eeName) # start removing the contact that will be broken in the next phase : # (This tell the solver that it should start minimizing the contact force on this contact, and ideally get to 0 at the given time) for eeName, contact in dic_contacts.items(): if phase_next and phase.isEffectorInContact(eeName) and not phase_next.isEffectorInContact(eeName): transition_time = phase.duration + dt/2. logger.info("\nTime %.3f Start breaking contact %s. transition time : %.3f\n", t, contact.name, transition_time) exist = invdyn.removeRigidContact(contact.name, transition_time) assert exist, "Try to remove a non existing contact !" # Remove all effectors not in contact at this phase, # This is required as the block above may not remove the contact exactly at the desired time # FIXME: why is it required ? Numerical approximation in the transition_time ? for eeName, contact in dic_contacts.items(): if not phase.isEffectorInContact(eeName): exist = invdyn.removeRigidContact(contact.name, 0.) if exist: logger.warning("Contact "+eeName+" was not remove after the given transition time.") if cfg.WB_STOP_AT_EACH_PHASE: input('start simulation') # save values at the beginning of the current phase q_begin = q.copy() v_begin = v.copy() phase.q_init = q_begin if phase_prev: phase_prev.q_final = q_begin phaseValid = False iter_for_phase = -1 # iterate until a valid motion for this phase is found (ie. collision free and which respect joint-limits) while not phaseValid: deletePhaseWBtrajectories(phase) # clean previous invalid trajectories t = phase.timeInitial k_t = 0 if iter_for_phase >= 0: # reset values to their value at the beginning of the current phase q = q_begin.copy() v = v_begin.copy() simulator.q = q simulator.v = v iter_for_phase += 1 logger.info("Start computation for phase %d , t = %f, try number : %d", pid, t, iter_for_phase) # loop to generate states (q,v,a) for the current contact phase : while t < phase.timeFinal - (dt / 2.): # set traj reference for current time : # com if comTask: sampleCom = curvesToTSID(com_traj,t) comTask.setReference(sampleCom) # am if amTask: if cfg.IK_trackAM: sampleAM = curvesToTSID(am_traj,t) else: sampleAM = tsid.TrajectorySample(3) amTask.setReference(sampleAM) # posture #print "postural task ref : ",samplePosture.pos() if postureTask: postureTask.setReference(samplePosture) # root orientation : if orientationRootTask: sampleRoot = curveSE3toTSID(root_traj,t) orientationRootTask.setReference(sampleRoot) # update weight of regularization tasks for the new contacts: if len(new_contacts_names) > 0 : # linearly decrease the weight of the tasks for the newly created contacts u_w_force = (t - phase.timeInitial) / (phase.duration * cfg.w_forceRef_time_ratio) if u_w_force <= 1.: current_w_force = cfg.w_forceRef_init * (1. - u_w_force) + cfg.w_forceRef_end * u_w_force for contact_name in new_contacts_names: success = invdyn.updateRigidContactWeights(contact_name, current_w_force) assert success, "Unable to change the weight of the force regularization task for contact "+contact_name logger.debug("### references given : ###") logger.debug("com pos : %s", sampleCom.pos()) logger.debug("com vel : %s", sampleCom.vel()) logger.debug("com acc : %s", sampleCom.acc()) if amTask: logger.debug("AM pos : %s", sampleAM.pos()) logger.debug("AM vel : %s", sampleAM.vel()) logger.debug("root pos : %s", sampleRoot.pos()) logger.debug("root vel : %s", sampleRoot.vel()) # end effector (if they exists) for eeName, traj in phase_ref.effectorTrajectories().items(): sampleEff = curveSE3toTSID(traj,t,True) dic_effectors_tasks[eeName].setReference(sampleEff) logger.debug("effector %s, pos = %s", eeName, sampleEff.pos()) logger.debug("effector %s, vel = %s", eeName, sampleEff.vel()) logger.debug("previous q = %s", q) logger.debug("previous v = %s", v) # solve HQP for the current time HQPData = invdyn.computeProblemData(t, q, v) if t < phase.timeInitial + dt: logger.info("final data for phase %d", pid) if logger.isEnabledFor(logging.INFO): HQPData.print_all() sol = solver.solve(HQPData) dv = invdyn.getAccelerations(sol) storeData(k_t == 0) q, v = simulator.simulate(dv) t += dt k_t += 1 if t >= phase.timeFinal - (dt / 2.): t = phase.timeFinal # avoid numerical imprecisions logger.debug("v = %s", v) logger.debug("dv = %s", dv) if int(t / dt) % cfg.IK_PRINT_N == 0: printIntermediate() try: checkDiverge() except ValueError: return stopHere() # end while t \in phase_t (loop for the current contact phase) if len(phase.effectorsWithTrajectory()) > 0 and cfg.EFF_CHECK_COLLISION: phaseValid, t_invalid = validator.check_motion(phase.q_t) #if iter_for_phase < 3 :# debug only, force limb-rrt # phaseValid = False # t_invalid = (phase.timeInitial + phase.timeFinal) / 2. if not phaseValid: if iter_for_phase == 0: # save the first q_t trajectory computed, for limb-rrt first_q_t = phase.q_t logger.warning("Phase %d not valid at t = %f", pid, t_invalid) logger.info("First invalid q : %s", phase.q_t(t_invalid)) if t_invalid <= (phase.timeInitial + cfg.EFF_T_PREDEF) \ or t_invalid >= (phase.timeFinal - cfg.EFF_T_PREDEF): logger.error("Motion is invalid during predefined phases, cannot change this.") return stopHere() if effectorCanRetry(): logger.warning("Try new end effector trajectory.") try: for eeName, ref_traj in phase_ref.effectorTrajectories().items(): placement_init = ref_traj.evaluateAsSE3(phase.timeInitial) placement_end = ref_traj.evaluateAsSE3(phase.timeFinal) traj = effectorMethod(cfg, time_interval, placement_init, placement_end, iter_for_phase + 1, first_q_t, phase_prev, phase_ref, phase_next, fullBody, eeName, viewer) # save the new trajectory in the phase with the references phase_ref.addEffectorTrajectory(eeName,traj) if cfg.DISPLAY_ALL_FEET_TRAJ and logger.isEnabledFor(logging.INFO): color = fullBody.dict_limb_color_traj[eeName] color[-1] = 0.6 # set small transparency display_tools.displaySE3Traj(phase_ref.effectorTrajectory(eeName), viewer.client.gui, viewer.sceneName, eeName + "_traj_" + str(pid) + "_" + str(iter_for_phase), color, [phase.timeInitial, phase.timeFinal], fullBody.dict_offset[eeName]) except ValueError as e: logging.error("ERROR in generateEndEffectorTraj :", exc_info=e) return stopHere() else: logging.error("End effector method choosen do not allow retries, abort here.") return stopHere() else: # no effector motions, phase always valid (or bypass the check) phaseValid = True logging.info("Phase %d valid.", pid) if phaseValid: setPreviousFinalValues(phase_prev, phase, cfg) # display the progress by moving the robot at the last configuration computed if viewer and cfg.IK_SHOW_PROGRESS: display_tools.displayWBconfig(viewer,q) #end while not phaseValid # end for all phases # store the data of the last point phase_prev = phase phase = ContactPhase(phase_prev) storeData(True) setPreviousFinalValues(phase_prev, phase, cfg) time_end = time.time() - time_start logger.warning("Whole body motion generated in : %f s.", time_end) logger.info("\nFinal COM Position %s", robot.com(invdyn.data())) logger.info("Desired COM Position %s", cs.contactPhases[-1].c_final) if queue_qt: queue_qt.put([phase.q_t.curve_at_index(phase.q_t.num_curves() - 1), None, True]) return cs, robot
def build_cs_from_sl1m_mip(fb, q_ref, root_end, pb, allfeetpos, use_orientation, use_interpolated_orientation, q_init = None, first_phase = None): """ Build a multicontact_api.ContactSequence from the SL1M outputs when using the MIP formulation :param fb: an instance of rbprm.Fullbody :param q_ref: the reference wholebody configuration of the robot :param root_end: the final base position :param pb: the SL1M problem dictionary, containing all the contact surfaces and data :param allfeetpos: the list of all foot position for each phase, computed by SL1M :param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide :param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation of the next phase :param q_init: the initial wholebody configuration (either this or first_phase should be provided) :param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided) :return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement """ # 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 num_steps = len(pb["phaseData"]) - 1 # number of contact repositionning limbs_names = fb.limbs_names num_effectors = len(limbs_names) logger.info(" limbs names : %s", limbs_names) cs = ContactSequence(0) if first_phase: cs.append(first_phase) elif q_init: # if only the configuration is provided, assume all effectors are in contact addPhaseFromConfig(fb, cs, q_init, limbs_names) else: raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined") logger.info("Initial phase added, contacts : %s ", cs.contactPhases[0].effectorsInContact()) # loop for all effector placements, and create the required contact phases previous_eff_placements = allfeetpos[0] if len(previous_eff_placements) != num_effectors: raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.") for pid, eff_placements in enumerate(allfeetpos[1:]): logger.info("Loop allfeetpos, id = %d", pid) if len(eff_placements) != num_effectors: raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.") switch = False # True if a repostionning have been detected for k, pos in enumerate(eff_placements): if norm(pos - previous_eff_placements[k]) > 1e-3: if switch: raise NotImplementedError("Several contact changes between two adjacent phases in SL1M output") switch = True ee_name = fb.dict_limb_joint[limbs_names[k]] #pos[2] += EPS_Z # FIXME: apply epsz along the normal pos = fb.dict_offset[ee_name].actInv(pos) logger.info("Move effector %s ", ee_name) logger.info("To position %s ", pos) placement = SE3.Identity() placement.translation = pos # compute orientation of the contact from the surface normal: phase_data = pb["phaseData"][pid+1] # +1 because the for loop start at id = 1 n = normal_from_ineq(phase_data["S"][phase_data["id_surface"]]) placement.rotation = rotationFromNormal(n) logger.debug("new contact placement : %s", placement) # TODO add yaw rotation from guide here ! cs.moveEffectorToPlacement(ee_name, placement) #if not switch: # raise RuntimeError("No contact changes between two adjacent phases in SL1M output") # assign com position to the last two phases : # swinging phase, same init and final position """ cs.contactPhases[-2].c_init = coms[pid * 2] cs.contactPhases[-2].c_final = coms[pid * 2 + 1] # phase with all the contacts: cs.contactPhases[-1].c_init = coms[pid * 2 + 1] if pid * 2 + 2 < len(coms): cs.contactPhases[-1].c_final = coms[pid * 2 + 2] else: cs.contactPhases[-1].c_final = cs.contactPhases[-1].c_init """ previous_eff_placements = eff_placements # final phase : # fixme : assume root is in the middle of the last 2 feet pos ... """ q_end = q_ref.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][0][2] logger.info("feet height final = %s", feet_height_end) q_end[2] = feet_height_end + q_ref[2] #q_end[2] += EPS_Z fb.setCurrentConfig(q_end) com = fb.getCenterOfMass() setFinalState(cs, com, q=q_end) """ p_final = cs.contactPhases[-1] p_final.c_final = computeCenterOfSupportPolygonFromPhase(p_final, fb.DEFAULT_COM_HEIGHT) p_final.c_init = p_final.c_final return cs