def test_pendulum_integration(self):
"""
@brief Compare pendulum motion, as simulated by Jiminy, against an
equivalent simulation done in python.
@details Since we don't have a simple analytical expression for the
solution of a (nonlinear) pendulum motion, we perform the
simulation in Python, with the same integrator, and compare
both results.
"""
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
setup_controller_and_engine(engine, self.robot)
# Run simulation and extract log data
x0 = np.array([0.1, 0.0])
tf = 2.0
time, x_jiminy = simulate_and_get_state_evolution(engine,
tf,
x0,
split=False)
# Pendulum dynamics
def dynamics(t, x):
return np.array([x[1], self.g / self.l * np.sin(x[0])])
# Integrate this non-linear dynamics
x_rk_python = integrate_dynamics(time, x0, dynamics)
# Compare the numerical and numerical integration of analytical model
# using Scipy
self.assertTrue(np.allclose(x_jiminy, x_rk_python, atol=TOLERANCE))
def test_fixed_body_constraint(self):
"""
@brief Test kinematic constraint: fixed second mass with a constaint.
"""
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, self.robot, internal_dynamics=self._spring_force)
# 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.set_options(engine_options)
# Add a kinematic constraint on the second mass
constraint = jiminy.FixedFrameConstraint("SecondMass")
self.robot.add_constraint("fixMass", constraint)
# The dynamics of the first mass is not changed, the acceleration of
# the second mass is the opposite of that of the first mass to provide
# a constant output position.
self.A[3, :] = -self.A[2, :]
# Compare the numerical and analytical solutions
_, x_jiminy, x_analytical = \
self._get_simulated_and_analytical_solutions(
engine, self.tf, self.x0)
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
def test_freeflyer_multiple_constraints(self):
"""
@brief Test having several constraints at once.
@details This test features:
- a freeflyer with a fixed body constraint on the
freeflyer. This gives a non-trivial constraint to solve
to effectively cancel the freeflyer.
- a fixed body constaint on the output mass.
"""
# Rebuild the model with a freeflyer
robot = load_urdf_default(
self.urdf_name, self.motors_names, has_freeflyer=True)
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, robot, internal_dynamics=self._spring_force)
# Configure the engine
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6) # Turn off gravity
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine.set_options(engine_options)
# Add a kinematic constraints.
freeflyer_constraint = jiminy.FixedFrameConstraint("world")
robot.add_constraint("world", freeflyer_constraint)
fix_mass_constraint = jiminy.FixedFrameConstraint("SecondMass")
robot.add_constraint("fixMass", fix_mass_constraint)
# Initialize with a random freeflyer configuration and zero velocity
x_init = np.zeros(17)
x_init[:7] = np.random.rand(7)
x_init[3:7] /= np.linalg.norm(x_init[3:7])
x_init[7:9], x_init[-2:] = np.split(self.x0, 2)
# The acceleration of the second mass should be the opposite of that of
# the first
self.A[3, :] = -self.A[2, :]
# Compare the numerical and analytical solutions
_, x_jiminy, x_analytical = \
self._get_simulated_and_analytical_solutions(
engine, self.tf, x_init)
self.assertTrue(np.allclose(
x_jiminy[:, [7, 8, 15, 16]], x_analytical, atol=TOLERANCE))
# Verify in addition that freeflyer has not moved
self.assertTrue(np.allclose(x_jiminy[:, 9:15], 0, atol=TOLERANCE))
self.assertTrue(np.allclose(
x_jiminy[:, :7], x_jiminy[0, :7], atol=TOLERANCE))
def _setup_controller_and_engine(self,
engine, robot,
compute_command=None,
internal_dynamics=None):
# Initialize the engine
setup_controller_and_engine(
engine, robot, compute_command, internal_dynamics)
# configure the engine
engine_options = engine.get_options()
engine_options['contacts']['stiffness'] = self.k_contact
engine_options['contacts']['damping'] = self.nu_contact
engine.set_options(engine_options)
return engine
def test_armature(self):
"""
@brief Verify the dynamics of the system when adding rotor inertia.
"""
# Configure the robot: set rotor inertia
J = 0.1
motor_options = self.robot.get_motors_options()
motor_options["PendulumJoint"]['enableArmature'] = True
motor_options["PendulumJoint"]['armature'] = J
self.robot.set_motors_options(motor_options)
# Dynamics: simulate a spring of stiffness k
k_spring = 500
def spring_force(t, q, v, sensors_data, u_custom):
u_custom[:] = -k_spring * q.flatten()
# Initialize the controller and setup the engine
engine = jiminy.Engine()
setup_controller_and_engine(engine,
self.robot,
internal_dynamics=spring_force)
# 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 log data
x0 = np.array([0.1, 0.0])
tf = 2.0
time, x_jiminy = simulate_and_get_state_evolution(engine,
tf,
x0,
split=False)
# Analytical solution: a simple mass on a spring
I_eq = self.I + J
A = np.array([[0, 1], [-k_spring / I_eq, 0]])
x_analytical = np.stack(
[scipy.linalg.expm(A * t).dot(x0) for t in time], axis=0)
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
def test_continuous_simulation(self):
"""
@brief Test simulation of this system using a continuous time
controller.
"""
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, self.robot, compute_command=self._spring_force)
# Configure the engine
engine_options = engine.get_options()
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0
engine_options["stepper"]["controllerUpdatePeriod"] = 0.0
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine.set_options(engine_options)
# Compare the numerical and analytical solutions
_, x_jiminy, x_analytical = \
self._get_simulated_and_analytical_solutions(
engine, self.tf, self.x0)
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
def test_fixed_body_constraint_armature(self):
"""
@brief Test fixed body constraint together with rotor inertia.
"""
# Create robot with freeflyer, set rotor inertia.
robot = load_urdf_default(self.urdf_name,
self.motors_names,
has_freeflyer=True)
# Enable rotor inertia
J = 0.1
motor_options = robot.get_motors_options()
motor_options["PendulumJoint"]['enableArmature'] = True
motor_options["PendulumJoint"]['armature'] = J
robot.set_motors_options(motor_options)
# Set fixed body constraint.
freeflyer_constraint = jiminy.FixedFrameConstraint("world")
robot.add_constraint("world", freeflyer_constraint)
# Create an engine: simulate a spring internal dynamics
k_spring = 500
def spring_force(t, q, v, sensors_data, u_custom):
u_custom[:] = -k_spring * q[-1]
engine = jiminy.Engine()
setup_controller_and_engine(engine,
robot,
internal_dynamics=spring_force)
# 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
x0 = np.array([0.1, 0.0])
qInit, vInit = neutral_state(robot, split=True)
qInit[-1], vInit[-1] = x0
xInit = np.concatenate((qInit, vInit))
tf = 2.0
time, q_jiminy, v_jiminy = simulate_and_get_state_evolution(engine,
tf,
xInit,
split=True)
# Analytical solution: dynamics should be unmodified by
# the constraint, so we have a simple mass on a spring.
I_eq = self.I + J
A = np.array([[0, 1], [-k_spring / I_eq, 0]])
x_analytical = np.stack(
[scipy.linalg.expm(A * t).dot(x0) for t in time], axis=0)
self.assertTrue(
np.allclose(q_jiminy[:, :-1], qInit[:-1], atol=TOLERANCE))
self.assertTrue(
np.allclose(v_jiminy[:, :-1], vInit[:-1], atol=TOLERANCE))
self.assertTrue(
np.allclose(q_jiminy[:, -1], x_analytical[:, 0], atol=TOLERANCE))
self.assertTrue(
np.allclose(v_jiminy[:, -1], x_analytical[:, 1], 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))
def test_pendulum_force_impulse(self):
"""
@brief Validate the impulse-momentum theorem
@details The analytical expression for the solution is exact for
impulse of force that are perfect dirac functions.
"""
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
setup_controller_and_engine(engine, self.robot)
# Analytical solution
def sys(t):
q = 0.0
v = 0.0
for i, force in enumerate(F_register):
if t > force["t"]:
pos = self.l * np.array(
[-np.cos(q - np.pi / 2), 0.0,
np.sin(q - np.pi / 2)])
n = pos / np.linalg.norm(pos)
d = np.cross(self.axis, n)
F_proj = force["F"][:3].T.dot(d)
v_delta = ((F_proj + force["F"][4] / self.l) *
min(force["dt"], t - force["t"])) / self.m
if (i < len(F_register) - 1):
q += (v + v_delta) * max(0, min(t,
F_register[i + 1]["t"]) - \
(force["t"] + force["dt"]))
else:
q += (v + v_delta) * max(0,
t - force["t"] + force["dt"])
q += (v + v_delta / 2) * min(force["dt"], t - force["t"])
v += v_delta
else:
break
return np.array([q, v])
# Register a set of impulse forces
np.random.seed(0)
F_register = [{
"t": 0.0,
"dt": 2.0e-3,
"F": np.array([1.0e3, 0.0, 0.0, 0.0, 0.0, 0.0])
}, {
"t": 0.1,
"dt": 1.0e-3,
"F": np.array([0.0, 1.0e3, 0.0, 0.0, 0.0, 0.0])
}, {
"t": 0.2,
"dt": 2.0e-5,
"F": np.array([-1.0e5, 0.0, 0.0, 0.0, 0.0, 0.0])
}, {
"t": 0.2,
"dt": 2.0e-4,
"F": np.array([0.0, 0.0, 1.0e4, 0.0, 0.0, 0.0])
}, {
"t": 0.4,
"dt": 1.0e-5,
"F": np.array([0.0, 0.0, 0.0, 0.0, 2.0e4, 0.0])
}, {
"t": 0.4,
"dt": 1.0e-5,
"F": np.array([1.0e3, 1.0e4, 3.0e4, 0.0, 0.0, 0.0])
}, {
"t": 0.6,
"dt": 1.0e-6,
"F": (2.0 * (np.random.rand(6) - 0.5)) * 4.0e6
}, {
"t": 0.8,
"dt": 2.0e-6,
"F": np.array([0.0, 0.0, 2.0e5, 0.0, 0.0, 0.0])
}]
for f in F_register:
engine.register_force_impulse("PendulumLink", f["t"], f["dt"],
f["F"])
# Configure the engine: No gravity + Continuous time simulation
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0
engine_options["stepper"]["controllerUpdatePeriod"] = 0.0
engine_options["stepper"]["logInternalStepperSteps"] = True
engine.set_options(engine_options)
# Run simulation and extract some information from log data
x0 = np.array([0.0, 0.0])
tf = 1.0
time, x_jiminy = simulate_and_get_state_evolution(engine,
tf,
x0,
split=False)
# Compute the associated analytical solution
x_analytical = np.stack([sys(t) for t in time], axis=0)
# Check if t = t_start / t_end were breakpoints.
# Note that the accuracy for the log is 1us.
t_break_err = np.concatenate([
np.array(
[min(abs(f["t"] - time)),
min(abs(f["t"] + f["dt"] - time))]) for f in F_register
])
self.assertTrue(np.allclose(t_break_err, 0.0, atol=TOLERANCE))
# This test has a specific tolerance because the analytical solution is
# an approximation since in practice, the external force is not
# constant over its whole application duration but rather depends on
# the orientation of the pole. For simplicity, the effect of the
# impulse forces is assumed to be constant. As a result, the tolerance
# cannot be tighter.
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=1e-6))
# Configure the engine: No gravity + Discrete time simulation
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0
engine_options["stepper"]["controllerUpdatePeriod"] = 0.0
engine_options["stepper"]["logInternalStepperSteps"] = True
engine.set_options(engine_options)
# Configure the engine: Continuous time simulation
engine_options["stepper"]["sensorsUpdatePeriod"] = 1.0e-3
engine_options["stepper"]["controllerUpdatePeriod"] = 1.0e-3
engine.set_options(engine_options)
# Run simulation
time, x_jiminy = simulate_and_get_state_evolution(engine,
tf,
x0,
split=False)
# Compute the associated analytical solution
x_analytical = np.stack([sys(t) for t in time], axis=0)
# Check if t = t_start / t_end were breakpoints
t_break_err = np.concatenate([
np.array(
[min(abs(f["t"] - time)),
min(abs(f["t"] + f["dt"] - time))]) for f in F_register
])
self.assertTrue(np.allclose(t_break_err, 0.0, atol=TOLERANCE))
# Compare the numerical and analytical solution
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=1e-6))
def test_sensor_noise_bias(self):
"""
@brief Test sensor noise and bias for an IMU sensor on a simple
pendulum in static pose.
"""
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
setup_controller_and_engine(engine, self.robot)
# Configure the engine: No gravity
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine.set_options(engine_options)
# Configure the IMU
imu_options = self.imu_sensor.get_options()
imu_options['noiseStd'] = np.linspace(0.0, 0.2, 9)
imu_options['bias'] = np.linspace(0.0, 1.0, 9)
self.imu_sensor.set_options(imu_options)
# Run simulation and extract log data
x0 = np.array([0.0, 0.0])
tf = 200.0
_, quat_jiminy, gyro_jiminy, accel_jiminy = \
SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=True)
# Convert quaternion to a rotation vector.
quat_axis = np.stack(
[log3(Quaternion(q[:, np.newaxis]).matrix()) for q in quat_jiminy],
axis=0)
# Estimate the quaternion noise and bias
# Because the IMU rotation is identity, the resulting rotation will
# simply be R_b R_noise. Since R_noise is a small rotation, we can
# consider that the resulting rotation is simply the rotation resulting
# from the sum of the rotation vector (this is only true at the first
# order) and thus directly recover the unbiased sensor data.
quat_axis_bias = np.mean(quat_axis, axis=0)
quat_axis_std = np.std(quat_axis, axis=0)
# Remove sensor rotation bias from gyro / accel data
quat_rot_bias = exp3(quat_axis_bias)
gyro_jiminy = np.vstack([quat_rot_bias @ v for v in gyro_jiminy])
accel_jiminy = np.vstack([quat_rot_bias @ v for v in accel_jiminy])
# Estimate the gyroscope and accelerometer noise and bias
gyro_std = np.std(gyro_jiminy, axis=0)
gyro_bias = np.mean(gyro_jiminy, axis=0)
accel_std = np.std(accel_jiminy, axis=0)
accel_bias = np.mean(accel_jiminy, axis=0)
# Compare estimated sensor noise and bias with the configuration
self.assertTrue(
np.allclose(imu_options['noiseStd'][:3],
quat_axis_std,
atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][:3], quat_axis_bias, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['noiseStd'][3:-3], gyro_std, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][3:-3], gyro_bias, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['noiseStd'][-3:], accel_std, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][-3:], accel_bias, atol=1.0e-2))
def test_sensor_delay(self):
"""
@brief Test sensor delay for an IMU sensor on a simple pendulum.
"""
# Configure the IMU
imu_options = self.imu_sensor.get_options()
imu_options['delayInterpolationOrder'] = 0
imu_options['delay'] = 0.0
self.imu_sensor.set_options(imu_options)
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
setup_controller_and_engine(engine, self.robot)
# Configure the engine: No gravity + Continuous time simulation
engine_options = engine.get_options()
engine_options["stepper"]["sensorsUpdatePeriod"] = 1.0e-3
engine.set_options(engine_options)
# Run simulation and extract imu data
x0 = np.array([0.1, 0.0])
tf = 2.0
time, imu_jiminy = \
SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=False)
# Deduce shifted imu data
imu_jiminy_shifted_0 = interp1d(time,
imu_jiminy,
kind='zero',
bounds_error=False,
fill_value=imu_jiminy[0],
axis=0)(time - 1.0e-2)
imu_jiminy_shifted_1 = interp1d(time,
imu_jiminy,
kind='linear',
bounds_error=False,
fill_value=imu_jiminy[0],
axis=0)(time - 1.0e-2)
# Configure the IMU
imu_options = self.imu_sensor.get_options()
imu_options['delayInterpolationOrder'] = 0
imu_options['delay'] = 1.0e-2
self.imu_sensor.set_options(imu_options)
# Run simulation and extract imu data
time, imu_jiminy_delayed_0 = \
SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=False)
# Configure the IMU
imu_options = self.imu_sensor.get_options()
imu_options['delayInterpolationOrder'] = 1
imu_options['delay'] = 1.0e-2
self.imu_sensor.set_options(imu_options)
# Run simulation
time, imu_jiminy_delayed_1 = \
SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=False)
# Compare sensor signals
self.assertTrue(
np.mean(imu_jiminy_delayed_0 - imu_jiminy_shifted_0) < 1.0e-5)
self.assertTrue(
np.allclose(imu_jiminy_delayed_1,
imu_jiminy_shifted_1,
atol=TOLERANCE))
def test_imu_sensor(self):
"""
@brief Test IMU sensor on pendulum motion.
@details Note that the actual expected solution of the pendulum motion
is used to compute the expected IMU data, instead of the
result of the simulation done by jiminy itself. So this test
is checking at the same time that the result of the simulation
matches the solution, and that the sensor IMU data are valid.
Though it is redundant, it validates that an IMU mounted on a
pendulum gives the signal one would expect from an IMU on a
pendulum, which is what a user would expect. Moreover, Jiminy
output log does not feature the acceleration - to this test is
indirectly checking that the acceleration computed by jiminy
is valid.
@remark Since we don't have a simple analytical expression for the
solution of a (nonlinear) pendulum motion, we perform the
simulation in python, with the same integrator.
"""
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
setup_controller_and_engine(engine, self.robot)
# Run simulation and extract log data
x0 = np.array([0.1, 0.1])
tf = 2.0
time, quat_jiminy, gyro_jiminy, accel_jiminy = \
SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=True)
# Pendulum dynamics
def dynamics(t, x):
return np.stack([x[..., 1], self.g / self.l * np.sin(x[..., 0])],
axis=-1)
# Integrate this non-linear dynamics
x_rk_python = integrate_dynamics(time, x0, dynamics)
# Compute sensor acceleration, i.e. acceleration in polar coordinates
theta = x_rk_python[:, 0]
dtheta = x_rk_python[:, 1]
dtheta = x_rk_python[:, 1]
# Acceleration: to resolve algebraic loop (current acceleration is
# function of input which itself is function of sensor signal, sensor
# data is computed using q_t, v_t, a_t
ddtheta = dynamics(0.0, x_rk_python)[:, 1]
expected_accel = np.stack([
-self.l * ddtheta + self.g * np.sin(theta),
np.zeros_like(theta), self.l * dtheta**2 - self.g * np.cos(theta)
],
axis=-1)
expected_gyro = np.stack(
[np.zeros_like(theta), dtheta,
np.zeros_like(theta)], axis=-1)
expected_quat = np.stack([
Quaternion(rpyToMatrix(np.array([0., t, 0.]))).coeffs()
for t in theta
],
axis=0)
# Compare sensor signal, ignoring first iterations that correspond to
# system initialization
self.assertTrue(
np.allclose(expected_quat[2:, :],
quat_jiminy[2:, :],
atol=TOLERANCE))
self.assertTrue(
np.allclose(expected_gyro[2:, :],
gyro_jiminy[2:, :],
atol=TOLERANCE))
self.assertTrue(
np.allclose(expected_accel[2:, :],
accel_jiminy[2:, :],
atol=TOLERANCE))
def test_external_force_profile(self):
"""
@brief Test adding an external force profile function to the system.
"""
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, self.robot, internal_dynamics=self._spring_force)
# 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.set_options(engine_options)
# Define and register the external force:
# a spring linking the second mass to the origin.
k_ext = 50.0
def external_force(t, q, v, f):
nonlocal k_ext
f[0] = - k_ext * (q[0] + q[1])
engine.register_force_profile("SecondMass", external_force)
# Add the extra external force to second mass
self.A[3, :] += np.array([
-k_ext / self.m[1], -k_ext / self.m[1], 0.0, 0.0])
# Compare the numerical and analytical solutions
_, x_jiminy, x_analytical = \
self._get_simulated_and_analytical_solutions(
engine, self.tf, self.x0)
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
# =====================================================================
# Now, apply a force / torque to a non-trivial rotation to verify
# internal projection of the force onto the joints.
# Rebuild the model with a freeflyer
robot = load_urdf_default(
self.urdf_name, self.motors_names, has_freeflyer=True)
# Initialize with a random freeflyer configuration and zero velocity
q_init = np.zeros(9)
q_init[:7] = np.random.rand(7)
q_init[3:7] /= np.linalg.norm(q_init[3:7])
v_init = np.zeros(8)
q_init[-2:], v_init[-2:] = np.split(self.x0, 2)
# Define the external wrench to apply on the system
f_local = np.array([1.0, 1.0, 0.0, 0.0, 0.5, 0.5])
joint_idx = robot.pinocchio_model.getJointId("FirstJoint")
# Create the controller
def internal_dynamics(t, q, v, sensors_data, u_custom):
# Apply torque on freeflyer to make it spin
self.assertTrue(np.allclose(
np.linalg.norm(q[3:7]), 1.0, atol=TOLERANCE))
u_custom[3:6] = 1.0
def compute_command(t, q, v, sensors_data, command):
# Check if local external force is properly computed
nonlocal f_local
if engine.is_initialized:
f_ext = engine.system_state.f_external[
joint_idx].vector
self.assertTrue(np.allclose(f_ext, f_local, atol=TOLERANCE))
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, robot, compute_command, internal_dynamics)
# Define and register the external force:
# a wrench in the local frame.
def external_force(t, q, v, f):
nonlocal f_local
# Rotate the wrench to project it to the world frame
R = robot.pinocchio_data.oMi[joint_idx].rotation
f[:3] = R @ f_local[:3]
f[3:] = R @ f_local[3:]
engine.register_force_profile("FirstJoint", external_force)
# Configure the engine
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["sensorsUpdatePeriod"] = 1e-3
engine_options["stepper"]["controllerUpdatePeriod"] = 1e-3
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine.set_options(engine_options)
# Run simulation: Check is done directly by control law
engine.simulate(self.tf, q_init, v_init)
