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))