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 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 generateZeroAMreference(cs): """ For all phases in the sequence, set an L_t and dL_t trajectory constant at 0 with the correct duration :param cs: the contact sequence to modify """ for phase in cs.contactPhases: phase.L_t = polynomial(np.zeros(3), np.zeros(3), phase.timeInitial, phase.timeFinal) phase.dL_t = polynomial(np.zeros(3), np.zeros(3), phase.timeInitial, phase.timeFinal)
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 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 genAMTrajFromPhaseStates(phase, constraintVelocity = True): """ Generate a cubic spline connecting (L_init, dL_init) to (L_final, dL_final) and set it as the phase AM trajectory :param phase: the ContactPhase to use :param constraintVelocity: if False, generate only a linear interpolation and ignore the values of dL """ if constraintVelocity: am_traj = polynomial(phase.L_init, phase.dL_init, phase.L_final, phase.dL_final, phase.timeInitial, phase.timeFinal) else: am_traj = polynomial(phase.L_init, phase.L_final, phase.timeInitial, phase.timeFinal) phase.L_t = am_traj phase.dL_t = am_traj.compute_derivate(1)
def connectPhaseTrajToFinalState(phase, duration = None): """ Append to the trajectory of c, dc and ddc a quintic spline connecting phase.c_final, dc_final and ddc_final and L and dL with a trajectory at 0 :param phase: the contact phase to modify :param duration: the duration of the new trajectories. If not provided, the duration is computed from the last point in the current trajectories and phase.timeFinal """ if phase.c_t is None or phase.dc_t is None or phase.ddc_t is None: # initialise empty trajectories phase.c_t = piecewise() phase.dc_t = piecewise() phase.ddc_t = piecewise() # get the initial state from the phase : c_init = phase.c_init dc_init = phase.dc_init ddc_init = phase.ddc_init t_init = phase.timeInitial else: # get the initial state from the last points of the trajectories : t_init = phase.c_t.max() c_init = phase.c_t(t_init) dc_init = phase.dc_t(t_init) ddc_init = phase.ddc_t(t_init) if phase.L_t is None or phase.dL_t is None : # initialise empty trajectories phase.L_t = piecewise() phase.dL_t = piecewise() # get the initial state from the phase : L_init = phase.L_init dL_init = phase.dL_init else : L_init = phase.c_t(t_init) dL_init = phase.dL_t(t_init) if not phase.c_final.any(): raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized c_final") if duration is not None: t_final = t_init + duration else: t_final = phase.timeFinal com_t = polynomial(c_init, dc_init, ddc_init, phase.c_final, phase.dc_final, phase.ddc_final, t_init, t_final) L_t = polynomial(L_init, dL_init, phase.L_final, phase.dL_final, t_init, t_final) phase.c_t.append(com_t) phase.dc_t.append(com_t.compute_derivate(1)) phase.ddc_t.append(com_t.compute_derivate(2)) phase.L_t.append(L_t) phase.dL_t.append(L_t.compute_derivate(1)) phase.timeFinal = t_final
def computeWrench(cs, Robot, dt): rp = RosPack() urdf = rp.get_path( Robot.packageName ) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf' #srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf' robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) model = robot.model data = robot.data q_t = cs.concatenateQtrajectories() dq_t = cs.concatenateQtrajectories() ddq_t = cs.concatenateQtrajectories() t = q_t.min() for phase in cs.contactPhases: phase.wrench_t = None t = phase.timeInitial while t <= phase.timeFinal: pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t)) pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t), phase.ddq_t(t)) # FIXME : why do I need to call com to have the correct values in data ?? phi0 = data.oMi[1].act(pin.Force(data.tau[:6])) if phase.wrench_t is None: phase.wrench_t = piecewise( polynomial(phi0.vector.reshape(-1, 1), t, t)) else: phase.wrench_t.append(phi0.vector, t) t += dt if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.: t = phase.timeFinal
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 connectPhaseTrajToInitialState(phase, duration): """ Insert at the beginning of the trajectory of c, dc and ddc a quintic spline connecting phase.c_init, dc_init and ddc__init and L and dL with a trajectory at 0 :param phase: the contact phase to modify :param duration: the duration of the new trajectories. """ if duration <= 0.: return if phase.c_t is None or phase.dc_t is None or phase.ddc_t is None: raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized COM trajectory") if phase.L_t is None or phase.dL_t is None : raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized AM trajectory") if not phase.c_init.any(): raise RuntimeError("connectPhaseTrajToFinalState can only be called with a phase with an initialized c_final") t_final = phase.c_t.min() t_init = t_final - duration c_final = phase.c_t(t_final) dc_final = phase.dc_t(t_final) ddc_final = phase.ddc_t(t_final) L_final = phase.L_t(t_final) dL_final = phase.dL_t(t_final) com_t = polynomial( phase.c_init, phase.dc_init, phase.ddc_init,c_final, dc_final, ddc_final, t_init, t_final) L_t = polynomial(phase.L_init, phase.dL_init, L_final, dL_final, t_init, t_final) # insert this trajectories at the beginning of the phase : piecewise_c= piecewise(com_t) piecewise_c.append(phase.c_t) phase.c_t = piecewise_c piecewise_dc= piecewise(com_t.compute_derivate(1)) piecewise_dc.append(phase.dc_t) phase.dc_t = piecewise_dc piecewise_ddc= piecewise(com_t.compute_derivate(2)) piecewise_ddc.append(phase.ddc_t) phase.ddc_t = piecewise_ddc piecewise_L= piecewise(L_t) piecewise_L.append(phase.L_t) phase.L_t = piecewise_L piecewise_dL= piecewise(L_t.compute_derivate(1)) piecewise_dL.append(phase.dL_t) phase.dL_t = piecewise_dL # set the new initial time phase.timeInitial = t_init
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 genCOMTrajFromPhaseStates(phase, constraintVelocity = True, constraintAcceleration = True): """ Generate a quintic spline connecting exactly (c, dc, ddc) init to final :param phase: :param constraintVelocity: if False, generate only a linear interpolation and ignore ddc, and dc values :param constraintAcceleration: if False, generate only a cubic spline and ignore ddc values """ if constraintAcceleration and not constraintVelocity: raise ValueError("Cannot constraints acceleration if velocity is not constrained.") if constraintAcceleration: com_traj = polynomial(phase.c_init, phase.dc_init, phase.ddc_init, phase.c_final, phase.dc_final, phase.ddc_final,phase.timeInitial, phase.timeFinal) elif constraintVelocity: com_traj = polynomial(phase.c_init, phase.dc_init, phase.c_final, phase.dc_final, phase.timeInitial, phase.timeFinal) else: com_traj = polynomial(phase.c_init, phase.c_final, phase.timeInitial, phase.timeFinal) phase.c_t = com_traj phase.dc_t = com_traj.compute_derivate(1) phase.ddc_t = com_traj.compute_derivate(2)
def constantSE3curve(placement, t_min, t_max = None): """ Create a constant SE3_curve at the given placement for the given duration :param placement: the placement :param t_min: the initial time :param t_max: final time, if not provided the curve will have a duration of 0 :return: the constant curve """ if t_max is None: t_max = t_min rot = SO3Linear(placement.rotation, placement.rotation, t_min, t_max) trans = polynomial(placement.translation.reshape(-1,1), t_min, t_max) return SE3Curve(trans, rot)
def computeZMPFromWrench(cs, Robot, dt): for phase in cs.contactPhases: t = phase.timeInitial phase.zmp_t = None while t <= phase.timeFinal: phi0 = Force(phase.wrench_t(t)) zmp = shiftZMPtoFloorAltitude(cs, t, phi0, Robot) if phase.zmp_t is None: phase.zmp_t = piecewise(polynomial(zmp.reshape(-1, 1), t, t)) else: phase.zmp_t.append(zmp, t) t += dt if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.: t = phase.timeFinal
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 computeWrenchRef(cs, mass, G, dt): Mcom = SE3.Identity() for phase in cs.contactPhases: t = phase.timeInitial phase.wrench_t = None while t <= phase.timeFinal: Mcom.translation = phase.c_t(t) Fcom = Force.Zero() Fcom.linear = mass * (phase.ddc_t(t) - G) Fcom.angular = phase.dL_t(t) F0 = Mcom.act(Fcom) if phase.wrench_t is None: phase.wrench_t = piecewise( polynomial(F0.vector.reshape(-1, 1), t, t)) else: phase.wrench_t.append(F0.vector, t) t += dt if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.: t = phase.timeFinal
com = array(fb.getCenterOfMass()) phase.c_init = com phase.c_final = com phase.q_final = phase.q_init # generate com motion : t = 0 # first go above the right feet in 2 seconds c0 = com.copy() dc0 = zeros(3) ddc0 = zeros(3) c1 = phase.contactPatch('leg_right_sole_fix_joint').placement.translation c1[2] = c0[2] - 0.03 # go down 3cm to avoid kinematic limits dc1 = zeros(3) ddc1 = zeros(3) c_t_right = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+2) t = c_t_right.max() c0 = c1.copy() # o above the left feet in 3 seconds: c1 = phase.contactPatch('leg_left_sole_fix_joint').placement.translation c1[2] = c0[2] c_t_left = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+3) t = c_t_left.max() c0 = c1.copy() # go back to the initial CoM position in 2 seconds c1 = com c_t_mid = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+2) c_t = piecewise(c_t_right) c_t.append(c_t_left) c_t.append(c_t_mid)