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 set_transform(origin, a, b): from pinocchio import rpy, Quaternion length = np.linalg.norm(b - a) z = (b - a) / length R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix() origin.attrib['xyz'] = " ".join([str(v) for v in ((a + b) / 2)]) origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R)])
def velocityXYZQuatToXYZRPY(xyzquat: np.ndarray, v: np.ndarray) -> np.ndarray: """Convert the derivative of [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw]. .. note: No need to estimate yaw angle to get RPY velocity, since `computeRpyJacobianInverse` only depends on Roll and Pitch angles. However, it is not the case for the linear velocity. .. warning:: Linear velocity in XYZRPY must be local-world-aligned frame, while returned linear velocity in XYZQuat is in local frame. """ quat = pin.Quaternion(xyzquat[3:]) rpy = matrixToRpy(quat.matrix()) J_rpy_inv = computeRpyJacobianInverse(rpy) return np.concatenate((quat * v[:3], J_rpy_inv @ v[3:]))
def moveArrow(self, name, pos1, pos2): if (ENABLE_VIEWER): length = norm(pos1 - pos2) self.robot.viewer.gui.resizeArrow('world/' + name, self.arrow_radius[name], length) # compute rotation matrix R = np.matlib.identity(3) a = pos2 - pos1 if (norm(a) != 0.0): a /= norm(a) R[:, 0] = a R[:, 1] = cross(a, np.matrix([1., 0., 0.]).T) if (norm(R[:, 1]) == 0.0): R[:, 1] = cross(a, np.matrix([0., 1., 0.]).T) R[:, 1] /= norm(R[:, 1]) R[:, 2] = cross(a, R[:, 1]) R[:, 2] /= norm(R[:, 2]) rpy = matrixToRpy(R) self.updateObjectConfigRpy(name, pos1, rpy)
def se3ToXYZRPY(M): p = np.zeros((6, )) p[:3] = M.translation p[3:] = matrixToRpy(M.rotation) return p
def XYZQuatToXYZRPY(xyzquat): """Convert [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw]. """ return np.concatenate(( xyzquat[:3], matrixToRpy(pin.Quaternion(xyzquat[3:]).matrix())))
def SE3ToXYZRPY(M): """Convert Pinocchio SE3 object to [X,Y,Z,Roll,Pitch,Yaw] vector. """ return np.concatenate((M.translation, matrixToRpy(M.rotation)))
def homoToXYZRPY(H): r = np.matrix([[H[0], H[1], H[2]], [H[4], H[5], H[6]], [H[8], H[9], H[10]]]) t = np.matrix([[H[3]], [H[7]], [H[11]]]) return np.vstack([t, matrixToRpy(r)]).A1
def se3ToXYZRPY(M): rpy = np.squeeze(np.asarray(matrixToRpy(M.rotation))).tolist() xyz = np.squeeze(np.asarray(M.translation)).tolist() return np.array(xyz + rpy)
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_flexibility_armature(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_flexibilty_model.pdf' for more information as to why this is not a true equality. """ # 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"] = [{ 'frameName': "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"]['enableArmature'] = True motor_options["PendulumJoint"]['armature'] = J self.robot.set_motors_options(motor_options) # Create an engine: PD controller on motor and no internal dynamics k_control, nu_control = 100.0, 1.0 def ControllerPD(t, q, v, sensors_data, command): command[:] = -k_control * q[4] - nu_control * v[3] engine = jiminy.Engine() setup_controller_and_engine(engine, self.robot, compute_command=ControllerPD) # 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. # Note that 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 time, x_jiminy = simulate_and_get_state_evolution(engine, tf, x0, split=False) # 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]] # Simulate the system: a perfect SEA system. A = np.array([[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [ -k * (1 / self.I + 1 / J), k_control / J, -nu * (1 / self.I + 1 / J), nu_control / J ], [k / J, -k_control / J, nu / J, -nu_control / J]]) x_analytical = np.stack( [scipy.linalg.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. self.assertTrue(np.allclose(x_jiminy_extract, x_analytical, atol=1e-4))