def __call__(self, t): assert t >= 0.0, "Time must be non-negative" i = int(t / self._dt) if (i >= self._x_ref.shape[1]): raise ValueError( "Specified time exceeds the duration of the trajectory: " + str(t)) M = self._M_ref[i] M.translation = self._x_ref[:, i] v = Motion.Zero() a = Motion.Zero() v.linear = self._v_ref[:, i] a.linear = self._a_ref[:, i] return (M, v, a)
def MotionFromVec(vect): if vect.shape[0] != 6 or vect.shape[1] != 1: raise ValueError("MotionFromVec take as input a vector of size 6") m = Motion.Zero() m.linear = np.matrix(vect[0:3]) m.angular = np.matrix(vect[3:6]) return m
def compute_for_normalized_time(self, t): if t < 0: print "Trajectory called with negative time." return self.compute_for_normalized_time(0) elif t > self.t_total: print "Trajectory called after final time." return self.compute_for_normalized_time(self.t_total) self.M = SE3.Identity() self.v = Motion.Zero() self.a = Motion.Zero() self.M.translation = self.curves(t) self.v.linear = self.curves.d(t) self.a.linear = self.curves.dd(t) #rotation : if self.curves.isInFirstNonZero(t): self.M.rotation = self.placement_init.rotation.copy() elif self.curves.isInLastNonZero(t): self.M.rotation = self.placement_end.rotation.copy() else: # make a slerp between self.effector_placement[id][0] and [1] : quat0 = Quaternion(self.placement_init.rotation) quat1 = Quaternion(self.placement_end.rotation) t_rot = t - self.t_mid_begin """ print "t : ",t print "t_mid_begin : ",self.t_mid_begin print "t_rot : ",t_rot print "t mid : ",self.t_mid """ assert t_rot >= 0, "Error in the time intervals of the polybezier" assert t_rot <= (self.t_mid + 1e-6 ), "Error in the time intervals of the polybezier" u = t_rot / self.t_mid # normalized time without the pre defined takeoff/landing phases self.M.rotation = (quat0.slerp(u, quat1)).matrix() # angular velocity : dt = 0.001 u_dt = dt / self.t_mid r_plus_dt = (quat0.slerp(u + u_dt, quat1)).matrix() self.v.angular = pin.log3(self.M.rotation.T * r_plus_dt) / dt r_plus2_dt = (quat0.slerp(u + (2. * u_dt), quat1)).matrix() next_angular_velocity = pin.log3(r_plus_dt.T * r_plus2_dt) / dt self.a.angular = (next_angular_velocity - self.v.angular) / dt #r_plus_dt = (quat0.slerp(u+u_dt,quat1)).matrix() #next_angular_vel = (pin.log3(self.M.rotation.T * r_plus_dt)/dt) #self.a.angular = (next_angular_vel - self.v.angular)/dt return self.M, self.v, self.a
def __call__(self, t): assert (t - self._t_init ) >= 0.0, "Time must be greater than or equal to t_init" i = int((t - self._t_init) / self._dt) if (i >= self._x_ref.shape[1]): i = self._x_ref.shape[1] - 1 # raise ValueError("Specified time exceeds the duration of the trajectory: "+str(t)); M = self._M_final v = Motion.Zero() a = Motion.Zero() M.translation = self._x_ref[:, i] v.linear = self._v_ref[:, i] a.linear = self._a_ref[:, i] return (M, v, a)
def __init__(self, placement_init, placement_final, time_interval): RefTrajectory.__init__(self, "TrajectorySE3LinearInterp") self.placement_init = placement_init self.placement_final = placement_final self.t0 = time_interval[0] self.t1 = time_interval[1] self.length = self.t1 - self.t0 self.quat0 = Quaternion(self.placement_init.rotation) self.quat1 = Quaternion(self.placement_final.rotation) self.M = SE3.Identity() self.v = Motion.Zero() self.a = Motion.Zero() # constant velocity and null acceleration : self.v.linear = (placement_final.translation - placement_final.translation) / self.length self.v.angular = pin.log3(placement_final.rotation.T * placement_final.rotation) / self.length
def MotionFromVec(vect): """ Convert a vector of size 6 to a pinocchio.Motion object. See MotiontoVec() :param vect: a numpy array or matrix of size 6 :return: a Motion object """ if vect.shape[0] != 6 or vect.shape[1] != 1: raise ValueError("MotionFromVec take as input a vector of size 6") m = Motion.Zero() m.linear = np.array(vect[0:3]) m.angular = np.array(vect[3:6]) return m
def __init__(self, curves, placement_init, placement_end, time_interval): RefTrajectory.__init__(self, "BezierTrajectory") self.placement_init = placement_init self.placement_end = placement_end self.curves = curves self.time_interval = time_interval self.t_total = curves.max() assert abs( self.t_total - (time_interval[1] - time_interval[0]) ) <= 1e-4, "time interval is not coherent with the length of the Bezier curves" assert len( curves.times ) % 2 == 0, "PolyBezier object contain an even number of curves, not implemented yet." id_mid = len(curves.times) / 2 # retrieve the timings of the middle segment (duration and begin/end wrt to the other curves) self.t_mid_begin = curves.times[id_mid - 1] self.t_mid_end = curves.times[id_mid] self.t_mid = curves.curves[curves.numCurves() / 2].max() curves.computeDerivates() self.M = SE3.Identity() self.v = Motion.Zero() self.a = Motion.Zero()
#descendants = lambda root,robot: filter( lambda i: root in ancestorOf(i,robot), range(root,robot.model.njoints) ) descendants = lambda root, robot: robot.model.subtrees[root] def setRobotArgs(robot): ancestors.__defaults__ = (robot, ) + ancestors.__defaults__ descendants.__defaults__ = (robot, ) #ancestorsOf.__init__.__defaults__ = (robot,) iv.__defaults__ = (robot, ) parent.__defaults__ = (robot, ) jFromIdx.__defaults__ = (robot, ) # --- SE3 operators Mcross = lambda x, y: Motion(x).cross(Motion(y)).vector Mcross.__doc__ = "Motion cross product" Fcross = lambda x, y: Motion(x).cross(Force(y)).vector Fcross.__doc__ = "Force cross product" MCross = lambda V, v: np.bmat([Mcross(V[:, i], v) for i in range(V.shape[1])]) FCross = lambda V, f: np.bmat([Fcross(V[:, i], f) for i in range(V.shape[1])]) adj = lambda nu: np.bmat([[skew(nu[3:]), skew(nu[:3])], [zero([3, 3]), skew(nu[3:])]]) adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)" adjdual = lambda nu: np.bmat([[skew(nu[3:]), zero([3, 3])], [skew(nu[:3]), skew(nu[3:])]]) adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' "
def createMultiphaseShootingProblem(rmodel, rdata, csw, timeStep): """ Create a Multiphase Shooting problem from the output of the centroidal solver. :params rmodel: robot model of type pinocchio::model :params rdata: robot data of type pinocchio::data :params csw: contact sequence wrapper of type ContactSequenceWrapper :params timeStep: Scalar timestep between nodes. :returns list of IntegratedActionModels """ # ----------------------- # Define Cost weights class Weights: com = 1e1 regx = 1e-3 regu = 0. swing_patch = 1e6 forces = 0. contactv = 1e3 # Define state cost vector for WeightedActivation ff_orientation = 1e1 xweight = np.array([0] * 3 + [ff_orientation] * 3 + [1.] * (rmodel.nv - 6) + [1.] * rmodel.nv) xweight[range(18, 20)] = ff_orientation # for patch in swing_patch: w.swing_patch.append(100.); # Define weights for the impact costs. imp_state = 1e2 imp_com = 1e2 imp_contact_patch = 1e6 imp_act_com = m2a([0.1, 0.1, 3.0]) # Define weights for the terminal costs term_com = 1e8 term_regx = 1e4 w = Weights() # ------------------------ problem_models = [] actuationff = ActuationModelFreeFloating(rmodel) State = StatePinocchio(rmodel) active_contact_patch = set() active_contact_patch_prev = set() for nphase, phase in enumerate(csw.cs.contact_phases): t0 = phase.time_trajectory[0] t1 = phase.time_trajectory[-1] N = int(round((t1 - t0) / timeStep)) + 1 contact_model = ContactModelMultiple(rmodel) # Add contact constraints for the active contact patches. # Add SE3 cost for the non-active contact patches. swing_patch = [] active_contact_patch_prev = active_contact_patch.copy() active_contact_patch.clear() for patch in csw.ee_map.keys(): if getattr(phase, patch).active: active_contact_patch.add(patch) active_contact = ContactModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=getattr(phase, patch).placement) contact_model.addContact(patch, active_contact) # print nphase, "Contact ",patch," added at ", getattr(phase,patch).placement.translation.T else: swing_patch.append(patch) # Check if contact has been added in this phase. If this phase is not zero, # add an impulse model to deal with this contact. new_contacts = active_contact_patch.difference(active_contact_patch_prev) if nphase != 0 and len(new_contacts) != 0: # print nphase, "Impact ",[p for p in new_contacts]," added" imp_model = ImpulseModelMultiple( rmodel, { "Impulse_" + patch: ImpulseModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch])) for patch in new_contacts }) # Costs for the impulse of a new contact cost_model = CostModelSum(rmodel, nu=0) # State cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState, nu=actuationff.nu, activation=ActivationModelWeightedQuad(w.xweight)) cost_model.addCost("imp_regx", cost_regx, w.imp_state) # CoM cost_com = CostModelImpactCoM(rmodel, activation=ActivationModelWeightedQuad(w.imp_act_com)) cost_model.addCost("imp_CoM", cost_com, w.imp_com) # Contact Frameplacement for patch in new_contacts: cost_contact = CostModelFramePlacement(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t0)[0]), nu=actuationff.nu) cost_model.addCost("imp_contact_" + patch, cost_contact, w.imp_contact_patch) imp_action_model = ActionModelImpact(rmodel, imp_model, cost_model) problem_models.append(imp_action_model) # Define the cost and action models for each timestep in the contact phase. # untill [:-1] because in contact sequence timetrajectory, the end-time is # also included. e.g., instead of being [0.,0.5], time trajectory is [0,0.5,1.] for t in np.linspace(t0, t1, N)[:-1]: cost_model = CostModelSum(rmodel, actuationff.nu) # For the first node of the phase, add cost v=0 for the contacting foot. if t == 0: for patch in active_contact_patch: cost_vcontact = CostModelFrameVelocity(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=m2a(Motion.Zero().vector), nu=actuationff.nu) cost_model.addCost("contactv_" + patch, cost_vcontact, w.contactv) # CoM Cost cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu) cost_model.addCost("CoM", cost_com, w.com) # Forces Cost for patch in contact_model.contacts.keys(): cost_force = CostModelForce(rmodel, contactModel=contact_model.contacts[patch], ref=m2a(csw.phi_c.forces[patch].eval(t)[0]), nu=actuationff.nu) cost_model.addCost("forces_" + patch, cost_force, w.forces) # Swing patch cost for patch in swing_patch: cost_swing = CostModelFramePlacement(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t)[0]), nu=actuationff.nu) cost_model.addCost("swing_" + patch, cost_swing, w.swing_patch) # print t, "Swing cost ",patch," added at ", csw.ee_splines[patch].eval(t)[0][:3].T # State Regularization cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState, nu=actuationff.nu, activation=ActivationModelWeightedQuad(w.xweight)) cost_model.addCost("regx", cost_regx, w.regx) # Control Regularization cost_regu = CostModelControl(rmodel, nu=actuationff.nu) cost_model.addCost("regu", cost_regu, w.regu) dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model) imodel = IntegratedActionModelEuler(dmodel, timeStep=timeStep) problem_models.append(imodel) # Create Terminal Model. contact_model = ContactModelMultiple(rmodel) # Add contact constraints for the active contact patches. swing_patch = [] t = t1 for patch in csw.ee_map.keys(): if getattr(phase, patch).active: active_contact = ContactModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=getattr(phase, patch).placement) contact_model.addContact(patch, active_contact) cost_model = CostModelSum(rmodel, actuationff.nu) # CoM Cost cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu) cost_model.addCost("CoM", cost_com, w.term_com) # State Regularization cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState, nu=actuationff.nu, activation=ActivationModelWeightedQuad(w.xweight)) cost_model.addCost("regx", cost_regx, w.term_regx) dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model) imodel = IntegratedActionModelEuler(dmodel) problem_models.append(imodel) problem_models.append return problem_models
def __init__(self, name, Mref): self._name = name self._dim = 6 self._Mref = Mref self._v_ref = Motion.Zero() self._a_ref = Motion.Zero()
# Spatial ALgebra #################### """ 1 The scene, and later the movement and forces in the scene are modeled following Featherstone's Spatial Algebra. 2 Placement, i.e. rotation and translation of frames (and bodies) are stored in objects of the class SE3. 3 Rigid velocities and acceleration are stored in the class Motion, 4 Forces are stored in the class Forces 5 Masses/Inertias can be found in the class Inertias. An important point to note is that these objects store linear and angular part separately, but we often have to come back to a plain vector/matrix representation. In that case, contrary to Featherstone, we rather store linear part first and angular second. """ M = SE3.Random() nu = Motion.Random() phi = Force.Random() Y = Inertia.Random() print(M, nu, phi, Y) # As mentioned before, the linear and the angular components are stored separately. However we can easily get back to a vector representation. print(nu, nu.vector.T) # Pinocchio strictly separates the constant model element in the Model class, and all the buffers for storing algorithmic quantities # We can create a data buffer through... rdata = model.createData() # By default the RobotWrapper already createsa data buffer through.... robot.data # The idea is that the same model can be used by different part of the algorithm to compute different values from different argument. # For example, in a optimal-control implementation of Pinocchio, you likely want to have a single robot model for all your problem, # but several data for each node of your optimal control solver. ##########################################################################
def __call__(self, t): # assert t <= self.time_intervals[-1][1], "t must be lower than the final time tf={}".format(self.time_intervals[-1][1]) index = len(self.t0_l) - 1 if t > self.time_intervals[1]: t = self.time_intervals[1] elif t < self.time_intervals[0]: t = self.time_intervals[0] index = 0 else: for k in range(len(self.t0_l)): if self.t0_l[k] > t: index = k - 1 break xyz_polycoeff = self.polycoeff_l[index] dxyz_polycoeff = self.dpolycoeff_l[index] ddxyz_polycoeff = self.ddpolycoeff_l[index] t0 = self.time_intervals[0] t1 = self.time_intervals[1] if t0 == t1: tau = 0. dtau_dt = 0. else: tau = (t - t0) / (t1 - t0) dtau_dt = 1. # Evaluate X x = polyval(tau, xyz_polycoeff[0]) if len(dxyz_polycoeff[0]): x_dot = polyval(tau, dxyz_polycoeff[0]) * dtau_dt else: x_dot = 0. if len(ddxyz_polycoeff[0]): x_dotdot = polyval(tau, ddxyz_polycoeff[0]) * dtau_dt**2 else: x_dotdot = 0. # Evaluate Y y = polyval(tau, xyz_polycoeff[1]) if len(dxyz_polycoeff[1]): y_dot = polyval(tau, dxyz_polycoeff[1]) * dtau_dt else: y_dot = 0. if len(ddxyz_polycoeff[1]): y_dotdot = polyval(tau, ddxyz_polycoeff[1]) * dtau_dt**2 else: y_dotdot = 0. # Evaluate Z x0 = polyval(0., xyz_polycoeff[0]) x1 = polyval(1., xyz_polycoeff[0]) if x0 == x1: tau_x = 0. dtau_x_dt = 0. else: tau_x = (x - x0) / (x1 - x0) dtau_x_dt = x_dot z = polyval(tau_x, xyz_polycoeff[2]) if len(dxyz_polycoeff[2]): z_dot = polyval(tau_x, dxyz_polycoeff[2]) * x_dot else: z_dot = 0. if len(ddxyz_polycoeff[2]): z_dotdot = polyval(tau_x, ddxyz_polycoeff[2]) * x_dot**2 + polyval(tau_x, dxyz_polycoeff[2]) * x_dotdot else: z_dotdot = 0. M = SE3.Identity() v = Motion.Zero() a = Motion.Zero() M.translation = np.matrix([x, y, z]).T M.rotation = self._R v.linear = np.matrix([x_dot, y_dot, z_dot]).T a.linear = np.matrix([x_dotdot, y_dotdot, z_dotdot]).T return M, v, a
sot.push(taskCom.task.name) sot.push(taskLH.task.name) sot.push(taskRH.task.name) toe = TOE("test_entity_toe") id4 = ((1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 0.0, 1.0)) epsilon = 0.1 ref_offset = 0.1 tau_offset = np.asarray((ref_offset, ) * njoints).squeeze() toe.init(urdfPath, id4, epsilon, OperationalPointsMap['waist'], OperationalPointsMap['chest']) plug(robot.device.state, toe.base6d_encoders) toe.gyroscope.value = (0., 0., 0.) gravity = Motion.Zero() gravity.linear = np.asarray((0.0, 0.0, -9.81)) # ------------------------------------------------------------------------------- # ----- MAIN LOOP --------------------------------------------------------------- # ------------------------------------------------------------------------------- robot.device.increment(dt) np.set_printoptions(suppress=True) def runner(n): for i in range(n): q_pin = fromSotToPinocchio(robot.device.state.value) q_pin[0, :6] = 0.0 q_pin[0, 6] = 1.0 nvZero = np.asarray((0.0, ) * pinocchioRobot.model.nv)
def __call__(self, t): return (self._Mref, Motion.Zero(), Motion.Zero())