def on_packet(packet):
""" Callback function that is called everytime a data packet arrives from QTM """
position = packet.get_6d()[1][body_id][0]
orientation = packet.get_6d()[1][body_id][1]
timestamp = packet.timestamp * 1e-6
# Get the last position and Rotation matrix from the shared memory.
last_position = np.array(
[shared_bodyPosition[0], shared_bodyPosition[1], shared_bodyPosition[2]])
last_rotation = np.array([[shared_bodyOrientationMat9[0], shared_bodyOrientationMat9[1], shared_bodyOrientationMat9[2]],
[shared_bodyOrientationMat9[3], shared_bodyOrientationMat9[4], shared_bodyOrientationMat9[5]],
[shared_bodyOrientationMat9[6], shared_bodyOrientationMat9[7], shared_bodyOrientationMat9[8]]])
last_se3 = pinocchio.SE3(last_rotation, last_position)
# Get the new position and Rotation matrix from the motion capture.
position = np.array([position.x, position.y, position.z]) * 1e-3
rotation = np.array(orientation.matrix).reshape(3, 3).transpose()
se3 = pinocchio.SE3(rotation, position)
xyzquat = se3ToXYZQUAT(se3)
# Get the position, Rotation matrix and Quaternion
shared_bodyPosition[0] = xyzquat[0]
shared_bodyPosition[1] = xyzquat[1]
shared_bodyPosition[2] = xyzquat[2]
shared_bodyOrientationQuat[0] = xyzquat[3]
shared_bodyOrientationQuat[1] = xyzquat[4]
shared_bodyOrientationQuat[2] = xyzquat[5]
shared_bodyOrientationQuat[3] = xyzquat[6]
shared_bodyOrientationMat9[0] = orientation.matrix[0]
shared_bodyOrientationMat9[1] = orientation.matrix[3]
shared_bodyOrientationMat9[2] = orientation.matrix[6]
shared_bodyOrientationMat9[3] = orientation.matrix[1]
shared_bodyOrientationMat9[4] = orientation.matrix[4]
shared_bodyOrientationMat9[5] = orientation.matrix[7]
shared_bodyOrientationMat9[6] = orientation.matrix[2]
shared_bodyOrientationMat9[7] = orientation.matrix[5]
shared_bodyOrientationMat9[8] = orientation.matrix[8]
# Compute world velocity.
if (shared_timestamp.value == -1):
shared_bodyVelocity[0] = 0
shared_bodyVelocity[1] = 0
shared_bodyVelocity[2] = 0
shared_bodyAngularVelocity[0] = 0.0
shared_bodyAngularVelocity[1] = 0.0
shared_bodyAngularVelocity[2] = 0.0
else:
dt = timestamp - shared_timestamp.value
shared_bodyVelocity[0] = (position[0] - last_position[0])/dt
shared_bodyVelocity[1] = (position[1] - last_position[1])/dt
shared_bodyVelocity[2] = (position[2] - last_position[2])/dt
bodyAngularVelocity = log(last_se3.rotation.T.dot(se3.rotation))/dt
shared_bodyAngularVelocity[0] = bodyAngularVelocity[0]
shared_bodyAngularVelocity[1] = bodyAngularVelocity[1]
shared_bodyAngularVelocity[2] = bodyAngularVelocity[2]
shared_timestamp.value = timestamp
def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = rand(3)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
self.assertApprox(log(exp(m)), m)
m = se3.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = rand(6)
self.assertTrue(
np.linalg.norm(m) <
np.pi) # necessary for next test (actually, only angular part)
self.assertApprox(log(exp(m)), m)
m = np.eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(np.eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(np.zeros(5))
with self.assertRaises(ValueError):
log(np.zeros((3, 1)))
def cost_log(x):
pin.forwardKinematics(robot.model, robot.data, np.matrix(x).T)
p = robot.data.oMi[-1].copy()
log_diff = log(p.inverse() * pdes)
norm_log_diff = np.linalg.norm(log_diff.vector)
return norm_log_diff
def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = rand(3)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
self.assertApprox(log(exp(m)), m)
m = pin.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = rand(6)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test (actually, only angular part)
self.assertApprox(log(exp(m)), m)
m = eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(zero(5))
with self.assertRaises(ValueError):
log(zero((3,1)))
def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = np.matrix(range(1, 4), np.double).T
self.assertApprox(log(exp(m)), m)
m = se3.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = np.matrix([float(i) / 10 for i in range(1, 7)]).T
self.assertApprox(log(exp(m)), m)
m = np.eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(np.eye(4))
with self.assertRaises(ValueError):
exp(range(3))
with self.assertRaises(ValueError):
log(range(3))
with self.assertRaises(ValueError):
log(np.zeros(5))