def se3Interp(s1, s2, alpha): t = (s1.translation * alpha + s2.translation * (1 - alpha)) r = se3.exp3( se3.log3(s1.rotation) * alpha + se3.log3(s2.rotation) * (1 - alpha)) return se3.SE3(r, t)
def test_exp3(self): v = zero(3) m = pin.exp3(v) self.assertApprox(m, eye(3))
def test_exp3(self): v = zero(3) m = pin.exp3(v) self.assertApprox(m, eye(3))
def compute_current_delta_IMU(b_ab, b_wb, dt): return [0.5 * b_ab * dt**2, b_ab * dt, pin.exp3(b_wb * dt)]
def test_sensor_noise_bias(self): """ @brief Test sensor noise and bias for an IMU sensor on a simple pendulum in static pose. """ # 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.0, 0.0]) tf = 200.0 # Configure the engine: No gravity engine_options = engine.get_options() engine_options["world"]["gravity"] = np.zeros(6) engine.set_options(engine_options) # Configure the IMU imu_options = imu_sensor.get_options() imu_options['noiseStd'] = np.linspace(0.0, 0.2, 9) imu_options['bias'] = np.linspace(0.0, 1.0, 9) imu_sensor.set_options(imu_options) # Run simulation engine.simulate(tf, x0) log_data, _ = engine.get_log() 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) # Convert quaternion to a rotation vector. quat_axis = np.stack( [log3(Quaternion(q[:, np.newaxis]).matrix()) for q in quat_jiminy], axis=0) # Estimate the quaternion noise and bias # Because the IMU rotation is identity, the resulting rotation will # simply be R_b R_noise. Since R_noise is a small rotation, we can # consider that the resulting rotation is simply the rotation resulting # from the sum of the rotation vector (this is only true at the first # order) and thus directly recover the unbiased sensor data. quat_axis_bias = np.mean(quat_axis, axis=0) quat_axis_std = np.std(quat_axis, axis=0) # Remove sensor rotation bias from gyro / accel data quat_rot_bias = exp3(quat_axis_bias) gyro_jiminy = np.vstack([quat_rot_bias @ v for v in gyro_jiminy]) accel_jiminy = np.vstack([quat_rot_bias @ v for v in accel_jiminy]) # Estimate the gyroscope and accelerometer noise and bias gyro_std = np.std(gyro_jiminy, axis=0) gyro_bias = np.mean(gyro_jiminy, axis=0) accel_std = np.std(accel_jiminy, axis=0) accel_bias = np.mean(accel_jiminy, axis=0) # Compare estimated sensor noise and bias with the configuration self.assertTrue( np.allclose(imu_options['noiseStd'][:3], quat_axis_std, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['bias'][:3], quat_axis_bias, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['noiseStd'][3:-3], gyro_std, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['bias'][3:-3], gyro_bias, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['noiseStd'][-3:], accel_std, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['bias'][-3:], accel_bias, atol=1.0e-2))
def test_sensor_noise_bias(self): """ @brief Test sensor noise and bias for an IMU sensor on a simple pendulum in static pose. """ # Create an engine: no controller and no internal dynamics engine = jiminy.Engine() setup_controller_and_engine(engine, self.robot) # Configure the engine: No gravity engine_options = engine.get_options() engine_options["world"]["gravity"] = np.zeros(6) engine.set_options(engine_options) # Configure the IMU imu_options = self.imu_sensor.get_options() imu_options['noiseStd'] = np.linspace(0.0, 0.2, 9) imu_options['bias'] = np.linspace(0.0, 1.0, 9) self.imu_sensor.set_options(imu_options) # Run simulation and extract log data x0 = np.array([0.0, 0.0]) tf = 200.0 _, quat_jiminy, gyro_jiminy, accel_jiminy = \ SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=True) # Convert quaternion to a rotation vector. quat_axis = np.stack( [log3(Quaternion(q[:, np.newaxis]).matrix()) for q in quat_jiminy], axis=0) # Estimate the quaternion noise and bias # Because the IMU rotation is identity, the resulting rotation will # simply be R_b R_noise. Since R_noise is a small rotation, we can # consider that the resulting rotation is simply the rotation resulting # from the sum of the rotation vector (this is only true at the first # order) and thus directly recover the unbiased sensor data. quat_axis_bias = np.mean(quat_axis, axis=0) quat_axis_std = np.std(quat_axis, axis=0) # Remove sensor rotation bias from gyro / accel data quat_rot_bias = exp3(quat_axis_bias) gyro_jiminy = np.vstack([quat_rot_bias @ v for v in gyro_jiminy]) accel_jiminy = np.vstack([quat_rot_bias @ v for v in accel_jiminy]) # Estimate the gyroscope and accelerometer noise and bias gyro_std = np.std(gyro_jiminy, axis=0) gyro_bias = np.mean(gyro_jiminy, axis=0) accel_std = np.std(accel_jiminy, axis=0) accel_bias = np.mean(accel_jiminy, axis=0) # Compare estimated sensor noise and bias with the configuration self.assertTrue( np.allclose(imu_options['noiseStd'][:3], quat_axis_std, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['bias'][:3], quat_axis_bias, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['noiseStd'][3:-3], gyro_std, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['bias'][3:-3], gyro_bias, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['noiseStd'][-3:], accel_std, atol=1.0e-2)) self.assertTrue( np.allclose(imu_options['bias'][-3:], accel_bias, atol=1.0e-2))