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

More simulation code minor updates

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-29 19:10:37 +03:00
parent 46ba00fca7
commit 78f3f6e3b3
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gazebo/simulator.cpp
View File

@ -7,13 +7,11 @@
#include <cmath> #include <cmath>
#include <gazebo/gazebo.hh> #include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh> #include <gazebo/physics/physics.hh>
#include <gazebo/rendering/rendering.hh>
#include <gazebo/common/common.hh> #include <gazebo/common/common.hh>
#include <gazebo/sensors/sensors.hh> #include <gazebo/sensors/sensors.hh>
#include <gazebo/msgs/msgs.hh> #include <gazebo/msgs/msgs.hh>
#include <ignition/math/Vector3.hh> #include <ignition/math/Vector3.hh>
#include <ignition/math/Pose3.hh> #include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
@ -55,7 +53,6 @@ public:
gzmsg << "Flix plugin loaded" << endl; gzmsg << "Flix plugin loaded" << endl;
} }
public:
void OnReset() { void OnReset() {
attitude = Quaternion(); // reset estimated attitude attitude = Quaternion(); // reset estimated attitude
gzmsg << "Flix plugin reset" << endl; gzmsg << "Flix plugin reset" << endl;
@ -82,12 +79,12 @@ public:
control(); control();
parseInput(); parseInput();
applyMotorsThrust(); applyMotorForces();
publishTopics(); publishTopics();
logData(); logData();
} }
void applyMotorsThrust() { void applyMotorForces() {
// thrusts // thrusts
const double dist = 0.035355; // motors shift from the center, m const double dist = 0.035355; // motors shift from the center, m
const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78 const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78
@ -115,6 +112,7 @@ public:
nodeHandle = transport::NodePtr(new transport::Node()); nodeHandle = transport::NodePtr(new transport::Node());
nodeHandle->Init(); nodeHandle->Init();
string ns = "~/" + model->GetName(); string ns = "~/" + model->GetName();
// create motors output topics for debugging and plotting
motorPub[0] = nodeHandle->Advertise<msgs::Int>(ns + "/motor0"); motorPub[0] = nodeHandle->Advertise<msgs::Int>(ns + "/motor0");
motorPub[1] = nodeHandle->Advertise<msgs::Int>(ns + "/motor1"); motorPub[1] = nodeHandle->Advertise<msgs::Int>(ns + "/motor1");
motorPub[2] = nodeHandle->Advertise<msgs::Int>(ns + "/motor2"); motorPub[2] = nodeHandle->Advertise<msgs::Int>(ns + "/motor2");