def setCOMfromCurve(phase, curve_normalized): """ Initialise the phase c_t dc_t and ddc_t from the given curve, Also set the final values for the CoM from the final points of the curve Also set or increase the final time from the duration of the curve :param phase: :param curve_normalized: the curve is defined in [0,max], we need to shift it :return: """ if phase.c_t is None: phase.c_t = piecewise() phase.dc_t = piecewise() phase.ddc_t = piecewise() t_min = phase.timeInitial # set initial values phase.c_init = curve_normalized(0.) phase.dc_init = curve_normalized.derivate(0., 1) phase.ddc_init = curve_normalized.derivate(0., 2) else: t_min = phase.c_t.max() t_max = t_min + curve_normalized.max() # shift the curve to the correct initial time : curve = bezier(curve_normalized.waypoints(), t_min, t_max) # set the trajectory in the phase : phase.c_t.append(curve) phase.dc_t.append(curve.compute_derivate(1)) phase.ddc_t.append(curve.compute_derivate(2)) # set the new final values : phase.timeFinal = t_max phase.c_final = phase.c_t(t_max) phase.dc_final = phase.dc_t(t_max) phase.ddc_final = phase.ddc_t(t_max)
def appendJointsDerivatives(first_iter_for_phase=False): 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 appendZMP(first_iter_for_phase=False): 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: :param duration: """ 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 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: :param duration: """ 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 appendJointsValues(first_iter_for_phase=False): 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)
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 appendContactForcesTrajs(first_iter_for_phase=False): if first_iter_for_phase and phase_prev: for eeName in phase_prev.effectorsInContact(): if t > phase_prev.contactForce(eeName).max(): if phase.isEffectorInContact(eeName): contact = dic_contacts[eeName] contact_forces = invdyn.getContactForce( contact.name, sol) contact_normal_force = np.array( contact.getNormalForce(contact_forces)) else: contact_normal_force = np.zeros(1) if cfg.Robot.cType == "_3_DOF": contact_forces = np.zeros(3) else: contact_forces = np.zeros(12) phase_prev.contactForce(eeName).append(contact_forces, t) phase_prev.contactNormalForce(eeName).append( contact_normal_force.reshape(1), t) 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: 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 appendCentroidal(first_iter_for_phase=False): 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 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 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
def initEmptyPhaseWholeBodyTrajectory(phase): phase.q_t = piecewise() phase.dq_t = piecewise() phase.ddq_t = piecewise() phase.tau_t = piecewise()
def initEmptyPhaseCentroidalTrajectory(phase): phase.c_t = piecewise() phase.dc_t = piecewise() phase.ddc_t = piecewise() phase.L_t = piecewise() phase.dL_t = piecewise()
def appendTorques(first_iter_for_phase=False): 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)
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) phase.c_t = c_t phase.dc_t = c_t.compute_derivate(1) phase.ddc_t = c_t.compute_derivate(2) # set the timings of the phase : phase.timeInitial = c_t.min() phase.timeFinal = c_t.max() # set a 0 AM trajectory generateZeroAMreference(cs) assert cs.haveTimings()