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())
