Skip to content

Make into library which can be linked via ROS #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
varunagrawal wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Make into library which can be linked via ROS #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a61ea4a
make into library which can be linked via ROS
varunagrawal Nov 29, 2020
018cf5b
rearranged code to library format
varunagrawal Nov 29, 2020
25765b9
make into library which can be linked via ROS
varunagrawal Nov 29, 2020
dc55ea6
updates to CMake
varunagrawal Dec 17, 2020
e109bf6
merge latest upstream
varunagrawal Feb 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
91 changes: 91 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
0 inertialMeasurementSim.hpp → ...ulticopter_sim/inertialMeasurementSim.hpp
View file
Open in desktop
File renamed without changes.
66 changes: 48 additions & 18 deletions multicopterDynamicsSim.hpp → ...ulticopter_sim/multicopterDynamicsSim.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@
#include <vector>
#include "inertialMeasurementSim.hpp"

#include <iomanip>
#include <fstream>

/**
* @brief Multicopter dynamics simulator class
*
Expand All @@ -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,
Expand Down Expand Up @@ -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);

Expand All @@ -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
//@{

Expand All @@ -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_;

Expand Down Expand Up @@ -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
41 changes: 41 additions & 0 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
2 changes: 1 addition & 1 deletion inertialMeasurementSim.cpp → src/inertialMeasurementSim.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
* @brief Inertial measurement unit (IMU) simulator class implementation
*
*/
#include "inertialMeasurementSim.hpp"
#include "multicopter_sim/inertialMeasurementSim.hpp"
#include <chrono>

/**
Expand Down
Loading