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 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: the ContactPhase to modify :param curve_normalized: the curve is defined in [0,max], we need to shift it """ 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 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 initEmptyPhaseWholeBodyTrajectory(phase): """ Initialize wholebody trajectories with empty piecewise curves :param phase: the ContactPhase """ phase.q_t = piecewise() phase.dq_t = piecewise() phase.ddq_t = piecewise() phase.tau_t = piecewise()
def initEmptyPhaseCentroidalTrajectory(phase): """ Initialize centroidal trajectories with empty piecewise curves :param phase: the ContactPhase """ phase.c_t = piecewise() phase.dc_t = piecewise() phase.ddc_t = piecewise() phase.L_t = piecewise() phase.dL_t = piecewise()
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 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 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
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()