def display(robot, q):
robot.realdisplay(q)
pio.updateFramePlacements(robot.model, robot.viz.data)
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-2].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-2]).flat))
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-1].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-1]).flat))
robot.viewer.gui.refresh()
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 setCameraTransform(self,
translation=np.zeros(3),
rotation=np.zeros(3),
relative=False):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
H_abs = SE3(R_pnc, np.array(translation))
if relative:
H_orig = XYZQUATToSe3(
self._client.getCameraTransform(self._window_id))
H_abs = H_abs * H_orig
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(H_abs).tolist())
else:
if relative:
raise RuntimeError(
"'relative'=True not available with meshcat.")
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def retrieve_freeflyer(trajectory_data, freeflyer_continuity=True):
"""
@brief Retrieves the freeflyer positions and velocities.
The reference frame is the support foot.
@param trajectory_data Sequence of States for which to retrieve the freeflyer.
"""
robot = trajectory_data['robot']
use_theoretical_model = trajectory_data['use_theoretical_model']
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, use_theoretical_model)
# 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 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 display(self, xs, fs=[], ps=[], dts=[], factor=1.):
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)
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 setCameraTransform(self, translation, rotation):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
T_pnc = np.array(translation)
T_R = SE3(R_pnc, T_pnc)
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(T_R).tolist())
else:
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def compute_freeflyer_state_from_fixed_body(robot, position, velocity=None, acceleration=None,
fixed_body_name=None, ground_profile=None,
use_theoretical_model=True):
"""
@brief Fill rootjoint data from articular data when a body is fixed parallel to world.
@details If 'fixed_body_name' is omitted, it will default to the contact point that ensures
no contact points are going through the ground and a single one is touching it.
@remark The hypothesis is that 'fixed_body_name' is fixed parallel to world frame.
So this method computes the position of freeflyer rootjoint in the fixed body frame.
@note This function modifies internal data.
@param robot The jiminy robot
@param[inout] position Must contain current articular data. The rootjoint data can
contain any value, it will be ignored and replaced.
The method fills in rootjoint data.
@param[inout] velocity See position
@param[inout] acceleration See position
@param fixed_body_name The name of the body frame that is considered fixed parallel to world frame
@param ground_profile The ground profile callback
"""
if not robot.has_freeflyer:
raise ValueError("The robot does not have a freeflyer.")
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
position[:6].fill(0.0)
position[6] = 1.0
if velocity is not None and acceleration is not None:
velocity[:6].fill(0.0)
acceleration[:6].fill(0.0)
pin.forwardKinematics(pnc_model, pnc_data, position, velocity, acceleration)
elif velocity is not None:
velocity[:6].fill(0.0)
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(pnc_model, pnc_data, position)
pin.framesForwardKinematics(pnc_model, pnc_data, position)
if fixed_body_name is None:
fixed_body_name = _compute_closest_contact_frame(
robot, ground_profile, use_theoretical_model)
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 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
robot = loadTiago(initViewer=True)
gv = robot.viewer.gui
gv.setCameraTransform(0, [-8, -8, 2, .6, -0.25, -0.25, .7])
NQ = robot.model.nq
NV = robot.model.nv
IDX_TOOL = robot.model.getFrameId('frametool')
IDX_BASIS = robot.model.getFrameId('framebasis')
oMgoal = pio.SE3(
pio.Quaternion(-0.4, 0.02, -0.5, 0.7).normalized().matrix(),
np.matrix([.2, -.4, .7]).T)
gv.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, .2)
gv.applyConfiguration('world/framegoal', list(pio.se3ToXYZQUAT(oMgoal).flat))
DT = 1e-2 # Integration step.
q0 = np.matrix([[
0., 0., 0., 1., 0.18, 1.37, -0.24, -0.98, 0.98, 0., 0., 0., 0., -0.13, 0.,
0., 0., 0.
]]).T
q = q0.copy()
herr = [] # Log the value of the error between tool and goal.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
pio.framesForwardKinematics(robot.model, robot.data,
q) # Compute frame placements
oMtool = robot.data.oMf[
IDX_TOOL] # Placement from world frame o to frame f oMtool
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)
