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 _get_simulated_and_analytical_solutions(self, engine, tf, xInit):
"""
@brief Simulate the system dynamics and compute the corresponding
analytical solution at the same timesteps.
"""
# Run simulation and extract some information from log data
time, x_jiminy = simulate_and_get_state_evolution(
engine, tf, xInit, split=False)
# Compute analytical solution
x_analytical = np.stack([
scipy.linalg.expm(self.A * t).dot(self.x0)
for t in time], axis=0)
return time, x_jiminy, x_analytical
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_multi_robot(self):
"""
@brief Simulate interaction between two robots.
@details The system simulated can be represented as such: each system
is a single mass linked to a spring, and a spring k_12 links
both systems.
k1 M1 k(1->2) M2
//| <><> |__| <><><><> | |
k2 | |
//| <><> <><> <><> <><> |__|
"""
# Specify model
urdf_name = "linear_single_mass.urdf"
motors_names = ["Joint"]
# Specify spring stiffness and damping for this simulation
# First two parameters are the stiffness of each system,
# third is the stiffness of the coupling spring
k = np.array([100, 20, 50])
nu = np.array([0.1, 0.2, 0.2])
# Create controllers
class Controllers:
def __init__(self, k, nu):
self.k = k
self.nu = nu
def compute_command(self, t, q, v, sensors_data, command):
pass
def internal_dynamics(self, t, q, v, sensors_data, u_custom):
u_custom[:] = - self.k * q - self.nu * v
# Create two identical robots
engine = jiminy.EngineMultiRobot()
# 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-2
engine.set_options(engine_options)
system_names = ['FirstSystem', 'SecondSystem']
robots = []
for i in range(2):
robots.append(load_urdf_default(urdf_name, motors_names))
# Create controller
controller = Controllers(k[i], nu[i])
controller = jiminy.BaseControllerFunctor(
controller.compute_command, controller.internal_dynamics)
controller.initialize(robots[i])
# Add system to engine.
engine.add_system(system_names[i], robots[i], controller)
# Add coupling force between both systems: a spring between both masses
def force(t, q1, v1, q2, v2, f):
f[0] = k[2] * (q2[0] - q1[0]) + nu[2] * (v2[0] - v1[0])
engine.register_force_coupling(
system_names[0], system_names[1], "Mass", "Mass", force)
# Run simulation and extract some information from log data
x0 = {'FirstSystem': np.array([0.1, 0.0]),
'SecondSystem': np.array([-0.1, 0.0])}
tf = 10.0
time, x_jiminy = simulate_and_get_state_evolution(
engine, tf, x0, split=False)
x_jiminy = np.concatenate(x_jiminy, axis=-1)
# Define analytical system dynamics: two masses linked by three springs
m = [r.pinocchio_model_th.inertias[1].mass for r in robots]
k_eq = [x + k[2] for x in k]
nu_eq = [x + nu[2] for x in nu]
A = np.array([[ 0.0, 1.0, 0.0, 0.0],
[-k_eq[0] / m[0], -nu_eq[0] / m[0], k[2] / m[0], nu[2] / m[0]],
[ 0.0, 0.0, 0.0, 1.0],
[ k[2] / m[1], nu[2] / m[1], -k_eq [1]/ m[1], -nu_eq[1] / m[1]]])
# Compute analytical solution
x_analytical = np.stack([
expm(A * t).dot(x_jiminy[0, :]) for t in time], axis=0)
# Compare the numerical and analytical solutions
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
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_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_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_constraint_external_force(self):
"""
@brief Test support of external force applied with constraints.
@details To provide a non-trivial test case with an external force
non-colinear to the constraints, simulate two masses
oscillating, one along the x axis and the other along the y
axis, with a spring between them (in diagonal). The system
may be represented as such ((f) indicates fixed bodies)
[M_22 (f)]
^
^
[M_21]\<>\
^ \<>\
^ \<>\
^ \<>\
[O (f)] <><> [M_11] <><> [M_12 (f)]
"""
# Build two robots with freeflyers, with a freeflyer and a fixed second
# body constraint.
# Rebuild the model with a freeflyer
robots = [jiminy.Robot(), jiminy.Robot()]
engine = jiminy.EngineMultiRobot()
# 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 some internal parameters
systems_names = ['FirstSystem', 'SecondSystem']
k = np.array([[100, 50], [80, 120]])
nu = np.array([[0.2, 0.01], [0.05, 0.1]])
k_cross = 100
# Initialize and configure the engine
for i in [0, 1]:
# Load robot
robots[i] = load_urdf_default(
self.urdf_name, self.motors_names, has_freeflyer=True)
# Apply constraints
freeflyer_constraint = jiminy.FixedFrameConstraint("world")
robots[i].add_constraint("world", freeflyer_constraint)
fix_mass_constraint = jiminy.FixedFrameConstraint("SecondMass")
robots[i].add_constraint("fixMass", fix_mass_constraint)
# Create controller
class Controller:
def __init__(self, k, nu):
self.k = k
self.nu = nu
def compute_command(self, t, q, v, sensors_data, command):
pass
def internal_dynamics(self, t, q, v, sensors_data, u_custom):
u_custom[6:] = - self.k * q[7:] - self.nu * v[6:]
controller = Controller(k[i, :], nu[i, :])
controller = jiminy.BaseControllerFunctor(
controller.compute_command, controller.internal_dynamics)
controller.initialize(robots[i])
# Add system to engine
engine.add_system(systems_names[i], robots[i], controller)
# Add coupling force
def force(t, q1, v1, q2, v2, f):
# Putting a linear spring between both systems would actually
# decouple them (the force applied on each system would be a
# function of this system state only). So we use a nonlinear
# stiffness, proportional to the square of the distance of both
# systems to the origin.
d2 = q1[7] ** 2 + q2[7] ** 2
f[0] = - k_cross * (1 + d2) * q1[7]
f[1] = + k_cross * (1 + d2) * q2[7]
engine.register_force_coupling(
systems_names[0], systems_names[1], "FirstMass", "FirstMass", force)
# Initialize the whole system.
x_init = {}
## First system: freeflyer at identity
x_init['FirstSystem'] = np.zeros(17)
x_init['FirstSystem'][7:9] = self.x0[:2]
x_init['FirstSystem'][-2:] = self.x0[2:]
x_init['FirstSystem'][6] = 1.0
## Second system: rotation by pi/2 around Z to bring X axis to Y axis
x_init['SecondSystem'] = x_init['FirstSystem'].copy()
x_init['SecondSystem'][5:7] = np.sqrt(2) / 2.0
# Run simulation and extract some information from log data
time, x_jiminy = simulate_and_get_state_evolution(
engine, self.tf, x_init, split=False)
# Verify that both freeflyers didn't moved
for x in x_jiminy:
self.assertTrue(np.allclose(x[:, 9:15], 0.0, atol=TOLERANCE))
self.assertTrue(np.allclose(x[:, :7], x[0, :7], atol=TOLERANCE))
# Extract coordinates in a minimum state vector
x_jiminy_extract = np.hstack([x[:, [7, 8, 15, 16]] for x in x_jiminy])
# Define dynamics of this system
def system_dynamics(t, x):
# Velocity to position
dx = np.zeros(8)
dx[:2] = x[2:4]
dx[4:6] = x[6:8]
# Compute acceleration linked to the spring
for i in range(2):
dx[2 + 4 * i] = (
- k[i, 0] * x[4 * i] - nu[i, 0] * x[2 + 4 * i]
+ k[i, 1] * x[1 + 4 * i] + nu[i, 1] * x[3 + 4 * i])
# Coupling force between both system
dsquared = x[0] ** 2 + x[4] ** 2
dx[2] += - k_cross * (1 + dsquared) * x[0]
dx[6] += - k_cross * (1 + dsquared) * x[4]
for i in [0, 1]:
# Divide forces by mass
dx[2 + 4 * i] /= self.m[0]
# Second mass accelration should be opposite of the first
dx[3 + 4 * i] = -dx[2 + 4 * i]
return dx
x0 = np.hstack([x_init[key][[7, 8, 15, 16]] for key in x_init])
x_python = integrate_dynamics(time, x0, system_dynamics)
self.assertTrue(np.allclose(
x_jiminy_extract, x_python, atol=TOLERANCE))