Public API

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)

multicopterDynamicsSim.hpp → include/multicopter_sim/multicopterDynamicsSim.hpp

@@ -13,6 +13,9 @@
 #include <vector>
 #include "inertialMeasurementSim.hpp"
 
+#include <iomanip>
+#include <fstream>
+
 /**
  * @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<double> getMotorSpeed();
+
+        Eigen::Vector3d getThrust(const std::vector<double> & motorSpeed);
+        Eigen::Vector3d getControlMoment(const std::vector<double> & motorSpeed,
+                                         const std::vector<double> & motorAcceleration);
+        Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity);
+        Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity);
+        Eigen::Vector3d getVehicleSpecificForce(void);
+
+        void getMotorSpeedDerivative(std::vector<double> & motorSpeedDer,
+                                     const std::vector<double> & motorSpeed,
+                                     const std::vector<double> & motorSpeedCommand);
+        Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce,
+                                              const Eigen::Vector3d & velocity, const std::vector<double> & motorSpeed);
+        Eigen::Vector3d getAngularVelocityDerivative(const std::vector<double> & motorSpeed,
+                                                     const std::vector<double>& 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<double> & motorSpeedCommand);
         void proceedState_RK4(double dt_secs, const std::vector<double> & 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<int> 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<double> & motorSpeed);
-        Eigen::Vector3d getControlMoment(const std::vector<double> & motorSpeed,
-                                         const std::vector<double> & motorAcceleration);
-        Eigen::Vector3d getAeroMoment(const Eigen::Vector3d & angularVelocity);
-        Eigen::Vector3d getDragForce(const Eigen::Vector3d & velocity);
-        Eigen::Vector3d getVehicleSpecificForce(void);
-
-        void getMotorSpeedDerivative(std::vector<double> & motorSpeedDer,
-                                     const std::vector<double> & motorSpeed,
-                                     const std::vector<double> & motorSpeedCommand);
-        Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond & attitude, const Eigen::Vector3d & stochForce,
-                                              const Eigen::Vector3d & velocity, const std::vector<double> & motorSpeed);
-        Eigen::Vector3d getAngularVelocityDerivative(const std::vector<double> & motorSpeed,
-                                                     const std::vector<double>& 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<double> & vec1, const std::vector<double> & vec2, 
                             std::vector<double> & vec3, double val);
         void vectorScalarProd(const std::vector<double> & vec1, std::vector<double> & vec2, double val);
         void vectorBoundOp(const std::vector<double> & vec1, std::vector<double> & vec2,
                            const std::vector<double> &  minvec, const std::vector<double> & 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<double>& motorSpeeds,
+                          const Eigen::Vector3d& force,
+                          const Eigen::Vector3d& torque) const;
 };
 
 #endif // MULTICOPTERDYNAMICSSIM_H

package.xml

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>multicopter_sim</name>
+  <version>0.0.0</version>
+  <description>Multicopter Measurements and Dynamics Simulation package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">Winter Guerra</maintainer>
+  <maintainer email="[email protected]">Varun Agrawal</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/flightgoggles_uav_dynamics</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+  <author>Varun Agrawal</author>
+  <author>Ezra Tal</author>
+  <author>Winter Guerra</author>
+
+  <depend>roscpp</depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

inertialMeasurementSim.cpp → src/inertialMeasurementSim.cpp

@@ -4,7 +4,7 @@
  * @brief Inertial measurement unit (IMU) simulator class implementation
  * 
  */
-#include "inertialMeasurementSim.hpp"
+#include "multicopter_sim/inertialMeasurementSim.hpp"
 #include <chrono>
 
 /**