diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f053416 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,91 @@ +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 +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} +) +include_directories(SYSTEM ${EIGEN3_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 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/package.xml b/package.xml new file mode 100644 index 0000000..410a489 --- /dev/null +++ b/package.xml @@ -0,0 +1,41 @@ + + + multicopter_sim + 0.0.0 + Multicopter Measurements and Dynamics Simulation package + + + + + Winter Guerra + Varun Agrawal + + + + + + 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 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..889682c 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))); -} \ No newline at end of file +} + +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(); +}