def test_rotate(self):
self.assertApprox(rotate('x', pi / 2), np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.'))
self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi))
m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7)
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.matrix(list(range(3))).T * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
def test_rotate(self):
self.assertApprox(
rotate('x', pi / 2),
np.array([[1., 0., 0.], [0., 0., -1.], [0., 1., 0.]]))
self.assertApprox(
rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi))
m = rotate('x', pi / 3).dot(rotate('y',
pi / 5)).dot(rotate('y', pi / 7))
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.array(list(range(3))) * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
self.assertApprox(
rpyToMatrix(rpy),
rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
try:
rotate('toto', 10.)
except ValueError:
self.assertTrue(True)
else:
self.assertTrue(False)
try:
rotate('w', 10.)
except ValueError:
self.assertTrue(True)
else:
self.assertTrue(False)
def test_rotate(self):
self.assertApprox(rotate('x', pi / 2),
np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.'))
self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi))
m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7)
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.matrix(list(range(3))).T * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
def test_rotate(self):
self.assertApprox(rotate('x', pi / 2), np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]]))
self.assertApprox(rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi))
m = rotate('x', pi / 3).dot(rotate('y', pi / 5)).dot(rotate('y', pi / 7))
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.array(list(range(3))) * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
self.assertApprox(rpyToMatrix(rpy), rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
def test_matrixToRpy(self):
n = 100
for _ in range(n):
quat = pin.Quaternion(np.random.rand(4, 1)).normalized()
R = quat.toRotationMatrix()
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
n2 = 100
# Test singular case theta = pi/2
Rp = np.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]])
for _ in range(n2):
r = random() * 2 * pi - pi
y = random() * 2 * pi - pi
R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(Rp) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
# Test singular case theta = -pi/2
Rp = np.array([[0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]])
for _ in range(n2):
r = random() * 2 * pi - pi
y = random() * 2 * pi - pi
R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
.dot(Rp) \
.dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
v = matrixToRpy(R)
Rprime = rpyToMatrix(v)
self.assertApprox(Rprime, R)
self.assertTrue(-pi <= v[0] and v[0] <= pi)
self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2)
self.assertTrue(-pi <= v[2] and v[2] <= pi)
def set_transform(origin, a, b):
from pinocchio import rpy, Quaternion
length = np.linalg.norm(b - a)
z = (b - a) / length
R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix()
origin.attrib['xyz'] = " ".join([str(v) for v in ((a + b) / 2)])
origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R)])
def velocityXYZQuatToXYZRPY(xyzquat: np.ndarray,
v: np.ndarray) -> np.ndarray:
"""Convert the derivative of [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw].
.. note:
No need to estimate yaw angle to get RPY velocity, since
`computeRpyJacobianInverse` only depends on Roll and Pitch angles.
However, it is not the case for the linear velocity.
.. warning::
Linear velocity in XYZRPY must be local-world-aligned frame, while
returned linear velocity in XYZQuat is in local frame.
"""
quat = pin.Quaternion(xyzquat[3:])
rpy = matrixToRpy(quat.matrix())
J_rpy_inv = computeRpyJacobianInverse(rpy)
return np.concatenate((quat * v[:3], J_rpy_inv @ v[3:]))
def moveArrow(self, name, pos1, pos2):
if (ENABLE_VIEWER):
length = norm(pos1 - pos2)
self.robot.viewer.gui.resizeArrow('world/' + name,
self.arrow_radius[name], length)
# compute rotation matrix
R = np.matlib.identity(3)
a = pos2 - pos1
if (norm(a) != 0.0):
a /= norm(a)
R[:, 0] = a
R[:, 1] = cross(a,
np.matrix([1., 0., 0.]).T)
if (norm(R[:, 1]) == 0.0):
R[:, 1] = cross(a,
np.matrix([0., 1., 0.]).T)
R[:, 1] /= norm(R[:, 1])
R[:, 2] = cross(a, R[:, 1])
R[:, 2] /= norm(R[:, 2])
rpy = matrixToRpy(R)
self.updateObjectConfigRpy(name, pos1, rpy)
def se3ToXYZRPY(M):
p = np.zeros((6, ))
p[:3] = M.translation
p[3:] = matrixToRpy(M.rotation)
return p
def XYZQuatToXYZRPY(xyzquat):
"""Convert [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw].
"""
return np.concatenate((
xyzquat[:3], matrixToRpy(pin.Quaternion(xyzquat[3:]).matrix())))
def SE3ToXYZRPY(M):
"""Convert Pinocchio SE3 object to [X,Y,Z,Roll,Pitch,Yaw] vector.
"""
return np.concatenate((M.translation, matrixToRpy(M.rotation)))
def homoToXYZRPY(H):
r = np.matrix([[H[0], H[1], H[2]], [H[4], H[5], H[6]], [H[8], H[9],
H[10]]])
t = np.matrix([[H[3]], [H[7]], [H[11]]])
return np.vstack([t, matrixToRpy(r)]).A1
def se3ToXYZRPY(M):
rpy = np.squeeze(np.asarray(matrixToRpy(M.rotation))).tolist()
xyz = np.squeeze(np.asarray(M.translation)).tolist()
return np.array(xyz + rpy)
def test_flexibility_rotor_inertia(self):
"""
@brief Test the addition of a flexibility in the system.
@details This test asserts that, by adding a flexibility and a rotor inertia,
the output is 'sufficiently close' to a SEA system:
see 'note_on_flexibli_model.pdf' for more information as to why this
is not a true equality.
"""
# Controller: PD controller on motor.
k_control = 100.0
nu_control = 1.0
def computeCommand(t, q, v, sensor_data, u):
u[:] = -k_control * q[4] - nu_control * v[3]
def internalDynamics(t, q, v, sensor_data, u):
u[:] = 0.0
# Physical parameters: rotor inertia, spring stiffness and damping.
J = 0.1
k = 20.0
nu = 0.1
# Enable flexibility
model_options = self.robot.get_model_options()
model_options["dynamics"]["enableFlexibleModel"] = True
model_options["dynamics"]["flexibilityConfig"] = [{
'jointName':
"PendulumJoint",
'stiffness':
k * np.ones(3),
'damping':
nu * np.ones(3)
}]
self.robot.set_model_options(model_options)
# Enable rotor inertia
motor_options = self.robot.get_motors_options()
motor_options["PendulumJoint"]['enableRotorInertia'] = True
motor_options["PendulumJoint"]['rotorInertia'] = J
self.robot.set_motors_options(motor_options)
controller = jiminy.ControllerFunctor(computeCommand, internalDynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6) # Turn off gravity
engine.set_options(engine_options)
# To avoid having to handle angle conversion,
# start with an initial velocity for the output mass.
v_init = 0.1
x0 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, v_init, 0.0, 0.0])
tf = 10.0
# Run simulation
engine.simulate(tf, x0)
# Get log data
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = np.stack([log_data['.'.join(['HighLevelController', s])]
for s in self.robot.logfile_position_headers + \
self.robot.logfile_velocity_headers], axis=-1)
# Convert quaternion to RPY
x_jiminy = np.stack([
np.concatenate(
(matrixToRpy(Quaternion(x[:4][:, np.newaxis]).matrix()).astype(
x.dtype, copy=False), x[4:])) for x in x_jiminy
],
axis=0)
# First, check that there was no motion other than along the Y axis.
self.assertTrue(np.allclose(x_jiminy[:, [0, 2, 4, 6]], 0))
# Now let's group x_jiminy to match the analytical system:
# flexibility angle, pendulum angle, flexibility velocity, pendulum velocity
x_jiminy_extract = x_jiminy[:, [1, 3, 5, 7]]
# And let's simulate the system: a perfect SEA system.
pnc_model = self.robot.pinocchio_model_th
I = pnc_model.inertias[1].mass * pnc_model.inertias[1].lever[2]**2
# Write system dynamics
A = np.array([[0, 0, 1, 0], [0, 0, 0, 1],
[
-k * (1 / I + 1 / J), k_control / J,
-nu * (1 / I + 1 / J), nu_control / J
], [k / J, -k_control / J, nu / J, -nu_control / J]])
x_analytical = np.stack(
[expm(A * t).dot(x_jiminy_extract[0]) for t in time], axis=0)
# This test has a specific tolerance because we know the dynamics don't exactly
# match: they are however very close, since the inertia of the flexible element
# is negligible before I.
TOLERANCE = 1e-4
self.assertTrue(
np.allclose(x_jiminy_extract, x_analytical, atol=TOLERANCE))
def test_flexibility_armature(self):
"""
@brief Test the addition of a flexibility in the system.
@details This test asserts that, by adding a flexibility and a rotor
inertia, the output is 'sufficiently close' to a SEA system:
see 'note_on_flexibilty_model.pdf' for more information as to
why this is not a true equality.
"""
# Physical parameters: rotor inertia, spring stiffness and damping.
J = 0.1
k = 20.0
nu = 0.1
# Enable flexibility
model_options = self.robot.get_model_options()
model_options["dynamics"]["enableFlexibleModel"] = True
model_options["dynamics"]["flexibilityConfig"] = [{
'frameName':
"PendulumJoint",
'stiffness':
k * np.ones(3),
'damping':
nu * np.ones(3)
}]
self.robot.set_model_options(model_options)
# Enable rotor inertia
motor_options = self.robot.get_motors_options()
motor_options["PendulumJoint"]['enableArmature'] = True
motor_options["PendulumJoint"]['armature'] = J
self.robot.set_motors_options(motor_options)
# Create an engine: PD controller on motor and no internal dynamics
k_control, nu_control = 100.0, 1.0
def ControllerPD(t, q, v, sensors_data, command):
command[:] = -k_control * q[4] - nu_control * v[3]
engine = jiminy.Engine()
setup_controller_and_engine(engine,
self.robot,
compute_command=ControllerPD)
# Configure the engine
engine_options = engine.get_options()
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine_options["world"]["gravity"] = np.zeros(6)
engine.set_options(engine_options)
# Run simulation and extract some information from log data.
# Note that to avoid having to handle angle conversion, start with an
# initial velocity for the output mass.
v_init = 0.1
x0 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, v_init, 0.0, 0.0])
tf = 10.0
time, x_jiminy = simulate_and_get_state_evolution(engine,
tf,
x0,
split=False)
# Convert quaternion to RPY
x_jiminy = np.stack([
np.concatenate((
matrixToRpy(Quaternion(x[:4][:, np.newaxis]).matrix())\
.astype(x.dtype, copy=False),
x[4:]
)) for x in x_jiminy
], axis=0)
# First, check that there was no motion other than along the Y axis.
self.assertTrue(np.allclose(x_jiminy[:, [0, 2, 4, 6]], 0))
# Now let's group x_jiminy to match the analytical system:
# flexibility angle, pendulum angle, flexibility velocity, pendulum
# velocity.
x_jiminy_extract = x_jiminy[:, [1, 3, 5, 7]]
# Simulate the system: a perfect SEA system.
A = np.array([[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0],
[
-k * (1 / self.I + 1 / J), k_control / J,
-nu * (1 / self.I + 1 / J), nu_control / J
], [k / J, -k_control / J, nu / J, -nu_control / J]])
x_analytical = np.stack(
[scipy.linalg.expm(A * t).dot(x_jiminy_extract[0]) for t in time],
axis=0)
# This test has a specific tolerance because we know the dynamics don't exactly
# match: they are however very close, since the inertia of the flexible element
# is negligible before I.
self.assertTrue(np.allclose(x_jiminy_extract, x_analytical, atol=1e-4))
