def setUp(self):
# Load URDF, create robot.
self.urdf_path = "data/linear_two_masses.urdf"
self.motor_names = ["FirstJoint", "SecondJoint"]
self.robot = load_urdf_default(self.urdf_path, self.motor_names)
# Specify spring stiffness and damping for this simulation
self.k = np.array([200, 20])
self.nu = np.array([0.1, 0.2])
# Define initial state and simulation duration
self.x0 = np.array([0.1, -0.1, 0.0, 0.0])
self.tf = 10.0
# Compute the matrix representing this linear system for analytical
# computation
m = np.stack([self.robot.pinocchio_model_th.inertias[i].mass
for i in range(1,3)], axis=0)
# Dynamics is linear: dX = A X
I = (1 / m[1] + 1 / m[0])
self.A = np.array([[ 0, 0, 1, 0],
[ 0, 0, 0, 1],
[-self.k[0] / m[0], self.k[1] / m[0], -self.nu[0] / m[0], self.nu[1] / m[0]],
[ self.k[0] / m[0], -self.k[1] * I, self.nu[0] / m[0], -self.nu[1] * I]])
def setUp(self):
# Create the robot and load the URDF
self.urdf_name = "linear_two_masses.urdf"
self.motors_names = ["FirstJoint", "SecondJoint"]
self.robot = load_urdf_default(
self.urdf_name, self.motors_names, has_freeflyer=False)
# Specify spring stiffness and damping for this simulation
self.k = np.array([200.0, 20.0])
self.nu = np.array([0.1, 0.2])
# Define initial state and simulation duration
self.x0 = np.array([0.1, -0.1, 0.0, 0.0])
self.tf = 10.0
# Extract some information about the model
self.m = np.stack([self.robot.pinocchio_model.inertias[i].mass
for i in [1, 2]], axis=0)
self.I = 1 / self.m[1] + 1 / self.m[0]
# Compute the matrix representing this linear system for
# analytical computation. Dynamics is linear: dX = A X
self.A = np.array([
[ 0.0, 0.0, 1.0, 0.0],
[ 0.0, 0.0, 0.0, 1.0],
[-self.k[0] / self.m[0], self.k[1] / self.m[0], -self.nu[0] / self.m[0], self.nu[1] / self.m[0]],
[ self.k[0] / self.m[0], -self.k[1] * self.I, self.nu[0] / self.m[0], -self.nu[1] * self.I]])
# Initialize numpy random seed
np.random.seed(42)
def setUp(self):
# Load URDF, create model.
self.urdf_path = "data/simple_pendulum.urdf"
# Create the jiminy model
# Instanciate model and engine
self.robot = load_urdf_default(self.urdf_path, ["PendulumJoint"])
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 _setup(self, shape):
# Load the right URDF, and create robot.
if shape == ShapeType.POINT:
urdf_name = "point_mass.urdf"
elif shape == ShapeType.BOX:
urdf_name = "box_collision_mesh.urdf"
elif shape == ShapeType.SPHERE:
urdf_name = "sphere_primitive.urdf"
# Create the jiminy robot and controller
robot = load_urdf_default(urdf_name, has_freeflyer=True)
# Add contact point or collision body, along with related sensor,
# depending on shape type.
if shape != ShapeType.POINT:
# Add collision body
robot.add_collision_bodies([self.body_name])
# Add a force sensor
force_sensor = jiminy.ForceSensor(self.body_name)
robot.attach_sensor(force_sensor)
force_sensor.initialize(self.body_name)
else:
# Add contact point
robot.add_contact_points([self.body_name])
# Add a contact sensor
contact_sensor = jiminy.ContactSensor(self.body_name)
robot.attach_sensor(contact_sensor)
contact_sensor.initialize(self.body_name)
# Extract some information about the engine and the robot
m = robot.pinocchio_model.inertias[-1].mass
g = robot.pinocchio_model.gravity.linear[2]
weight = m * np.linalg.norm(g)
if shape == ShapeType.POINT:
height = 0.0
elif shape == ShapeType.BOX:
geom = robot.collision_model.geometryObjects[0]
height = geom.geometry.points(0)[2] # It is a mesh, not an actual box primitive
elif shape == ShapeType.SPHERE:
geom = robot.collision_model.geometryObjects[0]
height = geom.geometry.radius
frame_idx = robot.pinocchio_model.getFrameId(self.body_name)
joint_idx = robot.pinocchio_model.frames[frame_idx].parent
frame_pose = robot.pinocchio_model.frames[frame_idx].placement
return robot, weight, height, joint_idx, frame_pose
def setUp(self):
# Load URDF, create model.
self.urdf_name = "simple_pendulum.urdf"
self.motors_names = ["PendulumJoint"]
self.robot = load_urdf_default(self.urdf_name, self.motors_names)
# Add IMU to the robot
self.imu_sensor = jiminy.ImuSensor("PendulumLink")
self.robot.attach_sensor(self.imu_sensor)
self.imu_sensor.initialize("PendulumLink")
# System dynamics: get length and inertia
axis_str = self.robot.pinocchio_model.joints[1].shortname()[-1]
self.axis = np.zeros(3)
self.axis[ord(axis_str) - ord('X')] = 1.0
self.l = abs(self.robot.pinocchio_model.inertias[1].lever[2])
self.m = self.robot.pinocchio_model.inertias[1].mass
self.g = self.robot.pinocchio_model.gravity.linear[2]
self.I = self.m * self.l**2
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_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)
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))
def test_external_force_profile(self):
"""
@brief Test adding an external force profile function to the system.
"""
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, self.robot, internal_dynamics=self._spring_force)
# Configure the engine
engine_options = engine.get_options()
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine.set_options(engine_options)
# Define and register the external force:
# a spring linking the second mass to the origin.
k_ext = 50.0
def external_force(t, q, v, f):
nonlocal k_ext
f[0] = - k_ext * (q[0] + q[1])
engine.register_force_profile("SecondMass", external_force)
# Add the extra external force to second mass
self.A[3, :] += np.array([
-k_ext / self.m[1], -k_ext / self.m[1], 0.0, 0.0])
# Compare the numerical and analytical solutions
_, x_jiminy, x_analytical = \
self._get_simulated_and_analytical_solutions(
engine, self.tf, self.x0)
self.assertTrue(np.allclose(x_jiminy, x_analytical, atol=TOLERANCE))
# =====================================================================
# Now, apply a force / torque to a non-trivial rotation to verify
# internal projection of the force onto the joints.
# Rebuild the model with a freeflyer
robot = load_urdf_default(
self.urdf_name, self.motors_names, has_freeflyer=True)
# Initialize with a random freeflyer configuration and zero velocity
q_init = np.zeros(9)
q_init[:7] = np.random.rand(7)
q_init[3:7] /= np.linalg.norm(q_init[3:7])
v_init = np.zeros(8)
q_init[-2:], v_init[-2:] = np.split(self.x0, 2)
# Define the external wrench to apply on the system
f_local = np.array([1.0, 1.0, 0.0, 0.0, 0.5, 0.5])
joint_idx = robot.pinocchio_model.getJointId("FirstJoint")
# Create the controller
def internal_dynamics(t, q, v, sensors_data, u_custom):
# Apply torque on freeflyer to make it spin
self.assertTrue(np.allclose(
np.linalg.norm(q[3:7]), 1.0, atol=TOLERANCE))
u_custom[3:6] = 1.0
def compute_command(t, q, v, sensors_data, command):
# Check if local external force is properly computed
nonlocal f_local
if engine.is_initialized:
f_ext = engine.system_state.f_external[
joint_idx].vector
self.assertTrue(np.allclose(f_ext, f_local, atol=TOLERANCE))
# Create and initialize the engine
engine = jiminy.Engine()
setup_controller_and_engine(
engine, robot, compute_command, internal_dynamics)
# Define and register the external force:
# a wrench in the local frame.
def external_force(t, q, v, f):
nonlocal f_local
# Rotate the wrench to project it to the world frame
R = robot.pinocchio_data.oMi[joint_idx].rotation
f[:3] = R @ f_local[:3]
f[3:] = R @ f_local[3:]
engine.register_force_profile("FirstJoint", external_force)
# Configure the engine
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine_options["stepper"]["solver"] = "runge_kutta_dopri5"
engine_options["stepper"]["sensorsUpdatePeriod"] = 1e-3
engine_options["stepper"]["controllerUpdatePeriod"] = 1e-3
engine_options["stepper"]["tolAbs"] = TOLERANCE * 1e-1
engine_options["stepper"]["tolRel"] = TOLERANCE * 1e-1
engine.set_options(engine_options)
# Run simulation: Check is done directly by control law
engine.simulate(self.tf, q_init, v_init)
