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)