diff --git a/.travis.sh b/.travis.sh index df8f561a0..69c23ecfb 100644 --- a/.travis.sh +++ b/.travis.sh @@ -69,6 +69,28 @@ if [[ "$ROS_DISTRO" = "kinetic" ]]; then export CC='gcc-7' fi +# use latest cmake +# https://qiita.com/sunrise_lover/items/810977fede4b979c382b +if [[ "$ROS_DISTRO" = "kinetic" ]]; then + cmake --version + wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null + sudo apt-get install -y software-properties-common + sudo apt-get install -y apt-transport-https + sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main" + sudo apt update + sudo apt install -y cmake + cmake --version +fi +if [[ "$ROS_DISTRO" = "melodic" ]]; then + cmake --version + wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null + sudo apt-get install -y software-properties-common + sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main" + sudo apt update + sudo apt install -y cmake + cmake --version +fi + # Build catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON catkin build -p1 -j1 --no-status diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h b/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h index a67a4ada8..fde5b9e7b 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h @@ -453,7 +453,7 @@ namespace aerial_robot_estimation std::string tf_prefix_; /* 9: x_w, y_w, z_w, roll_w, pitch_w, yaw_cog_w, x_b, y_b, yaw_board_w */ - array state_; + std::array state_; /* for calculate the sensor to baselink with the consideration of time delay */ int qu_size_; @@ -464,7 +464,7 @@ namespace aerial_robot_estimation /* sensor fusion */ boost::shared_ptr< pluginlib::ClassLoader > sensor_fusion_loader_ptr_; bool sensor_fusion_flag_; - array fuser_; //0: egomotion; 1: experiment + std::array fuser_; //0: egomotion; 1: experiment /* sensor (un)health level */ uint8_t unhealth_level_; diff --git a/aerial_robot_kinetic.rosinstall b/aerial_robot_kinetic.rosinstall index fc7b02212..ea109b3c6 100644 --- a/aerial_robot_kinetic.rosinstall +++ b/aerial_robot_kinetic.rosinstall @@ -1,8 +1,8 @@ # aerial robot third party - git: local-name: aerial_robot_3rdparty - uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: b10f5bf + uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git + version: develop/casadi/sample # kalman filter - git: diff --git a/aerial_robot_melodic.rosinstall b/aerial_robot_melodic.rosinstall index b07b161d4..370c8b871 100644 --- a/aerial_robot_melodic.rosinstall +++ b/aerial_robot_melodic.rosinstall @@ -1,8 +1,8 @@ # aerial robot third party - git: local-name: aerial_robot_3rdparty - uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: b10f5bf + uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git + version: develop/casadi/sample # kalman filter - git: diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt index 43babfb41..8a70892e3 100644 --- a/aerial_robot_model/CMakeLists.txt +++ b/aerial_robot_model/CMakeLists.txt @@ -7,6 +7,7 @@ add_compile_options(-std=c++17) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + casadi_eigen eigen_conversions geometry_msgs interactive_markers @@ -32,6 +33,10 @@ find_package(orocos_kdl REQUIRED) find_package(urdfdom_headers REQUIRED) find_package(Eigen3 REQUIRED) +set(CMAKE_CXX_STANDARD 17) +find_package(pinocchio REQUIRED) +find_package(casadi REQUIRED) + include_directories(${orocos_kdl_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS}) if(NOT CMAKE_BUILD_TYPE) @@ -52,8 +57,8 @@ generate_messages( catkin_package( INCLUDE_DIRS include test - LIBRARIES aerial_robot_model aerial_robot_model_ros numerical_jacobians robot_model_pluginlib - CATKIN_DEPENDS eigen_conversions geometry_msgs interactive_markers kalman_filter kdl_parser message_runtime pluginlib sensor_msgs spinal std_msgs tf tf_conversions tf2_eigen tf2_geometry_msgs tf2_kdl tf2_ros tf_conversions urdf visualization_msgs + LIBRARIES aerial_robot_model aerial_robot_model_ros numerical_jacobians pinocchio_robot_model pinocchio_robot_model_ros robot_model_pluginlib + CATKIN_DEPENDS casadi_eigen eigen_conversions geometry_msgs interactive_markers kalman_filter kdl_parser message_runtime pluginlib sensor_msgs spinal std_msgs tf tf_conversions tf2_eigen tf2_geometry_msgs tf2_kdl tf2_ros tf_conversions urdf visualization_msgs DEPENDS orocos_kdl urdfdom_headers ) @@ -92,6 +97,13 @@ add_library(robot_model_pluginlib src/model/plugin/underactuated_tilted_robot_model.cpp) target_link_libraries(robot_model_pluginlib ${catkin_LIBRARIES} aerial_robot_model) +add_library(pinocchio_robot_model src/model/base_model/pinocchio_robot_model.cpp) +target_link_libraries(pinocchio_robot_model ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} casadi) +add_library(pinocchio_robot_model_ros src/model/base_model/pinocchio_robot_model_ros.cpp) +target_link_libraries(pinocchio_robot_model_ros ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} pinocchio_robot_model) +add_executable(pinocchio_test src/model/base_model/pinocchio_test.cpp) +target_link_libraries(pinocchio_test ${catkin_LIBRARIES} ${Eigen3_LIBRARIES} ${pinocchio_LIBRARIES} pinocchio_robot_model_ros) + add_library(servo_bridge src/servo_bridge/servo_bridge.cpp) target_link_libraries(servo_bridge ${catkin_LIBRARIES}) add_executable(servo_bridge_node src/servo_bridge/servo_bridge_node.cpp) @@ -111,11 +123,11 @@ target_link_libraries(numerical_jacobians aerial_robot_model ${catkin_LIBRARIES} install(DIRECTORY include/${PROJECT_NAME}/ test/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(TARGETS aerial_robot_model aerial_robot_model_ros robot_model_pluginlib servo_bridge numerical_jacobians +install(TARGETS aerial_robot_model aerial_robot_model_ros pinocchio_robot_model pinocchio_robot_model_ros robot_model_pluginlib servo_bridge numerical_jacobians DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) -install(TARGETS rotor_tf_publisher interactive_marker_tf_broadcaster servo_bridge_node +install(TARGETS rotor_tf_publisher interactive_marker_tf_broadcaster servo_bridge_node pinocchio_test DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h new file mode 100644 index 000000000..2151b1990 --- /dev/null +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h @@ -0,0 +1,147 @@ +#pragma once + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include + +// #include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace aerial_robot_model { + class PinocchioRobotModel + { + public: + PinocchioRobotModel(); + ~PinocchioRobotModel() = default; + + pinocchio::Model getModelDbl() {return model_dbl_;} + pinocchio::Data getDataDbl() {return data_dbl_;} + pinocchio::ModelTpl getModel() {return model_;} + pinocchio::DataTpl getData() {return data_;} + + std::vector getRotorsOriginFromRoot() {return rotors_origin_from_root_;} + std::vector getRotorsOriginFromCog() {return rotors_origin_from_cog_;} + std::vector getRotorsNormalFromRoot() {return rotors_normal_from_root_;} + std::vector getRotorsNormalFromCog() {return rotors_normal_from_cog_;} + + casadi::SX getQCs() {return q_cs_;} + Eigen::Matrix getQ() {return q_;} + casadi::DM getQDbl() {return q_dbl_;} + + casadi::SX getMass() {return mass_;} + casadi::SX getInertia() {return inertia_;} + pinocchio::SE3Tpl getoMcog() {return oMcog_;} + + std::map getJointIndexMap() {return joint_index_map_;} + + void updateRobotModel(); + void updateRobotModel(casadi::SX q_cs); + void updateRobotModel(Eigen::Matrix q); + void updateRobotModelImpl(Eigen::Matrix q); + + private: + std::string getRobotModelXml(const std::string param, ros::NodeHandle nh = ros::NodeHandle()); + + void modelInit(); + void kinematicsInit(); + void inertialInit(); + void inertialUpdate(); + void rotorInit(); + void rotorUpdate(); + + pinocchio::Model model_dbl_; + pinocchio::Data data_dbl_; + pinocchio::ModelTpl model_; + pinocchio::DataTpl data_; + + std::mutex mutex_cog_pos_; + std::mutex mutex_cog_rot_; + std::mutex mutex_inertia_; + std::mutex mutex_rotor_origin_; + std::mutex mutex_rotor_normal_; + + std::vector rotors_origin_from_root_; + std::vector rotors_origin_from_cog_; + std::vector rotors_normal_from_root_; + std::vector rotors_normal_from_cog_; + + casadi::SX q_cs_; + Eigen::Matrix q_; + casadi::DM q_dbl_; + + casadi::SX mass_; + casadi::SX inertia_; + pinocchio::SE3Tpl oMcog_; + + std::map joint_index_map_; + + int rotor_num_; + std::string baselink_; + + protected: + void setCogPos(const Eigen::Matrix cog_pos) + { + std::lock_guard lock(mutex_cog_pos_); + oMcog_.translation() = cog_pos; + } + void setCogRot(const Eigen::Matrix cog_rot) + { + std::lock_guard lock(mutex_cog_rot_); + oMcog_.rotation() = cog_rot; + } + void setInertia(const Eigen::Matrix inertia) + { + std::lock_guard lock(mutex_inertia_); + pinocchio::casadi::copy(inertia, inertia_); + } + void setRotorsOriginFromRoot(const std::vector rotors_origin_from_root) + { + std::lock_guard lock(mutex_rotor_origin_); + rotors_origin_from_root_ = rotors_origin_from_root; + } + void setRotorsOriginFromCog(const std::vector rotors_origin_from_cog) + { + std::lock_guard lock(mutex_rotor_origin_); + rotors_origin_from_cog_ = rotors_origin_from_cog; + } + void setRotorsNormalFromRoot(const std::vector rotors_normal_from_root) + { + std::lock_guard lock(mutex_rotor_normal_); + rotors_normal_from_root_ = rotors_normal_from_root; + } + void setRotorsNormalFromCog(const std::vector rotors_normal_from_cog) + { + std::lock_guard lock(mutex_rotor_normal_); + rotors_normal_from_cog_ = rotors_normal_from_cog; + } + }; +} diff --git a/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h new file mode 100644 index 000000000..14934f2fa --- /dev/null +++ b/aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include +#include + +namespace aerial_robot_model +{ + class PinocchioRobotModelRos + { + public: + PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp); + virtual ~PinocchioRobotModelRos() = default; + boost::shared_ptr getPinocchioRobotModel() {return pinocchio_robot_model_;} + + private: + ros::NodeHandle nh_; + ros::NodeHandle nhp_; + + pluginlib::ClassLoader pinocchio_robot_model_loader_; + boost::shared_ptr pinocchio_robot_model_; + }; +} diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml index b8006e441..6a8ffeb84 100644 --- a/aerial_robot_model/package.xml +++ b/aerial_robot_model/package.xml @@ -12,12 +12,15 @@ catkin eigen_conversions + casadi + casadi_eigen geometry_msgs interactive_markers kalman_filter kdl_parser liburdfdom-headers-dev message_generation + pinocchio pluginlib sensor_msgs spinal @@ -32,6 +35,8 @@ urdf visualization_msgs + casadi + casadi_eigen eigen_conversions geometry_msgs interactive_markers @@ -39,6 +44,7 @@ kalman_filter kdl_parser message_runtime + pinocchio pluginlib robot_state_publisher rviz diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp new file mode 100644 index 000000000..685bd7e77 --- /dev/null +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model.cpp @@ -0,0 +1,201 @@ +#include + +namespace aerial_robot_model { + PinocchioRobotModel::PinocchioRobotModel() + { + modelInit(); + kinematicsInit(); + inertialInit(); + rotorInit(); + + updateRobotModel(); + } + + + void PinocchioRobotModel::modelInit() + { + // Load the urdf model from ros parameter server + std::string robot_model_string = getRobotModelXml("robot_description"); + pinocchio::urdf::buildModelFromXML(robot_model_string, pinocchio::JointModelFreeFlyer(), model_dbl_); + ROS_WARN_STREAM("[model][pinocchio] model name: " << model_dbl_.name); + + // Create data required by the algorithms + pinocchio::Data data_dbl_(model_dbl_); + + // declaraion of model and data with casadi + model_ = model_dbl_.cast(); + data_ = pinocchio::DataTpl(model_); + + // get baselink name from urdf + TiXmlDocument robot_model_xml; + robot_model_xml.Parse(robot_model_string.c_str()); + TiXmlElement* baselink_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("baselink"); + if(!baselink_attr) + ROS_DEBUG("Can not get baselink attribute from urdf model"); + else + baselink_ = std::string(baselink_attr->Attribute("name")); + + ROS_WARN_STREAM("[model][pinocchio] model_.nq: " << model_.nq); + ROS_WARN_STREAM("[model][pinocchio] model_.nv: " << model_.nv); + ROS_WARN_STREAM("[model][pinocchio] model_.njoints: " << model_.njoints); + + // make map for joint position and index + std::vector q_dims(model_.njoints); + for(int i = 0; i < model_.njoints; i++) + { + std::string joint_type = model_.joints[i].shortname(); + if(joint_type == "JointModelFreeFlyer") // floating joint is expressed by seven variables in joint position space (position and quaternion) + q_dims.at(i) = 7; + else if(joint_type == "JointModelRUBX" || joint_type == "JointModelRUBY" || joint_type == "JointModelRUBZ" || joint_type == "JointModelRevoluteUnboundedUnaligned") // continuous joint is expressed by two variables in joint position space (cos and sin) + q_dims.at(i) = 2; + else // revolute joint is expressed by one variable in joint position space + q_dims.at(i) = 1; + } + + int joint_index = 0; + rotor_num_ = 0; + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + { + if(model_.names[joint_id] != "universe") + { + joint_index_map_[model_.names[joint_id]] = joint_index; + joint_index += q_dims.at(joint_id); + + // special process for rotor + if(model_.names[joint_id].find("rotor") != std::string::npos) + { + rotor_num_++; + } + } + } + + for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model_.njoints; ++joint_id) + ROS_WARN_STREAM("[model][pinocchio] joint " << std::setw(4) << joint_id << ": " << std::setw(24) << std::left << model_.names[joint_id] << ": " << std::setw(24) << std::left << model_.joints[joint_id].shortname()); + for(int i = 0; i < model_.frames.size(); i++) + ROS_WARN_STREAM("[model][pinocchio] frame " << std::setw(4) << i << ": " << std::setw(24) << std::left << model_.frames[i].name); + } + + + void PinocchioRobotModel::kinematicsInit() + { + // init q + q_cs_ = casadi::SX::sym("q", model_.nq); + for(int i = 0; i < 7; i++) // set 0 to free flyer joint + q_cs_(i) = 0; + + q_dbl_ = casadi::DM(model_.nq, 1); + q_.resize(model_.nq, 1); + for(int i = 0; i < model_.nq; i++) + { + q_dbl_(i) = 0.0; + q_(i) = q_cs_(i); + } + } + + void PinocchioRobotModel::inertialInit() + { + // get mass (caution: type of this variable is casadi::SX) + mass_ = pinocchio::computeTotalMass(model_); + ROS_WARN_STREAM("[model][pinocchio] robot mass: " << mass_); + } + + void PinocchioRobotModel::rotorInit() + { + rotors_origin_from_root_.resize(rotor_num_); + rotors_origin_from_cog_.resize(rotor_num_); + rotors_normal_from_root_.resize(rotor_num_); + rotors_normal_from_cog_.resize(rotor_num_); + } + + void PinocchioRobotModel::inertialUpdate() + { + // set cog frame + setCogPos(pinocchio::centerOfMass(model_, data_, q_, true)); + setCogRot(data_.oMf[model_.getFrameId(baselink_)].rotation()); + + // get inertia matrix expressed in cog frame. (Hint: pinocchio/unittest/centroidal.cpp) + pinocchio::crba(model_, data_, q_); // Composite Rigid Body Algorithm + data_.M.triangularView() = data_.M.transpose().triangularView(); + data_.Ycrb[0] = data_.liMi[1].act(data_.Ycrb[1]); + + pinocchio::InertiaTpl Ig(oMcog_.inverse().act(data_.Ycrb[0])); + pinocchio::Symmetric3Tpl I = Ig.inertia(); + setInertia(I.matrix()); + } + + void PinocchioRobotModel::rotorUpdate() + { + std::vector rotors_origin_from_root(rotor_num_), rotors_origin_from_cog(rotor_num_), rotors_normal_from_root(rotor_num_), rotors_normal_from_cog(rotor_num_); + // get rotor origin and normal from root and cog + for(int i = 0; i < rotor_num_; i++) + { + std::string rotor_name = "rotor" + std::to_string(i + 1); + int joint_id = model_.getJointId(rotor_name); + + // origin + pinocchio::casadi::copy(data_.oMi[joint_id].translation(), rotors_origin_from_root.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).translation(), rotors_origin_from_cog.at(i)); + + // normal (assume rotational axis is corresponding to z axis) + pinocchio::casadi::copy(data_.oMi[joint_id].rotation().middleCols(2, 1), rotors_normal_from_root.at(i)); + pinocchio::casadi::copy((oMcog_.inverse() * data_.oMi[joint_id]).rotation().middleCols(2, 1), rotors_normal_from_cog.at(i)); + } + + setRotorsOriginFromRoot(rotors_origin_from_root); + setRotorsOriginFromCog(rotors_origin_from_cog); + setRotorsNormalFromRoot(rotors_normal_from_root); + setRotorsNormalFromCog(rotors_normal_from_cog); + } + + void PinocchioRobotModel::updateRobotModel() + { + Eigen::Matrix q; + q.resize(model_.nq, 1); + pinocchio::casadi::copy(q_cs_, q); + + updateRobotModel(q); + } + + void PinocchioRobotModel::updateRobotModel(casadi::SX q_cs) + { + assert((q_cs.size1() == model_.nq) || (q_cs.size2() == model_.nq)); + + Eigen::Matrix q; + q.resize(model_.nq, 1); + pinocchio::casadi::copy(q_cs, q); + + updateRobotModel(q); + } + + void PinocchioRobotModel::updateRobotModel(Eigen::Matrix q) + { + assert(q.size() == model_.nq); + + updateRobotModelImpl(q); + } + + void PinocchioRobotModel::updateRobotModelImpl(Eigen::Matrix q) + { + q_ = q; + + // solve FK + pinocchio::forwardKinematics(model_, data_, q_); + pinocchio::updateFramePlacements(model_, data_); + + inertialUpdate(); + rotorUpdate(); + } + + std::string PinocchioRobotModel::getRobotModelXml(const std::string param, ros::NodeHandle nh) + { + std::string xml_string; + + if(!nh.hasParam(param)) + { + ROS_ERROR("Could not find parameter %s on parameter server with namespace '%s'", param.c_str(), nh.getNamespace().c_str()); + return xml_string; + } + nh.getParam(param, xml_string); + return xml_string; + } +} diff --git a/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp b/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp new file mode 100644 index 000000000..dcb551d7e --- /dev/null +++ b/aerial_robot_model/src/model/base_model/pinocchio_robot_model_ros.cpp @@ -0,0 +1,29 @@ +#include + +namespace aerial_robot_model +{ + PinocchioRobotModelRos::PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp): + nh_(nh), + nhp_(nhp), + pinocchio_robot_model_loader_("aerial_robot_model", "aerial_robot_model::PinocchioRobotModel") + { + // load robot model plugin + std::string plugin_name; + if(nh_.getParam("pinocchio_robot_model_plugin_name", plugin_name)) + { + try + { + pinocchio_robot_model_ = pinocchio_robot_model_loader_.createInstance(plugin_name); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); + } + } + else + { + ROS_ERROR("can not find plugin rosparameter for robot model, use default class: aerial_robot_model::PinocchioRobotModel"); + pinocchio_robot_model_ = boost::make_shared(); + } + } +} diff --git a/aerial_robot_model/src/model/base_model/pinocchio_test.cpp b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp new file mode 100644 index 000000000..221bb16ca --- /dev/null +++ b/aerial_robot_model/src/model/base_model/pinocchio_test.cpp @@ -0,0 +1,14 @@ +#include + +int main(int argc, char ** argv) +{ + ros::init (argc, argv, "pinocchio_robot_model"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + aerial_robot_model::PinocchioRobotModelRos* pinocchio_robot_model_ros = new aerial_robot_model::PinocchioRobotModelRos(nh, nh_private); + ros::spin(); + + delete pinocchio_robot_model_ros; + return 0; +} diff --git a/aerial_robot_noetic.rosinstall b/aerial_robot_noetic.rosinstall index b07b161d4..370c8b871 100644 --- a/aerial_robot_noetic.rosinstall +++ b/aerial_robot_noetic.rosinstall @@ -1,8 +1,8 @@ # aerial robot third party - git: local-name: aerial_robot_3rdparty - uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git - version: b10f5bf + uri: https://github.com/sugikazu75/aerial_robot_3rdparty.git + version: develop/casadi/sample # kalman filter - git: