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 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 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_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_walk_20cm_quasistatic_COM(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs")) self.assertEqual(cs.size(), 23) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) self.assertFalse(cs.haveFriction()) self.assertFalse(cs.haveContactModelDefined()) checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def test_step_in_place_REF(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "step_in_place_REF.cs")) self.assertEqual(cs.size(), 9) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) self.assertTrue(cs.haveEffectorsTrajectories()) self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False)) self.assertTrue(cs.haveFriction()) self.assertTrue(cs.haveContactModelDefined()) checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_com_motion_above_feet_WB(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs")) self.assertEqual(cs.size(), 1) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) self.assertTrue(cs.haveJointsTrajectories()) self.assertTrue(cs.haveJointsDerivativesTrajectories()) self.assertTrue(cs.haveContactForcesTrajectories()) self.assertTrue(cs.haveZMPtrajectories()) checkCS(self, cs, wholeBody=True)
def test_walk_20cm_quasistatic_WB(self): cs = ContactSequence() cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs")) self.assertEqual(cs.size(), 23) self.assertTrue(cs.haveConsistentContacts()) self.assertTrue(cs.haveTimings()) self.assertTrue(cs.haveCentroidalValues()) self.assertTrue(cs.haveCentroidalTrajectories()) self.assertTrue(cs.haveEffectorsTrajectories(1e-1)) self.assertTrue(cs.haveJointsTrajectories()) self.assertTrue(cs.haveJointsDerivativesTrajectories()) self.assertTrue(cs.haveContactForcesTrajectories()) self.assertTrue(cs.haveZMPtrajectories()) self.assertTrue(cs.haveFriction()) self.assertTrue(cs.haveContactModelDefined()) checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
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
for pid, phase in enumerate(cs.contactPhases): # define timing : phase.timeInitial = t if phase.numContacts() == 2: phase.timeFinal = t + 2. # DS duration else: phase.timeFinal = t + 1.5 ## SS duration t = phase.timeFinal # define init/end CoM position : if phase.numContacts() == 1: phase.c_init = cs.contactPhases[pid - 1].c_final phase.c_final = cs.contactPhases[pid - 1].c_final else: if pid > 0: phase.c_init = cs.contactPhases[pid - 1].c_final if pid < cs.size() - 1: if cs.contactPhases[pid + 1].isEffectorInContact(fb.lfoot): phase.c_final = c_l elif cs.contactPhases[pid + 1].isEffectorInContact(fb.rfoot): phase.c_final = c_r else: phase.c_final = c_mid for phase in cs.contactPhases: genCOMTrajFromPhaseStates(phase) genAMTrajFromPhaseStates(phase) assert cs.haveConsistentContacts() assert cs.haveConsistentContacts() assert cs.haveCentroidalValues() assert cs.haveCentroidalTrajectories()
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
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
class LocoPlannerReactive(LocoPlanner): 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 init_guide_planner(self): """ Initialize an rbprm.AbstractPathPlanner class :return: an instance of rbprm.AbstractPathPlanner initialized with the current settings """ # the following script must produce a if hasattr(self.cfg, 'SCRIPT_ABSOLUTE_PATH'): scriptName = self.cfg.SCRIPT_ABSOLUTE_PATH else: scriptName = self.cfg.RBPRM_SCRIPT_PATH + "." + self.cfg.SCRIPT_PATH + '.' + self.cfg.DEMO_NAME scriptName += "_path" logger.warning("Run Guide script : %s", scriptName) module = importlib.import_module(scriptName) planner = module.PathPlanner("guide_planning") planner.init_problem() # greatly increase the number of loops of the random shortcut planner.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50) # force the base orientation to follow the direction of motion along the Z axis planner.ps.setParameter("Kinodynamic/forceYawOrientation", True) planner.q_init[:2] = [0, 0] # FIXME : defined somewhere ?? planner.init_viewer(cfg.ENV_NAME) planner.init_planner() return planner def init_viewer(self): """ Build a pinocchio wrapper and a gepetto-gui instance and assign them as class members: self.robot and self.gui Also start (or restart) a gepetto-gui process """ subprocess.run(["killall", "gepetto-gui"]) self.process_gepetto_gui = subprocess.Popen("gepetto-gui", stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, preexec_fn=os.setpgrp) atexit.register(self.process_gepetto_gui.kill) time.sleep(3) self.robot, self.gui = initScenePinocchio(cfg.Robot.urdfName + cfg.Robot.urdfSuffix, cfg.Robot.packageName, cfg.ENV_NAME) self.robot.display(self.cfg.IK_REFERENCE_CONFIG) def init_pybullet(self): self.pyb_sim = pybullet_simulator(dt=self.cfg.IK_dt, q_init=self.cfg.IK_REFERENCE_CONFIG[7:].reshape(-1, 1), root_init=[0, 0, 0.1], env_name=cfg.ENV_NAME + ".urdf", env_package="hpp_environments", use_gui=False) def execute_motion(self, q_t, dq_t): if USE_PYB: self.viewer_lock.acquire() SimulatorLoop(self.pyb_sim, self.cfg.IK_dt, q_t, dq_t, self.robot.display) self.viewer_lock.release() else: self.viewer_lock.acquire() disp_wb_pinocchio(self.robot, q_t, self.cfg.DT_DISPLAY) self.viewer_lock.release() def plan_guide(self, root_goal): """ Plan a guide from the current last_phase root position to the given root position :param root_goal: list of size 3 or 7: translation and quaternion for the desired root position If the quaternion part is not specified, the final orientation is not constrained Store the Id of the new path in self.current_guide_id """ self.guide_planner.q_goal = self.guide_planner.q_init[::] self.guide_planner.q_goal[:3] = root_goal[:3] self.guide_planner.q_goal[3:7] = [0, 0, 0, 1] self.guide_planner.q_goal[-6:] = [0] * 6 last_phase = self.get_last_phase() if last_phase: logger.warning("Last phase not None") logger.warning("Last phase q_final : %s", last_phase.q_final[:7]) self.guide_planner.q_init[:7] = last_phase.q_final[:7] self.guide_planner.q_init[2] = self.guide_planner.rbprmBuilder.ref_height # FIXME #add small velocity in order to have smooth change of orientation at the beginning/end quat_init = Quaternion(self.guide_planner.q_init[6], self.guide_planner.q_init[3], self.guide_planner.q_init[4], self.guide_planner.q_init[5]) dir_init = quat_init * np.array([1, 0, 0]) self.guide_planner.q_init[-6] = dir_init[0] * V_INIT self.guide_planner.q_init[-5] = dir_init[1] * V_INIT if len(root_goal) >= 7: self.guide_planner.q_goal[3:7] = root_goal[3:7] quat_goal = Quaternion(self.guide_planner.q_goal[6], self.guide_planner.q_goal[3], self.guide_planner.q_goal[4], self.guide_planner.q_goal[5]) dir_goal = quat_goal * np.array([1, 0, 0]) self.guide_planner.q_goal[-6] = dir_goal[0] * V_GOAL self.guide_planner.q_goal[-5] = dir_goal[1] * V_GOAL logger.warning("Guide init = %s", self.guide_planner.q_init) logger.warning("Guide goal = %s", self.guide_planner.q_goal) self.guide_planner.ps.resetGoalConfigs() self.guide_planner.ps.clearRoadmap() self.current_root_goal = root_goal self.guide_planner.solve(True) self.current_guide_id = self.guide_planner.ps.numberPaths() - 1 def compute_cs_from_guide(self): """ Call SL1M to produce a contact sequence following the root path stored in self.current_guide_id Store the result in self.cs :param guide_id: Id of the path stored in self.guide_planner.ps :return: """ initial_contacts = None last_phase = self.get_last_phase() if self.cfg.SL1M_MAX_STEP > 0: # compute the pathlength corresponding to this number of steps: max_path_length = self.cfg.SL1M_MAX_STEP * self.cfg.GUIDE_STEP_SIZE total_path_length = self.guide_planner.ps.pathLength(self.current_guide_id) if total_path_length > max_path_length: # split the guide path in several segment of max_path_length length guide_ids = [] t = 0. while t < total_path_length: t_next = t + max_path_length if t_next > total_path_length: t_next = total_path_length self.guide_planner.ps.extractPath(self.current_guide_id, t, t_next) guide_ids += [self.guide_planner.ps.numberPaths() - 1] t = t_next else: guide_ids = [self.current_guide_id] else: guide_ids = [self.current_guide_id] print("##### compute sl1m from guide id(s) : ", guide_ids) self.cs = ContactSequence() for guide_id in guide_ids: self.guide_planner.pathId = guide_id self.guide_planner.q_goal = self.guide_planner.ps.configAtParam( guide_id, self.guide_planner.ps.pathLength(guide_id)) print("### FORLOOP, guide id = ", guide_id) # compute initial contacts position, either from last_phase or from the wholebody configuration if last_phase: q_init = None initial_contacts = [last_phase.contactPatch(ee_name).placement.translation + self.fullBody.dict_offset[ee_name].translation for ee_name in [self.fullBody.dict_limb_joint[limb] for limb in self.fullBody.limbs_names]] first_phase = ContactPhase() tools.copyContactPlacement(last_phase, first_phase) tools.setInitialFromFinalValues(last_phase, first_phase) else: first_phase = None q_init = self.fullBody.getCurrentConfig() initial_contacts = sl1m.initial_foot_pose_from_fullbody(self.fullBody, q_init) pathId, pb, coms, footpos, allfeetpos, res = sl1m.solve(self.guide_planner, self.cfg, False, initial_contacts) root_end = self.guide_planner.ps.configAtParam(pathId, self.guide_planner.ps.pathLength(pathId) - 0.001)[0:7] logger.info("SL1M, root_end = %s", root_end) current_cs = sl1m.build_cs_from_sl1m(self.cfg.SL1M_USE_MIP, self.fullBody, self.cfg.IK_REFERENCE_CONFIG, root_end, pb, sl1m.sl1m.RF, allfeetpos, cfg.SL1M_USE_ORIENTATION, cfg.SL1M_USE_INTERPOLATED_ORIENTATION, q_init, first_phase) last_phase = current_cs.contactPhases[-1] # Merge current_cs in cs if self.cs.size() == 0: [self.cs.append(phase) for phase in current_cs.contactPhases] else: # first new phase is the same as last previous phase [self.cs.append(phase) for phase in current_cs.contactPhases[1:]] logger.warning("## Compute cs from guide done.") process_stones = Process(target=self.display_stones_lock) process_stones.start() atexit.register(process_stones.terminate) 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 hide_stones_lock(self): """ Wait for the lock to access to gepetto-gui and then remove all the stepping stones from the scene """ logger.info("Waiting lock to hide stepping stones ...") self.viewer_lock.acquire() logger.info("Hide stepping stones ...") hideSteppingStone(self.gui) logger.info("Hiding done.") self.viewer_lock.release() def get_last_phase(self): """ Retrieve the last phase stored in the shared memory If self.last_phase exist and the flag last_phase_flag is False, return the phase stored in self.last_phase Otherwise, retrieve the data in the shared memory and deserialize it :return: the last phase """ if self.last_phase is None or self.last_phase_flag.value: self.last_phase_lock.acquire() try: pic = bytes(self.last_phase_pickled) self.last_phase = pickle.loads(pic) self.last_phase_flag.value = False except: #logger.error("Cannot de serialize the last_phase") self.last_phase = None self.last_phase_lock.release() return self.last_phase def set_last_phase(self, last_phase): """ set pickled last_phase data to last_phase_pickled shared memory :param last_phase: """ logger.info("set_last_phase: pickle serialization ...") arr = pickle.dumps(last_phase) logger.info("set_last_phase: waiting for lock ...") self.last_phase_lock.acquire() if len(arr) >= MAX_PICKLE_SIZE: raise ValueError("In copy array: given array is too big, size = " + str(len(arr))) logger.info("set_last_phase: writing data ... ") for i, el in enumerate(arr): self.last_phase_pickled[i] = el self.last_phase_flag.value = True logger.info("Add a last_phase, q_final = %s", last_phase.q_final) self.last_phase_lock.release() def is_at_stop(self): """ Return True if the last phase have a null CoM and joint velocities :return: """ p = self.get_last_phase() if p: if not np.isclose(p.dc_final, np.zeros(3), atol=1e-2).all(): return False dq = p.dq_t(p.timeFinal) if not np.isclose(dq, np.zeros(dq.shape), atol=1e-1).all(): return False return True 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 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 compute_wholebody_queue(self, cs_ref): """ Call the wholebody motion generation with the given cs_ref and the queue_qt and store the last compute phase with self.set_last_phase :param cs_ref: :return: """ logger.warning("@@ Start compute_wholebody_queue") cs_wb, _ = self.generate_wholebody(self.cfg, cs_ref, None, None, None, self.queue_qt) last_phase = ContactPhase(cs_wb.contactPhases[-1]) tools.deletePhaseTrajectories(last_phase) tools.deleteEffectorsTrajectories(last_phase) last_phase.root_t = cs_ref.contactPhases[-1].root_t self.set_last_phase(last_phase) logger.warning("@@ End compute_wholebody_queue") def loop_centroidal(self): """ Loop waiting for data in pipe_cs, solving the centroidal problem for each new data and send the results in pipe_cs_com """ last_centroidal_phase = None last_iter = False timeout = False try: while not last_iter and not timeout: if self.pipe_cs_out.poll(TIMEOUT_CONNECTIONS): cs, last_iter = self.pipe_cs_out.recv() logger.info("## Run centroidal") cs_com, last_centroidal_phase = self.compute_centroidal(cs, last_centroidal_phase, last_iter) logger.info("-- Add a cs_com to the queue") self.pipe_cs_com_in.send([cs_com, last_iter]) else: timeout = True logger.warning("Loop centroidal closed because pipe is empty since 10 seconds") if last_iter: logger.info("Centroidal last iter received, close the pipe and terminate process.") except: logger.error("FATAL ERROR in loop centroidal: ") traceback.print_exc() sys.exit(0) self.pipe_cs_com_in.close() def loop_wholebody(self): """ Loop waiting for data in pipe_cs_com, computing the wholebody motion for each new data and sending the results in queue_qt """ last_v = None robot = None last_iter = False timeout = False # Set the current config, either from the planned ContactSequence or from the data stored in last_phase last_q = self.cs.contactPhases[0].q_init if last_q is None or last_q.shape[0] < self.robot.nq: logger.info("initial config not defined in CS, set it from last phase.") # get current last_phase config: last_phase = self.get_last_phase() if logger.isEnabledFor(logging.INFO) and last_phase: logger.info("last_phase.q_final shape: %d", last_phase.q_final.shape[0]) while last_phase is None or last_phase.q_final.shape[0] < self.robot.nq: # Wait for the data to be updated by another process last_phase = self.get_last_phase() last_q = last_phase.q_final logger.info("Got last_q from last_phase, start wholebody loop ...") logger.info("last_q in loop_wholebody = %s", last_q) try: while not last_iter and not timeout: if self.pipe_cs_com_out.poll(TIMEOUT_CONNECTIONS): cs_com, last_iter = self.pipe_cs_com_out.recv() logger.info("## Run wholebody") cs_wb, last_q, last_v, last_phase, robot = self.compute_wholebody( robot, cs_com, last_q, last_v, last_iter) logger.info("-- Add a cs_wb to the queue") self.queue_qt.put([cs_wb.concatenateQtrajectories(), cs_wb.concatenateDQtrajectories(), last_phase, last_iter]) else: timeout = True logger.warning("Loop wholebody closed because pipe is empty since 10 seconds") if last_iter: logger.info("Wholebody last iter received, close the pipe and terminate process.") except: logger.error("FATAL ERROR in loop wholebody: ") traceback.print_exc() sys.exit(0) def loop_viewer(self): """ Loop waiting for data in queue_qt and displaying each new trajectories. Before displaying each new data, it store the new last_phase in shared memory, this phase correspond to the last configuration and contacts that will be displayed for the current iteration. It watch for the "stop_motion" flag, if received the loop stop at the end of the current iteration """ self.loop_viewer_lock.acquire() logger.warning("## Start a loop_viewer") self.stop_motion_flag.value = False last_iter = False timeout = TIMEOUT_CONNECTIONS try: while not last_iter: q_t, dq_t, last_phase, last_iter = self.queue_qt.get(timeout=timeout) timeout = 0.1 if last_phase: self.set_last_phase(last_phase) self.execute_motion(q_t, dq_t) if self.stop_motion_flag.value: logger.info("STOP MOTION in viewer") last_iter = True except queue_empty: logger.warning("Loop viewer closed because queue is empty since 10 seconds") except: logger.error("FATAL ERROR in loop viewer: ") traceback.print_exc() sys.exit(0) self.queue_qt.close() self.loop_viewer_lock.release() logger.warning("## End of loop_viewer") def stop_process(self): """ Terminate the compute_cs, centroidal and wholebody process, close all the pipes and send the "stop motion" flag to the viewer """ self.stop_motion_flag.value = True logger.warning("STOP MOTION flag sent") if self.process_compute_cs: self.process_compute_cs.terminate() if self.pipe_cs_in: self.pipe_cs_in.close() if self.pipe_cs_out: self.pipe_cs_out.close() if self.pipe_cs_com_in: self.pipe_cs_com_in.close() if self.pipe_cs_com_out: self.pipe_cs_com_out.close() #if self.queue_qt: # self.queue_qt.close() if self.process_centroidal: self.process_centroidal.terminate() if self.process_wholebody: self.process_wholebody.terminate() def start_viewer_process(self): """ Create a new queue_qt object and start the loop_viewer method in a new process """ self.queue_qt = Queue() self.process_viewer = Process(target=self.loop_viewer) self.process_viewer.start() atexit.register(self.process_viewer.terminate) def start_process(self): """ Create new pipes and queue objects and start the centroidal, wholebody and viewer loops in new processes Also delete the last_phase stored """ self.pipe_cs_out, self.pipe_cs_in = Pipe(False) self.pipe_cs_com_out, self.pipe_cs_com_in = Pipe(False) #self.last_phase_pickled = Array(c_ubyte, MAX_PICKLE_SIZE) self.last_phase = None self.start_viewer_process() if self.process_centroidal: self.process_centroidal.terminate() self.process_centroidal = Process(target=self.loop_centroidal) self.process_centroidal.start() atexit.register(self.process_centroidal.terminate) if self.process_wholebody: self.process_wholebody.terminate() self.process_wholebody = Process(target=self.loop_wholebody) self.process_wholebody.start() atexit.register(self.process_wholebody.terminate) 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 compute_cs_requirements(self): """ Compute all the required data to use the ContactSequence stored in self.cs as input for the centroidal method or the wholebody method """ tools.computePhasesTimings(self.cs, self.cfg) tools.computePhasesCOMValues(self.cs, self.cfg.Robot.DEFAULT_COM_HEIGHT) tools.setAllUninitializedContactModel(self.cs, cfg.Robot) tools.computeRootTrajFromContacts(self.fullBody, self.cs) tools.setAllUninitializedFrictionCoef(self.cs, self.cfg.MU) 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 run_zero_step_capturability(self, move_to_support_polygon=True): """ Compute the centroidal trajectory to bring the current last_phaseto a stop without contact changes. Then start a viewer and a wholebody processes to generate and display the motion corresponding to this centroidal trajectory. :param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon """ cs_ref = self.compute_stopping_cs(move_to_support_polygon) self.start_viewer_process() self.cfg.IK_dt = 0.02 p = Process(target=self.compute_wholebody_queue, args=(cs_ref, )) p.start() def stop_motion(self, move_to_support_polygon=True): """ Terminate all the running contact, centroidal and wholebody processes and remove the stepping stones from the display. If the robot is not at a stop, compute and display a motion bringing it at a steady state :param move_to_support_polygon: if True, add a trajectory to put the CoM above the center of the support polygon """ self.stop_process() process_stones = Process(target=self.hide_stones_lock) process_stones.start() atexit.register(process_stones.terminate) if not self.is_at_stop(): logger.warning("!!!!!! REQUIRE STOP MOTION: compute 0-step capturability") self.run_zero_step_capturability(move_to_support_polygon) def move_to_goal(self, root_goal): """ Plan and execute a motion connecting the current configuration to one with the given root position If the robot is in motion, start by computing a safe stop motion. :param root_goal: list of size 3 or 7: translation and quaternion for the desired root position If the quaternion part is not specified, the final orientation is not constrained :return: """ self.stop_motion(False) self.plan_guide(root_goal) logger.info("Guide planning solved, path id = %d", self.current_guide_id) self.compute_cs_from_guide() self.compute_cs_requirements() logger.info("Start process") self.start_process() time.sleep(0.1) logger.info("@@@ Start compute_from_cs @@@") self.process_compute_cs = Process(target=self.compute_from_cs) self.process_compute_cs.start() atexit.register(self.process_compute_cs.terminate) logger.info("@@@ END compute_from_cs @@@") def find_closest_guide_time(self, path_id): """ Look for the index in the guide path that give the closest distance between the root position at this index and the wholebody root position in the last_phase configuration :param path_id: :return: """ last_phase = self.get_last_phase() if last_phase is None: return 0. root_wb = np.array(last_phase.q_final[0:2]) DT = 0.01 current_t = 0. min_t = 0. min_dist = math.inf ps = self.guide_planner.ps t_max = ps.pathLength(path_id) while current_t <= t_max: root_guide = np.array(ps.configAtParam(path_id, current_t)[0:2]) dist = np.linalg.norm(root_wb - root_guide) if dist < min_dist: min_dist = dist min_t = current_t current_t += DT return min_t def is_path_valid(self, path_id): """ Check if the given path id stored in self.guide_planner.ps is valid or not :param path_id: the id of the path :return: True if the path is completely valid, False otherwise """ DT = 0.01 # FIXME: timestep of the discretization for the collision checking of the path ps = self.guide_planner.ps robot_rom = self.guide_planner.rbprmBuilder t = self.find_closest_guide_time(path_id) - 0.1 if t < 0: t = 0. t_max = ps.pathLength(path_id) while t <= t_max: report = robot_rom.isConfigValid(ps.configAtParam(path_id, t)) if not report[0]: return False t += DT report = robot_rom.isConfigValid(ps.configAtParam(path_id, t_max)) if not report[0]: return False else: return True def add_obstacle_to_viewer(self, name, size, position, color=[0, 0, 1, 1]): """ Add an obstacle (a box) to the viewer. This call is blocking as it wait for a lock to access to the gepetto-gui API :param name: the node name of the new obstacle :param size: the size of the box [x, y, z] :param position: the placement (translation + quaternion) of the obstacle in the world frame :param color: the color (rgba) of the obstacle """ node_name = "world/environments/" + name #FIXME: change the prefix if there is changes in pinocchio ... logger.info("Waiting lock to add obstacles ...") self.viewer_lock.acquire() # add the obstacle to the viewer: self.gui.addBox(node_name, size[0], size[1], size[2], color) # move the obstacle to the given placement: self.gui.applyConfiguration(node_name, position) self.gui.refresh() self.viewer_lock.release() logger.info("Obstacles added in the viewer") def add_obstacle_to_problem_solvers(self, name, size, position, obstacle_client): """ Add an obstacle to the environment of the planner :param name: the name of the obstacle :param size: the size of the box (x, y, z) :param position: the placement (translation + quaternion) of the obstacle :param obstacle_client: a corba-server Obstacle client instance """ # add the obstacle to the problem solver: obstacle_client.createBox(name, size[0] + SCALE_OBSTACLE_COLLISION, size[1] + SCALE_OBSTACLE_COLLISION, size[2] + SCALE_OBSTACLE_COLLISION) obstacle_client.addObstacle(name, True, False) # move the obstacle to the given placement: obstacle_client.moveObstacle(name, position) def add_obstacle(self, size, position, color=[0, 0, 1, 1]): """ Add a cube to the environment, and recompute a motion if the current computed motion become invalid :param size: The size of the cube [x, y, z] :param position: The placement of the cube: either a list of length 3 for the translation or a list of size 7 for translation + quaternion :param color: color of the obstacle in the viewer, blue by default :return: """ logger.warning("!!!! ADD OBSTACLE REQUESTED") name = "obstacle_0" #FIXME: change the prefix if there is changes in pinocchio ... # Add an id until it is an unused name: i = 1 obs_names = self.guide_planner.ps.client.obstacle.getObstacleNames(True, False) while name in obs_names: name = "obstacle_" + str(i) i += 1 if len(position) == 3: position += [0, 0, 0, 1] logger.info("!!!! Addobstacle name : %s", name) self.add_obstacle_to_problem_solvers(name, size, position, self.guide_planner.ps.client.obstacle) self.add_obstacle_to_problem_solvers(name, size, position, self.fullBody.client.obstacle) logger.info("!!!! obstacle added to the problem") # add obstacle to the viewer, do it in a process as this call is blocking: process_obstacle = Process(target=self.add_obstacle_to_viewer, args=(name, size, position, color)) process_obstacle.start() atexit.register(process_obstacle.terminate) logger.info("!!!! start thread to display obstacle") # Check if the motion must be re-planned : if not self.is_at_stop(): logger.info("!!!!!! Add obstacle during motion, check path ...") valid = self.is_path_valid(self.current_guide_id) if valid: logger.warning("!!!!!! Current path is still valid, continue ...") else: logger.warning("!!!!!! Current path is now invalid ! Compute a new one ...") # Re plan the motion self.move_to_goal(self.current_root_goal) else: logger.warning("!!!!!! Add obstacle: The robot is not in motion")
# 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) # 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) print("number of contact phases in the contact sequence : ", cs.size()) # there is now one phase in our 'cs' object # we can now add new contact phases by changing the contacts of the last phase of the sequence: cs.breakContact( fb.rfoot ) # add a new contact phase to the sequence: break the right feet contact # create a new contact for the right feet at a different position : placement = cs.contactPhases[0].contactPatch( fb.rfoot).placement.copy() # take the previous position of the right feet pos = placement.translation pos[0] += 0.15 # move of 15cm in the x direction placement.translation = pos # add a new contact phase, with a contact created for the right feet at the given placement : cs.createContact(fb.rfoot, ContactPatch(placement))