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_continuous_simulation(self):
"""
@brief Test simulation of this system using a continuous time controller.
"""
def compute_command(t, q, v, sensors_data, u):
u[:] = - self.k * q - self.nu * v
def internal_dynamics(t, q, v, sensors_data, u):
u[:] = 0.0
controller = jiminy.ControllerFunctor(compute_command, internal_dynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
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.set_options(engine_options)
# Run simulation
engine.simulate(self.tf, self.x0)
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)
# Compute analytical solution
x_analytical = np.stack([expm(self.A * t).dot(self.x0) for t in time], axis=0)
# Compare the numerical and analytical solutions
self.assertTrue(np.allclose(x_jiminy, x_analytical, 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_contact_point_dynamics(self):
"""
@brief Validate the contact dynamics.
@details The energy is expected to decrease slowly when penetrating into the ground,
but should stay constant otherwise. Then, the equilibrium point must also
match the physics. Note that the friction model is not assessed here.
"""
# Create the engine
engine = jiminy.Engine()
engine.initialize(self.robot)
engine_options = engine.get_options()
engine_options['contacts']['stiffness'] = self.k_contact
engine_options['contacts']['damping'] = self.nu_contact
engine_options['contacts']['transitionEps'] = 1.0 / self.k_contact # To avoid assertion failure because of problem regularization
engine_options["stepper"]["dtMax"] = self.dtMax
engine_options["stepper"]["logInternalStepperSteps"] = True
engine.set_options(engine_options)
# Extract some information about the engine and the robot
mass = self.robot.pinocchio_model.inertias[-1].mass
gravity = engine.get_options()['world']['gravity'][2]
# Run simulation
x0 = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ]) # [TX,TY,TZ],[QX,QY,QZ,QW]
tf = 1.5
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = np.stack([log_data['HighLevelController.' + s]
for s in self.robot.logfile_position_headers + \
self.robot.logfile_velocity_headers], axis=-1)
# Total energy and derivative
E_contact = 1/2 * self.k_contact * np.minimum(x_jiminy[:, 2], 0.0) ** 2
E_robot = log_data['HighLevelController.energy']
E_tot = E_robot + E_contact
E_diff_robot = np.concatenate((np.diff(E_robot) / np.diff(time), np.array([0.0], dtype=E_robot.dtype)))
E_diff_tot = np.concatenate((np.diff(E_tot) / np.diff(time), np.array([0.0], dtype=E_robot.dtype)))
# Check that the total energy never increases
# One must use a specific, less restrictive, tolerance, because of numerical differentiation error of float32.
TOLERANCE_diff = 5e-2
self.assertTrue(np.all(E_diff_tot < TOLERANCE_diff))
# Check that the energy of robot only increases when the robot is moving upward while still in the ground.
# This is done by check that there is not two consecutive samples violating this law.
self.assertTrue(np.all(np.diff(np.where((E_diff_robot > 0.0) != \
np.logical_and(x_jiminy[:, 9] > 0.0, x_jiminy[:, 2] < 0.0))) > 1))
# Compare the numerical and analytical equilibrium state
idx = self.robot.pinocchio_model.frames[self.robot.pinocchio_model.getFrameId("MassBody")].parent
self.assertTrue(np.allclose(-engine.system_state.f_external[idx].linear[2], mass * gravity, atol=TOLERANCE))
self.assertTrue(np.allclose(self.k_contact * x_jiminy[-1, 2], mass * gravity, atol=TOLERANCE))
def test_force_sensor(self):
"""
@brief Validate output of force sensor.
@details The energy is expected to decrease slowly when penetrating into the ground,
but should stay constant otherwise. Then, the equilibrium point must also
match the physics. Note that the friction model is not assessed here.
"""
# Create the engine
engine = jiminy.Engine()
engine.initialize(self.robot)
engine_options = engine.get_options()
engine_options['contacts']['stiffness'] = self.k_contact
engine_options['contacts']['damping'] = self.nu_contact
engine_options['contacts']['transitionEps'] = 1.0 / self.k_contact # To avoid assertion failure because of problem regularization
engine_options["stepper"]["dtMax"] = self.dtMax
engine_options["stepper"]["logInternalStepperSteps"] = True
engine.set_options(engine_options)
idx = self.robot.pinocchio_model.getFrameId("MassBody")
def computeCommand(t, q, v, sensors_data, u):
# Verify sensor data.
f = Force(sensors_data[jiminy.ForceSensor.type, "MassBody"], np.zeros(3))
f_joint_sensor = self.robot.pinocchio_model.frames[idx].placement * f
f_jiminy = engine.system_state.f_external[self.robot.pinocchio_model.frames[idx].parent]
self.assertTrue(np.allclose(f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE))
u[:] = 0.0
# Internal dynamics: make the mass spin to generate nontrivial rotations.
def internalDynamics(t, q, v, sensors_data, u):
u[3:6] = 1.0
controller = jiminy.ControllerFunctor(computeCommand, internalDynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
# Run simulation
x0 = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ]) # [TX,TY,TZ],[QX,QY,QZ,QW]
tf = 1.5
engine.simulate(tf, x0)
def test_fixed_body_constraint_rotor_inertia(self):
"""
@brief Test fixed body constraint together with rotor inertia.
"""
# Create robot with freeflyer, set rotor inertia.
self.robot = load_urdf_default(self.urdf_path, ["PendulumJoint"])
J = 0.1
motor_options = self.robot.get_motors_options()
motor_options["PendulumJoint"]['enableRotorInertia'] = True
motor_options["PendulumJoint"]['rotorInertia'] = J
self.robot.set_motors_options(motor_options)
# No controller
def computeCommand(t, q, v, sensor_data, u):
u[:] = 0.0
# Dynamics: simulate a spring of stifness k
k_spring = 500
def internalDynamics(t, q, v, sensor_data, u):
u[:] = -k_spring * q[:]
controller = jiminy.ControllerFunctor(computeCommand, internalDynamics)
controller.initialize(self.robot)
# Set fixed body constraint.
freeflyer_constraint = jiminy.FixedFrameConstraint("world")
self.robot.add_constraint("world", freeflyer_constraint)
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)
x0 = np.array([0.1, 0.0])
tf = 2.0
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = np.stack([log_data['HighLevelController.' + s]
for s in self.robot.logfile_position_headers + \
self.robot.logfile_velocity_headers], axis=-1)
# Analytical solution: dynamics should be unmodifed by
# the constraint, so we have a simple mass on a spring.
pnc_model = self.robot.pinocchio_model_th
I = pnc_model.inertias[1].mass * pnc_model.inertias[1].lever[2]**2
# Write system dynamics
I_eq = I + J
A = np.array([[0, 1], [-k_spring / I_eq, 0]])
x_analytical = np.stack([expm(A * t).dot(x0) for t in time], axis=0)
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 test_rotor_inertia(self):
"""
@brief Verify the dynamics of the system when adding rotor inertia.
"""
# No controller
def computeCommand(t, q, v, sensors_data, u):
u[:] = 0.0
# Dynamics: simulate a spring of stiffness k
k_spring = 500
def internalDynamics(t, q, v, sensors_data, u):
u[:] = -k_spring * q[:]
controller = jiminy.ControllerFunctor(computeCommand, internalDynamics)
controller.initialize(self.robot)
# Set rotor inertia
J = 0.1
motor_options = self.robot.get_motors_options()
motor_options["PendulumJoint"]['enableRotorInertia'] = True
motor_options["PendulumJoint"]['rotorInertia'] = J
self.robot.set_motors_options(motor_options)
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)
x0 = np.array([0.1, 0.0])
tf = 2.0
# Run simulation
engine.simulate(tf, x0)
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)
# Analytical solution: a simple mass on a spring
pnc_model = self.robot.pinocchio_model_th
I = pnc_model.inertias[1].mass * pnc_model.inertias[1].lever[2]**2
# Write system dynamics
I_eq = I + J
A = np.array([[0, 1], [-k_spring / I_eq, 0]])
x_analytical = np.stack([expm(A * t).dot(x0) for t in time], axis=0)
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
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_external_force_profile(self):
"""
@brief Test adding an external force profile function to the system.
"""
# Set same springs as usual
def compute_command(t, q, v, sensors_data, u):
u[:] = 0.0
def internal_dynamics(t, q, v, sensors_data, u):
u[:] = -self.k * q - self.nu * v
controller = jiminy.ControllerFunctor(compute_command,
internal_dynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
# Define external force: a spring linking the second mass to the origin.
k_ext = 50
def external_force(t, q, v, f):
f[0] = -k_ext * (q[0] + q[1])
engine.register_force_profile("SecondMass", external_force)
# Run simulation
engine.simulate(self.tf, self.x0)
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)
# Compute analytical solution
# Add extra external force to second mass.
m = self.robot.pinocchio_model_th.inertias[2].mass
self.A[3, :] += np.array([-k_ext / m, -k_ext / m, 0, 0])
x_analytical = np.stack([expm(self.A * t).dot(self.x0) for t in time],
axis=0)
# Compare the numerical and analytical solutions
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
def test_contact_sensor(self):
"""
@brief Validate output of contact sensor.
@details The energy is expected to decrease slowly when penetrating
into the ground, but should stay constant otherwise. Then,
the equilibrium point must also match the physics. Note that
the friction model is not assessed here.
"""
# Create the robot
robot, *_, joint_idx, frame_pose = self._setup(ShapeType.POINT)
# Create the engine
engine = jiminy.Engine()
# No control law, only check sensors data
def check_sensors_data(t, q, v, sensors_data, command):
nonlocal engine, frame_pose
# Verify sensor data, if the engine has been initialized
if engine.is_initialized:
contact_data = sensors_data[
jiminy.ContactSensor.type, self.body_name]
f = Force(contact_data, np.zeros(3))
f_joint_sensor = frame_pose * f
f_jiminy = engine.system_state.f_external[joint_idx]
self.assertTrue(np.allclose(
f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE))
# Internal dynamics: make the mass spin to generate nontrivial
# rotations.
def spinning_force(t, q, v, sensors_data, u_custom):
u_custom[3:6] = 1.0
# Initialize and configure the engine
self._setup_controller_and_engine(engine, robot,
compute_command=check_sensors_data,
internal_dynamics=spinning_force)
# Run simulation
q0, v0 = neutral_state(robot, split=True)
tf = 1.5
engine.simulate(tf, q0, v0)
def test_fixed_body_constraint(self):
"""
@brief Test kinematic constraint: fixed second mass with a constaint.
"""
# Set same spings as usual
def compute_command(t, q, v, sensors_data, u):
u[:] = 0.0
def internal_dynamics(t, q, v, sensors_data, u):
u[:] = -self.k * q - self.nu * v
controller = jiminy.ControllerFunctor(compute_command,
internal_dynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
# Add a kinematic constraint on the second mass
constraint = jiminy.FixedFrameConstraint("SecondMass")
self.robot.add_constraint("fixMass", constraint)
# Run simulation
engine.simulate(self.tf, self.x0)
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)
# Compute analytical solution
# 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, :]
x_analytical = np.stack([expm(self.A * t).dot(self.x0) for t in time],
axis=0)
# Compare the numerical and analytical solutions
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
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()
engine.initialize(self.robot)
x0 = np.array([0.1, 0.0])
tf = 2.0
# Run simulation
engine.simulate(tf, x0)
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)
# System dynamics: get length and inertia.
l = -self.robot.pinocchio_model_th.inertias[1].lever[2]
g = self.robot.pinocchio_model.gravity.linear[2]
# Pendulum dynamics
def dynamics(t, x):
return np.array([x[1], g / 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_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_collision_and_contact_dynamics(self, shape):
"""
@brief Validate the collision body and contact point dynamics.
@details The energy is expected to decrease slowly when penetrating
into the ground, but should stay constant otherwise. Then,
the equilibrium point must also match the physics. Note that
the friction model is not assessed here.
"""
# Create the robot
robot, weight, height, joint_idx, _ = self._setup(shape)
# Create, initialize, and configure the engine
engine = jiminy.Engine()
self._setup_controller_and_engine(engine, robot)
# Set some extra options of the engine, to avoid assertion failure
# because of problem regularization and outliers
engine_options = engine.get_options()
engine_options['contacts']['transitionEps'] = 1.0e-6
engine_options["stepper"]["controllerUpdatePeriod"] = self.dtMax
engine.set_options(engine_options)
# Run simulation and extract some information from log data
x0 = neutral_state(robot, split=False)
x0[2] = 1.0
tf = 1.5
_, x_jiminy = simulate_and_get_state_evolution(
engine, tf, x0, split=False)
q_z_jiminy = x_jiminy[:, 2]
v_z_jiminy = x_jiminy[:, 9]
penetration_depth = np.minimum(q_z_jiminy - height, 0.0)
# Total energy and derivative
log_data, _ = engine.get_log()
E_robot = log_data['HighLevelController.energy']
E_contact = 1 / 2 * self.k_contact * penetration_depth ** 2
E_tot = E_robot + E_contact
E_diff_robot = np.concatenate((
np.diff(E_robot) / self.dtMax,
np.zeros((1,), dtype=E_robot.dtype)))
E_diff_tot = savgol_filter(E_tot, 201, 2, deriv=1, delta=self.dtMax)
# Check that the total energy never increases.
# One must use a specific, less restrictive, tolerance, because of
# numerical differentiation and filtering error.
self.assertTrue(np.all(E_diff_tot < 1.0e-3))
# Check that the energy of robot only increases when the robot is
# moving upward while still in the ground. This is done by check
# that there is not two consecutive samples violating this law.
# Note that the energy must be non-zero to this test to make
# sense, otherwise the integration error and log accuracy makes
# the test fail.
tolerance_depth = 1e-9
self.assertTrue(np.all(np.diff(np.where(
(abs(E_diff_robot) > tolerance_depth) & ((E_diff_robot > 0.0) != \
((v_z_jiminy > 0.0) & (penetration_depth < 0.0))))[0]) > 1))
# Compare the numerical and analytical equilibrium state.
f_ext_z = engine.system_state.f_external[joint_idx].linear[2]
self.assertTrue(np.allclose(f_ext_z, weight, atol=TOLERANCE))
self.assertTrue(np.allclose(
-penetration_depth[-1], weight / self.k_contact, 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.
"""
# Add IMU
imu_sensor = jiminy.ImuSensor("PendulumLink")
self.robot.attach_sensor(imu_sensor)
imu_sensor.initialize("PendulumLink")
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
engine.initialize(self.robot)
x0 = np.array([0.1, 0.1])
tf = 2.0
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
time = log_data['Global.Time']
quat_jiminy = np.stack(
[log_data['PendulumLink.Quat' + s] for s in ['x', 'y', 'z', 'w']],
axis=-1)
gyro_jiminy = np.stack(
[log_data['PendulumLink.Gyro' + s] for s in ['x', 'y', 'z']],
axis=-1)
accel_jiminy = np.stack(
[log_data['PendulumLink.Accel' + s] for s in ['x', 'y', 'z']],
axis=-1)
# System dynamics: get length and inertia
l = -self.robot.pinocchio_model_th.inertias[1].lever[2]
g = self.robot.pinocchio_model.gravity.linear[2]
# Pendulum dynamics
def dynamics(t, x):
return np.stack([x[..., 1], g / 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]
# 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-1)
ddtheta = np.concatenate((np.zeros(1), dynamics(0.0, x_rk_python)[:-1,
1]))
expected_accel = np.stack([
-l * ddtheta + g * np.sin(theta),
np.zeros_like(theta), l * dtheta**2 - 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_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_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_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_friction_model(self, shape):
"""
@brief Validate the friction model.
@details The transition between dry, dry-viscous, and viscous friction
is assessed. The energy variation and the steady state are
also compared to the theoretical model.
"""
# Create the robot and engine
robot, weight, height, *_ = self._setup(shape)
# Create, initialize, and configure the engine
engine = jiminy.Engine()
self._setup_controller_and_engine(engine, robot)
# Set some extra options of the engine
engine_options = engine.get_options()
engine_options['contacts']['transitionEps'] = 1.0e-6
engine_options['contacts']['friction'] = self.friction
engine_options['contacts']['transitionVelocity'] = self.transtion_vel
engine_options["stepper"]["controllerUpdatePeriod"] = self.dtMax
engine.set_options(engine_options)
# Register an impulse of force
t0, dt, Fx = 0.05, 0.8, 5.0
F = np.array([Fx, 0.0, 0.0, 0.0, 0.0, 0.0])
engine.register_force_impulse(self.body_name, t0, dt, F)
# Run simulation
x0 = neutral_state(robot, split=False)
x0[2] = height
tf = 1.5
time, _, v_jiminy = simulate_and_get_state_evolution(
engine, tf, x0, split=True)
v_x_jiminy = v_jiminy[:, 0]
# Validate the stiction model: check the transition between dry and
# viscous friction because of stiction phenomena.
log_data, _ = engine.get_log()
acceleration = log_data[
'HighLevelController.currentFreeflyerAccelerationLinX']
jerk = np.diff(acceleration) / np.diff(time)
snap = np.diff(jerk) / np.diff(time[1:])
snap_rel = np.abs(snap / np.max(snap))
snap_disc = time[1:-1][snap_rel > 1.0e-5]
snap_disc = snap_disc[np.concatenate((
[False], np.diff(snap_disc) > 2 * self.dtMax))]
snap_disc_analytical_dry = time[(
(v_x_jiminy > (self.transtion_vel - 2.0e-5)) &
(v_x_jiminy < (self.transtion_vel + 2.0e-5)))]
snap_disc_analytical = np.sort(np.concatenate(
(snap_disc_analytical_dry,
np.array([t0, t0 + self.dtMax, t0 + dt, t0 + dt + self.dtMax]))))
snap_disc_analytical = snap_disc_analytical[np.concatenate((
[False], np.diff(snap_disc_analytical) > 2 * self.dtMax))]
self.assertTrue(len(snap_disc) == len(snap_disc_analytical))
self.assertTrue(np.allclose(
snap_disc, snap_disc_analytical, atol=2*self.dtMax))
# Check that the energy increases only when the force is applied
tolerance_E = 1e-9
E_robot = log_data['HighLevelController.energy']
E_diff_robot = np.concatenate((
np.diff(E_robot) / np.diff(time),
np.zeros((1,), dtype=E_robot.dtype)))
E_inc_range = time[np.where(E_diff_robot > tolerance_E)[0][[0, -1]]]
E_inc_range_analytical = np.array([t0, t0 + dt - self.dtMax])
self.assertTrue(np.allclose(
E_inc_range, E_inc_range_analytical, atol=tolerance_E))
# Check that the steady state matches the theory.
# Note that a specific tolerance is used for the acceleration since the
# steady state is not perfectly reached.
tolerance_acc = 1e-6
v_steady = v_x_jiminy[np.isclose(time, t0 + dt)]
v_steady_analytical = Fx / (self.friction * weight)
a_steady = acceleration[
(time > t0 + dt - self.dtMax) & (time < t0 + dt + self.dtMax)]
self.assertTrue(len(a_steady) == 1)
self.assertTrue(a_steady < tolerance_acc)
self.assertTrue(np.allclose(
v_steady, v_steady_analytical, atol=TOLERANCE))
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()
engine.initialize(self.robot)
# Analytical solution
pnc_model = self.robot.pinocchio_model_th
mass = pnc_model.inertias[1].mass
length = abs(pnc_model.inertias[1].lever[2])
axis = np.array([0.0, 1.0, 0.0])
def sys(t):
q = 0.0
v = 0.0
for i in range(len(F_register)):
if t > F_register[i]["t"]:
pos = length * 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(axis, n)
F_proj = F_register[i]["F"][:3].T.dot(d)
v_delta = ((F_proj + F_register[i]["F"][4] / length) * min(
F_register[i]["dt"], t - F_register[i]["t"])) / mass
if (i < len(F_register) - 1):
q += (v + v_delta) * max(
0,
min(t, F_register[i + 1]["t"]) -
(F_register[i]["t"] + F_register[i]["dt"]))
else:
q += (v + v_delta) * max(
0, t - F_register[i]["t"] + F_register[i]["dt"])
q += (v + v_delta / 2) * min(F_register[i]["dt"],
t - F_register[i]["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"])
# Set the initial state and simulation duration
x0 = np.array([0.0, 0.0])
tf = 1.0
# 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
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = np.stack([log_data['HighLevelController.' + s]
for s in self.robot.logfile_position_headers + \
self.robot.logfile_velocity_headers], axis=-1)
# 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 (the accuracy for the log is 1us)
t_break_err = np.concatenate([
np.array([
min(abs(f["t"] - log_data['Global.Time'])),
min(abs(f["t"] + f["dt"] - log_data['Global.Time']))
]) for f in F_register
])
self.assertTrue(np.allclose(t_break_err, 0.0, atol=1e-12))
# 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.
TOLERANCE = 1e-6
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
# 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
engine.simulate(tf, x0)
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)
# 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 (the accuracy for the log is 1us)
t_break_err = np.concatenate([
np.array([
min(abs(f["t"] - log_data['Global.Time'])),
min(abs(f["t"] + f["dt"] - log_data['Global.Time']))
]) for f in F_register
])
self.assertTrue(np.allclose(t_break_err, 0.0, atol=1e-12))
# Compare the numerical and analytical solution
TOLERANCE = 1e-6
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
# Instantiate the controller
def computeCommand(t, q, v, sensors_data, u):
u[0] = 0.0
def internalDynamics(t, q, v, sensors_data, u):
u[:] = 0.0
controller = jiminy.ControllerFunctor(computeCommand, internalDynamics)
controller.initialize(robot)
# Instantiate the engine
engine = jiminy.Engine()
engine.initialize(robot, controller)
# ######################### Configuration the simulation ################################
robot_options = robot.get_options()
engine_options = engine.get_options()
ctrl_options = controller.get_options()
robot_options["telemetry"]["enableImuSensors"] = True
engine_options["telemetry"]["enableConfiguration"] = True
engine_options["telemetry"]["enableVelocity"] = True
engine_options["telemetry"]["enableAcceleration"] = True
engine_options["telemetry"]["enableTorque"] = True
engine_options["telemetry"]["enableEnergy"] = True
engine_options["world"]["gravity"][2] = -9.81
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_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.
self.robot = load_urdf_default(self.urdf_path, self.motor_names, has_freeflyer = True)
# Set same spings as usual
def compute_command(t, q, v, sensors_data, u):
u[:] = 0.0
def internal_dynamics(t, q, v, sensors_data, u):
u[6:] = - self.k * q[7:] - self.nu * v[6:]
controller = jiminy.ControllerFunctor(compute_command, internal_dynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
# Disable gravity.
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6) # Turn off gravity
engine.set_options(engine_options)
# Add a kinematic constraints.
freeflyer_constraint = jiminy.FixedFrameConstraint("world")
self.robot.add_constraint("world", freeflyer_constraint)
fix_mass_constraint = jiminy.FixedFrameConstraint("SecondMass")
self.robot.add_constraint("fixMass", fix_mass_constraint)
# Initialize with zero freeflyer velocity...
x_init = np.zeros(17)
x_init[7:9] = self.x0[:2]
x_init[-2:] = self.x0[2:]
# ... and a "random" (but fixed) freeflyer quaternion
np.random.seed(42)
x_init[:7] = np.random.rand(7)
x_init[3:7] /= np.linalg.norm(x_init[3:7])
# Run simulation
engine.simulate(self.tf, x_init)
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)
# Verify that freeflyer hasn't 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))
# Compute analytical solution - the acceleration of the second mass should
# be the opposite of that of the first.
self.A[3, :] = -self.A[2, :]
x_analytical = np.stack([expm(self.A * t).dot(self.x0) for t in time], axis=0)
# Compare the numerical and analytical solutions
self.assertTrue(np.allclose(x_jiminy[:, [7,8,15,16]], x_analytical, atol=TOLERANCE))
def test_external_force_profile(self):
"""
@brief Test adding an external force profile function to the system.
"""
# Set same springs as usual
def compute_command(t, q, v, sensors_data, u):
u[:] = 0.0
def internal_dynamics(t, q, v, sensors_data, u):
u[:] = - self.k * q - self.nu * v
controller = jiminy.ControllerFunctor(compute_command, internal_dynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
# Define external force: a spring linking the second mass to the origin.
k_ext = 50
def external_force(t, q, v, f):
f[0] = - k_ext * (q[0] + q[1])
engine.register_force_profile("SecondMass", external_force)
# Run simulation
engine.simulate(self.tf, self.x0)
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)
# Compute analytical solution
# Add extra external force to second mass.
m = self.robot.pinocchio_model_th.inertias[2].mass
self.A[3, :] += np.array([-k_ext / m, -k_ext / m, 0, 0])
x_analytical = np.stack([expm(self.A * t).dot(self.x0) for t in time], axis=0)
# Compare the numerical and analytical solutions
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.
self.robot = load_urdf_default(self.urdf_path, self.motor_names, has_freeflyer = True)
# Initialize with zero freeflyer velocity...
x_init = np.zeros(17)
x_init[7:9] = self.x0[:2]
x_init[-2:] = self.x0[2:]
# ... and a "random" (but fixed) freeflyer quaternion
np.random.seed(42)
x_init[:7] = np.random.rand(7)
x_init[3:7] /= np.linalg.norm(x_init[3:7])
# Define a wrench in the local frame.
f_local = np.array([1.0, 1.0, 0., 0., 0.5, 0.5])
idx = self.robot.pinocchio_model.getJointId("FirstJoint")
def external_force(t, q, v, f):
# Rotate the wrench to project it to the world frame.
R = self.robot.pinocchio_data.oMi[idx].rotation
f[:3] = R @ f_local[:3]
f[3:] = R @ f_local[3:]
def internal_dynamics(t, q, v, sensors_data, u):
# Apply torque on freeflyer to make it spin.
self.assertTrue(np.allclose(np.linalg.norm(q[3:7]), 1.0, atol=TOLERANCE))
u[3:6] = 1.0
def compute_command(t, q, v, sensors_data, u):
# Check force computation: is the local external force what we expected ?
# Exclude first computation as simulator init is bit peculiar.
if t > 0.0:
self.assertTrue(np.allclose(engine.system_state.f_external[idx].vector, f_local, atol=TOLERANCE))
u[:] = 0.0
controller = jiminy.ControllerFunctor(compute_command, internal_dynamics)
controller.initialize(self.robot)
engine = jiminy.Engine()
engine.initialize(self.robot, controller)
engine.register_force_profile("FirstJoint", external_force)
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine_options["stepper"]["sensorsUpdatePeriod"] = 1e-3
engine_options["stepper"]["controllerUpdatePeriod"] = 1e-3
engine.set_options(engine_options)
engine.simulate(self.tf, x_init)
def test_sensor_delay(self):
"""
@brief Test sensor delay for an IMU sensor on a simple pendulum.
"""
# Add IMU.
imu_sensor = jiminy.ImuSensor("PendulumLink")
self.robot.attach_sensor(imu_sensor)
imu_sensor.initialize("PendulumLink")
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
engine.initialize(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)
x0 = np.array([0.1, 0.0])
tf = 2.0
# Configure the IMU
imu_options = imu_sensor.get_options()
imu_options['delayInterpolationOrder'] = 0
imu_options['delay'] = 0.0
imu_sensor.set_options(imu_options)
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
time = log_data['Global.Time']
imu_jiminy = np.stack([
log_data['PendulumLink.' + f] for f in jiminy.ImuSensor.fieldnames
],
axis=-1)
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 = imu_sensor.get_options()
imu_options['delayInterpolationOrder'] = 0
imu_options['delay'] = 1.0e-2
imu_sensor.set_options(imu_options)
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
imu_jiminy_delayed_0 = np.stack([
log_data['PendulumLink.' + f] for f in jiminy.ImuSensor.fieldnames
],
axis=-1)
# Configure the IMU
imu_options = imu_sensor.get_options()
imu_options['delayInterpolationOrder'] = 1
imu_options['delay'] = 1.0e-2
imu_sensor.set_options(imu_options)
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
imu_jiminy_delayed_1 = np.stack([
log_data['PendulumLink.' + f] for f in jiminy.ImuSensor.fieldnames
],
axis=-1)
# 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_sensor_noise_bias(self):
"""
@brief Test sensor noise and bias for an IMU sensor on a simple pendulum in static pose.
"""
# Add IMU.
imu_sensor = jiminy.ImuSensor("PendulumLink")
self.robot.attach_sensor(imu_sensor)
imu_sensor.initialize("PendulumLink")
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
engine.initialize(self.robot)
x0 = np.array([0.0, 0.0])
tf = 200.0
# 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 = imu_sensor.get_options()
imu_options['noiseStd'] = np.linspace(0.0, 0.2, 9)
imu_options['bias'] = np.linspace(0.0, 1.0, 9)
imu_sensor.set_options(imu_options)
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
quat_jiminy = np.stack(
[log_data['PendulumLink.Quat' + s] for s in ['x', 'y', 'z', 'w']],
axis=-1)
gyro_jiminy = np.stack(
[log_data['PendulumLink.Gyro' + s] for s in ['x', 'y', 'z']],
axis=-1)
accel_jiminy = np.stack(
[log_data['PendulumLink.Accel' + s] for s in ['x', 'y', 'z']],
axis=-1)
# 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_friction_model(self):
"""
@brief Validate the friction model.
@details The transition between dry, dry-viscous, and viscous friction is assessed.
The energy variation and the steady state are also compared to the theoretical model.
"""
# Create the engine
engine = jiminy.Engine()
engine.initialize(self.robot)
engine_options = engine.get_options()
engine_options['contacts']['stiffness'] = self.k_contact
engine_options['contacts']['damping'] = self.nu_contact
engine_options['contacts']['frictionDry'] = self.dry_friction
engine_options['contacts']['frictionViscous'] = self.visc_friction
engine_options['contacts']['frictionStictionVel'] = self.v_stiction
engine_options['contacts']['frictionStictionRatio'] = self.r_stiction
engine_options['contacts'][
'transitionEps'] = 1.0 / self.k_contact # To avoid assertion failure because of problem regularization
engine_options["stepper"]["dtMax"] = self.dtMax
engine_options["stepper"]["logInternalStepperSteps"] = True
engine.set_options(engine_options)
# Extract some information about the engine and the robot
mass = self.robot.pinocchio_model.inertias[-1].mass
gravity = engine.get_options()['world']['gravity'][2]
# Register a impulse of force
t0 = 0.05
dt = 0.8
F = 5.0
engine.register_force_impulse("MassBody", t0, dt,
np.array([F, 0.0, 0.0, 0.0, 0.0, 0.0]))
# Run simulation
x0 = np.array([
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
]) # [TX,TY,TZ],[QX,QY,QZ,QW]
tf = 1.5
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = np.stack([log_data['HighLevelController.' + s]
for s in self.robot.logfile_position_headers + \
self.robot.logfile_velocity_headers], axis=-1)
# Validate the stiction model:
# check the transition between dry and viscous friction because of stiction phenomena
acceleration = log_data[
'HighLevelController.currentFreeflyerAccelerationLinX']
jerk = np.diff(acceleration) / np.diff(time)
snap = np.diff(jerk) / np.diff(time[1:])
snap_rel = np.abs(snap / np.max(snap))
snap_disc = time[1:-1][snap_rel > 2.0e-4]
snap_disc_analytical_dry = log_data['Global.Time'][np.logical_and(
x_jiminy[:, 7] > self.v_stiction - self.dtMax,
x_jiminy[:, 7] < self.v_stiction + self.dtMax)]
snap_disc_analytical_viscous = log_data['Global.Time'][np.logical_and(
x_jiminy[:, 7] >
(1.0 + self.r_stiction) * self.v_stiction - self.dtMax,
x_jiminy[:, 7] <
(1.0 + self.r_stiction) * self.v_stiction + self.dtMax)]
snap_disc_analytical = np.sort(
np.concatenate(
(snap_disc_analytical_dry, snap_disc_analytical_viscous,
np.array([t0, t0 + self.dtMax, t0 + dt,
t0 + dt + self.dtMax]))))
self.assertTrue(len(snap_disc) == len(snap_disc_analytical))
self.assertTrue(
np.allclose(snap_disc, snap_disc_analytical, atol=1e-12))
# Check that the energy increases only when the force is applied
E_robot = log_data['HighLevelController.energy']
E_diff_robot = np.concatenate((np.diff(E_robot) / np.diff(time),
np.array([0.0], dtype=E_robot.dtype)))
E_inc_range = log_data['Global.Time'][np.where(
E_diff_robot > 1e-5)[0][[0, -1]]]
E_inc_range_analytical = np.array([t0, t0 + dt - self.dtMax])
self.assertTrue(
np.allclose(E_inc_range, E_inc_range_analytical, atol=5e-3))
# Check that the steady state matches the theory
# Note that a specific tolerance is used for the acceleration since the steady state is not perfectly reached
TOLERANCE_acc = 1e-5
v_steady = x_jiminy[log_data['Global.Time'] == t0 + dt, 7]
v_steady_analytical = -F / (self.visc_friction * mass * gravity)
a_steady = acceleration[np.logical_and(
log_data['Global.Time'] > t0 + dt - self.dtMax,
log_data['Global.Time'] < t0 + dt + self.dtMax)]
self.assertTrue(len(a_steady) == 1)
self.assertTrue(a_steady < TOLERANCE_acc)
self.assertTrue(
np.allclose(v_steady, v_steady_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))
