def computeCommand(t, q, v, sensors_data, u):
# Verify sensor data.
f = Force(sensors_data[jiminy.ForceSensor.type, "MassBody"], np.zeros(3))
f_joint_sensor = self.robot.pinocchio_model.frames[idx].placement * f
f_jiminy = engine.system_state.f_external[self.robot.pinocchio_model.frames[idx].parent]
self.assertTrue(np.allclose(f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE))
u[:] = 0.0
def computeWrenchRef(res):
Mcom = SE3.Identity()
for k, t in enumerate(res.t_t):
Mcom.translation = res.c_reference[:, k]
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (res.ddc_reference[:, k] - cfg.GRAVITY)
Fcom.angular = res.dL_reference[:, k]
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k] = F0.vector
return res
def check_sensors_data(t, q, v, sensors_data, command):
nonlocal engine, frame_pose
# Verify sensor data, if the engine has been initialized
if engine.is_initialized:
contact_data = sensors_data[
jiminy.ContactSensor.type, self.body_name]
f = Force(contact_data, np.zeros(3))
f_joint_sensor = frame_pose * f
f_jiminy = engine.system_state.f_external[joint_idx]
self.assertTrue(np.allclose(
f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE))
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 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 computeZMPFromWrench(cs, time_t, wrench_t):
N = len(time_t)
ZMP_t = np.matrix(np.empty((3, N)))
# smooth wrench traj :
Wrench_trajectory = Trajectories.DifferentiableEuclidianTrajectory()
Wrench_trajectory.computeFromPoints(np.asmatrix(time_t), wrench_t,
0 * wrench_t)
for k in range(N):
wrench = Wrench_trajectory(time_t[k])[0]
phi0 = Force(wrench)
ZMP_t[:, k] = shiftZMPtoFloorAltitude(cs, time_t[k], phi0)
return ZMP_t
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 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 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)' "
td = np.tensordot
quad = lambda H, v: np.matrix(td(td(H, v, [2, 0]), v, [1, 0])).T
#################### """ 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. ########################################################################## # Forward Kinematics
def storeData(k_t, res, q, v, dv, invdyn, sol):
# store current state
res.q_t[:, k_t] = q
res.dq_t[:, k_t] = v
res.ddq_t[:, k_t] = dv
res.tau_t[:, k_t] = invdyn.getActuatorForces(
sol) # actuator forces, with external forces (contact forces)
#store contact info (force and status)
if cfg.IK_store_contact_forces:
for eeName, contact in dic_contacts.iteritems():
if invdyn.checkContact(contact.name, sol):
res.contact_forces[eeName][:,
k_t] = invdyn.getContactForce(
contact.name, sol)
res.contact_normal_force[
eeName][:, k_t] = contact.getNormalForce(
res.contact_forces[eeName][:, k_t])
res.contact_activity[eeName][:, k_t] = 1
# store centroidal info (real one and reference) :
if cfg.IK_store_centroidal:
pcom, vcom, acom = pinRobot.com(q, v, dv)
res.c_t[:, k_t] = pcom
res.dc_t[:, k_t] = vcom
res.ddc_t[:, k_t] = acom
res.L_t[:, k_t] = pinRobot.centroidalMomentum(q, v).angular
#res.dL_t[:,k_t] = pinRobot.centroidalMomentumVariation(q,v,dv) # FIXME : in robot wrapper, use * instead of .dot() for np matrices
pin.dccrba(pinRobot.model, pinRobot.data, q, v)
res.dL_t[:, k_t] = Force(
pinRobot.data.Ag.dot(dv) + pinRobot.data.dAg.dot(v)).angular
# same for reference data :
res.c_reference[:, k_t] = com_desired
res.dc_reference[:, k_t] = vcom_desired
res.ddc_reference[:, k_t] = acom_desired
res.L_reference[:, k_t] = L_desired
res.dL_reference[:, k_t] = dL_desired
if cfg.IK_store_zmp:
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]))
res.wrench_t[:, k_t] = phi0.vector
res.zmp_t[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], phi0, cfg.EXPORT_OPENHRP)
# same but from the 'reference' values :
Mcom = SE3.Identity()
Mcom.translation = com_desired
Fcom = Force.Zero()
Fcom.linear = cfg.MASS * (acom_desired - cfg.GRAVITY)
Fcom.angular = dL_desired
F0 = Mcom.act(Fcom)
res.wrench_reference[:, k_t] = F0.vector
res.zmp_reference[:, k_t] = shiftZMPtoFloorAltitude(
cs, res.t_t[k_t], F0, cfg.EXPORT_OPENHRP)
if cfg.IK_store_effector:
for eeName in usedEffectors: # real position (not reference)
res.effector_trajectories[eeName][:, k_t] = SE3toVec(
getCurrentEffectorPosition(robot, invdyn.data(), eeName))
res.d_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorVelocity(robot, invdyn.data(), eeName))
res.dd_effector_trajectories[eeName][:, k_t] = MotiontoVec(
getCurrentEffectorAcceleration(robot, invdyn.data(),
eeName))
return res
