[WIP][RobotModel] symbolic robot model calculation framework with pinocchio and casadi

.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

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<AxisState, State::TOTAL_NUM> state_;
+    std::array<AxisState, State::TOTAL_NUM> 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<kf_plugin::KalmanFilter> > sensor_fusion_loader_ptr_;
     bool sensor_fusion_flag_;
-    array<SensorFuser, 2> fuser_; //0: egomotion; 1: experiment
+    std::array<SensorFuser, 2> fuser_; //0: egomotion; 1: experiment
 
     /* sensor (un)health level */
     uint8_t unhealth_level_;

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:

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:

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}
 )
 

aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model.h

@@ -0,0 +1,147 @@
+#pragma once
+
+#include <pinocchio/fwd.hpp>
+
+#include <pinocchio/parsers/urdf.hpp>
+
+#include <pinocchio/algorithm/center-of-mass.hxx>
+#include <pinocchio/algorithm/centroidal.hxx>
+#include <pinocchio/algorithm/crba.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/algorithm/kinematics-derivatives.hpp>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/algorithm/model.hxx>
+#include <pinocchio/autodiff/casadi.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/spatial/se3-tpl.hpp>
+#include <pinocchio/spatial/inertia.hpp>
+
+
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+
+// #include <casadi/casadi.hpp>
+#include <CasadiEigen/CasadiEigen.h>
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <Eigen/LU>
+
+#include <iostream>
+#include <mutex>
+#include <tinyxml.h>
+
+
+namespace aerial_robot_model {
+  class PinocchioRobotModel
+  {
+  public:
+    PinocchioRobotModel();
+    ~PinocchioRobotModel() = default;
+
+    pinocchio::Model getModelDbl() {return model_dbl_;}
+    pinocchio::Data getDataDbl() {return data_dbl_;}
+    pinocchio::ModelTpl<casadi::SX> getModel() {return model_;}
+    pinocchio::DataTpl<casadi::SX> getData() {return data_;}
+
+    std::vector<casadi::SX> getRotorsOriginFromRoot() {return rotors_origin_from_root_;}
+    std::vector<casadi::SX> getRotorsOriginFromCog() {return rotors_origin_from_cog_;}
+    std::vector<casadi::SX> getRotorsNormalFromRoot() {return rotors_normal_from_root_;}
+    std::vector<casadi::SX> getRotorsNormalFromCog() {return rotors_normal_from_cog_;}
+
+    casadi::SX getQCs() {return q_cs_;}
+    Eigen::Matrix<casadi::SX, Eigen::Dynamic, 1> getQ() {return q_;}
+    casadi::DM getQDbl() {return q_dbl_;}
+
+    casadi::SX getMass() {return mass_;}
+    casadi::SX getInertia() {return inertia_;}
+    pinocchio::SE3Tpl<casadi::SX> getoMcog() {return oMcog_;}
+
+    std::map<std::string, int> getJointIndexMap() {return joint_index_map_;}
+
+    void updateRobotModel();
+    void updateRobotModel(casadi::SX q_cs);
+    void updateRobotModel(Eigen::Matrix<casadi::SX, Eigen::Dynamic, 1> q);
+    void updateRobotModelImpl(Eigen::Matrix<casadi::SX, Eigen::Dynamic, 1> 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<casadi::SX> model_;
+    pinocchio::DataTpl<casadi::SX> 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<casadi::SX> rotors_origin_from_root_;
+    std::vector<casadi::SX> rotors_origin_from_cog_;
+    std::vector<casadi::SX> rotors_normal_from_root_;
+    std::vector<casadi::SX> rotors_normal_from_cog_;
+
+    casadi::SX q_cs_;
+    Eigen::Matrix<casadi::SX, Eigen::Dynamic, 1> q_;
+    casadi::DM q_dbl_;
+
+    casadi::SX mass_;
+    casadi::SX inertia_;
+    pinocchio::SE3Tpl<casadi::SX> oMcog_;
+
+    std::map<std::string, int> joint_index_map_;
+
+    int rotor_num_;
+    std::string baselink_;
+
+  protected:
+    void setCogPos(const Eigen::Matrix<casadi::SX, 3, 1> cog_pos)
+    {
+      std::lock_guard<std::mutex> lock(mutex_cog_pos_);
+      oMcog_.translation() = cog_pos;
+    }
+    void setCogRot(const Eigen::Matrix<casadi::SX, 3, 3> cog_rot)
+    {
+      std::lock_guard<std::mutex> lock(mutex_cog_rot_);
+      oMcog_.rotation() = cog_rot;
+    }
+    void setInertia(const Eigen::Matrix<casadi::SX, 3, 3> inertia)
+    {
+      std::lock_guard<std::mutex> lock(mutex_inertia_);
+      pinocchio::casadi::copy(inertia, inertia_);
+    }
+    void setRotorsOriginFromRoot(const std::vector<casadi::SX> rotors_origin_from_root)
+    {
+      std::lock_guard<std::mutex> lock(mutex_rotor_origin_);
+      rotors_origin_from_root_ = rotors_origin_from_root;
+    }
+    void setRotorsOriginFromCog(const std::vector<casadi::SX> rotors_origin_from_cog)
+    {
+      std::lock_guard<std::mutex> lock(mutex_rotor_origin_);
+      rotors_origin_from_cog_ = rotors_origin_from_cog;
+    }
+    void setRotorsNormalFromRoot(const std::vector<casadi::SX> rotors_normal_from_root)
+    {
+      std::lock_guard<std::mutex> lock(mutex_rotor_normal_);
+      rotors_normal_from_root_ = rotors_normal_from_root;
+    }
+    void setRotorsNormalFromCog(const std::vector<casadi::SX> rotors_normal_from_cog)
+    {
+      std::lock_guard<std::mutex> lock(mutex_rotor_normal_);
+      rotors_normal_from_cog_ = rotors_normal_from_cog;
+    }
+  };
+}

aerial_robot_model/include/aerial_robot_model/model/pinocchio_robot_model_ros.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <aerial_robot_model/model/pinocchio_robot_model.h>
+#include <pluginlib/class_loader.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+
+namespace aerial_robot_model
+{
+  class PinocchioRobotModelRos
+  {
+  public:
+    PinocchioRobotModelRos(ros::NodeHandle nh, ros::NodeHandle nhp);
+    virtual ~PinocchioRobotModelRos() = default;
+    boost::shared_ptr<aerial_robot_model::PinocchioRobotModel> getPinocchioRobotModel() {return pinocchio_robot_model_;}
+
+  private:
+    ros::NodeHandle nh_;
+    ros::NodeHandle nhp_;
+
+    pluginlib::ClassLoader<aerial_robot_model::PinocchioRobotModel> pinocchio_robot_model_loader_;
+    boost::shared_ptr<aerial_robot_model::PinocchioRobotModel> pinocchio_robot_model_;
+  };
+}

aerial_robot_model/package.xml

@@ -12,12 +12,15 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>eigen_conversions</build_depend>
+  <build_depend>casadi</build_depend>
+  <build_depend>casadi_eigen</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>interactive_markers</build_depend>
   <build_depend>kalman_filter</build_depend>
   <build_depend>kdl_parser</build_depend>
   <build_depend>liburdfdom-headers-dev</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>pinocchio</build_depend>
   <build_depend>pluginlib</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>spinal</build_depend>
@@ -32,13 +35,16 @@
   <build_depend>urdf</build_depend>
   <build_depend>visualization_msgs</build_depend>
 
+  <run_depend>casadi</run_depend>
+  <run_depend>casadi_eigen</run_depend>
   <run_depend>eigen_conversions</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>interactive_markers</run_depend>
   <run_depend>joint_state_publisher_gui</run_depend>
   <run_depend>kalman_filter</run_depend>
   <run_depend>kdl_parser</run_depend>
   <run_depend>message_runtime</run_depend>
+  <run_depend>pinocchio</run_depend>
   <run_depend>pluginlib</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>rviz</run_depend>