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 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 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)
robot.display(q)
print("The robot is display with end effector on the red box.")
time.sleep(2)
#
# MOVE #############################################################
#
print("Let's start the movement ...")
# Random velocity of the robot driving the movement
vq = np.matrix([ 2.,0,0,4.,0,0]).T
idx = robot.index('wrist_3_joint')
oMeff = robot.placement(q, idx) # Placement of end-eff wrt world at current configuration
oMbox = pio.XYZQUATToSe3(q_box) # Placement of box wrt world
effMbox = oMeff.inverse() * oMbox # Placement of box wrt eff
for i in range(1000):
# Chose new configuration of the robot
q += vq / 40
q[2] = 1.71 + math.sin(i * 0.05) / 2
# Gets the new position of the box
oMbox = robot.placement(q, idx) * effMbox
# Display new configuration for robot and box
gv.applyConfiguration(boxID, pio.se3ToXYZQUATtuple(oMbox))
robot.display(q)
time.sleep(0.1)