ImuSimulator< T > Class Template Reference

#include <ImuSimulator.h>

Inheritance diagram for ImuSimulator< T >:

Collaboration diagram for ImuSimulator< T >:

Public Member Functions
	ImuSimulator (SimulatorControlParameters &simSettings, u64 seed=0)
void	updateVectornav (const FBModelState< T > &robotState, const FBModelStateDerivative< T > &robotStateD, VectorNavData *data)
void	computeAcceleration (const FBModelState< T > &robotState, const FBModelStateDerivative< T > &robotStateD, Vec3< float > &acc, std::uniform_real_distribution< float > &dist, const RotMat< float > &R_body)
void	updateCheaterState (const FBModelState< T > &robotState, const FBModelStateDerivative< T > &robotStateD, CheaterState< T > &state)

Private Attributes
SimulatorControlParameters &	_simSettings
std::mt19937	_mt
std::uniform_real_distribution< float >	_vectornavGyroDistribution
std::uniform_real_distribution< float >	_vectornavAccelerometerDistribution
std::uniform_real_distribution< float >	_vectornavQuatDistribution
bool	_vectorNavOrientationNoise = false

Detailed Description

template<typename T>
class ImuSimulator< T >

Definition at line 16 of file ImuSimulator.h.

Constructor & Destructor Documentation

template<typename T>

ImuSimulator< T >::ImuSimulator ( SimulatorControlParameters &  simSettings, u64  seed = 0  )

inlineexplicit

Definition at line 18 of file ImuSimulator.h.

      : _simSettings(simSettings),
        _mt(seed),
        _vectornavGyroDistribution(-simSettings.vectornav_imu_gyro_noise,
                                   simSettings.vectornav_imu_gyro_noise),
        _vectornavAccelerometerDistribution(
            -simSettings.vectornav_imu_accelerometer_noise,
            simSettings.vectornav_imu_accelerometer_noise),
        _vectornavQuatDistribution(-simSettings.vectornav_imu_quat_noise,
                                   simSettings.vectornav_imu_quat_noise) {
    if (simSettings.vectornav_imu_quat_noise != 0) {
      _vectorNavOrientationNoise = true;
    }
  }

Member Function Documentation

template<typename T>

void ImuSimulator< T >::computeAcceleration	(	const FBModelState< T > &	robotState,
		const FBModelStateDerivative< T > &	robotStateD,
		Vec3< float > &	acc,
		std::uniform_real_distribution< float > &	dist,
		const RotMat< float > &	R_body
	)

Definition at line 10 of file ImuSimulator.cpp.

References FBModelState< T >::bodyVelocity, FBModelStateDerivative< T >::dBodyVelocity, fillEigenWithRandom(), and spatial::spatialToLinearAcceleration().

                                                                            {
  // accelerometer noise
  fillEigenWithRandom(acc, _mt, dist);

  // gravity (should be positive when robot is upright)
  acc += (R_body * Vec3<float>(0, 0, 9.81));

  // acceleration
  acc += spatial::spatialToLinearAcceleration(robotStateD.dBodyVelocity,
                                              robotState.bodyVelocity)
             .template cast<float>();
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

void ImuSimulator< T >::updateCheaterState	(	const FBModelState< T > &	robotState,
		const FBModelStateDerivative< T > &	robotStateD,
		CheaterState< T > &	state
	)

Definition at line 74 of file ImuSimulator.cpp.

References CheaterState< T >::acceleration, FBModelState< T >::bodyOrientation, FBModelState< T >::bodyPosition, FBModelState< T >::bodyVelocity, FBModelStateDerivative< T >::dBodyVelocity, CheaterState< T >::omegaBody, CheaterState< T >::orientation, CheaterState< T >::position, ori::quaternionToRotationMatrix(), spatial::spatialToLinearAcceleration(), and CheaterState< T >::vBody.

                                                                          {
  RotMat<T> R_body = quaternionToRotationMatrix(robotState.bodyOrientation);
  state.acceleration = (R_body * Vec3<T>(0, 0, 9.81)) +
                       spatial::spatialToLinearAcceleration(
                           robotStateD.dBodyVelocity, robotState.bodyVelocity);
  state.orientation = robotState.bodyOrientation;
  state.position = robotState.bodyPosition;
  state.omegaBody = robotState.bodyVelocity.template head<3>();
  state.vBody = robotState.bodyVelocity.template tail<3>();
}

Here is the call graph for this function:

Here is the caller graph for this function:

template<typename T>

void ImuSimulator< T >::updateVectornav	(	const FBModelState< T > &	robotState,
		const FBModelStateDerivative< T > &	robotStateD,
		VectorNavData *	data
	)

Definition at line 28 of file ImuSimulator.cpp.

References VectorNavData::accelerometer, FBModelState< T >::bodyOrientation, FBModelState< T >::bodyVelocity, f(), fillEigenWithRandom(), VectorNavData::gyro, ori::integrateQuat(), VectorNavData::quat, and ori::quaternionToRotationMatrix().

                                                                       {
  // body orientation
  RotMat<float> R_body = quaternionToRotationMatrix(
      robotState.bodyOrientation.template cast<float>());

  Quat<float> ori_quat;
  // printf("imu ori: %f, %f, %f, %f\n",
  // robotState.bodyOrientation[0],
  // robotState.bodyOrientation[1],
  // robotState.bodyOrientation[2],
  // robotState.bodyOrientation[3] );

  // acceleration
  computeAcceleration(robotState, robotStateD, data->accelerometer,
                      _vectornavAccelerometerDistribution, R_body);

  // gyro
  fillEigenWithRandom(data->gyro, _mt, _vectornavGyroDistribution);
  data->gyro +=
      robotState.bodyVelocity.template head<3>().template cast<float>();

  // quaternion
  if (_vectorNavOrientationNoise) {
    Vec3<float> omegaNoise;
    fillEigenWithRandom(omegaNoise, _mt, _vectornavQuatDistribution);
    Quat<float> floatQuat = robotState.bodyOrientation.template cast<float>();
    // data->quat = integrateQuat(floatQuat, omegaNoise, 1.0f);
    ori_quat = integrateQuat(floatQuat, omegaNoise, 1.0f);
    // printf("imu noised ori: %f, %f, %f, %f\n",
    // robotState.bodyOrientation[0],
    // robotState.bodyOrientation[1],
    // robotState.bodyOrientation[2],
    // robotState.bodyOrientation[3] );
  } else {
    // data->quat = robotState.bodyOrientation.template cast<float>();
    ori_quat = robotState.bodyOrientation.template cast<float>();
  }
  data->quat[3] = ori_quat[0];
  data->quat[0] = ori_quat[1];
  data->quat[1] = ori_quat[2];
  data->quat[2] = ori_quat[3];
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

template<typename T>

std::mt19937 ImuSimulator< T >::_mt

private

Definition at line 50 of file ImuSimulator.h.

template<typename T>

SimulatorControlParameters& ImuSimulator< T >::_simSettings

private

Definition at line 49 of file ImuSimulator.h.

template<typename T>

std::uniform_real_distribution<float> ImuSimulator< T >::_vectornavAccelerometerDistribution

private

Definition at line 52 of file ImuSimulator.h.

template<typename T>

std::uniform_real_distribution<float> ImuSimulator< T >::_vectornavGyroDistribution

private

Definition at line 51 of file ImuSimulator.h.

template<typename T>

bool ImuSimulator< T >::_vectorNavOrientationNoise = false

private

Definition at line 54 of file ImuSimulator.h.

template<typename T>

std::uniform_real_distribution<float> ImuSimulator< T >::_vectornavQuatDistribution

private

Definition at line 53 of file ImuSimulator.h.

---

The documentation for this class was generated from the following files:

• ImuSimulator.h
• ImuSimulator.cpp