Hello, I faced the same problem. The reason is that the acceleration
calculation only takes into account the offset position (
m_offsetPose.GetPos()) without the orientation (m_offsetPose.GetRot()) (
chrono_sensor/sensors/ChIMUSensor.cpp
<https://github.com/projectchrono/chrono/blob/30cd3f2702cb58182e46d5b2724d2d2850a50e21/src/chrono_sensor/sensors/ChIMUSensor.cpp#L34-L39>
):
CH_SENSOR_API void ChAccelerometerSensor::PushKeyFrame() {
ChVector<double> tran_acc_no_offset =
m_parent->PointAccelerationLocalToParent(m_offsetPose.GetPos());
ChVector<double> tran_acc_offset = -m_parent->GetSystem()->Get_G_acc();
tran_acc_offset = m_parent->GetRot().Rotate(tran_acc_offset);
m_keyframes.push_back(tran_acc_no_offset + tran_acc_offset);
}
I made my own implementation of the accelerometer. I'm attaching it.
Maybe when I have time I'll make a pull request with these edits to the
main repository.
четверг, 19 декабря 2024 г. в 23:08:29 UTC+7, Yan Dong:
> Hi, I'm using Chrono 8.0.0, C++.
>
> I want to output accelerometer attached on a ChBody, but the acc output
> seems to be the acceleration on world frame not on the box frame. Also, I
> have find the acc output of gravity is strange.
>
> Here're my findings:
> 1. About the gravity: I set the gravity to be (0,0,-10), and I release the
> acc attached on a box to fall freely. The acc output is zero which is
> desired because no external force on box.
> Now I rotate the box, making the z-axis pointing downward, by:
>
> box->SetRot(Q_from_AngAxis(chrono::CH_C_PI, { 1, 0, 0 }));
>
> then the IMU output is now (0,0,-20). Which I think should be 0 becase the
> free falling has no external force. The real imu is 0 indeed.
>
>
> 2. About the extrinsics: I change the IMU attached extrinsics to the body,
> and I find the IMU output do not change at all no matter the what the
> extrinsics is:"
>
> auto ext_zero = chrono::ChFrame<double>({ 0, 0, 0 },
> Q_from_AngAxis(chrono::CH_C_PI, { 1, 0, 0 }));
> auto acc_ = chrono_types::make_shared<ChAccelerometerSensor>(box, 100,
> ext_zero, nullptr);
>
> 3. then I further find that the acc output seems to be the acceleration of
> the body in terms of the world frame but not its own frame. I add the an
> external world's X direction force and I find no matter the box is rotated
> (to negtive x direction) or the extrinsics is rotated, the Accelerometer
> output is always on x+ direction.
>
>
> So my questions are:
> a). How you implement Accelerometer calculation? What's the Accelerometer
> output's reference frame?
> b). How is the gravity effect on Accelerometer calculated? I guess is by
> relative moving frame together with the global gravity settings? But it's
> not true for a real Accelerometer.
>
>
> Here's my code. The 3 above findings are tested by codes with comments:
>
> ---------------------------------------------------
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
> *ChSystemNSC sys; chrono::sensor::ChSensorManager manager_(&sys);
> sys.SetCollisionSystemType(ChCollisionSystem::Type::BULLET);
> sys.Set_G_acc({ 0, 0, -10 }); sys.SetSolverMaxIterations(20); auto
> ground_mat = chrono_types::make_shared<ChMaterialSurfaceNSC>(); double
> friction_mu = 1.0; ground_mat->SetSfriction(friction_mu);
> ground_mat->SetKfriction(friction_mu); auto box =
> chrono_types::make_shared<ChBodyEasyBox>(1, 1, 1, 1.0, true, true,
> ground_mat); box->SetPos(ChVector<>(0, 0, 0.5)); // 1. Gravity Issue
> Rotate or Not. // box->SetRot(Q_from_AngAxis(chrono::CH_C_PI / 1, { 1,
> 0, 0 })); box->SetMass(1); box->SetInertiaXX(ChVector<>(1, 1, 1));
> box->SetBodyFixed(false); box->SetCollide(true); //
> sys.AddBody(box); // 2. Extrinsics Issue auto ext_zero =
> chrono::ChFrame<double>({ 0, 0, 0 }, Q_from_AngAxis(0, { 1, 0, 0 }));
> auto acc_ = chrono_types::make_shared<ChAccelerometerSensor>(box, 100,
> ext_zero, nullptr); acc_->SetName("IMU - Accelerometer");
> acc_->SetLag(0.0f); acc_->SetCollectionWindow(0.01);
> acc_->PushFilter(chrono_types::make_shared<ChFilterAccelAccess>());
> manager_.AddSensor(acc_); int imu_last_count_ = 0; // 3.
> Accelerometer direction issue by external force //
> std::shared_ptr<ChBody> ground; // create_ground(sys, ground);
> // I created a ground. // auto force1 =
> chrono_types::make_shared<ChForce>(); // box->AddForce(force1); //
> force1->SetMode(ChForce::FORCE); //
> force1->SetAlign(ChForce::WORLD_DIR); //
> force1->SetF_x(chrono_types::make_shared<ChFunction_Const>(20)); //
> force1->SetF_y(chrono_types::make_shared<ChFunction_Const>(0)); //
> force1->SetF_z(chrono_types::make_shared<ChFunction_Const>(0));
> while(1){ auto bufferAcc =
> acc_->GetMostRecentBuffer<UserAccelBufferPtr>(); if
> (bufferAcc->Buffer && bufferAcc->LaunchedCount > imu_last_count_) {
> AccelData acc_data = bufferAcc->Buffer[0];
> std::vector<double> acc(3); acc[0] = acc_data.X;
> acc[1] = acc_data.Y; acc[2] = acc_data.Z; cout <<
> acc[0] << ", " << acc[1] << ", " << acc[2] << endl;
> imu_last_count_ = bufferAcc->LaunchedCount; }
> manager_.Update(); sys.DoStepDynamics(0.001); }*
> ---------------------------------------------------
--
You received this message because you are subscribed to the Google Groups
"ProjectChrono" group.
To unsubscribe from this group and stop receiving emails from it, send an email
to [email protected].
To view this discussion visit
https://groups.google.com/d/msgid/projectchrono/8aaa69a5-e48a-4742-8335-74981fdfd895n%40googlegroups.com.
#ifndef ACCELEROMETERSENSOR_HPP
#define ACCELEROMETERSENSOR_HPP
#include <vector>
#include "chrono/core/ChVector3.h"
#include "chrono_sensor/sensors/ChSensor.h"
#include "chrono_sensor/filters/ChFilter.h"
#include "chrono_sensor/sensors/ChNoiseModel.h"
using namespace chrono;
using namespace chrono::sensor;
class AccelerometerSensor : public ChDynamicSensor {
public:
/// Class constructor for an accelerometer sensor
/// @param parent Body to which the sensor is attached.
/// @param updateRate Rate at which the sensor should update.
/// @param offsetPose Relative position and orientation of the sensor with
respect to its parent object.
/// @param noise_model Noise model for the sensor to use when augmentating
data
AccelerometerSensor(std::shared_ptr<chrono::ChBody> parent,
float updateRate,
chrono::ChFrame<double> offsetPose,
std::shared_ptr<ChNoiseModel> noise_model)
: ChDynamicSensor(parent, updateRate, offsetPose) {
auto update_filter =
chrono_types::make_shared<FilterAccelerometerUpdate>(noise_model);
m_filters.push_front(std::static_pointer_cast<ChFilter>(update_filter));
}
/// Class destructor
~AccelerometerSensor() {}
virtual void PushKeyFrame() {
ChVector3d tran_acc_no_offset =
m_offsetPose.TransformDirectionParentToLocal(
m_parent->PointAccelerationLocalToParent(m_offsetPose.GetPos()));
ChVector3d g = m_offsetPose.TransformDirectionParentToLocal(
m_parent->TransformDirectionParentToLocal(m_parent->GetSystem()->GetGravitationalAcceleration()));
m_keyframes.push_back(tran_acc_no_offset + g);
}
virtual void ClearKeyFrames() { m_keyframes.clear(); }
private:
std::vector<ChVector3d> m_keyframes; ///< stores keyframes for sensor
friend class FilterAccelerometerUpdate;
};
/// Class for generating IMU data
class FilterAccelerometerUpdate : public ChFilter {
public:
/// Class constructor
/// @param noise_model The noise model to use when augmenting the IMU data
FilterAccelerometerUpdate(std::shared_ptr<ChNoiseModel> noise_model)
: m_noise_model(noise_model), ChFilter("Accelerometer Updater") {}
/// Apply function. Generates IMU data.
virtual void Apply() {
// default sensor values
ChVector3d acc = {0, 0, 0};
// get last measurements
if (m_accSensor->m_keyframes.size() > 0) {
// acc = m_accSensor->m_keyframes.back();
for (auto c : m_accSensor->m_keyframes) {
acc += c;
}
acc /= m_accSensor->m_keyframes.size();
}
if (m_noise_model) {
m_noise_model->AddNoise(acc);
}
// load IMU data
m_bufferOut->Buffer[0].X = acc.x();
m_bufferOut->Buffer[0].Y = acc.y();
m_bufferOut->Buffer[0].Z = acc.z();
m_bufferOut->LaunchedCount = m_accSensor->GetNumLaunches();
m_bufferOut->TimeStamp =
static_cast<float>(m_accSensor->GetParent()->GetSystem()->GetChTime());
}
/// Initializes all data needed by the filter access apply function.
/// @param pSensor A pointer to the sensor.
/// @param bufferInOut pointer to the process buffer
virtual void Initialize(std::shared_ptr<ChSensor> pSensor,
std::shared_ptr<SensorBuffer>& bufferInOut) {
if (bufferInOut) {
throw std::runtime_error("Accelerometer update filter must be
applied first in filter graph");
}
m_accSensor = std::dynamic_pointer_cast<AccelerometerSensor>(pSensor);
if (!m_accSensor) {
throw std::runtime_error("Accelerometer Update filter can only be
used on an accelerometer\n");
}
m_bufferOut = chrono_types::make_shared<SensorHostAccelBuffer>();
m_bufferOut->Buffer = std::make_unique<AccelData[]>(1);
m_bufferOut->Width = m_bufferOut->Height = 1;
m_bufferOut->LaunchedCount = m_accSensor->GetNumLaunches();
m_bufferOut->TimeStamp = 0.f;
bufferInOut = m_bufferOut;
}
private:
std::shared_ptr<AccelerometerSensor> m_accSensor;
std::shared_ptr<SensorHostAccelBuffer> m_bufferOut; ///< For holding
generated IMU data
std::shared_ptr<ChNoiseModel> m_noise_model; ///< The noise model
for augmenting data
};
#endif
