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_velocity(robot: jiminy.Model,
body_name: str,
use_theoretical_model: bool = True) -> pin.SE3:
"""Get the spatial velocity wrt world in body frame for a given body.
.. warning::
It is assumed that `update_quantities` has been called beforehand.
: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 velocity.
"""
# 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.getFrameVelocity(
pnc_model, pnc_data, body_id, pin.LOCAL_WORLD_ALIGNED)
def BaseVelocityFromKinAndIMU(self, contactFrameId):
"""Estimate the velocity of the base with forward kinematics using a contact point
that is supposed immobile in world frame
Args:
contactFrameId (int): ID of the contact point frame (foot frame)
"""
frameVelocity = pin.getFrameVelocity(self.model, self.data,
contactFrameId,
pin.ReferenceFrame.LOCAL)
framePlacement = pin.updateFramePlacement(self.model, self.data,
contactFrameId)
# Angular velocity of the base wrt the world in the base frame (Gyroscope)
_1w01 = self.IMU_ang_vel.reshape((3, 1))
# Linear velocity of the foot wrt the base in the foot frame
_Fv1F = frameVelocity.linear
# Level arm between the base and the foot
_1F = np.array(framePlacement.translation)
# Orientation of the foot wrt the base
_1RF = framePlacement.rotation
# Linear velocity of the base wrt world in the base frame
_1v01 = self.cross3(_1F.ravel(), _1w01.ravel()) - \
(_1RF @ _Fv1F.reshape((3, 1)))
# IMU and base frames have the same orientation
# _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel())
return np.array(_1v01)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel)
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_linear_vel = joint_velocity.linear
joint_angular_vel = joint_velocity.angular
joint_6v_vel = joint_velocity.vector
err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_linear_vel = frame_velocity.linear
frame_angular_vel = frame_velocity.angular
frame_6v_vel = frame_velocity.vector
err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def get_velocity(self):
M = self.data.oMf[self.frame_id]
R = se3.SE3(
M.rotation,
0 * M.translation) # same as M but with translation set to zero
v_local = se3.getFrameVelocity(self.model, self.data, self.frame_id)
v_world = (R.act(v_local)
).linear # convert velocity from local frame to world frame
return v_world
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 get_link_vel(self, link_id):
ret = np.zeros(6)
frame_id = self._model.getFrameId(link_id)
spatial_vel = pin.getFrameVelocity(
self._model, self._data, frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
ret[0:3] = spatial_vel.angular
ret[3:6] = spatial_vel.linear
return np.copy(ret)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
# IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = joint_velocity.vector - \
joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = frame_velocity.vector - \
frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def createEESplines(self, rmodel, rdata, xs, t_sampling=0.005):
N = len(xs)
abscissa = a2m(np.linspace(0., t_sampling * (N - 1), N))
self.ee_splines = EESplines()
for patch in self.ee_map.keys():
p = np.zeros((3, N))
m = np.zeros((3, N))
for i in range(N):
q = a2m(xs[i][:rmodel.nq])
v = a2m(xs[i][-rmodel.nv:])
pinocchio.forwardKinematics(rmodel, rdata, q, v)
p[:, i] = m2a(
pinocchio.updateFramePlacement(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).translation)
m[:, i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).linear)
self.ee_splines.update([[patch, CubicHermiteSpline(abscissa, p, m)]])
return
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):
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 forward_kinematics(
self,
joint_positions: typing.List[np.ndarray],
joint_velocities: typing.Optional[np.ndarray] = None,
) -> typing.Union[typing.List[np.ndarray], typing.Tuple[
typing.List[np.ndarray], typing.List[np.ndarray]], ]:
"""Compute end-effector positions (and velocities) for the given joint configuration.
Args:
joint_positions: Flat list of angular joint positions.
joint_velocities: Optional. Flat list of angular joint
velocities.
Returns:
If only joint positions are given: List of end-effector
positions. Each position is given as an np.array with x,y,z
positions.
If joint positions and velocities are given: Tuple with
(i) list of end-effector positions and (ii) list of
end-effector velocities. Each position and velocity is given
as an np.array with x,y,z components.
"""
pinocchio.framesForwardKinematics(self.robot_model, self.data,
joint_positions)
positions = [
np.asarray(
self.data.oMf[link_id].translation).reshape(-1).tolist()
for link_id in self.tip_link_ids
]
if joint_velocities is None:
return positions
else:
pinocchio.forwardKinematics(self.robot_model, self.data,
joint_positions, joint_velocities)
velocities = []
for link_id in self.tip_link_ids:
local_to_world_transform = pinocchio.SE3.Identity()
local_to_world_transform.rotation = self.data.oMf[
link_id].rotation
v_local = pinocchio.getFrameVelocity(self.robot_model,
self.data, link_id)
v_world = local_to_world_transform.act(v_local)
velocities.append(v_world.linear)
return positions, velocities
def ForwardKinematics(self, q, v=np.zeros(3)):
"""
Description:
1. Computes the Cartesian Position & Velocity of the robot
2. The TCP position is w.r.t. the 'tip_link' of the robot.
Args:
q -> np.array (3,) : Joint Positions of the robot.
v -> np.array (3,) : Joint Velocities of the robot.
Returns:
np.array (3,) : Cartesian Position of the robot.
np.array (3,) : Cartesian Velocity of the robot.
"""
index = self.EOAT_ID
frame = pin.ReferenceFrame.LOCAL
pin.forwardKinematics(self.model, self.model_data, q, v)
pin.updateFramePlacements(self.model, self.model_data)
pos = pin.updateFramePlacement(self.model, self.model_data, index)
vel = pin.getFrameVelocity(self.model, self.model_data, index, frame)
return np.array(pos)[:3, -1], np.array(vel)[:3]
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)
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 calc(self, data, x, u):
data.r = (pinocchio.getFrameVelocity(self.state.pinocchio,
data.pinocchio, self.vref.frame) -
self.vref.oMf).vector
self.activation.calc(data.activation, data.r)
data.cost = data.activation.a
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
def calc(self, data, x, u):
data.residuals[:] = m2a(
pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).linear) - self.ref
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
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 get_frame_velocity(self, q, v, frame_id) -> pin.Motion:
pin.forwardKinematics(self.model, self.data, q, v)
return pin.getFrameVelocity(self.model, self.data,
self.model.getFrameId(frame_id),
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def writeToMessage(self,
t,
q,
v=None,
tau=None,
p=dict(),
pd=dict(),
f=dict(),
s=dict()):
# Filling the time information
self._msg.header.stamp = rospy.Time(t)
self._msg.time = t
if v is None:
v = np.zeros(self._model.nv)
if tau is None:
tau = np.zeros(self._model.njoints - 2)
# Filling the centroidal state
pinocchio.centerOfMass(self._model, self._data, q, v)
c = self._data.com[0]
cd = self._data.vcom[0]
# Center of mass
self._msg.centroidal.com_position.x = c[0]
self._msg.centroidal.com_position.y = c[1]
self._msg.centroidal.com_position.z = c[2]
self._msg.centroidal.com_velocity.x = cd[0]
self._msg.centroidal.com_velocity.y = cd[1]
self._msg.centroidal.com_velocity.z = cd[2]
# Base
self._msg.centroidal.base_orientation.x = q[3]
self._msg.centroidal.base_orientation.y = q[4]
self._msg.centroidal.base_orientation.z = q[5]
self._msg.centroidal.base_orientation.w = q[6]
self._msg.centroidal.base_angular_velocity.x = v[3]
self._msg.centroidal.base_angular_velocity.y = v[4]
self._msg.centroidal.base_angular_velocity.z = v[5]
# Momenta
momenta = pinocchio.computeCentroidalMomentum(self._model, self._data)
momenta_rate = pinocchio.computeCentroidalMomentumTimeVariation(
self._model, self._data)
self._msg.centroidal.momenta.linear.x = momenta.linear[0]
self._msg.centroidal.momenta.linear.y = momenta.linear[1]
self._msg.centroidal.momenta.linear.z = momenta.linear[2]
self._msg.centroidal.momenta.angular.x = momenta.angular[0]
self._msg.centroidal.momenta.angular.y = momenta.angular[1]
self._msg.centroidal.momenta.angular.z = momenta.angular[2]
self._msg.centroidal.momenta_rate.linear.x = momenta_rate.linear[0]
self._msg.centroidal.momenta_rate.linear.y = momenta_rate.linear[1]
self._msg.centroidal.momenta_rate.linear.z = momenta_rate.linear[2]
self._msg.centroidal.momenta_rate.angular.x = momenta_rate.angular[0]
self._msg.centroidal.momenta_rate.angular.y = momenta_rate.angular[1]
self._msg.centroidal.momenta_rate.angular.z = momenta_rate.angular[2]
# Filling the joint state
njoints = self._model.njoints - 2
for j in range(njoints):
self._msg.joints[j].position = q[7 + j]
self._msg.joints[j].velocity = v[6 + j]
self._msg.joints[j].effort = tau[j]
# Filling the contact state
names = list(p.keys()) + list(pd.keys()) + list(f.keys()) + list(
s.keys())
names = list(dict.fromkeys(names))
self._msg.contacts = [None] * len(names)
if len(names) != 0 and (len(p.keys()) == 0 or len(pd.keys()) == 0):
pinocchio.forwardKinematics(self._model, self._data, q, v)
for i, name in enumerate(names):
contact_msg = ContactState()
contact_msg.name = name
frame_id = self._model.getFrameId(name)
# Retrive the contact position
if name in p:
pose = pinocchio.SE3ToXYZQUAT(p[name])
else:
oMf = pinocchio.updateFramePlacement(self._model, self._data,
frame_id)
pose = pinocchio.SE3ToXYZQUAT(oMf)
# Retrieve the contact velocity
if name in pd:
ovf = pd[name]
else:
ovf = pinocchio.getFrameVelocity(
self._model, self._data, frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# Storing the contact position and velocity inside the message
contact_msg.pose.position.x = pose[0]
contact_msg.pose.position.y = pose[1]
contact_msg.pose.position.z = pose[2]
contact_msg.pose.orientation.x = pose[3]
contact_msg.pose.orientation.y = pose[4]
contact_msg.pose.orientation.z = pose[5]
contact_msg.pose.orientation.w = pose[6]
contact_msg.velocity.linear.x = ovf.linear[0]
contact_msg.velocity.linear.y = ovf.linear[1]
contact_msg.velocity.linear.z = ovf.linear[2]
contact_msg.velocity.angular.x = ovf.angular[0]
contact_msg.velocity.angular.y = ovf.angular[1]
contact_msg.velocity.angular.z = ovf.angular[2]
# Retrieving and storing force data
if name in f:
contact_info = f[name]
ctype = contact_info[0]
force = contact_info[1]
contact_msg.type = ctype
contact_msg.wrench.force.x = force.linear[0]
contact_msg.wrench.force.y = force.linear[1]
contact_msg.wrench.force.z = force.linear[2]
contact_msg.wrench.torque.x = force.angular[0]
contact_msg.wrench.torque.y = force.angular[1]
contact_msg.wrench.torque.z = force.angular[2]
if name in s:
terrain_info = s[name]
norm = terrain_info[0]
friction = terrain_info[1]
contact_msg.surface_normal.x = norm[0]
contact_msg.surface_normal.y = norm[1]
contact_msg.surface_normal.z = norm[2]
contact_msg.friction_coefficient = friction
self._msg.contacts[i] = contact_msg
return copy.deepcopy(self._msg)
## Calculate l2 placement
l2_frame = model.getFrameId('l2')
l2_translation = pin.updateFramePlacement(model, data, l2_frame)
print("l2 translation")
print(l2_translation)
## Calculate j2 jacobian
pin.computeJointJacobians(model, data, q)
j2_jacobian = pin.getFrameJacobian(model, data, j2_frame,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
print("j2 jacobian")
print(j2_jacobian)
## Calculate l2 jacobian
l2_jacobian = pin.getFrameJacobian(model, data, l2_frame,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
print("l2 jacobian")
print(l2_jacobian)
## Calculate j2 spatial velocity
j2_vel = pin.getFrameVelocity(model, data, j2_frame)
print("j2 vel")
print(j2_vel)
## Calculate l2 spatial velocity
l2_vel = pin.getFrameVelocity(model, data, l2_frame,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
print("l2 vel")
print(l2_vel)
print(np.dot(l2_jacobian, qdot))
