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_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_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_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_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_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_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))
