From ac60222383ff1e108d15ffb8cfc769a76ff1da99 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 Oct 2020 16:26:51 -0400 Subject: [PATCH 1/9] make all functions in public domain so user can access various properties --- multicopterDynamicsSim.hpp | 39 ++++++++++++++++++++------------------ 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/multicopterDynamicsSim.hpp b/multicopterDynamicsSim.hpp index 4be551f..20a42f6 100644 --- a/multicopterDynamicsSim.hpp +++ b/multicopterDynamicsSim.hpp @@ -30,6 +30,8 @@ class MulticopterDynamicsSim{ double forceProcessNoiseAutoCorrelation, const Eigen::Vector3d & gravity); MulticopterDynamicsSim(int numCopter); + + int getNumCopter() { return numCopter_; } void setVehicleProperties(double vehicleMass, const Eigen::Matrix3d & vehicleInertia, const Eigen::Matrix3d & aeroMomentCoefficient, double dragCoefficient, @@ -59,7 +61,25 @@ class MulticopterDynamicsSim{ Eigen::Quaterniond getVehicleAttitude(void); Eigen::Vector3d getVehicleVelocity(void); Eigen::Vector3d getVehicleAngularVelocity(void); - + + Eigen::Vector3d getThrust(const std::vector & motorSpeed); + Eigen::Vector3d getControlMoment(const std::vector & motorSpeed, + const std::vector & motorAcceleration); + Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity); + Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity); + Eigen::Vector3d getVehicleSpecificForce(void); + + void getMotorSpeedDerivative(std::vector & motorSpeedDer, + const std::vector & motorSpeed, + const std::vector & motorSpeedCommand); + Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce, + const Eigen::Vector3d & velocity, const std::vector & motorSpeed); + Eigen::Vector3d getAngularVelocityDerivative(const std::vector & motorSpeed, + const std::vector& motorAcceleration, + const Eigen::Vector3d & angularVelocity, + const Eigen::Vector3d & stochMoment); + Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & angularVelocity); + void proceedState_ExplicitEuler(double dt_secs, const std::vector & motorSpeedCommand); void proceedState_RK4(double dt_secs, const std::vector & motorSpeedCommand); @@ -130,23 +150,6 @@ class MulticopterDynamicsSim{ /// @name Vehicle stochastic force vector Eigen::Vector3d stochForce_ = Eigen::Vector3d::Zero(); // N - Eigen::Vector3d getThrust(const std::vector & motorSpeed); - Eigen::Vector3d getControlMoment(const std::vector & motorSpeed, - const std::vector & motorAcceleration); - Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity); - Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity); - Eigen::Vector3d getVehicleSpecificForce(void); - - void getMotorSpeedDerivative(std::vector & motorSpeedDer, - const std::vector & motorSpeed, - const std::vector & motorSpeedCommand); - Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce, - const Eigen::Vector3d & velocity, const std::vector & motorSpeed); - Eigen::Vector3d getAngularVelocityDerivative(const std::vector & motorSpeed, - const std::vector& motorAcceleration, - const Eigen::Vector3d & angularVelocity, - const Eigen::Vector3d & stochMoment); - Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & angularVelocity); void vectorAffineOp(const std::vector & vec1, const std::vector & vec2, std::vector & vec3, double val); void vectorScalarProd(const std::vector & vec1, std::vector & vec2, double val); From c0d0d9fe10e325de3b34359969003c960f09514a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Nov 2020 17:00:06 -0500 Subject: [PATCH 2/9] flag to set stochasticity --- multicopterDynamicsSim.cpp | 74 +++++++++++++++++++++++++++----------- multicopterDynamicsSim.hpp | 13 +++++++ 2 files changed, 67 insertions(+), 20 deletions(-) diff --git a/multicopterDynamicsSim.cpp b/multicopterDynamicsSim.cpp index 424a820..8b993b7 100644 --- a/multicopterDynamicsSim.cpp +++ b/multicopterDynamicsSim.cpp @@ -48,6 +48,7 @@ const Eigen::Vector3d & gravity , motorTimeConstant_(numCopter) , maxMotorSpeed_(numCopter) , minMotorSpeed_(numCopter) +, enableStochasticity_(true) { randomNumberGenerator_.seed(std::chrono::system_clock::now().time_since_epoch().count()); @@ -90,6 +91,7 @@ MulticopterDynamicsSim::MulticopterDynamicsSim(int numCopter) , motorTimeConstant_(numCopter) , maxMotorSpeed_(numCopter) , minMotorSpeed_(numCopter) +, enableStochasticity_(true) { randomNumberGenerator_.seed(std::chrono::system_clock::now().time_since_epoch().count()); @@ -324,17 +326,29 @@ Eigen::Vector3d MulticopterDynamicsSim::getVehicleAngularVelocity(void){ return angularVelocity_; } +/** + * @brief Get the speeds of the motors + * + * @return std::vector Vector with motor speeds for all motor outputs. + */ +std::vector MulticopterDynamicsSim::getMotorSpeed() { + return motorSpeed_; +} + /** * @brief Get total specific force acting on vehicle, excluding gravity force * * @return Eigen::Vector3d Specific force in vehicle-fixed reference frame */ Eigen::Vector3d MulticopterDynamicsSim::getVehicleSpecificForce(void){ - Eigen::Vector3d specificForce = (getThrust(motorSpeed_) + - attitude_.inverse()*(getDragForce(velocity_) + stochForce_)) - / vehicleMass_; - - return specificForce; + Eigen::Vector3d specificForce = getThrust(motorSpeed_); + + if (enableStochasticity_) { + specificForce += (attitude_.inverse()*(getDragForce(velocity_) + stochForce_)); + } else { + specificForce += (attitude_.inverse()*getDragForce(velocity_)); + } + return specificForce / vehicleMass_; } /** @@ -363,19 +377,22 @@ Eigen::Vector3d MulticopterDynamicsSim::getThrust(const std::vector & mo * @return Eigen::Vector3d Moment vector */ Eigen::Vector3d MulticopterDynamicsSim::getControlMoment(const std::vector & motorSpeed, const std::vector & motorAcceleration){ - Eigen::Vector3d controlMoment = Eigen::Vector3d::Zero(); + Eigen::Vector3d thrustMoment = Eigen::Vector3d::Zero(); + Eigen::Vector3d motorTorqueMoment = Eigen::Vector3d::Zero(); for (int indx = 0; indx < numCopter_; indx++){ // Moment due to thrust Eigen::Vector3d motorThrust(0.,0.,fabs(motorSpeed.at(indx))*motorSpeed.at(indx)*thrustCoefficient_.at(indx)); - controlMoment += motorFrame_.at(indx).translation().cross(motorFrame_.at(indx).linear()*motorThrust); + thrustMoment += motorFrame_.at(indx).translation().cross(motorFrame_.at(indx).linear()*motorThrust); // Moment due to torque Eigen::Vector3d motorTorque(0.,0.,motorDirection_.at(indx)*fabs(motorSpeed.at(indx))*motorSpeed.at(indx)*torqueCoefficient_.at(indx)); motorTorque(2) += motorDirection_.at(indx)*motorRotationalInertia_.at(indx)*motorAcceleration.at(indx); - controlMoment += motorFrame_.at(indx).linear()*motorTorque; + motorTorqueMoment += motorFrame_.at(indx).linear()*motorTorque; } + Eigen::Vector3d controlMoment = thrustMoment + motorTorqueMoment; + return controlMoment; } @@ -429,9 +446,13 @@ void MulticopterDynamicsSim::proceedState_ExplicitEuler(double dt_secs, const st stochForce_ /= sqrt(dt_secs); Eigen::Vector3d stochMoment; - stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochMoment << 0, 0, 0; + } std::vector motorSpeedDer(numCopter_); getMotorSpeedDerivative(motorSpeedDer,motorSpeed,motorSpeedCommandBounded); @@ -449,9 +470,13 @@ void MulticopterDynamicsSim::proceedState_ExplicitEuler(double dt_secs, const st attitude_.normalize(); - stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochForce_ << 0, 0, 0; + } imu_.proceedBiasDynamics(dt_secs); } @@ -476,9 +501,14 @@ void MulticopterDynamicsSim::proceedState_RK4(double dt_secs, const std::vector< stochForce_ /= sqrt(dt_secs); Eigen::Vector3d stochMoment; - stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochMoment << 0, 0, 0; + } + // k1 std::vector motorSpeedDer(numCopter_); @@ -564,9 +594,13 @@ void MulticopterDynamicsSim::proceedState_RK4(double dt_secs, const std::vector< attitude_.normalize(); angularVelocity_ = angularVelocity + angularVelocityDer*(1./6.); - stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochForce_ << 0, 0, 0; + } imu_.proceedBiasDynamics(dt_secs); } diff --git a/multicopterDynamicsSim.hpp b/multicopterDynamicsSim.hpp index 20a42f6..c8280e9 100644 --- a/multicopterDynamicsSim.hpp +++ b/multicopterDynamicsSim.hpp @@ -13,6 +13,8 @@ #include #include "inertialMeasurementSim.hpp" +#include + /** * @brief Multicopter dynamics simulator class * @@ -61,6 +63,7 @@ class MulticopterDynamicsSim{ Eigen::Quaterniond getVehicleAttitude(void); Eigen::Vector3d getVehicleVelocity(void); Eigen::Vector3d getVehicleAngularVelocity(void); + std::vector getMotorSpeed(); Eigen::Vector3d getThrust(const std::vector & motorSpeed); Eigen::Vector3d getControlMoment(const std::vector & motorSpeed, @@ -92,6 +95,10 @@ class MulticopterDynamicsSim{ /// @name Number of rotors int numCopter_; + /// @name Flag indicating whether to add stochastic forces + //TODO add ROS parameter querying to set this. + bool enableStochasticity_; + /// @name Motor properties //@{ @@ -104,6 +111,12 @@ class MulticopterDynamicsSim{ /* +1 if positive motor speed corresponds to positive moment around the motor frame z-axis -1 if positive motor speed corresponds to negative moment around the motor frame z-axis i.e. -1 indicates a positive motor speed corresponds to a positive rotation rate around the motor z-axis + + Values: + motor0 -1 (front left) + motor1 1 (back left) + motor2 -1 (back right) + motor3 1 (front right) */ std::vector motorDirection_; From a61ea4afcc82daa9414c7eced35b65a5be60e230 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Nov 2020 00:33:40 -0500 Subject: [PATCH 3/9] make into library which can be linked via ROS --- CMakeLists.txt | 88 +++++++++++++++++++ .../inertialMeasurementSim.hpp | 0 .../multicopterDynamicsSim.hpp | 11 ++- package.xml | 40 +++++++++ .../inertialMeasurementSim.cpp | 2 +- .../multicopterDynamicsSim.cpp | 14 ++- 6 files changed, 150 insertions(+), 5 deletions(-) create mode 100644 CMakeLists.txt rename inertialMeasurementSim.hpp => include/multicopter_sim/inertialMeasurementSim.hpp (100%) rename multicopterDynamicsSim.hpp => include/multicopter_sim/multicopterDynamicsSim.hpp (97%) create mode 100644 package.xml rename inertialMeasurementSim.cpp => src/inertialMeasurementSim.cpp (99%) rename multicopterDynamicsSim.cpp => src/multicopterDynamicsSim.cpp (99%) diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..403bea7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 2.8.3) +project(multicopter_sim) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp + # DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/multicopterDynamicsSim.cpp + src/inertialMeasurementSim.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_flightgoggles_capture.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/inertialMeasurementSim.hpp b/include/multicopter_sim/inertialMeasurementSim.hpp similarity index 100% rename from inertialMeasurementSim.hpp rename to include/multicopter_sim/inertialMeasurementSim.hpp diff --git a/multicopterDynamicsSim.hpp b/include/multicopter_sim/multicopterDynamicsSim.hpp similarity index 97% rename from multicopterDynamicsSim.hpp rename to include/multicopter_sim/multicopterDynamicsSim.hpp index 4be551f..021baf5 100644 --- a/multicopterDynamicsSim.hpp +++ b/include/multicopter_sim/multicopterDynamicsSim.hpp @@ -30,6 +30,8 @@ class MulticopterDynamicsSim{ double forceProcessNoiseAutoCorrelation, const Eigen::Vector3d & gravity); MulticopterDynamicsSim(int numCopter); + + int getNumCopter() { return numCopter_; } void setVehicleProperties(double vehicleMass, const Eigen::Matrix3d & vehicleInertia, const Eigen::Matrix3d & aeroMomentCoefficient, double dragCoefficient, @@ -59,7 +61,8 @@ class MulticopterDynamicsSim{ Eigen::Quaterniond getVehicleAttitude(void); Eigen::Vector3d getVehicleVelocity(void); Eigen::Vector3d getVehicleAngularVelocity(void); - + std::vector getMotorSpeed(); + void proceedState_ExplicitEuler(double dt_secs, const std::vector & motorSpeedCommand); void proceedState_RK4(double dt_secs, const std::vector & motorSpeedCommand); @@ -84,6 +87,12 @@ class MulticopterDynamicsSim{ /* +1 if positive motor speed corresponds to positive moment around the motor frame z-axis -1 if positive motor speed corresponds to negative moment around the motor frame z-axis i.e. -1 indicates a positive motor speed corresponds to a positive rotation rate around the motor z-axis + + Values: + motor0 -1 (front left) + motor1 1 (back left) + motor2 -1 (back right) + motor3 1 (front right) */ std::vector motorDirection_; diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6c3313b --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + multicopter_sim + 0.0.0 + Multicopter Measurements and Dynamics Simulation package + + + + + Winter Guerra + + + + + + MIT + + + + + + + + + + + + Varun Agrawal + Ezra Tal + Winter Guerra + + roscpp + catkin + + + + + + + diff --git a/inertialMeasurementSim.cpp b/src/inertialMeasurementSim.cpp similarity index 99% rename from inertialMeasurementSim.cpp rename to src/inertialMeasurementSim.cpp index f38fca4..dd6865b 100644 --- a/inertialMeasurementSim.cpp +++ b/src/inertialMeasurementSim.cpp @@ -4,7 +4,7 @@ * @brief Inertial measurement unit (IMU) simulator class implementation * */ -#include "inertialMeasurementSim.hpp" +#include "multicopter_sim/inertialMeasurementSim.hpp" #include /** diff --git a/multicopterDynamicsSim.cpp b/src/multicopterDynamicsSim.cpp similarity index 99% rename from multicopterDynamicsSim.cpp rename to src/multicopterDynamicsSim.cpp index 424a820..0937ead 100644 --- a/multicopterDynamicsSim.cpp +++ b/src/multicopterDynamicsSim.cpp @@ -4,7 +4,7 @@ * @brief Multicopter dynamics simulator class implementation * */ -#include "multicopterDynamicsSim.hpp" +#include "multicopter_sim/multicopterDynamicsSim.hpp" #include #include @@ -324,6 +324,15 @@ Eigen::Vector3d MulticopterDynamicsSim::getVehicleAngularVelocity(void){ return angularVelocity_; } +/** + * @brief Get the speeds of the motors + * + * @return std::vector Vector with motor speeds for all motor outputs. + */ +std::vector MulticopterDynamicsSim::getMotorSpeed() { + return motorSpeed_; +} + /** * @brief Get total specific force acting on vehicle, excluding gravity force * @@ -333,7 +342,6 @@ Eigen::Vector3d MulticopterDynamicsSim::getVehicleSpecificForce(void){ Eigen::Vector3d specificForce = (getThrust(motorSpeed_) + attitude_.inverse()*(getDragForce(velocity_) + stochForce_)) / vehicleMass_; - return specificForce; } @@ -678,4 +686,4 @@ Eigen::Vector3d MulticopterDynamicsSim::getAngularVelocityDerivative(const std:: return (vehicleInertia_.inverse()*(getControlMoment(motorSpeed,motorAcceleration) + getAeroMoment(angularVelocity) + stochMoment - angularVelocity.cross(angularMomentum))); -} \ No newline at end of file +} From b0e46b00934cf1ff807d8cb5574829b86ad5dc29 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Nov 2020 08:02:25 -0500 Subject: [PATCH 4/9] don't expose functions which are internal --- multicopterDynamicsSim.hpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/multicopterDynamicsSim.hpp b/multicopterDynamicsSim.hpp index c8280e9..cc2b37d 100644 --- a/multicopterDynamicsSim.hpp +++ b/multicopterDynamicsSim.hpp @@ -65,24 +65,8 @@ class MulticopterDynamicsSim{ Eigen::Vector3d getVehicleAngularVelocity(void); std::vector getMotorSpeed(); - Eigen::Vector3d getThrust(const std::vector & motorSpeed); - Eigen::Vector3d getControlMoment(const std::vector & motorSpeed, - const std::vector & motorAcceleration); - Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity); - Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity); Eigen::Vector3d getVehicleSpecificForce(void); - void getMotorSpeedDerivative(std::vector & motorSpeedDer, - const std::vector & motorSpeed, - const std::vector & motorSpeedCommand); - Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce, - const Eigen::Vector3d & velocity, const std::vector & motorSpeed); - Eigen::Vector3d getAngularVelocityDerivative(const std::vector & motorSpeed, - const std::vector& motorAcceleration, - const Eigen::Vector3d & angularVelocity, - const Eigen::Vector3d & stochMoment); - Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & angularVelocity); - void proceedState_ExplicitEuler(double dt_secs, const std::vector & motorSpeedCommand); void proceedState_RK4(double dt_secs, const std::vector & motorSpeedCommand); From 5675654220217c1e88275d279e947317926f340e Mon Sep 17 00:00:00 2001 From: ghryou Date: Sat, 29 May 2021 11:25:59 -0400 Subject: [PATCH 5/9] add python wrapper --- pyMulticopterDynamicsSim.cpp | 215 +++++++++++++++++++++++++++++++++++ 1 file changed, 215 insertions(+) create mode 100644 pyMulticopterDynamicsSim.cpp diff --git a/pyMulticopterDynamicsSim.cpp b/pyMulticopterDynamicsSim.cpp new file mode 100644 index 0000000..f3e2353 --- /dev/null +++ b/pyMulticopterDynamicsSim.cpp @@ -0,0 +1,215 @@ +#include "pybind11/pybind11.h" +#include "pybind11/stl.h" +#include "pybind11/eigen.h" +#include "multicopterDynamicsSim.hpp" + +namespace py = pybind11; + +void bind_MulticopterDynamicsSim(py::module &m); + +PYBIND11_MODULE(multicopter_dynamics_sim, m) { + m.doc() = "Python package of multicopterDynamicsSim"; + bind_MulticopterDynamicsSim(m); +} + +void bind_MulticopterDynamicsSim(py::module &m) { + py::class_(m, "Isometry3d", py::buffer_protocol()) + .def(py::init([](const Eigen::Matrix & arg) { + Eigen::Isometry3d r = Eigen::Isometry3d::Identity(); + r.rotate(Eigen::Quaterniond(arg.topLeftCorner<3, 3>())); + r.translation() = Eigen::Vector3d(arg.col(3)); + return r;})) + .def(py::init([](Eigen::VectorXd arg) { + Eigen::Isometry3d r = Eigen::Isometry3d::Identity(); + r.rotate(Eigen::Quaterniond(arg[3],arg[4],arg[5],arg[6])); + r.translation() = Eigen::Vector3d(arg.segment(0,3)); + return r;})) + .def_buffer([](Eigen::Isometry3d& r)->py::buffer_info { + return py::buffer_info( + r.data(), // Pointer to buffer + sizeof(double), // Size of one scalar + py::format_descriptor::format(), // Python struct-style format descriptor + 2, // Number of dimensions + { 3, 4 }, // Buffer dimensions + { sizeof(double) * 3, sizeof(double) } // Strides (in bytes) for each index + ); + }); + + py::class_(m, "Quaterniond", py::buffer_protocol()) + .def(py::init([](const Eigen::Vector4d & arg) { + Eigen::Quaterniond q(arg[0],arg[1],arg[2],arg[3]); + return q;})) + .def_buffer([](Eigen::Quaterniond& r)->py::buffer_info { + return py::buffer_info( + r.coeffs().data(), // Pointer to buffer + sizeof(double), // Size of one scalar + py::format_descriptor::format(), // Python struct-style format descriptor + 1, // Number of dimensions + { 4 }, // Buffer dimensions + { sizeof(double) } // Strides (in bytes) for each index + ); + }); + + py::class_(m, "MulticopterDynamicsSim") + .def(py::init< + int, double, double, double, double, double, double, double, + const Eigen::Matrix3d &, const Eigen::Matrix3d & , + double, double, double, const Eigen::Vector3d &>(), + py::arg("numCopter")=4, py::arg("thrustCoefficient")=1.91e-6, py::arg("torqueCoefficient")=2.6e-7, + py::arg("minMotorSpeed")=0, py::arg("maxMotorSpeed")=2200., + py::arg("motorTimeConstant")=0.02, py::arg("motorRotationalInertia")=6.62e-6, + py::arg("vehicleMass")=1.0, + py::arg("vehicleInertia")=Eigen::Vector3d(.0049,0.0049,0.0069).matrix().asDiagonal(), + py::arg("aeroMomentCoefficient")=Eigen::Matrix3d::Zero(), + py::arg("dragCoefficient")=0.1, + py::arg("momentProcessNoiseAutoCorrelation")=0.00025, + py::arg("forceProcessNoiseAutoCorrelation")=0.0005, + py::arg("gravity")=Eigen::Vector3d(0.,0.,9.81)) + .def(py::init(), py::arg("numCopter")) + .def("setRandomSeed", &MulticopterDynamicsSim::setRandomSeed, "set random seed", + py::arg("multicopterSeed"), py::arg("imuSeed")) + .def("setVehicleProperties", &MulticopterDynamicsSim::setVehicleProperties, "set vehicle properties", + py::arg("vehicleMass"), py::arg("vehicleInertia"), py::arg("aeroMomentCoefficient"), + py::arg("dragCoefficient"), + py::arg("momentProcessNoiseAutoCorrelation"), + py::arg("forceProcessNoiseAutoCorrelation")) + .def("setGravityVector", &MulticopterDynamicsSim::setGravityVector, "set gravity vector", py::arg("gravity")) + + .def("setMotorFrame", &MulticopterDynamicsSim::setMotorFrame, "set motor frame", + py::arg("motorFrame"), py::arg("motorDirection"), py::arg("motorIndex")) + .def("setMotorFrame", [](MulticopterDynamicsSim & multicopterSim, + const Eigen::Matrix & motorFrame, int motorDirection, int motorIndex) { + Eigen::Isometry3d r = Eigen::Isometry3d::Identity(); + r.rotate(Eigen::Quaterniond(motorFrame.topLeftCorner<3, 3>())); + r.translation() = Eigen::Vector3d(motorFrame.col(3)); + multicopterSim.setMotorFrame(r, motorDirection, motorIndex); + return; + }, "set motor frame", + py::arg("motorFrame"), py::arg("motorDirection"), py::arg("motorIndex")) + + .def("setMotorProperties", (void (MulticopterDynamicsSim::*)(double, double, double, double, double, double, int)) + &MulticopterDynamicsSim::setMotorProperties, "set motor properties", + py::arg("thrustCoefficient"), py::arg("torqueCoefficient"), py::arg("motorTimeConstant"), + py::arg("minMotorSpeed"), py::arg("maxMotorSpeed"), py::arg("rotationalInertia"), py::arg("motorIndex")) + .def("setMotorProperties", (void (MulticopterDynamicsSim::*)(double, double, double, double, double, double)) + &MulticopterDynamicsSim::setMotorProperties, "set motor properties", + py::arg("thrustCoefficient"), py::arg("torqueCoefficient"), py::arg("motorTimeConstant"), + py::arg("minMotorSpeed"), py::arg("maxMotorSpeed"), py::arg("rotationalInertia")) + .def("setMotorSpeed", (void (MulticopterDynamicsSim::*)(double)) + &MulticopterDynamicsSim::setMotorSpeed, "set motor speed", py::arg("motorSpeed")) + .def("setMotorSpeed", (void (MulticopterDynamicsSim::*)(double, int)) + &MulticopterDynamicsSim::setMotorSpeed, "set motor speed", py::arg("motorSpeed"), py::arg("motorIndex")) + .def("resetMotorSpeeds", &MulticopterDynamicsSim::resetMotorSpeeds, "reset motor speed to zero") + + .def("setVehiclePosition", &MulticopterDynamicsSim::setVehiclePosition, "set vehicle position", + py::arg("position"), py::arg("attitude")) + .def("setVehiclePosition", [](MulticopterDynamicsSim & multicopterSim, + const Eigen::Vector3d & position, const Eigen::Vector4d & attitude) { + Eigen::Quaterniond q(attitude[0],attitude[1],attitude[2],attitude[3]); + multicopterSim.setVehiclePosition(position, q); + return; + }, "set vehicle position", + py::arg("position"), py::arg("attitude")) + .def("setVehicleState", &MulticopterDynamicsSim::setVehicleState, "set vehicle state", + py::arg("position"), py::arg("velocity"), py::arg("angularVelocity"), + py::arg("attitude"), py::arg("motorSpeed")) + .def("setVehicleState", [](MulticopterDynamicsSim & multicopterSim, + const Eigen::Vector3d & position, + const Eigen::Vector3d & velocity, + const Eigen::Vector3d & angularVelocity, + const Eigen::Vector4d & attitude, + const std::vector & motorSpeed) { + Eigen::Quaterniond q(attitude[0],attitude[1],attitude[2],attitude[3]); + multicopterSim.setVehicleState(position, velocity, angularVelocity, q, motorSpeed); + }, "set vehicle state", + py::arg("position"), py::arg("velocity"), py::arg("angularVelocity"), + py::arg("attitude"), py::arg("motorSpeed")) + + // .def("getVehicleState", &MulticopterDynamicsSim::getVehicleState, "get vehicle state") + .def("getVehicleState", [](MulticopterDynamicsSim & multicopterSim) { + Eigen::Vector3d position, velocity, angularVelocity; + Eigen::Quaterniond attitude; + std::vector motorSpeed; + multicopterSim.getVehicleState(position, velocity, angularVelocity, attitude, motorSpeed); + Eigen::Vector4d attitude_vec; + attitude_vec << attitude.w(), attitude.x(), attitude.y(), attitude.z(); + Eigen::VectorXd motorSpeed_vec = Eigen::Map(motorSpeed.data(), motorSpeed.size()); + // std::list vec; + // vec.push_back(position); + // vec.push_back(velocity); + // vec.push_back(angularVelocity); + // vec.push_back(attitude_vec); + // vec.push_back(motorSpeed_vec); + std::map vec; + vec.insert(std::pair("position",position)); + vec.insert(std::pair("velocity",velocity)); + vec.insert(std::pair("angularVelocity",angularVelocity)); + vec.insert(std::pair("attitude",attitude_vec)); + vec.insert(std::pair("motorSpeed",motorSpeed_vec)); + return vec; + }, "get vehicle state") + + .def("getVehiclePosition", &MulticopterDynamicsSim::getVehiclePosition, "get vehicle position") + .def("getVehicleVelocity", &MulticopterDynamicsSim::getVehicleVelocity, "get vehicle velocity") + .def("getVehicleAttitude", &MulticopterDynamicsSim::getVehicleAttitude, "get vehicle attitude") + .def("getVehicleAngularVelocity", &MulticopterDynamicsSim::getVehicleAngularVelocity, "get vehicle angular velocity") + + .def("proceedState", [](MulticopterDynamicsSim & multicopterSim, + double dt_secs, const std::vector & motorSpeedCommand) { + multicopterSim.proceedState_RK4(dt_secs, motorSpeedCommand); + }, "proceed state", + py::arg("dt_secs"), py::arg("motorSpeedCommand")) + .def("proceedState_ExplicitEuler", &MulticopterDynamicsSim::proceedState_ExplicitEuler, "proceed state", + py::arg("dt_secs"), py::arg("motorSpeedCommand")) + .def("proceedState_RK4", &MulticopterDynamicsSim::proceedState_RK4, "proceed state", + py::arg("dt_secs"), py::arg("motorSpeedCommand")) + + .def("getIMUMeasurement", [](MulticopterDynamicsSim & multicopterSim) { + Eigen::Vector3d accOutput; + Eigen::Vector3d gyroOutput; + multicopterSim.getIMUMeasurement(accOutput, gyroOutput); + std::map vec; + vec.insert(std::pair("acc",accOutput)); + vec.insert(std::pair("gyro",gyroOutput)); + return vec; + }, "get imu measurement") + + .def("setIMUBias", [](MulticopterDynamicsSim & multicopterSim, + const Eigen::Vector3d & accBias, const Eigen::Vector3d & gyroBias, + double accBiasProcessNoiseAutoCorrelation, + double gyroBiasProcessNoiseAutoCorrelation) { + multicopterSim.imu_.setBias(accBias, gyroBias, + accBiasProcessNoiseAutoCorrelation, + gyroBiasProcessNoiseAutoCorrelation); + }, "set IMU bias", + py::arg("accBias"), py::arg("gyroBias"), + py::arg("accBiasProcessNoiseAutoCorrelation"), + py::arg("gyroBiasProcessNoiseAutoCorrelation")) + .def("setIMUBias", [](MulticopterDynamicsSim & multicopterSim, + double accBiasVariance, double gyroBiasVariance, + double accBiasProcessNoiseAutoCorrelation, + double gyroBiasProcessNoiseAutoCorrelation) { + multicopterSim.imu_.setBias(accBiasVariance, gyroBiasVariance, + accBiasProcessNoiseAutoCorrelation, + gyroBiasProcessNoiseAutoCorrelation); + }, "set IMU bias", + py::arg("accBiasVariance"), py::arg("gyroBiasVariance"), + py::arg("accBiasProcessNoiseAutoCorrelation"), + py::arg("gyroBiasProcessNoiseAutoCorrelation")) + .def("setIMUBias", [](MulticopterDynamicsSim & multicopterSim, + double accBiasVariance, double gyroBiasVariance) { + multicopterSim.imu_.setBias(accBiasVariance, gyroBiasVariance); + }, "set IMU bias", + py::arg("accBiasVariance"), py::arg("gyroBiasVariance")) + .def("setIMUNoiseVariance", [](MulticopterDynamicsSim & multicopterSim, + double accMeasNoiseVariance, double gyroMeasNoiseVariance) { + multicopterSim.imu_.setNoiseVariance(accMeasNoiseVariance, gyroMeasNoiseVariance); + }, "set IMU noise variance", + py::arg("accMeasNoiseVariance"), py::arg("gyroMeasNoiseVariance")) + .def("setIMUOrientation", [](MulticopterDynamicsSim & multicopterSim, + const Eigen::Vector4d & imuOrient) { + Eigen::Quaterniond q(imuOrient[0],imuOrient[1],imuOrient[2],imuOrient[3]); + multicopterSim.imu_.setOrientation(q); + }, "set IMU orientation", + py::arg("imuOrient")); +} From d1d6c81f1fb4d80a88e5b94a3057f2e7ab26aa60 Mon Sep 17 00:00:00 2001 From: ghryou Date: Sat, 29 May 2021 14:57:24 -0400 Subject: [PATCH 6/9] set the seed of random generator --- inertialMeasurementSim.cpp | 20 ++++++++++++++++++++ inertialMeasurementSim.hpp | 4 ++++ multicopterDynamicsSim.cpp | 12 ++++++++++++ multicopterDynamicsSim.hpp | 2 ++ 4 files changed, 38 insertions(+) diff --git a/inertialMeasurementSim.cpp b/inertialMeasurementSim.cpp index f38fca4..683619e 100644 --- a/inertialMeasurementSim.cpp +++ b/inertialMeasurementSim.cpp @@ -27,6 +27,15 @@ inertialMeasurementSim::inertialMeasurementSim(double accMeasNoiseVariance, doub gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation; } +/** + * @brief Set seed of random number generator + * + * @param imuSeed Seed for IMU measurement noise and bias dynamics RNG. + */ +void inertialMeasurementSim::setRandomSeed(const unsigned imuSeed){ + randomNumberGenerator_.seed(imuSeed); +} + /** * @brief Set bias properties * @@ -44,6 +53,17 @@ void inertialMeasurementSim::setBias(const Eigen::Vector3d & accBias, const Eige gyroBiasProcessNoiseAutoCorrelation_ = gyroBiasProcessNoiseAutoCorrelation; } +/** + * @brief Get bias values + * + * @param accBias Accelerometer bias value + * @param gyroBias Gyroscope bias value + */ +void inertialMeasurementSim::getBias(Eigen::Vector3d & accBias, Eigen::Vector3d & gyroBias){ + accBias = accBias_; + gyroBias = gyroBias_; +} + /** * @brief Set bias properties * diff --git a/inertialMeasurementSim.hpp b/inertialMeasurementSim.hpp index 2c904a8..ea881cf 100644 --- a/inertialMeasurementSim.hpp +++ b/inertialMeasurementSim.hpp @@ -22,6 +22,8 @@ class inertialMeasurementSim inertialMeasurementSim(double accMeasNoiseVariance, double gyroMeasNoiseVariance, double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation); + void setRandomSeed(const unsigned imuSeed); + void setBias(const Eigen::Vector3d & accBias, const Eigen::Vector3d & gyroBias, double accBiasProcessNoiseAutoCorrelation, double gyroBiasProcessNoiseAutoCorrelation); @@ -31,6 +33,8 @@ class inertialMeasurementSim void setBias(double accBiasVariance, double gyroBiasVariance); + void getBias(Eigen::Vector3d & accBias, Eigen::Vector3d & gyroBias); + void setNoiseVariance(double accMeasNoiseVariance, double gyroMeasNoiseVariance); void setOrientation(const Eigen::Quaterniond & imuOrient); diff --git a/multicopterDynamicsSim.cpp b/multicopterDynamicsSim.cpp index 8b993b7..0dc7b48 100644 --- a/multicopterDynamicsSim.cpp +++ b/multicopterDynamicsSim.cpp @@ -144,6 +144,18 @@ void MulticopterDynamicsSim::setGravityVector(const Eigen::Vector3d & gravity){ gravity_ = gravity; } +/** + * @brief Set seed of random number generators + * + * @param multicopterSeed Seed for multicopter process noise RNG. + * @param imuSeed Seed for IMU measurement noise and bias dynamics RNG. + */ +void MulticopterDynamicsSim::setRandomSeed(const unsigned multicopterSeed, + const unsigned imuSeed){ + randomNumberGenerator_.seed(multicopterSeed); + imu_.setRandomSeed(imuSeed); +} + /** * @brief Set orientation and position for individual motor * diff --git a/multicopterDynamicsSim.hpp b/multicopterDynamicsSim.hpp index cc2b37d..7aaf818 100644 --- a/multicopterDynamicsSim.hpp +++ b/multicopterDynamicsSim.hpp @@ -40,6 +40,8 @@ class MulticopterDynamicsSim{ double momentProcessNoiseAutoCorrelation, double forceProcessNoiseAutoCorrelation); void setGravityVector(const Eigen::Vector3d & gravity); + void setRandomSeed(const unsigned multicopterSeed, + const unsigned imuSeed); void setMotorFrame(const Eigen::Isometry3d & motorFrame, int motorDirection, int motorIndex); void setMotorProperties(double thrustCoefficient, double torqueCoefficient, double motorTimeConstant, double minMotorSpeed, double maxMotorSpeed, double rotationalInertia, int motorIndex); From 018cf5b5d4bae0545bd72f0dac93c893cfb9aa6c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Nov 2020 00:33:40 -0500 Subject: [PATCH 7/9] rearranged code to library format --- .../inertialMeasurementSim.hpp | 0 .../multicopterDynamicsSim.hpp | 66 ++++++++--- .../inertialMeasurementSim.cpp | 2 +- .../multicopterDynamicsSim.cpp | 112 ++++++++++++++---- 4 files changed, 140 insertions(+), 40 deletions(-) rename inertialMeasurementSim.hpp => include/multicopter_sim/inertialMeasurementSim.hpp (100%) rename multicopterDynamicsSim.hpp => include/multicopter_sim/multicopterDynamicsSim.hpp (89%) rename inertialMeasurementSim.cpp => src/inertialMeasurementSim.cpp (99%) rename multicopterDynamicsSim.cpp => src/multicopterDynamicsSim.cpp (85%) diff --git a/inertialMeasurementSim.hpp b/include/multicopter_sim/inertialMeasurementSim.hpp similarity index 100% rename from inertialMeasurementSim.hpp rename to include/multicopter_sim/inertialMeasurementSim.hpp diff --git a/multicopterDynamicsSim.hpp b/include/multicopter_sim/multicopterDynamicsSim.hpp similarity index 89% rename from multicopterDynamicsSim.hpp rename to include/multicopter_sim/multicopterDynamicsSim.hpp index b2cd747..106f951 100644 --- a/multicopterDynamicsSim.hpp +++ b/include/multicopter_sim/multicopterDynamicsSim.hpp @@ -13,6 +13,9 @@ #include #include "inertialMeasurementSim.hpp" +#include +#include + /** * @brief Multicopter dynamics simulator class * @@ -30,6 +33,8 @@ class MulticopterDynamicsSim{ double forceProcessNoiseAutoCorrelation, const Eigen::Vector3d & gravity); MulticopterDynamicsSim(int numCopter); + + int getNumCopter() { return numCopter_; } void setVehicleProperties(double vehicleMass, const Eigen::Matrix3d & vehicleInertia, const Eigen::Matrix3d & aeroMomentCoefficient, double dragCoefficient, @@ -61,7 +66,26 @@ class MulticopterDynamicsSim{ Eigen::Quaterniond getVehicleAttitude(void); Eigen::Vector3d getVehicleVelocity(void); Eigen::Vector3d getVehicleAngularVelocity(void); - + std::vector getMotorSpeed(); + + Eigen::Vector3d getThrust(const std::vector & motorSpeed); + Eigen::Vector3d getControlMoment(const std::vector & motorSpeed, + const std::vector & motorAcceleration); + Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity); + Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity); + Eigen::Vector3d getVehicleSpecificForce(void); + + void getMotorSpeedDerivative(std::vector & motorSpeedDer, + const std::vector & motorSpeed, + const std::vector & motorSpeedCommand); + Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce, + const Eigen::Vector3d & velocity, const std::vector & motorSpeed); + Eigen::Vector3d getAngularVelocityDerivative(const std::vector & motorSpeed, + const std::vector& motorAcceleration, + const Eigen::Vector3d & angularVelocity, + const Eigen::Vector3d & stochMoment); + Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & angularVelocity); + void proceedState_ExplicitEuler(double dt_secs, const std::vector & motorSpeedCommand); void proceedState_RK4(double dt_secs, const std::vector & motorSpeedCommand); @@ -74,6 +98,10 @@ class MulticopterDynamicsSim{ /// @name Number of rotors int numCopter_; + /// @name Flag indicating whether to add stochastic forces + //TODO add ROS parameter querying to set this. + bool enableStochasticity_; + /// @name Motor properties //@{ @@ -86,6 +114,12 @@ class MulticopterDynamicsSim{ /* +1 if positive motor speed corresponds to positive moment around the motor frame z-axis -1 if positive motor speed corresponds to negative moment around the motor frame z-axis i.e. -1 indicates a positive motor speed corresponds to a positive rotation rate around the motor z-axis + + Values: + motor0 -1 (front left) + motor1 1 (back left) + motor2 -1 (back right) + motor3 1 (front right) */ std::vector motorDirection_; @@ -132,28 +166,24 @@ class MulticopterDynamicsSim{ /// @name Vehicle stochastic force vector Eigen::Vector3d stochForce_ = Eigen::Vector3d::Zero(); // N - Eigen::Vector3d getThrust(const std::vector & motorSpeed); - Eigen::Vector3d getControlMoment(const std::vector & motorSpeed, - const std::vector & motorAcceleration); - Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity); - Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity); - Eigen::Vector3d getVehicleSpecificForce(void); - - void getMotorSpeedDerivative(std::vector & motorSpeedDer, - const std::vector & motorSpeed, - const std::vector & motorSpeedCommand); - Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce, - const Eigen::Vector3d & velocity, const std::vector & motorSpeed); - Eigen::Vector3d getAngularVelocityDerivative(const std::vector & motorSpeed, - const std::vector& motorAcceleration, - const Eigen::Vector3d & angularVelocity, - const Eigen::Vector3d & stochMoment); - Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & angularVelocity); void vectorAffineOp(const std::vector & vec1, const std::vector & vec2, std::vector & vec3, double val); void vectorScalarProd(const std::vector & vec1, std::vector & vec2, double val); void vectorBoundOp(const std::vector & vec1, std::vector & vec2, const std::vector & minvec, const std::vector & maxvec); + + /// State index + double index_ = 0; + /// Time stamp after integration step + double time_ = 0; + + void saveDynamics(const Eigen::Vector3d& position, + const Eigen::Quaterniond& attitude, + const Eigen::Vector3d& velocity, + const Eigen::Vector3d& angularVelocity, + const std::vector& motorSpeeds, + const Eigen::Vector3d& force, + const Eigen::Vector3d& torque) const; }; #endif // MULTICOPTERDYNAMICSSIM_H \ No newline at end of file diff --git a/inertialMeasurementSim.cpp b/src/inertialMeasurementSim.cpp similarity index 99% rename from inertialMeasurementSim.cpp rename to src/inertialMeasurementSim.cpp index 683619e..d83133e 100644 --- a/inertialMeasurementSim.cpp +++ b/src/inertialMeasurementSim.cpp @@ -4,7 +4,7 @@ * @brief Inertial measurement unit (IMU) simulator class implementation * */ -#include "inertialMeasurementSim.hpp" +#include "multicopter_sim/inertialMeasurementSim.hpp" #include /** diff --git a/multicopterDynamicsSim.cpp b/src/multicopterDynamicsSim.cpp similarity index 85% rename from multicopterDynamicsSim.cpp rename to src/multicopterDynamicsSim.cpp index 5765734..eb57e2a 100644 --- a/multicopterDynamicsSim.cpp +++ b/src/multicopterDynamicsSim.cpp @@ -4,7 +4,7 @@ * @brief Multicopter dynamics simulator class implementation * */ -#include "multicopterDynamicsSim.hpp" +#include "multicopter_sim/multicopterDynamicsSim.hpp" #include #include @@ -48,6 +48,7 @@ const Eigen::Vector3d & gravity , motorTimeConstant_(numCopter) , maxMotorSpeed_(numCopter) , minMotorSpeed_(numCopter) +, enableStochasticity_(false) { randomNumberGenerator_.seed(std::chrono::system_clock::now().time_since_epoch().count()); @@ -71,6 +72,14 @@ const Eigen::Vector3d & gravity forceProcessNoiseAutoCorrelation_ = forceProcessNoiseAutoCorrelation; gravity_ = gravity; + + std::ofstream states_file; + states_file.open("states.csv", std::ios::ate); + states_file << "index,seq,timestamp,"; + states_file << "position x,position y,position z,rotation w,rotation x,rotation y,rotation z,"; + states_file << "velocity x,velocity y,velocity z,ang x,ang y,ang z,"; + states_file << "motor 1,motor 2,motor 3,motor 4,force x,force y,force z,torque x,torque y,torque z" << std::endl; + states_file.close(); } /** @@ -90,6 +99,7 @@ MulticopterDynamicsSim::MulticopterDynamicsSim(int numCopter) , motorTimeConstant_(numCopter) , maxMotorSpeed_(numCopter) , minMotorSpeed_(numCopter) +, enableStochasticity_(false) { randomNumberGenerator_.seed(std::chrono::system_clock::now().time_since_epoch().count()); @@ -336,17 +346,29 @@ Eigen::Vector3d MulticopterDynamicsSim::getVehicleAngularVelocity(void){ return angularVelocity_; } +/** + * @brief Get the speeds of the motors + * + * @return std::vector Vector with motor speeds for all motor outputs. + */ +std::vector MulticopterDynamicsSim::getMotorSpeed() { + return motorSpeed_; +} + /** * @brief Get total specific force acting on vehicle, excluding gravity force * * @return Eigen::Vector3d Specific force in vehicle-fixed reference frame */ Eigen::Vector3d MulticopterDynamicsSim::getVehicleSpecificForce(void){ - Eigen::Vector3d specificForce = (getThrust(motorSpeed_) + - attitude_.inverse()*(getDragForce(velocity_) + stochForce_)) - / vehicleMass_; - - return specificForce; + Eigen::Vector3d specificForce = getThrust(motorSpeed_); + + if (enableStochasticity_) { + specificForce += (attitude_.inverse()*(getDragForce(velocity_) + stochForce_)); + } else { + specificForce += (attitude_.inverse()*getDragForce(velocity_)); + } + return specificForce / vehicleMass_; } /** @@ -375,19 +397,22 @@ Eigen::Vector3d MulticopterDynamicsSim::getThrust(const std::vector & mo * @return Eigen::Vector3d Moment vector */ Eigen::Vector3d MulticopterDynamicsSim::getControlMoment(const std::vector & motorSpeed, const std::vector & motorAcceleration){ - Eigen::Vector3d controlMoment = Eigen::Vector3d::Zero(); + Eigen::Vector3d thrustMoment = Eigen::Vector3d::Zero(); + Eigen::Vector3d motorTorqueMoment = Eigen::Vector3d::Zero(); for (int indx = 0; indx < numCopter_; indx++){ // Moment due to thrust Eigen::Vector3d motorThrust(0.,0.,fabs(motorSpeed.at(indx))*motorSpeed.at(indx)*thrustCoefficient_.at(indx)); - controlMoment += motorFrame_.at(indx).translation().cross(motorFrame_.at(indx).linear()*motorThrust); + thrustMoment += motorFrame_.at(indx).translation().cross(motorFrame_.at(indx).linear()*motorThrust); // Moment due to torque Eigen::Vector3d motorTorque(0.,0.,motorDirection_.at(indx)*fabs(motorSpeed.at(indx))*motorSpeed.at(indx)*torqueCoefficient_.at(indx)); motorTorque(2) += motorDirection_.at(indx)*motorRotationalInertia_.at(indx)*motorAcceleration.at(indx); - controlMoment += motorFrame_.at(indx).linear()*motorTorque; + motorTorqueMoment += motorFrame_.at(indx).linear()*motorTorque; } + Eigen::Vector3d controlMoment = thrustMoment + motorTorqueMoment; + return controlMoment; } @@ -441,9 +466,13 @@ void MulticopterDynamicsSim::proceedState_ExplicitEuler(double dt_secs, const st stochForce_ /= sqrt(dt_secs); Eigen::Vector3d stochMoment; - stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochMoment << 0, 0, 0; + } std::vector motorSpeedDer(numCopter_); getMotorSpeedDerivative(motorSpeedDer,motorSpeed,motorSpeedCommandBounded); @@ -452,6 +481,10 @@ void MulticopterDynamicsSim::proceedState_ExplicitEuler(double dt_secs, const st Eigen::Vector4d attitudeDer = getAttitudeDerivative(attitude,angularVelocity); Eigen::Vector3d angularVelocityDer = getAngularVelocityDerivative(motorSpeed,motorSpeedDer,angularVelocity,stochMoment); + saveDynamics(position_, attitude_, velocity_, angularVelocity_, motorSpeed_, velocityDer, angularVelocityDer); + index_ += 1; + time_ += dt_secs; + vectorAffineOp(motorSpeed,motorSpeedDer,motorSpeed_,dt_secs); vectorBoundOp(motorSpeed_,motorSpeed_,minMotorSpeed_,maxMotorSpeed_); position_ = position + positionDer*dt_secs; @@ -461,9 +494,13 @@ void MulticopterDynamicsSim::proceedState_ExplicitEuler(double dt_secs, const st attitude_.normalize(); - stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochForce_ << 0, 0, 0; + } imu_.proceedBiasDynamics(dt_secs); } @@ -488,9 +525,14 @@ void MulticopterDynamicsSim::proceedState_RK4(double dt_secs, const std::vector< stochForce_ /= sqrt(dt_secs); Eigen::Vector3d stochMoment; - stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochMoment << sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(momentProcessNoiseAutoCorrelation_/dt_secs)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochMoment << 0, 0, 0; + } + // k1 std::vector motorSpeedDer(numCopter_); @@ -576,9 +618,13 @@ void MulticopterDynamicsSim::proceedState_RK4(double dt_secs, const std::vector< attitude_.normalize(); angularVelocity_ = angularVelocity + angularVelocityDer*(1./6.); - stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), - sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + if (enableStochasticity_) { + stochForce_ << sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_), + sqrt(forceProcessNoiseAutoCorrelation_)*standardNormalDistribution_(randomNumberGenerator_); + } else { + stochForce_ << 0, 0, 0; + } imu_.proceedBiasDynamics(dt_secs); } @@ -690,4 +736,28 @@ Eigen::Vector3d MulticopterDynamicsSim::getAngularVelocityDerivative(const std:: return (vehicleInertia_.inverse()*(getControlMoment(motorSpeed,motorAcceleration) + getAeroMoment(angularVelocity) + stochMoment - angularVelocity.cross(angularMomentum))); +} + +void MulticopterDynamicsSim::saveDynamics( + const Eigen::Vector3d& position, const Eigen::Quaterniond& attitude, + const Eigen::Vector3d& velocity, const Eigen::Vector3d& angularVelocity, + const std::vector& motorSpeeds, const Eigen::Vector3d& force, + const Eigen::Vector3d& torque) const { + std::ofstream states_file; + states_file.open("states.csv", std::ios::app); + states_file << "0,"; + states_file << index_ << ","; + states_file << time_ << ","; + states_file << position(0) << "," << position(1) << "," << position(2) << ","; + states_file << attitude.w() << "," << attitude.x() << "," << attitude.y() + << "," << attitude.z() << ","; + states_file << velocity(0) << "," << velocity(1) << "," << velocity(2) << ","; + states_file << angularVelocity(0) << "," << angularVelocity(1) << "," + << angularVelocity(2) << ","; + states_file << motorSpeeds.at(0) << "," << motorSpeeds.at(1) << "," + << motorSpeeds.at(2) << "," << motorSpeeds.at(3) << ","; + states_file << force(0) << "," << force(1) << "," << force(2) << ","; + states_file << torque(0) << "," << torque(1) << "," << torque(2) << std::endl; + + states_file.close(); } \ No newline at end of file From 25765b936d0573e171b9cab328a5aa589bf15453 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Nov 2020 00:33:57 -0500 Subject: [PATCH 8/9] make into library which can be linked via ROS --- CMakeLists.txt | 88 ++++++++++++++++++++++++++++++++++++++++++++++++++ package.xml | 40 +++++++++++++++++++++++ 2 files changed, 128 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..403bea7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 2.8.3) +project(multicopter_sim) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp + # DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/multicopterDynamicsSim.cpp + src/inertialMeasurementSim.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_flightgoggles_capture.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..d471f89 --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + multicopter_sim + 0.0.0 + Multicopter Measurements and Dynamics Simulation package + + + + + Varun Agrawal + + + + + + MIT + + + + + + + + + + + + Varun Agrawal + Ezra Tal + Winter Guerra + + roscpp + catkin + + + + + + + From dc55ea6366e8b43d2024a7ae9152f75e4b0e1a59 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 17 Dec 2020 16:44:06 -0500 Subject: [PATCH 9/9] updates to CMake --- CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 403bea7..f053416 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,12 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0) project(multicopter_sim) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") +find_package(Eigen3 REQUIRED) + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -41,6 +43,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ) +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) ## Declare a C++ library add_library(${PROJECT_NAME}