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