def test_getters(self):
data = self.model.createData()
q = pin.randomConfiguration(self.model)
v = np.random.rand(self.model.nv)
a = np.random.rand(self.model.nv)
pin.forwardKinematics(self.model, data, q, v, a)
T = pin.updateFramePlacement(self.model, data, self.frame_idx)
self.assertApprox(T,
data.oMi[self.parent_idx].act(self.frame_placement))
v = pin.getFrameVelocity(self.model, data, self.frame_idx)
self.assertApprox(v,
self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
self.assertApprox(v,
self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
self.assertApprox(
v, data.oMi[self.parent_idx].act(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(
v,
pin.SE3(T.rotation, np.zeros(3)).act(
self.frame_placement.actInv(data.v[self.parent_idx])))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx)
self.assertApprox(a,
self.frame_placement.actInv(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
self.assertApprox(a,
self.frame_placement.actInv(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
self.assertApprox(
a, data.oMi[self.parent_idx].act(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(
a,
pin.SE3(T.rotation, np.zeros(3)).act(
self.frame_placement.actInv(data.a[self.parent_idx])))
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx)
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
a = pin.getFrameClassicalAcceleration(
self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def get_body_world_acceleration(robot: jiminy.Model,
body_name: str,
use_theoretical_model: bool = True) -> pin.SE3:
"""Get the body spatial acceleration in world frame.
The moment of this tensor (i.e linear part) is NOT the linear acceleration
of the center of the body frame, expressed in the world frame.
.. warning::
It is assumed that `update_quantities` has been called.
:param robot: Jiminy robot.
:param body_name: Name of the body.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
:returns: Spatial acceleration.
"""
# Pick the right pinocchio model and data
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
# Get frame index and make sure it exists
body_id = pnc_model.getFrameId(body_name)
assert body_id < pnc_model.nframes, f"Frame '{body_name}' does not exits."
return pin.getFrameAcceleration(
pnc_model, pnc_data, body_id, pin.LOCAL_WORLD_ALIGNED)
def calc(self, data, x):
assert (self.Mref.oMf is not None or self.gains[0] == 0.)
data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
if self.gains[0] != 0.:
self.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame]
data.a0 += np.asscalar(self.gains[0]) * pinocchio.log6(self.rMf).vector
if self.gains[1] != 0.:
v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector
data.a0 += np.asscalar(self.gains[1]) * v
def calc(self, data, x):
assert (self.xref.oxf is not None or self.gains[0] == 0.)
v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.xref.frame)
self.vw = v.angular
self.vv = v.linear
fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.xref.frame,
pinocchio.ReferenceFrame.LOCAL)
data.Jc = fJf[:3, :]
self.Jw = fJf[3:, :]
data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio,
self.xref.frame).linear + pinocchio.utils.cross(self.vw, self.vv)
if self.gains[0] != 0.:
data.a0 += np.asscalar(self.gains[0]) * (data.pinocchio.oMf[self.xref.frame].translation - self.xref.oxf)
if self.gains[1] != 0.:
data.a0 += np.asscalar(self.gains[1]) * self.vv
def calc(self, data, x):
# We suppose forwardKinematics(q,v,a), computeJointJacobian and updateFramePlacement already
# computed.
assert (self.ref is not None or self.gains[0] == 0.)
data.J[:, :] = pinocchio.getFrameJacobian(
self.pinocchio, data.pinocchio, self.frame,
pinocchio.ReferenceFrame.LOCAL)
data.a0[:] = pinocchio.getFrameAcceleration(self.pinocchio,
data.pinocchio,
self.frame).vector.flat
if self.gains[0] != 0.:
data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame]
data.a0[:] += self.gains[0] * m2a(pinocchio.log(data.rMf).vector)
if self.gains[1] != 0.:
data.a0[:] += self.gains[1] * m2a(
pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).vector)
def calc(self, data, x):
# We suppose forwardKinematics(q,v,np.zero), computeJointJacobian and updateFramePlacement already
# computed.
assert (self.ref is not None or self.gains[0] == 0.)
data.v = pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).copy()
vw = data.v.angular
vv = data.v.linear
J6 = pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio,
self.frame,
pinocchio.ReferenceFrame.LOCAL)
data.J[:, :] = J6[:3, :]
data.Jw[:, :] = J6[3:, :]
data.a0[:] = (pinocchio.getFrameAcceleration(
self.pinocchio, data.pinocchio, self.frame).linear +
cross(vw, vv)).flat
if self.gains[0] != 0.:
data.a0[:] += self.gains[0] * (
m2a(data.pinocchio.oMf[self.frame].translation) - self.ref)
if self.gains[1] != 0.:
data.a0[:] += self.gains[1] * m2a(vv)
M = data.M
Mi = pinocchio.computeMinverse(model, data, q)
da = data.ddq_dq
dtau = data.dtau_dq
assert (absmax(M * da + dtau) < 1e-9)
assert (absmax(da + Mi * dtau) < 1e-9)
pinocchio.forwardKinematics(model, data, q, v, a)
pinocchio.updateFramePlacements(model, data)
frameJacobian = pinocchio.getFrameJacobian(model, data, 10,
pinocchio.ReferenceFrame.LOCAL)
frameJacobianTimeVariation = pinocchio.getFrameJacobianTimeVariation(
model, data, 10, pinocchio.ReferenceFrame.LOCAL)
frameAcceleration = pinocchio.getFrameAcceleration(model, data, 10)
assert (absmax(frameJacobian * a + frameJacobianTimeVariation * v -
frameAcceleration.vector) < 1e-9)
'''
(a,f) = K^-1 (tau-b,-gamma)
avec K = [ M J* ; J 0 ]
(a',f') = -K^-1 K' K^-1 (tau-b,-gamma) - K^-1 (b';gamma')
= -Ki K' (a,f) - Ki (b';g')
= -Ki ( K'(a,f) - (b',g') )
'''
# Define finite-diff routines.
# Check ABA derivatives (without forces)
def compute_force(self, project_in_friction_cone):
M = self.data.oMf[self.frame_id]
self.p = M.translation
delta_p = self.p0 - self.p
R = se3.SE3(M.rotation, 0 * M.translation)
v_local = se3.getFrameVelocity(self.model, self.data, self.frame_id)
v_world = (R.act(v_local)).linear
dJv_local = se3.getFrameAcceleration(self.model, self.data,
self.frame_id)
dJv_local.linear += np.cross(v_local.angular, v_local.linear, axis=0)
dJv_world = (R.act(dJv_local)).linear
self.v = v_world
self.dJv = dJv_world
if (self.slipping):
self.dp0 = self.v - self.v.dot(self.normal) * self.normal
self.f = self.K @ delta_p + self.B @ (self.dp0 - v_world)
if (project_in_friction_cone):
self.dp0 = zero(3)
# check whether point is in contact
if (self.f.dot(self.normal) <= 0.0):
# print("\nINFO: Negative normal force %s!"%(self.frame_name), delta_p.T, self.f.T)
self.f = zero(3)
# do not reset v and dJv because they could be used by ExponentialSimulator
# self.v = zero(3)
# self.dJv = zero(3)
else:
# if(not self.active):
# self.active = True
# print("\nINFO: contact %s made!"%(self.frame_name))
# check whether contact force is outside friction cone
f_N = self.f.dot(self.normal) # norm of normal force
f_T = self.f - f_N * self.normal # tangential force (3d)
f_T_norm = norm(f_T) # norm of tangential force
if (f_T_norm > self.mu * f_N): # contact is slipping
t_dir = f_T / f_T_norm # direction of tangential force
# saturate force at the friction cone boundary
f_T = self.mu * f_N * t_dir
self.f = f_N * self.normal + f_T
# update anchor point assuming anchor point vel is equal to contact point vel
self.dp0 = self.v - self.v.dot(self.normal) * self.normal
# f = K@(p0-p) + B@(v0-v) => p0 = p + f/K - B@(v0-v)/K
self.p0 = self.p + self.Kinv @ (
self.f - self.B @ (self.dp0 - self.v))
if (self.slipping == False):
self.slipping = True
print(
'INFO: contact %s started slipping' %
(self.frame_name), f_T_norm - self.mu * f_N)
elif (self.slipping == True):
self.slipping = False
print(
'INFO: contact %s stopped slipping' %
(self.frame_name), f_T_norm - self.mu * f_N)
return self.f
def update(self, solo, qmes12, vmes12):
"""Update the quantities of the Interface based on the last measurements from the simulation
Args:
solo (object): Pinocchio wrapper for the quadruped
qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
vmes12 (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators
"""
# Rotation matrix from the world frame to the base frame
self.oRb = pin.Quaternion(qmes12[3:7]).matrix()
# Linear and angular velocity in base frame
self.vmes12_base = vmes12.copy()
self.vmes12_base[0:3,
0:1] = self.oRb.transpose() @ self.vmes12_base[0:3,
0:1]
self.vmes12_base[3:6,
0:1] = self.oRb.transpose() @ self.vmes12_base[3:6,
0:1]
"""# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
for i in self.indexes[1:]:
self.mean_feet_z = np.min((self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
qmes12[2, 0] -= self.mean_feet_z"""
# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
pin.framesForwardKinematics(solo.model, solo.data, qmes12)
# Get center of mass from Pinocchio
pin.centerOfMass(solo.model, solo.data, qmes12, self.vmes12_base)
# Update position/orientation of frames
pin.updateFramePlacements(solo.model, solo.data)
pin.ccrba(solo.model, solo.data, qmes12, self.vmes12_base)
# Update minimum height of feet
# TODO: Rename mean_feet_z into min_feet_z
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
"""for i in self.indexes:
self.mean_feet_z += solo.data.oMf[i].translation[2, 0]
self.mean_feet_z *= 0.25"""
for i in self.indexes[1:]:
self.mean_feet_z = np.min(
(self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
self.mean_feet_z = 0.0
# Store position, linear velocity and angular velocity in global frame
self.oC = solo.data.com[0]
self.oV = solo.data.vcom[0]
self.oW = vmes12[3:6]
pin.crba(solo.model, solo.data, qmes12)
# Get SE3 object from world frame to base frame
self.oMb = pin.SE3(pin.Quaternion(qmes12[3:7]), self.oC)
self.RPY = pin.rpy.matrixToRpy(self.oMb.rotation)
# Get SE3 object from world frame to local frame
self.oMl = pin.SE3(
pin.utils.rotate('z', self.RPY[2, 0]),
np.array([qmes12[0, 0], qmes12[1, 0], self.mean_feet_z]))
# Get position, linear velocity and angular velocity in local frame
self.lC = self.oMl.inverse() * self.oC
self.lV = self.oMl.rotation.transpose() @ self.oV
self.lW = self.oMl.rotation.transpose() @ self.oW
# Pos, vel and acc of feet
for i, j in enumerate(self.indexes):
# Position of feet in local frame
self.o_feet[:, i:(i + 1)] = solo.data.oMf[j].translation
self.l_feet[:, i:(
i + 1)] = self.oMl.inverse() * solo.data.oMf[j].translation
# getFrameVelocity output is in the frame of the foot so a transform is required
self.ov_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameVelocity(
solo.model, solo.data, j).vector[0:3, 0:1]
self.lv_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.ov_feet[:, i:(i + 1)]
# getFrameAcceleration output is in the frame of the foot so a transform is required
self.oa_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameAcceleration(
solo.model, solo.data, j).vector[0:3, 0:1]
self.la_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.oa_feet[:, i:(i + 1)]
# Orientation of the base in local frame
# Base and local frames have the same yaw orientation in world frame
self.abg[0:2] = self.RPY[0:2]
# Position of shoulders in world frame
for i in range(4):
self.o_shoulders[:, i:(i + 1)] = self.oMl * self.l_shoulders[:, i]
return 0
def compute(self, q, dq):
### FEET
Jfeet = []
afeet = []
pfeet_err = []
vfeet_ref = []
pin.forwardKinematics(self.rmodel, self.rdata, q, dq,
np.zeros(self.rmodel.nv))
pin.updateFramePlacements(self.rmodel, self.rdata)
for i_ee in range(4):
idx = int(self.foot_ids[i_ee])
pos = self.rdata.oMf[idx].translation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
ref = self.feet_position_ref[i_ee]
vref = self.feet_velocity_ref[i_ee]
aref = self.feet_acceleration_ref[i_ee]
J1 = pin.computeFrameJacobian(self.robot.model, self.robot.data, q,
idx, pin.LOCAL_WORLD_ALIGNED)[:3]
e1 = ref - pos
acc1 = -self.Kp_flyingfeet * (pos - ref) - self.Kd_flyingfeet * (
nu.linear - vref) + aref
if self.flag_in_contact[i_ee]:
acc1 *= 0 # In contact = no feedback
drift1 = np.zeros(3)
drift1 += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).linear
drift1 += self.cross3(nu.angular, nu.linear)
acc1 -= drift1
Jfeet.append(J1)
afeet.append(acc1)
pfeet_err.append(e1)
vfeet_ref.append(vref)
### BASE POSITION
idx = self.BASE_ID
pos = self.rdata.oMf[idx].translation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
ref = self.base_position_ref
Jbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q,
idx, pin.LOCAL_WORLD_ALIGNED)[:3]
e_basispos = ref - pos
accbasis = -self.Kp_base_position * (
pos - ref) - self.Kd_base_position * nu.linear
drift = np.zeros(3)
drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).linear
drift += self.cross3(nu.angular, nu.linear)
accbasis -= drift
### BASE ROTATION
idx = self.BASE_ID
rot = self.rdata.oMf[idx].rotation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
rotref = self.base_orientation_ref
Jwbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data,
q, idx, pin.LOCAL_WORLD_ALIGNED)[3:]
e_basisrot = -rotref @ pin.log3(rotref.T @ rot)
accwbasis = -self.Kp_base_orientation * rotref @ pin.log3(
rotref.T @ rot) - self.Kd_base_orientation * nu.angular
drift = np.zeros(3)
drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).angular
accwbasis -= drift
J = np.vstack(Jfeet + [Jbasis, Jwbasis])
acc = np.concatenate(afeet + [accbasis, accwbasis])
x_err = np.concatenate(pfeet_err + [e_basispos, e_basisrot])
dx_ref = np.concatenate(
vfeet_ref +
[self.base_linearvelocity_ref, self.base_angularvelocity_ref])
invJ = self.dinv(J) #or np.linalg.inv(J) since full rank
ddq = invJ @ acc
self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err)
self.dq_cmd = invJ @ dx_ref
return ddq
