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)