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_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_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_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_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_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))
for joint_name in motor_joint_names:
motor = jiminy.SimpleMotor(joint_name)
robot.attach_motor(motor)
motor.initialize(joint_name)
# 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
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_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 | |
//| <><> <><> <><> <><> |__|
"""
# Load URDF, create robot.
urdf_path = "data/linear_single_mass.urdf"
# 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, u):
u[:] = 0
def internal_dynamics(self, t, q, v, sensors_data, u):
u[:] = -self.k * q - self.nu * v
# Define initial state (for both systems) and simulation duration
x0 = {
'FirstSystem': np.array([0.1, 0.0]),
'SecondSystem': np.array([-0.1, 0.0])
}
tf = 10.0
# Create two identical robots
engine = jiminy.EngineMultiRobot()
system_names = ['FirstSystem', 'SecondSystem']
robots = []
for i in range(2):
robots.append(load_urdf_default(urdf_path, ["Joint"]))
# Create controller
controller = Controllers(k[i], nu[i])
controller = jiminy.ControllerFunctor(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 coupling_force(t, q1, v1, q2, v2, f):
f[0] = k[2] * (q2[0] - q1[0]) + nu[2] * (v2[0] - v1[0])
engine.add_coupling_force(system_names[0], system_names[1], "Mass",
"Mass", coupling_force)
# Run multi-robot simulation.
engine.simulate(tf, x0)
# Extract log data
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = np.stack([log_data['.'.join(('HighLevelController', system_names[i], s))]
for i in range(len(system_names)) \
for s in robots[i].logfile_position_headers + \
robots[i].logfile_velocity_headers] , axis=-1)
# Write 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, 1, 0, 0],
[-k_eq[0] / m[0], -nu_eq[0] / m[0], k[2] / m[0], nu[2] / m[0]],
[0, 0, 0, 1],
[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_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 = []
engine = jiminy.EngineMultiRobot()
system_names = ['FirstSystem', 'SecondSystem']
k = np.array([[100, 50], [80, 120]])
nu = np.array([[0.2, 0.01], [0.05, 0.1]])
k_cross = 100
class Controllers():
def __init__(self, k, nu):
self.k = k
self.nu = nu
def compute_command(self, t, q, v, sensors_data, u):
u[:] = 0
def internal_dynamics(self, t, q, v, sensors_data, u):
u[6:] = - self.k * q[7:] - self.nu * v[6:]
for i in range(2):
# Load robot.
robots.append(load_urdf_default(self.urdf_path, self.motor_names, 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
controller = Controllers(k[i, :], nu[i, :])
controller = jiminy.ControllerFunctor(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.
def coupling_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.
dsquared = q1[7] ** 2 + q2[7] ** 2
f[0] = - k_cross * (1 + dsquared) * q1[7]
f[1] = k_cross * (1 + dsquared) * q2[7]
engine.add_coupling_force(system_names[0], system_names[1], "FirstMass", "FirstMass", coupling_force)
# Initialize the whole system.
# First sytem: freeflyer at identity.
x_init = {'FirstSystem': np.zeros(17)}
x_init['FirstSystem'][7:9] = self.x0[:2]
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'] = np.copy(x_init['FirstSystem'])
x_init['SecondSystem'][5:7] = np.sqrt(2) / 2.0
# Run simulation
engine.simulate(self.tf, x_init)
# Extract log data
log_data, _ = engine.get_log()
time = log_data['Global.Time']
x_jiminy = [np.stack([log_data['.'.join(['HighLevelController', system_names[i], s])]
for s in robots[i].logfile_position_headers + \
robots[i].logfile_velocity_headers] , axis=-1)
for i in range(len(system_names))]
# Verify that both freeflyers didn't moved.
for i in range(len(system_names)):
self.assertTrue(np.allclose(x_jiminy[i][:, 9:15], 0.0, atol=TOLERANCE))
self.assertTrue(np.allclose(x_jiminy[i][:, :7], x_jiminy[i][0, :7], atol=TOLERANCE))
# Extract coordinates in a minimum state vector.
x_jiminy_extract = np.hstack([x_jiminy[i][:, [7,8,15,16]]
for i in range(len(system_names))])
# Define dynamics of this system
def system_dynamics(t, x):
dx = np.zeros(8)
# Velocity to position.
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 range(2):
# Devide forces by mass.
m = robots[i].pinocchio_model_th.inertias[1].mass
dx[2 + 4 * i] /= m
# 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))
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)
