def test_rotate(self): self.assertApprox(rotate('x', pi / 2), np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.')) self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi)) m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7) self.assertApprox(rpyToMatrix(matrixToRpy(m)), m) rpy = np.matrix(list(range(3))).T * pi / 2 self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
def test_rotate(self): self.assertApprox( rotate('x', pi / 2), np.array([[1., 0., 0.], [0., 0., -1.], [0., 1., 0.]])) self.assertApprox( rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi)) m = rotate('x', pi / 3).dot(rotate('y', pi / 5)).dot(rotate('y', pi / 7)) self.assertApprox(rpyToMatrix(matrixToRpy(m)), m) rpy = np.array(list(range(3))) * pi / 2 self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy) self.assertApprox( rpyToMatrix(rpy), rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2]))) try: rotate('toto', 10.) except ValueError: self.assertTrue(True) else: self.assertTrue(False) try: rotate('w', 10.) except ValueError: self.assertTrue(True) else: self.assertTrue(False)
def test_rotate(self): self.assertApprox(rotate('x', pi / 2), np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]])) self.assertApprox(rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi)) m = rotate('x', pi / 3).dot(rotate('y', pi / 5)).dot(rotate('y', pi / 7)) self.assertApprox(rpyToMatrix(matrixToRpy(m)), m) rpy = np.array(list(range(3))) * pi / 2 self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy) self.assertApprox(rpyToMatrix(rpy), rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
def test_matrixToRpy(self): n = 100 for _ in range(n): quat = pin.Quaternion(np.random.rand(4, 1)).normalized() R = quat.toRotationMatrix() v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2) self.assertTrue(-pi <= v[2] and v[2] <= pi) n2 = 100 # Test singular case theta = pi/2 Rp = np.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]) for _ in range(n2): r = random() * 2 * pi - pi y = random() * 2 * pi - pi R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(Rp) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2) self.assertTrue(-pi <= v[2] and v[2] <= pi) # Test singular case theta = -pi/2 Rp = np.array([[0.0, 0.0, -1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]) for _ in range(n2): r = random() * 2 * pi - pi y = random() * 2 * pi - pi R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(Rp) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) v = matrixToRpy(R) Rprime = rpyToMatrix(v) self.assertApprox(Rprime, R) self.assertTrue(-pi <= v[0] and v[0] <= pi) self.assertTrue(-pi / 2 <= v[1] and v[1] <= pi / 2) self.assertTrue(-pi <= v[2] and v[2] <= pi)
def test_computeRpyJacobianTimeDerivative(self): # Check zero at zero velocity r = random() * 2 * pi - pi p = random() * pi - pi / 2 y = random() * 2 * pi - pi rpy = np.array([r, p, y]) rpydot = np.zeros(3) dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot) self.assertTrue((dj0 == np.zeros((3, 3))).all()) djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL) self.assertTrue((djL == np.zeros((3, 3))).all()) djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD) self.assertTrue((djW == np.zeros((3, 3))).all()) djA = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((djA == np.zeros((3, 3))).all()) # Check correct identities between different versions rpydot = np.random.rand(3) dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot) djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL) djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD) djA = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((dj0 == djL).all()) self.assertTrue((djW == djA).all()) R = rpyToMatrix(rpy) jL = computeRpyJacobian(rpy, pin.LOCAL) jW = computeRpyJacobian(rpy, pin.WORLD) omegaL = jL.dot(rpydot) omegaW = jW.dot(rpydot) self.assertApprox(omegaW, R.dot(omegaL)) self.assertApprox(djW, pin.skew(omegaW).dot(R).dot(jL) + R.dot(djL)) self.assertApprox(djW, R.dot(pin.skew(omegaL)).dot(jL) + R.dot(djL))
def setCameraTransform(self, translation=np.zeros(3), rotation=np.zeros(3), relative=False): # translation : [Px, Py, Pz], # rotation : [Roll, Pitch, Yaw] R_pnc = rpyToMatrix(np.array(rotation)) if Viewer.backend == 'gepetto-gui': H_abs = SE3(R_pnc, np.array(translation)) if relative: H_orig = XYZQUATToSe3( self._client.getCameraTransform(self._window_id)) H_abs = H_abs * H_orig self._client.setCameraTransform(self._window_id, se3ToXYZQUAT(H_abs).tolist()) else: if relative: raise RuntimeError( "'relative'=True not available with meshcat.") import meshcat.transformations as tf # Transformation of the camera object T_meshcat = tf.translation_matrix(translation) self._client.viewer[ "/Cameras/default/rotated/<object>"].set_transform(T_meshcat) # Orientation of the camera object Q_pnc = Quaternion(R_pnc).coeffs() Q_meshcat = np.roll(Q_pnc, shift=1) R_meshcat = tf.quaternion_matrix(Q_meshcat) self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def config_sot_to_urdf(q): qUrdf = mat_zeros(39) qUrdf[:3, 0] = q[:3, 0] quatMat = rpyToMatrix(q[3:6, 0]) quatVec = Quaternion(quatMat) qUrdf[3:7, 0] = quatVec.coeffs() qUrdf[7:, 0] = q[6:, 0] return qUrdf
def test_rpyToMatrix(self): r = random() * 2 * pi - pi p = random() * pi - pi / 2 y = random() * 2 * pi - pi R = rpyToMatrix(r, p, y) Raa = AngleAxis(y, np.array([0., 0., 1.])).matrix() \ .dot(AngleAxis(p, np.array([0., 1., 0.])).matrix()) \ .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix()) self.assertApprox(R, Raa) v = np.array([r, p, y]) Rv = rpyToMatrix(v) self.assertApprox(Rv, Raa) self.assertApprox(Rv, R)
def _origin_info_to_se3(origin_info: Optional[ET.Element]) -> pin.SE3: """Convert an XML element with str attribute 'xyz' [X, Y, Z] encoding the translation and 'rpy' encoding [Roll, Pitch, Yaw] into a Pinocchio.SE3 object. """ if origin_info is not None: origin_xyz = _string_to_array(origin_info.attrib['xyz']) origin_rpy = _string_to_array(origin_info.attrib['rpy']) return pin.SE3(rpyToMatrix(origin_rpy), origin_xyz) else: return pin.SE3.Identity()
def test_computeRpyJacobian(self): # Check identity at zero rpy = np.zeros(3) j0 = computeRpyJacobian(rpy) self.assertTrue((j0 == np.eye(3)).all()) jL = computeRpyJacobian(rpy, pin.LOCAL) self.assertTrue((jL == np.eye(3)).all()) jW = computeRpyJacobian(rpy, pin.WORLD) self.assertTrue((jW == np.eye(3)).all()) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((jA == np.eye(3)).all()) # Check correct identities between different versions r = random() * 2 * pi - pi p = random() * pi - pi / 2 y = random() * 2 * pi - pi rpy = np.array([r, p, y]) R = rpyToMatrix(rpy) j0 = computeRpyJacobian(rpy) jL = computeRpyJacobian(rpy, pin.LOCAL) jW = computeRpyJacobian(rpy, pin.WORLD) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((j0 == jL).all()) self.assertTrue((jW == jA).all()) self.assertApprox(jW, R.dot(jL)) # Check against finite differences rpydot = np.random.rand(3) eps = 1e-7 tol = 1e-4 dRdr = (rpyToMatrix(r + eps, p, y) - R) / eps dRdp = (rpyToMatrix(r, p + eps, y) - R) / eps dRdy = (rpyToMatrix(r, p, y + eps) - R) / eps Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2] omegaL = jL.dot(rpydot) self.assertTrue(np.allclose(Rdot, R.dot(pin.skew(omegaL)), atol=tol)) omegaW = jW.dot(rpydot) self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol))
def velocityXYZRPYToXYZQuat(xyzrpy: np.ndarray, dxyzrpy: np.ndarray) -> np.ndarray: """Convert the derivative of [X,Y,Z,Roll,Pitch,Yaw] to [X,Y,Z,Qx,Qy,Qz,Qw]. .. warning:: Linear velocity in XYZRPY must be local-world-aligned frame, while returned linear velocity in XYZQuat is in local frame. """ rpy = xyzrpy[3:] R = rpyToMatrix(rpy) J_rpy = computeRpyJacobian(rpy) return np.concatenate((R.T @ dxyzrpy[:3], J_rpy @ dxyzrpy[3:]))
def config_sot_to_urdf(q): # GEPETTO VIEWER Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36 # ROBOT VIEWER # Free flyer0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35 qUrdf = mat_zeros(37); qUrdf[:3,0] = q[:3,0]; quatMat = rpyToMatrix(q[3:6,0]); quatVec = Quaternion(quatMat); qUrdf[3:7,0] = quatVec.coeffs(); qUrdf[7:11,0] = q[18:22,0]; # chest-head qUrdf[11:18,0] = q[29:,0]; # larm qUrdf[18:25,0] = q[22:29,0]; # rarm qUrdf[25:31,0] = q[12:18,0]; # lleg qUrdf[31:,0] = q[6:12,0]; # rleg return qUrdf;
def setCameraTransform(self, translation, rotation): # translation : [Px, Py, Pz], # rotation : [Roll, Pitch, Yaw] R_pnc = rpyToMatrix(np.array(rotation)) if Viewer.backend == 'gepetto-gui': T_pnc = np.array(translation) T_R = SE3(R_pnc, T_pnc) self._client.setCameraTransform(self._window_id, se3ToXYZQUAT(T_R).tolist()) else: import meshcat.transformations as tf # Transformation of the camera object T_meshcat = tf.translation_matrix(translation) self._client.viewer[ "/Cameras/default/rotated/<object>"].set_transform(T_meshcat) # Orientation of the camera object Q_pnc = Quaternion(R_pnc).coeffs() Q_meshcat = np.roll(Q_pnc, shift=1) R_meshcat = tf.quaternion_matrix(Q_meshcat) self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def initialize(self, urdf_path: str, hardware_path: Optional[str] = None, mesh_path: Optional[str] = None, has_freeflyer: bool = True, avoid_instable_collisions: bool = True, verbose: bool = True) -> None: """Initialize the robot. :param urdf_path: Path of the URDF file of the robot. :param hardware_path: Path of Jiminy hardware description toml file. Optional: Looking for '*_hardware.toml' file in the same folder and with the same name. If not found, then no hardware is added to the robot, which is valid and can be used for display. :param mesh_path: Path to the folder containing the URDF meshes. It will overwrite any absolute mesh path. Optional: Env variable 'JIMINY_DATA_PATH' will be used if available. :param has_freeflyer: Whether the robot is fixed-based wrt its root link, or can move freely in the world. :param avoid_instable_collisions: Prevent numerical instabilities by replacing collision mesh by vertices of associated minimal volume bounding box, and replacing primitive box by its vertices. :param verbose: Whether or not to print warnings. """ # Handle verbosity level if verbose: logger.setLevel(logging.DEBUG) else: logger.setLevel(logging.ERROR) # Backup the original URDF path self.urdf_path_orig = urdf_path # Fix the URDF mesh paths if mesh_path is not None: urdf_path = _fix_urdf_mesh_path(urdf_path, mesh_path) # Initialize the robot without motors nor sensors if mesh_path is not None: mesh_root_dirs = [mesh_path] else: mesh_root_dirs = [os.path.dirname(urdf_path)] mesh_env_path = os.environ.get('JIMINY_DATA_PATH', None) if mesh_env_path is not None: mesh_root_dirs += [mesh_env_path] return_code = super().initialize(urdf_path, has_freeflyer, mesh_root_dirs) if return_code != jiminy.hresult_t.SUCCESS: raise ValueError( "Impossible to load the URDF file. Either the file is " "corrupted or does not exit.") # Load the hardware description file if available if hardware_path is None: hardware_path = str( pathlib.Path( self.urdf_path_orig).with_suffix('')) + '_hardware.toml' self.hardware_path = hardware_path if not os.path.exists(hardware_path): if hardware_path: logger.warning( "Hardware configuration file not found. Not adding any " "hardware to the robot.\n Default file can be generated " "using 'generate_hardware_description_file' method.") return hardware_info = toml.load(hardware_path) self.extra_info = hardware_info.pop('Global', {}) motors_info = hardware_info.pop('Motor', {}) sensors_info = hardware_info.pop('Sensor', {}) # Parse the URDF tree = ET.parse(urdf_path) root = tree.getroot() # Extract the list of bodies having visual and collision meshes or # primitives. geometry_info = { 'visual': { 'primitive': None, 'mesh': None }, 'collision': { 'primitive': None, 'mesh': None } } for geometry_type in geometry_info.keys(): geometry_links = set( link.get('name') for link in root.findall( f"./link/{geometry_type}/geometry/../..")) mesh_links = [ name for name in geometry_links if 'mesh' in set( link[0].tag for link in root.find(f"./link[@name='{name}']").findall( f'{geometry_type}/geometry')) ] primitive_links = [ name for name in geometry_links if set( link[0].tag for link in root.find(f"./link[@name='{name}']").findall( f'{geometry_type}/geometry')).difference({'mesh'}) ] geometry_info[geometry_type]['mesh'] = mesh_links geometry_info[geometry_type]['primitive'] = primitive_links # Checking the collision bodies, to make sure they are associated with # supported collision geometries. If not, fixing the issue after # throwing a warning. collision_bodies_names = self.extra_info.pop('collisionBodiesNames', []) contact_frames_names = self.extra_info.pop('contactFramesNames', []) for body_name in collision_bodies_names.copy(): # Filter out the different cases. # After this filter, we know that their is no collision geometry # associated with the body but their is a visual mesh, or there is # only collision meshes. if body_name in geometry_info['collision']['mesh'] and \ body_name in geometry_info['collision']['primitive']: if not avoid_instable_collisions: continue logger.warning( "Collision body having both primitive and mesh geometries " "is not supported. Enabling only primitive collision for " "this body.") continue elif body_name in geometry_info['collision']['primitive']: pass elif body_name in geometry_info['collision']['mesh']: if not avoid_instable_collisions: continue logger.warning( "Collision body associated with mesh geometry is not " "supported for now. Replacing it by contact points at the " "vertices of the minimal volume bounding box.") elif body_name not in geometry_info['visual']['mesh']: logger.warning( "No visual mesh nor collision geometry associated with " "the collision body. Fallback to adding a single contact " "point at body frame.") contact_frames_names.append(body_name) continue else: logger.warning( "No collision geometry associated with the collision " "body. Fallback to replacing it by contact points at " "the vertices of the minimal volume bounding box of the " "available visual meshes.") # Check if collision primitive box are available body_link = root.find(f"./link[@name='{body_name}']") collision_box_sizes_info, collision_box_origin_info = [], [] for link in body_link.findall('collision'): box_link = link.find('geometry/box') if box_link is not None: collision_box_sizes_info.append(box_link.get('size')) collision_box_origin_info.append(link.find('origin')) # Replace the collision boxes by contact points, if any if collision_box_sizes_info: if not avoid_instable_collisions: continue logger.warning( "Collision body associated with box geometry is not " "numerically stable for now. Replacing it by contact " "points at the vertices.") for i, (box_size_info, box_origin_info) in enumerate( zip(collision_box_sizes_info, collision_box_origin_info)): box_size = list(map(float, box_size_info.split())) box_origin = _origin_info_to_se3(box_origin_info) vertices = [ e.flatten() for e in np.meshgrid(*[ 0.5 * v * np.array([-1.0, 1.0]) for v in box_size ]) ] for j, (x, y, z) in enumerate(zip(*vertices)): frame_name = "_".join( (body_name, "CollisionBox", str(i), str(j))) vertex_pos_rel = pin.SE3(np.eye(3), np.array([x, y, z])) frame_transform = box_origin.act(vertex_pos_rel) self.add_frame(frame_name, body_name, frame_transform) contact_frames_names.append(frame_name) elif body_name in geometry_info['collision']['primitive']: # Do nothing if the primitive is not a box. It should be fine. continue # Remove the body from the collision detection set collision_bodies_names.remove(body_name) # Early return if collision box primitives have been replaced if collision_box_sizes_info: continue # Extract meshes paths and origins. if body_name in geometry_info['collision']['mesh']: mesh_links = body_link.findall('collision') else: mesh_links = body_link.findall('visual') mesh_paths = [ link.find('geometry/mesh').get('filename') for link in mesh_links ] mesh_scales = [ _string_to_array( link.find('geometry/mesh').get('scale', '1.0 1.0 1.0')) for link in mesh_links ] mesh_origins = [] for link in mesh_links: mesh_origin_info = link.find('origin') mesh_origin_transform = \ _origin_info_to_se3(mesh_origin_info) mesh_origins.append(mesh_origin_transform) # Replace the collision body by contact points for mesh_path, mesh_scale, mesh_origin in zip( mesh_paths, mesh_scales, mesh_origins): # Replace relative mesh path by absolute one if mesh_path.startswith("package://"): mesh_path_orig = mesh_path for root_dir in mesh_root_dirs: mesh_path = mesh_path_orig.replace( "package:/", root_dir) if os.path.exists(mesh_path): break # Compute the minimal volume bounding box, then add new frames # to the robot model at its vertices and register contact # points at their location. try: mesh = trimesh.load(mesh_path) except ValueError: # Mesh file is not available continue box = mesh.bounding_box_oriented for i in range(8): frame_name = "_".join((body_name, "BoundingBox", str(i))) frame_transform_rel = pin.SE3( np.eye(3), mesh_scale * np.asarray(box.vertices[i])) frame_transform = mesh_origin.act(frame_transform_rel) self.add_frame(frame_name, body_name, frame_transform) contact_frames_names.append(frame_name) # Add the collision bodies and contact points. # Note that it must be done before adding the sensors because # Contact sensors requires contact points to be defined. # Mesh collisions is not numerically stable for now, so disabling it. self.add_collision_bodies(collision_bodies_names, ignore_meshes=avoid_instable_collisions) self.add_contact_points(list(set(contact_frames_names))) # Add the motors to the robot for motor_type, motors_descr in motors_info.items(): for motor_name, motor_descr in motors_descr.items(): # Create the sensor and attach it motor = getattr(jiminy, motor_type)(motor_name) self.attach_motor(motor) # Initialize the motor joint_name = motor_descr.pop('joint_name') motor.initialize(joint_name) # Set the motor options options = motor.get_options() option_fields = options.keys() for name, value in motor_descr.items(): if name not in option_fields: logger.warning( f"'{name}' is not a valid option for the motor " f"{motor_name} of type {motor_type}.") options[name] = value options['enableArmature'] = True motor.set_options(options) # Add the sensors to the robot for sensor_type, sensors_descr in sensors_info.items(): for sensor_name, sensor_descr in sensors_descr.items(): # Create the sensor and attach it sensor = getattr(jiminy, sensor_type)(sensor_name) self.attach_sensor(sensor) # Initialize the sensor if sensor_type == encoder.type: joint_name = sensor_descr.pop('joint_name') sensor.initialize(joint_name) elif sensor_type == effort.type: motor_name = sensor_descr.pop('motor_name') sensor.initialize(motor_name) elif sensor_type == contact.type: frame_name = sensor_descr.pop('frame_name') sensor.initialize(frame_name) elif sensor_type in [force.type, imu.type]: # Create the frame and add it to the robot model frame_name = sensor_descr.pop('frame_name', None) # Create a frame if a frame name has been specified. # In such a case, the body name must be specified. if frame_name is None: # Get the body name body_name = sensor_descr.pop('body_name') # Generate a frame name both intelligible and available i = 0 frame_name = "_".join( (sensor_name, sensor_type, "Frame")) while self.pinocchio_model.existFrame(frame_name): frame_name = "_".join( (sensor_name, sensor_type, "Frame", str(i))) i += 1 # Compute SE3 object representing the frame placement frame_pose_xyzrpy = np.array( sensor_descr.pop('frame_pose')) frame_trans = frame_pose_xyzrpy[:3] frame_rot = rpyToMatrix(frame_pose_xyzrpy[3:]) frame_placement = pin.SE3(frame_rot, frame_trans) # Add the frame to the robot model self.add_frame(frame_name, body_name, frame_placement) # Initialize the sensor sensor.initialize(frame_name) else: raise ValueError(f"Unsupported sensor type {sensor_type}.") # Set the sensor options options = sensor.get_options() option_fields = options.keys() for name, value in sensor_descr.items(): if name not in option_fields: logger.warning( f"'{name}' is not a valid option for the sensor " f"{sensor_name} of type {sensor_type}.") options[name] = value sensor.set_options(options)
def XYZRPYToSe3(xyzrpy): return SE3(rpyToMatrix(xyzrpy[3:]), xyzrpy[:3])
def XYZRPYToSE3(xyzrpy): """Convert [X,Y,Z,Roll,Pitch,Yaw] vector to Pinocchio SE3 object. """ return pin.SE3(rpyToMatrix(xyzrpy[3:]), xyzrpy[:3])
def test_imu_sensor(self): """ @brief Test IMU sensor on pendulum motion. @details Note that the actual expected solution of the pendulum motion is used to compute the expected IMU data, instead of the result of the simulation done by jiminy itself. So this test is checking at the same time that the result of the simulation matches the solution, and that the sensor IMU data are valid. Though it is redundant, it validates that an IMU mounted on a pendulum gives the signal one would expect from an IMU on a pendulum, which is what a user would expect. Moreover, Jiminy output log does not feature the acceleration - to this test is indirectly checking that the acceleration computed by jiminy is valid. @remark Since we don't have a simple analytical expression for the solution of a (nonlinear) pendulum motion, we perform the simulation in python, with the same integrator. """ # Create an engine: no controller and no internal dynamics engine = jiminy.Engine() setup_controller_and_engine(engine, self.robot) # Run simulation and extract log data x0 = np.array([0.1, 0.1]) tf = 2.0 time, quat_jiminy, gyro_jiminy, accel_jiminy = \ SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=True) # Pendulum dynamics def dynamics(t, x): return np.stack([x[..., 1], self.g / self.l * np.sin(x[..., 0])], axis=-1) # Integrate this non-linear dynamics x_rk_python = integrate_dynamics(time, x0, dynamics) # Compute sensor acceleration, i.e. acceleration in polar coordinates theta = x_rk_python[:, 0] dtheta = x_rk_python[:, 1] dtheta = x_rk_python[:, 1] # Acceleration: to resolve algebraic loop (current acceleration is # function of input which itself is function of sensor signal, sensor # data is computed using q_t, v_t, a_t ddtheta = dynamics(0.0, x_rk_python)[:, 1] expected_accel = np.stack([ -self.l * ddtheta + self.g * np.sin(theta), np.zeros_like(theta), self.l * dtheta**2 - self.g * np.cos(theta) ], axis=-1) expected_gyro = np.stack( [np.zeros_like(theta), dtheta, np.zeros_like(theta)], axis=-1) expected_quat = np.stack([ Quaternion(rpyToMatrix(np.array([0., t, 0.]))).coeffs() for t in theta ], axis=0) # Compare sensor signal, ignoring first iterations that correspond to # system initialization self.assertTrue( np.allclose(expected_quat[2:, :], quat_jiminy[2:, :], atol=TOLERANCE)) self.assertTrue( np.allclose(expected_gyro[2:, :], gyro_jiminy[2:, :], atol=TOLERANCE)) self.assertTrue( np.allclose(expected_accel[2:, :], accel_jiminy[2:, :], atol=TOLERANCE))
def test_imu_sensor(self): """ @brief Test IMU sensor on pendulum motion. @details Note that the actual expected solution of the pendulum motion is used to compute the expected IMU data, instead of the result of the simulation done by jiminy itself. So this test is checking at the same time that the result of the simulation matches the solution, and that the sensor IMU data are valid. Though it is redundant, it validates that an IMU mounted on a pendulum gives the signal one would expect from an IMU on a pendulum, which is what a user would expect. Moreover, Jiminy output log does not feature the acceleration - to this test is indirectly checking that the acceleration computed by jiminy is valid. @remark Since we don't have a simple analytical expression for the solution of a (nonlinear) pendulum motion, we perform the simulation in python, with the same integrator. """ # Add IMU imu_sensor = jiminy.ImuSensor("PendulumLink") self.robot.attach_sensor(imu_sensor) imu_sensor.initialize("PendulumLink") # Create an engine: no controller and no internal dynamics engine = jiminy.Engine() engine.initialize(self.robot) x0 = np.array([0.1, 0.1]) tf = 2.0 # Run simulation engine.simulate(tf, x0) log_data, _ = engine.get_log() time = log_data['Global.Time'] quat_jiminy = np.stack( [log_data['PendulumLink.Quat' + s] for s in ['x', 'y', 'z', 'w']], axis=-1) gyro_jiminy = np.stack( [log_data['PendulumLink.Gyro' + s] for s in ['x', 'y', 'z']], axis=-1) accel_jiminy = np.stack( [log_data['PendulumLink.Accel' + s] for s in ['x', 'y', 'z']], axis=-1) # System dynamics: get length and inertia l = -self.robot.pinocchio_model_th.inertias[1].lever[2] g = self.robot.pinocchio_model.gravity.linear[2] # Pendulum dynamics def dynamics(t, x): return np.stack([x[..., 1], g / l * np.sin(x[..., 0])], axis=-1) # Integrate this non-linear dynamics x_rk_python = integrate_dynamics(time, x0, dynamics) # Compute sensor acceleration, i.e. acceleration in polar coordinates theta = x_rk_python[:, 0] dtheta = x_rk_python[:, 1] # Acceleration: to resolve algebraic loop (current acceleration is # function of input which itself is function of sensor signal, sensor # data is computed using q_t, v_t, a_(t-1) ddtheta = np.concatenate((np.zeros(1), dynamics(0.0, x_rk_python)[:-1, 1])) expected_accel = np.stack([ -l * ddtheta + g * np.sin(theta), np.zeros_like(theta), l * dtheta**2 - g * np.cos(theta) ], axis=-1) expected_gyro = np.stack( [np.zeros_like(theta), dtheta, np.zeros_like(theta)], axis=-1) expected_quat = np.stack([ Quaternion(rpyToMatrix(np.array([0., t, 0.]))).coeffs() for t in theta ], axis=0) # Compare sensor signal, ignoring first iterations that correspond to system initialization self.assertTrue( np.allclose(expected_quat[2:, :], quat_jiminy[2:, :], atol=TOLERANCE)) self.assertTrue( np.allclose(expected_gyro[2:, :], gyro_jiminy[2:, :], atol=TOLERANCE)) self.assertTrue( np.allclose(expected_accel[2:, :], accel_jiminy[2:, :], atol=TOLERANCE))
def get_transform(origin): from pinocchio import SE3, rpy _xyz = [float(v) for v in origin.attrib.get('xyz', '0 0 0').split(' ')] _rpy = [float(v) for v in origin.attrib.get('rpy', '0 0 0').split(' ')] return SE3(rpy.rpyToMatrix(*_rpy), np.array(_xyz))