def test_xyzquat_operator(self):
# Setup
mul = Multiply_poseQuaternion_vector("mul")
mul_inv = MultiplyInv_poseQuaternion_vector("mul_inv")
# Create a Random SE3 object.
xyzq_rand = pinocchio.SE3ToXYZQUAT(pinocchio.SE3.Random())
xyzq_out = pinocchio.SE3ToXYZQUAT(pinocchio.SE3.Random())
#
mul.sin1.value = xyzq_rand
mul.sin2.value = xyzq_out
#
mul_inv.sin1.value = xyzq_rand
plug(mul.sout, mul_inv.sin2)
#
mul_inv.sout.recompute(1)
out = mul_inv.sout.value
# debug print
print()
print("mul.sin1.value =", mul.sin1.value)
print("mul.sin2.value =", mul.sin2.value)
print("mul.sout.value =", mul.sout.value)
print("mul_inv.sin1.value =", mul_inv.sin1.value)
print("mul_inv.sin2.value =", mul_inv.sin2.value)
print("mul_inv.sout.value =", mul_inv.sout.value)
# Testing
np.testing.assert_allclose(out, xyzq_out, rtol=1e-4)
def test_no_update_no_select(self):
entity = CreateWorldFrame("")
# Random input
identity_frame = pinocchio.SE3.Identity()
random_frame = pinocchio.SE3.Random()
entity.frame_sin.value = pinocchio.SE3ToXYZQUAT(random_frame)
# checkout that the output is not changed
entity.world_frame_sout.recompute(1)
np.testing.assert_almost_equal(entity.world_frame_sout.value,
pinocchio.SE3ToXYZQUAT(identity_frame))
def _build_from_mesh(self, model_wrapper, mesh, bounds=None):
model = model_wrapper.model
geom_model = model_wrapper.geom_model
# save geom_obj placement
geom_obj = mesh.geom_obj()
placement = geom_obj.placement.copy()
geom_obj.placement = pin.SE3.Identity()
geom_obj.q_placement = placement
q_freeflyer = pin.SE3ToXYZQUAT(placement).copy()
# create a freeflyer joint and add it to the model
free_flyer = pin.JointModelFreeFlyer()
universe_joint = 0
joint_name = f"freeflyer_{mesh.name}"
if bounds is not None:
parent_joint = model.addJoint(
universe_joint,
free_flyer,
pin.SE3.Identity(),
joint_name=joint_name,
max_effort=np.array([1000]),
max_velocity=np.array([1000]),
min_config=bounds[0],
max_config=bounds[1],
)
else:
parent_joint = model.addJoint(
universe_joint, free_flyer, pin.SE3.Identity(), joint_name
)
# add geom_obj to the geom_model
geom_obj.parentJoint = parent_joint
geom_model.addGeometryObject(geom_obj)
def compute(m):
tq_vec = pin.SE3ToXYZQUAT(m)
tq_tup = pin.SE3ToXYZQUATtuple(m)
mm_vec = pin.XYZQUATToSE3(tq_vec)
mm_tup = pin.XYZQUATToSE3(tq_tup)
mm_lis = pin.XYZQUATToSE3(list(tq_tup))
return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
def attach_to_link(model, link, gripper=None, handle=None, contact=None):
"""
Attach a gripper, handle or contact to a different link.
- model: a pinocchio.Model that represents the kinematic chain.
- link: the link onto which to attach.
- gripper, handle, contact: exactly one of them should be provided
"""
import pinocchio
if int(gripper is None) + int(handle is None) + int(contact is None) != 2:
raise ValueError(
"Exactly one of {gripper, handle, contact} should be provided")
srdf = (
contact if handle is None else handle) if gripper is None else gripper
oid = model.getFrameId(srdf['link'])
nid = model.getFrameId(link)
if oid >= model.nframes or nid >= model.nframes:
raise ValueError("Could not find one of the frames")
if oid == nid: return
of = model.frames[oid]
nf = model.frames[nid]
if of.parent != nf.parent:
raise RuntimeError("The frames are not attached to the same joint")
srdf['link'] = nf.name
if contact is not None:
# Change srdf['points'] from old link to new link
raise NotImplementedError(
"Need to change srdf['points'] from old link to new link")
else:
olMf = pinocchio.XYZQUATToSE3(srdf["position"])
jMnl = nf.placement
jMol = of.placement
nlMf = jMnl.inverse() * jMol * olMf
srdf["position"] = pinocchio.SE3ToXYZQUAT(nlMf)
def test_double_convertion(self):
# Setup
# Create a Random SE3 object.
pin_se3_pose_rot_mat = pinocchio.SE3.Random()
# Convert the rotation in quaternion.
init_se3_pose_quat = np.array(
pinocchio.SE3ToXYZQUAT(pin_se3_pose_rot_mat))
# Create the small graph.
quat2rpy = PoseQuaternionToPoseRPY("")
rpy2quat = PoseRPYToPoseQuaternion("")
plug(quat2rpy.sout, rpy2quat.sin)
# Feed the graph.
quat2rpy.sin.value = init_se3_pose_quat
# Evaluate the graph.
rpy2quat.sout.recompute(1)
# Extract the result.
final_se3_pose_quat = rpy2quat.sout.value
# Make the quaternions w positive for testing:
if init_se3_pose_quat[-1] < 0.0:
init_se3_pose_quat[-4:] *= -1
if final_se3_pose_quat[-1] < 0.0:
final_se3_pose_quat[-4:] *= -1
# Testing
np.testing.assert_allclose(init_se3_pose_quat,
final_se3_pose_quat,
rtol=1e-4)
def test_against_pinocchio(self):
""" Test against pinocchio conversion tools """
# Setup
# Create a Random SE3 object.
pin_se3_pose_rot_mat = pinocchio.SE3.Random()
# Convert the rotation in roll-pitch-yaw.
pin_rpy = pinocchio.rpy.matrixToRpy(pin_se3_pose_rot_mat.rotation).T
pin_se3_pose_rpy = np.array(
np.hstack([pin_se3_pose_rot_mat.translation, pin_rpy]))
# Convert the rotation in quaternion.
pin_se3_pose_quat = pinocchio.SE3ToXYZQUAT(pin_se3_pose_rot_mat)
# Create the entity and convert the se3(pose, quat) -> se3(pose, rpy).
pq2pr = PoseQuaternionToPoseRPY("")
pq2pr.sin.value = np.array(pin_se3_pose_quat)
pq2pr.sout.recompute(1)
entity_se3_pose_rpy = pq2pr.sout.value
# Testing
np.testing.assert_allclose(pin_se3_pose_rpy,
entity_se3_pose_rpy,
rtol=1e-6)
def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m = pin.SE3.Identity()
m.translation = np.matrix('1. 2. 3.').T
m.rotation = np.matrix(
'1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2)
self.assertApprox(
pin.SE3ToXYZQUAT(m).T,
[1., 2., 3., sqrt(2) / 2, 0, 0,
sqrt(2) / 2])
self.assertApprox(
pin.XYZQUATToSE3([1., 2., 3.,
sqrt(2) / 2, 0, 0,
sqrt(2) / 2]), m)
def test_use_all(self):
entity = CreateWorldFrame("")
entity.update()
entity.set_which_dofs(np.array(6 * [1]))
# Random input
random_frame = pinocchio.SE3.Random()
entity.frame_sin.value = pinocchio.SE3ToXYZQUAT(random_frame)
# checkout that the output is not changed
entity.world_frame_sout.recompute(1)
np.testing.assert_almost_equal(entity.world_frame_sout.value,
entity.frame_sin.value)
def test_full_offset_xyz_null(self):
entity = CreateWorldFrame("")
entity.update()
entity.set_which_dofs(np.array([0, 0, 0, 1, 1, 1]))
# Random input
random_frame = pinocchio.SE3.Random()
entity.frame_sin.value = pinocchio.SE3ToXYZQUAT(random_frame)
# checkout that the output is not changed
entity.world_frame_sout.recompute(1)
np.testing.assert_almost_equal(
entity.world_frame_sout.value,
3 * [0] + entity.frame_sin.value[3:].tolist())
def compute_freeflyer(trajectory_data: TrajectoryDataType,
freeflyer_continuity: bool = True) -> None:
"""Compute the freeflyer positions and velocities.
.. warning::
This function modifies the internal robot data.
:param trajectory_data: Sequence of States for which to retrieve the
freeflyer.
:param freeflyer_continuity: Whether or not to enforce the continuity
in position of the freeflyer.
Optional: True by default.
"""
robot = trajectory_data['robot']
contact_frame_prev = None
w_M_ff_offset = pin.SE3.Identity()
w_M_ff_prev = None
for s in trajectory_data['evolution_robot']:
# Compute freeflyer using contact frame as reference frame
s.contact_frame = compute_freeflyer_state_from_fixed_body(
robot, s.q, s.v, s.a, s.contact_frame, None)
# Move freeflyer to ensure continuity over time, if requested
if freeflyer_continuity:
# Extract the current freeflyer transform
w_M_ff = pin.XYZQUATToSE3(s.q[:7])
# Update the internal buffer of the freeflyer transform
if contact_frame_prev is not None \
and contact_frame_prev != s.contact_frame:
w_M_ff_offset = w_M_ff_offset * w_M_ff_prev * w_M_ff.inverse()
contact_frame_prev = s.contact_frame
w_M_ff_prev = w_M_ff
# Add the appropriate offset to the freeflyer
w_M_ff = w_M_ff_offset * w_M_ff
s.q[:7] = pin.SE3ToXYZQUAT(w_M_ff)
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)
def display(self, xs, fs=[], ps=[], dts=[], factor=1.):
numpy_conversion = False
if libcrocoddyl_pywrap.getNumpyType() == np.matrix:
numpy_conversion = True
libcrocoddyl_pywrap.switchToNumpyMatrix()
warnings.warn(
"Numpy matrix supports will be removed in future release",
DeprecationWarning,
stacklevel=2)
if ps:
for key, p in ps.items():
self.robot.viewer.gui.setCurvePoints(
self.frameTrajGroup + "/" + key, p)
if not dts:
dts = [0.] * len(xs)
S = 1 if self.rate <= 0 else max(len(xs) / self.rate, 1)
for i, x in enumerate(xs):
if not i % S:
if fs:
self.activeContacts = {
k: False
for k, c in self.activeContacts.items()
}
for f in fs[i]:
key = f["key"]
pose = f["oMf"]
wrench = f["f"]
# Display the contact forces
R = rotationMatrixFromTwoVectors(
self.x_axis, wrench.linear)
forcePose = pinocchio.SE3ToXYZQUATtuple(
pinocchio.SE3(R, pose.translation))
forceMagnitud = np.linalg.norm(
wrench.linear) / self.totalWeight
forceName = self.forceGroup + "/" + key
self.robot.viewer.gui.setVector3Property(
forceName, "Scale", [1. * forceMagnitud, 1., 1.])
self.robot.viewer.gui.applyConfiguration(
forceName, forcePose)
self.robot.viewer.gui.setVisibility(forceName, "ON")
# Display the friction cones
position = pose
position.rotation = rotationMatrixFromTwoVectors(
self.z_axis, f["nsurf"])
frictionName = self.frictionGroup + "/" + key
self._setConeMu(key, f["mu"])
self.robot.viewer.gui.applyConfiguration(
frictionName,
list(
np.array(pinocchio.SE3ToXYZQUAT(
position)).squeeze()))
self.robot.viewer.gui.setVisibility(frictionName, "ON")
self.activeContacts[key] = True
for key, c in self.activeContacts.items():
if c == False:
self.robot.viewer.gui.setVisibility(
self.forceGroup + "/" + key, "OFF")
self.robot.viewer.gui.setVisibility(
self.frictionGroup + "/" + key, "OFF")
self.robot.display(x[:self.robot.nq])
time.sleep(dts[i] * factor)
if numpy_conversion:
numpy_conversion = False
libcrocoddyl_pywrap.switchToNumpyMatrix()
def compute_freeflyer_state_from_fixed_body(
robot: jiminy.Model,
position: np.ndarray,
velocity: Optional[np.ndarray] = None,
acceleration: Optional[np.ndarray] = None,
fixed_body_name: Optional[str] = None,
ground_profile: Optional[Callable[
[np.ndarray], Tuple[float, np.ndarray]]] = None,
use_theoretical_model: Optional[bool] = None) -> str:
"""Fill rootjoint data from articular data when a body is fixed and
aligned with world frame.
This method computes the position of freeflyer in the fixed body frame.
If **fixed_body_name** is omitted, it will default to either:
- the set of three contact points
- a single collision body
In such a way that no contact points nor collision bodies are going through
the ground and at least contact points or a collision body are touching it.
`None` is returned if their is no contact frame or if the robot does not
have a freeflyer.
.. warning::
This function modifies the internal robot data.
.. warning::
The method fills in freeflyer data instead of returning an updated copy
for efficiency.
:param robot: Jiminy robot.
:param position: Must contain current articular data. The freeflyer data
can contain any value, it will be ignored and replaced.
:param velocity: See position.
:param acceleration: See position.
:param fixed_body_name: Name of the body frame that is considered fixed
parallel to world frame.
Optional: It will be infered from the set of
contact points and collision bodies.
:param ground_profile: Ground profile callback.
:param use_theoretical_model:
Whether the state corresponds to the theoretical model when updating
and fetching the robot's state. Must be False if `fixed_body_name` is
not speficied.
Optional: True by default if `fixed_body_name` is specified, False
otherwise.
:returns: Name of the contact frame, if any.
"""
# Early return if no freeflyer
if not robot.has_freeflyer:
return None
# Handling of default arguments
if use_theoretical_model is None:
use_theoretical_model = fixed_body_name is not None
position[:6].fill(0.0)
position[6] = 1.0
if velocity is not None:
velocity[:6].fill(0.0)
if acceleration is not None:
acceleration[:6].fill(0.0)
update_quantities(
robot, position, velocity, acceleration, update_physics=False,
use_theoretical_model=use_theoretical_model)
if fixed_body_name is None:
if use_theoretical_model:
raise RuntimeError(
"Cannot infer contact transform for theoretical model.")
w_M_ff = compute_transform_contact(robot, ground_profile)
else:
ff_M_fixed_body = get_body_world_transform(
robot, fixed_body_name, use_theoretical_model, copy=False)
if ground_profile is not None:
ground_translation = np.zeros(3)
ground_translation[2], normal = ground_profile(
ff_M_fixed_body.translation)
ground_rotation = pin.Quaternion.FromTwoVectors(
np.array([0.0, 0.0, 1.0]), normal).matrix()
w_M_ground = pin.SE3(ground_rotation, ground_translation)
else:
w_M_ground = pin.SE3.Identity()
w_M_ff = w_M_ground.act(ff_M_fixed_body.inverse())
position[:7] = pin.SE3ToXYZQUAT(w_M_ff)
if fixed_body_name is not None:
if velocity is not None:
ff_v_fixed_body = get_body_world_velocity(
robot, fixed_body_name, use_theoretical_model)
base_link_velocity = - ff_v_fixed_body
velocity[:6] = base_link_velocity.vector
if acceleration is not None:
ff_a_fixedBody = get_body_world_acceleration(
robot, fixed_body_name, use_theoretical_model)
base_link_acceleration = - ff_a_fixedBody
acceleration[:6] = base_link_acceleration.vector
return fixed_body_name
def XYZRPYToXYZQuat(xyzrpy):
"""Convert [X,Y,Z,Roll,Pitch,Yaw] to [X,Y,Z,Qx,Qy,Qz,Qw].
"""
return pin.SE3ToXYZQUAT(XYZRPYToSE3(xyzrpy))
def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m = pin.SE3.Identity()
m.translation = np.array([1., 2., 3.])
m.rotation = np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]]) # rotate('x', pi / 2)
self.assertApprox(pin.SE3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
self.assertApprox(pin.XYZQUATToSE3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
gn.maxIter = 100
gn.verbose = False
penalty = mno.Penalty(variables.space.nq, variables.space.nv,
constraints.dimension())
penalty.etol2 = 1e-12
penalty.fxtol2 = 1e-10
penalty.maxIter = 40
penalty.verbose = True
# Compute initial guess
X0 = variables.space.neutral
for k in range(variables.nplanes):
variables.plane(X0, k)[0] = 1
# Assume w is the object base link
variables.tag(X0, 0, False)[:] = pinocchio.SE3ToXYZQUAT(wMt0)
unknown_tags = set(range(1, variables.ntags))
unknown_cameras = set(range(variables.ncameras))
unhandled_images = images[:]
while len(unknown_tags) > 0:
if len(unhandled_images) == 0:
raise RuntimeError("some tags were not seen")
for im, image in enumerate(images):
if image not in unhandled_images: continue
cMw = None
for tag, cMi in zip(image.tags, image.guess_cMis):
if tag in unknown_tags: continue
# compute camera pose
cMw = cMi * variables.tag(X0, tag).inverse()
variables.camera(X0, im, False)[:] = pinocchio.SE3ToXYZQUAT(cMw)
unhandled_images.remove(image)
gn.verbose = False
penalty = mno.Penalty(variables.space.nq, variables.space.nv,
constraints.dimension())
penalty.etol2 = 1e-12
penalty.fxtol2 = 1e-10
penalty.maxIter = 40
penalty.verbose = True
# Compute initial guess
X0 = variables.space.neutral
for k in range(variables.nplanes):
variables.plane(X0, k)[0] = 1
# Assume w is the object base link
variables.tag(X0, 0, False)[:] = pinocchio.SE3ToXYZQUAT(wMt0)
unknown_tags = set(range(1, variables.ntags))
unknown_cameras = set(range(variables.ncameras))
unhandled_images = images[:]
while len(unknown_tags) > 0:
if len(unhandled_images) == 0:
raise RuntimeError("some tags were not seen")
for im, image in enumerate(images):
if image not in unhandled_images: continue
cMw = None
for tag, cMi in zip(image.tags, image.guess_cMis):
if tag in unknown_tags: continue
# compute camera pose
cMw = cMi * variables.tag(X0, tag).inverse()
variables.camera(X0, im, False)[:] = pinocchio.SE3ToXYZQUAT(cMw)
unhandled_images.remove(image)
