aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup simulation code, remove debug model showing current attitude estimation

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-29 18:45:19 +03:00
parent 3207fdb43c
commit 645b148564
3 changed files with 30 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gazebo/flix.world
View File

@ -25,18 +25,5 @@
<uri>model://flix</uri> <uri>model://flix</uri>
<pose>0 0 0.2 0 0 0</pose> <pose>0 0 0.2 0 0 0</pose>
</include> </include>
<model name="flix_estimate">
<static>true</static>
<link name="estimate">
<visual name="estimate">
<pose>0 0 0 0 0 1.57</pose>
<geometry>
<box>
<size>0.125711 0.125711 0.022</size>
</box>
</geometry>
</visual>
</link>
</model>
</world> </world>
</sdf> </sdf>

17
gazebo/joystick.h
View File

@ -7,12 +7,10 @@
#include <gazebo/gazebo.hh> #include <gazebo/gazebo.hh>
#include <iostream> #include <iostream>
using namespace std; // NOTE: this should be changed to the actual calibration values
const int16_t channelNeutralMin[] = {-1290, -258, -26833, 0, 0, 0};
static const int16_t channelNeutralMin[] = {-1290, -258, -26833, 0, 0, 0}; const int16_t channelNeutralMax[] = {-1032, -258, -27348, 3353, 0, 0};
static const int16_t channelNeutralMax[] = {-1032, -258, -27348, 3353, 0, 0}; const int16_t channelMax[] = {27090, 27090, 27090, 27090, 0, 0};
static const int16_t channelMax[] = {27090, 27090, 27090, 27090, 0, 0};
#define RC_CHANNEL_ROLL 0 #define RC_CHANNEL_ROLL 0
#define RC_CHANNEL_PITCH 1 #define RC_CHANNEL_PITCH 1
@ -21,8 +19,7 @@ static const int16_t channelMax[] = {27090, 27090, 27090, 27090, 0, 0};
#define RC_CHANNEL_AUX 4 #define RC_CHANNEL_AUX 4
#define RC_CHANNEL_MODE 5 #define RC_CHANNEL_MODE 5
static SDL_Joystick *joystick; SDL_Joystick *joystick;
bool joystickInitialized = false, warnShown = false; bool joystickInitialized = false, warnShown = false;
void normalizeRC(); void normalizeRC();
@ -33,9 +30,9 @@ void joystickInit()
joystick = SDL_JoystickOpen(0); joystick = SDL_JoystickOpen(0);
if (joystick != NULL) { if (joystick != NULL) {
joystickInitialized = true; joystickInitialized = true;
gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << endl; gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << std::endl;
} else if (!warnShown) { } else if (!warnShown) {
gzwarn << "Joystick not found, begin waiting for joystick..." << endl; gzwarn << "Joystick not found, begin waiting for joystick..." << std::endl;
warnShown = true; warnShown = true;
} }
} }

117
gazebo/simulator.cpp
View File

@ -19,6 +19,7 @@
#include "Arduino.h" #include "Arduino.h"
#include "flix.h" #include "flix.h"
#include "util.h"
#include "util.ino" #include "util.ino"
#include "joystick.h" #include "joystick.h"
#include "time.ino" #include "time.ino"
@ -29,25 +30,13 @@
#include "lpf.h" #include "lpf.h"
using ignition::math::Vector3d; using ignition::math::Vector3d;
using ignition::math::Pose3d;
using namespace gazebo; using namespace gazebo;
using namespace std; using namespace std;
Pose3d flu2frd(const Pose3d& p)
{
return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(),
p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());
}
Vector flu2frd(const Vector3d& v)
{
return Vector(v.X(), -v.Y(), -v.Z());
}
class ModelFlix : public ModelPlugin class ModelFlix : public ModelPlugin
{ {
private: private:
physics::ModelPtr model, estimateModel; physics::ModelPtr model;
physics::LinkPtr body; physics::LinkPtr body;
sensors::ImuSensorPtr imu; sensors::ImuSensorPtr imu;
event::ConnectionPtr updateConnection, resetConnection; event::ConnectionPtr updateConnection, resetConnection;
@ -60,32 +49,18 @@ public:
{ {
this->model = _parent; this->model = _parent;
this->body = this->model->GetLink("body"); this->body = this->model->GetLink("body");
this->imu = dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu
this->imu = std::dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu this->updateConnection = event::Events::ConnectWorldUpdateBegin(bind(&ModelFlix::OnUpdate, this));
if (imu == nullptr) { this->resetConnection = event::Events::ConnectWorldReset(bind(&ModelFlix::OnReset, this));
gzerr << "IMU sensor not found" << std::endl;
return;
}
this->estimateModel = model->GetWorld()->ModelByName("flix_estimate");
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&ModelFlix::OnUpdate, this));
this->resetConnection = event::Events::ConnectWorldReset(
std::bind(&ModelFlix::OnReset, this));
initNode(); initNode();
Serial.begin(0); Serial.begin(0);
gzmsg << "Flix plugin loaded" << endl; gzmsg << "Flix plugin loaded" << endl;
} }
public: public:
void OnReset() void OnReset()
{ {
attitude = Quaternion(); attitude = Quaternion(); // reset estimated attitude
gzmsg << "Flix plugin reset" << endl; gzmsg << "Flix plugin reset" << endl;
} }
@ -112,7 +87,6 @@ public:
parseInput(); parseInput();
applyMotorsThrust(); applyMotorsThrust();
updateEstimatePose();
publishTopics(); publishTopics();
logData(); logData();
} }
@ -120,71 +94,26 @@ public:
void applyMotorsThrust() void applyMotorsThrust()
{ {
// thrusts // thrusts
const double d = 0.035355; const double dist = 0.035355; // motors shift from the center, m
const double maxThrust = 0.03 * ONE_G; // 30 g, https://www.youtube.com/watch?v=VtKI4Pjx8Sk const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78
// 65 mm prop ~40 g
const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05; const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05; // imitating motors asymmetry
const float minThrustRel = 0; float mfl = scale0 * maxThrust * abs(motors[MOTOR_FRONT_LEFT]);
float mfr = scale1 * maxThrust * abs(motors[MOTOR_FRONT_RIGHT]);
float mrl = scale2 * maxThrust * abs(motors[MOTOR_REAR_LEFT]);
float mrr = scale3 * maxThrust * abs(motors[MOTOR_REAR_RIGHT]);
// apply min thrust body->AddLinkForce(Vector3d(0.0, 0.0, mfl), Vector3d(dist, dist, 0.0));
float mfl = mapff(motors[MOTOR_FRONT_LEFT], 0, 1, minThrustRel, 1); body->AddLinkForce(Vector3d(0.0, 0.0, mfr), Vector3d(dist, -dist, 0.0));
float mfr = mapff(motors[MOTOR_FRONT_RIGHT], 0, 1, minThrustRel, 1); body->AddLinkForce(Vector3d(0.0, 0.0, mrl), Vector3d(-dist, dist, 0.0));
float mrl = mapff(motors[MOTOR_REAR_LEFT], 0, 1, minThrustRel, 1); body->AddLinkForce(Vector3d(0.0, 0.0, mrr), Vector3d(-dist, -dist, 0.0));
float mrr = mapff(motors[MOTOR_REAR_RIGHT], 0, 1, minThrustRel, 1);
if (motors[MOTOR_FRONT_LEFT] < 0.001) mfl = 0;
if (motors[MOTOR_FRONT_RIGHT] < 0.001) mfr = 0;
if (motors[MOTOR_REAR_LEFT] < 0.001) mrl = 0;
if (motors[MOTOR_REAR_RIGHT] < 0.001) mrr = 0;
// TODO: min_thrust
body->AddLinkForce(Vector3d(0.0, 0.0, scale0 * maxThrust * abs(mfl)), Vector3d(d, d, 0.0));
body->AddLinkForce(Vector3d(0.0, 0.0, scale1 * maxThrust * abs(mfr)), Vector3d(d, -d, 0.0));
body->AddLinkForce(Vector3d(0.0, 0.0, scale2 * maxThrust * abs(mrl)), Vector3d(-d, d, 0.0));
body->AddLinkForce(Vector3d(0.0, 0.0, scale3 * maxThrust * abs(mrr)), Vector3d(-d, -d, 0.0));
// TODO: indicate if > 1
// torque // torque
const double maxTorque = 0.0023614413; // 24.08 g*cm const double maxTorque = 0.0024; // 24 g*cm ≈ 24 mN*m
int direction = 1; body->AddRelativeTorque(Vector3d(0.0, 0.0, scale0 * maxTorque * motors[MOTOR_FRONT_LEFT]));
// z is counter clockwise, normal rotation direction is minus body->AddRelativeTorque(Vector3d(0.0, 0.0, scale1 * -maxTorque * motors[MOTOR_FRONT_RIGHT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale0 * maxTorque * motors[MOTOR_FRONT_LEFT])); body->AddRelativeTorque(Vector3d(0.0, 0.0, scale2 * -maxTorque * motors[MOTOR_REAR_LEFT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale1 * -maxTorque * motors[MOTOR_FRONT_RIGHT])); body->AddRelativeTorque(Vector3d(0.0, 0.0, scale3 * maxTorque * motors[MOTOR_REAR_RIGHT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale2 * -maxTorque * motors[MOTOR_REAR_LEFT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale3 * maxTorque * motors[MOTOR_REAR_RIGHT]));
}
void updateEstimatePose() {
if (estimateModel == nullptr) {
return;
}
if (!attitude.finite()) {
// gzerr << "attitude is nan" << std::endl;
return;
}
Pose3d pose(
model->WorldPose().Pos().X(), model->WorldPose().Pos().Y(), model->WorldPose().Pos().Z(),
attitude.w, attitude.x, -attitude.y, -attitude.z // frd to flu
);
// std::cout << pose.Pos().X() << " " << pose.Pos().Y() << " " << pose.Pos().Z() <<
// " " << pose.Rot().W() << " " << pose.Rot().X() << " " << pose.Rot().Y() << " " << pose.Rot().Z() << std::endl;
// calculate attitude estimation error
Quaternion groundtruthAttitude(estimateModel->WorldPose().Rot().W(), estimateModel->WorldPose().Rot().X(), -estimateModel->WorldPose().Rot().Y(), -estimateModel->WorldPose().Rot().Z());
float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), groundtruthAttitude.rotate(Vector(0, 0, -1)));
if (angle < 0.3) {
//gzwarn << "att err: " << angle << endl;
// TODO: warning
// position under the floor to make it invisible
pose.SetZ(-5);
}
estimateModel->SetWorldPose(pose);
} }
void initNode() { void initNode() {
@ -200,7 +129,7 @@ public:
void publishTopics() { void publishTopics() {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
msgs::Int msg; msgs::Int msg;
msg.set_data(static_cast<int>(std::round(motors[i] * 1000))); msg.set_data(static_cast<int>(round(motors[i] * 1000)));
motorPub[i]->Publish(msg); motorPub[i]->Publish(msg);
} }
} }