[MuJoCo] enable to use mujoco simulator

.gitignore

@@ -107,3 +107,7 @@ tags
 # Jetbrains
 .idea/
 *cmake-build-debug/
+
+# mujoco
+MUJOCO_LOG.txt
+robots/**/mujoco

.travis.sh

@@ -33,7 +33,7 @@ rosdep update --include-eol-distros
 # Install source code
 mkdir -p ~/catkin_ws/src
 cd ~/catkin_ws
-ln -sf ${CI_SOURCE_PATH} src/${REPOSITORY_NAME}
+cp -r ${CI_SOURCE_PATH} src/${REPOSITORY_NAME} # copy the whole contents instead of create symbolic link
 wstool init src
 wstool merge -t src src/${REPOSITORY_NAME}/aerial_robot_${ROS_DISTRO}.rosinstall
 wstool update -t src

aerial_robot_kinetic.rosinstall

@@ -2,7 +2,7 @@
 - git:
     local-name: aerial_robot_3rdparty
     uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git
-    version: ros-kinetic
+    version: b21caa3
 
 # kalman filter
 - git:

aerial_robot_melodic.rosinstall

@@ -2,7 +2,7 @@
 - git:
     local-name: aerial_robot_3rdparty
     uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git
-    version: b936f88
+    version: b21caa3
 
 # kalman filter
 - git:

aerial_robot_model/include/aerial_robot_model/servo_bridge.h

@@ -199,6 +199,7 @@ class ServoBridge
   ros::NodeHandle nhp_;
 
   ros::Publisher servo_states_pub_;
+  ros::Publisher mujoco_control_input_pub_;
   map<string, ros::Subscriber> servo_states_subs_;
   map<string, ros::Subscriber> servo_ctrl_subs_;
   map<string, bool> no_real_state_flags_;
@@ -212,6 +213,7 @@ class ServoBridge
   double moving_angle_thresh_;
   bool send_init_joint_pose_;
   bool simulation_mode_;
+  bool use_mujoco_;
   int send_init_joint_pose_cnt_;
 
   void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name);

aerial_robot_model/src/model/base_model/robot_model.cpp

@@ -360,6 +360,7 @@ namespace aerial_robot_model {
     cog.p = link_inertia.getCOG();
     setCog(cog);
     mass_ = link_inertia.getMass();
+    ROS_INFO_STREAM_ONCE("[aerial_robot_model] robot mass is " << mass_);
 
     setInertia((cog.Inverse() * link_inertia).getRotationalInertia());
     setCog2Baselink(cog.Inverse() * f_baselink);

aerial_robot_model/src/servo_bridge/servo_bridge.cpp

@@ -39,6 +39,12 @@
 ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(nhp)
 {
   nh_.param("/use_sim_time", simulation_mode_, false);
+  nhp_.param("use_mujoco", use_mujoco_, false);
+  if(use_mujoco_)
+    {
+      ROS_WARN("use mujoco simulator");
+      simulation_mode_ = false;
+    }
 
   if(simulation_mode_)
     {
@@ -64,6 +70,7 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
   servo_states_subs_.insert(make_pair("common", nh_.subscribe<spinal::ServoStates>(state_sub_topic, 10, boost::bind(&ServoBridge::servoStatesCallback, this, _1, "common"))));
   /* common publisher: target servo state to real machine (spinal_ros_bridge) */
   servo_ctrl_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoControlCmd>(ctrl_pub_topic, 1)));
+  mujoco_control_input_pub_ = nh_.advertise<sensor_msgs::JointState>("mujoco/ctrl_input", 1);
   /* common publisher: torque on/off command */
   servo_torque_ctrl_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoTorqueCmd>(torque_pub_topic, 1)));
 
@@ -291,19 +298,22 @@ void ServoBridge::servoStatesCallback(const spinal::ServoStatesConstPtr& state_m
 void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo_ctrl_msg, const string& servo_group_name)
 {
   spinal::ServoControlCmd target_angle_msg;
+  sensor_msgs::JointState mujoco_control_input_msg;
 
   if(servo_ctrl_msg->name.size() > 0)
     {
       for(int i = 0; i < servo_ctrl_msg->name.size(); i++)
         {/* servo name is assigned */
-
           if(servo_ctrl_msg->position.size() !=  servo_ctrl_msg->name.size())
             {
               ROS_ERROR("[servo bridge, servo control control]: the servo position num and name num are different in ros msgs [%d vs %d]",
                         (int)servo_ctrl_msg->position.size(), (int)servo_ctrl_msg->name.size());
               return;
             }
 
+          mujoco_control_input_msg.name.push_back(servo_ctrl_msg->name.at(i));
+          mujoco_control_input_msg.position.push_back(servo_ctrl_msg->position.at(i));
+
           // use servo_name to search the servo_handler
           auto servo_handler = find_if(servos_handler_[servo_group_name].begin(), servos_handler_[servo_group_name].end(),
                                        [&](SingleServoHandlePtr s) {return servo_ctrl_msg->name.at(i)  == s->getName();});
@@ -344,6 +354,9 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo
           target_angle_msg.index.push_back(servo_handler->getId());
           target_angle_msg.angles.push_back(servo_handler->getTargetAngleVal(ValueType::BIT));
 
+          mujoco_control_input_msg.name.push_back(servo_handler->getName());
+          mujoco_control_input_msg.position.push_back(servo_ctrl_msg->position.at(i));
+
           if(simulation_mode_)
             {
               std_msgs::Float64 msg;
@@ -353,6 +366,8 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo
         }
     }
 
+  mujoco_control_input_pub_.publish(mujoco_control_input_msg);
+
   if (servo_ctrl_pubs_.find(servo_group_name) != servo_ctrl_pubs_.end())
     servo_ctrl_pubs_[servo_group_name].publish(target_angle_msg);
   else

aerial_robot_noetic.rosinstall

@@ -2,7 +2,7 @@
 - git:
     local-name: aerial_robot_3rdparty
     uri: https://github.com/JSKAerialRobot/aerial_robot_3rdparty.git
-    version: b936f88
+    version: b21caa3
 
 # kalman filter
 - git:

aerial_robot_simulation/.gitignore

@@ -0,0 +1 @@
+build

aerial_robot_simulation/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
   aerial_robot_model
   gazebo_ros_control
   kdl_parser
+  mujoco_ros_control
   roscpp
   spinal
   tf
@@ -38,24 +39,32 @@ include_directories(
   include
   )
 
+
 add_definitions(-DSIMULATION)
 
 add_library(flight_controllers
+  src/mujoco/mujoco_attitude_controller.cpp
   src/simulation_attitude_controller.cpp)
-target_link_libraries(flight_controllers ${catkin_LIBRARIES})
 add_dependencies(flight_controllers spinal_generate_messages_cpp)
+target_link_libraries(flight_controllers ${catkin_LIBRARIES})
 
 add_library(spinal_interface src/spinal_interface.cpp)
 target_link_libraries(spinal_interface ${catkin_LIBRARIES})
 
+add_library(mujoco_spinal_interface src/mujoco/mujoco_spinal_interface.cpp)
+target_link_libraries(mujoco_spinal_interface ${catkin_LIBRARIES})
+
 add_library(aerial_robot_hw_sim src/aerial_robot_hw_sim.cpp)
 target_link_libraries(aerial_robot_hw_sim spinal_interface ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
+add_library(mujoco_aerial_robot_hw_sim src/mujoco/mujoco_aerial_robot_hw_sim.cpp )
+target_link_libraries(mujoco_aerial_robot_hw_sim ${catkin_LIBRARIES} mujoco_spinal_interface)
+
+
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
-
-install(TARGETS aerial_robot_hw_sim spinal_interface flight_controllers
+install(TARGETS aerial_robot_hw_sim spinal_interface flight_controllers mujoco_spinal_interface mujoco_aerial_robot_hw_sim
   DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 
@@ -64,6 +73,6 @@ install(DIRECTORY scripts
   USE_SOURCE_PERMISSIONS
 )
 
-install(FILES aerial_robot_hw_sim_plugins.xml flight_controllers_plugins.xml
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(FILES aerial_robot_hw_sim_plugins.xml flight_controllers_plugins.xml mujoco_robot_hw_sim_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

aerial_robot_simulation/config/Mujoco.yaml

@@ -0,0 +1,17 @@
+# control
+attitude_controller:
+  type: flight_controllers/MujocoAttitudeController
+
+
+# sensor -> aerial_robot_hw_sim
+## mocap
+simulation:
+  robot_hw_sim_plugin_name: mujoco_ros_control/AerialRobotHWSim
+
+  mocap_pub_rate: 0.01
+  mocap_pos_noise: 0.001
+  mocap_rot_noise: 0.001
+
+  # ground truth -> aerial_robot_hw_sim
+  ground_truth_pub_rate: 0.01
+

aerial_robot_simulation/flight_controllers_plugins.xml

@@ -6,4 +6,10 @@
     </description>
   </class>
 
+  <class name="flight_controllers/MujocoAttitudeController" type="flight_controllers::MujocoAttitudeController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The Attitude Controller using part of the flight controller function in micro-baord.
+    </description>
+  </class>
+
 </library>

aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_aerial_robot_hw_sim.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <aerial_robot_simulation/mujoco/mujoco_spinal_interface.h>
+#include <aerial_robot_simulation/noise_model.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <mujoco_ros_control/mujoco_default_robot_hw_sim.h>
+#include <nav_msgs/Odometry.h>
+
+namespace mujoco_ros_control
+{
+  class AerialRobotHWSim : public mujoco_ros_control::DefaultRobotHWSim
+  {
+  public:
+    AerialRobotHWSim() {};
+    ~AerialRobotHWSim() {}
+
+    bool init(const std::string& robot_namespace,
+              ros::NodeHandle model_nh,
+              mjModel* mujoco_model,
+              mjData* mujoco_data
+              ) override;
+
+    void read(const ros::Time& time, const ros::Duration& period) override;
+
+    void write(const ros::Time& time, const ros::Duration& period) override;
+
+  protected:
+    hardware_interface::MujocoSpinalInterface spinal_interface_;
+
+    std::vector<std::string> rotor_list_;
+    ros::Publisher ground_truth_pub_;
+    ros::Publisher mocap_pub_;
+    double ground_truth_pub_rate_;
+    double mocap_pub_rate_;
+    double mocap_rot_noise_, mocap_pos_noise_;
+    double ground_truth_pos_noise_, ground_truth_vel_noise_, ground_truth_rot_noise_, ground_truth_angular_noise_;
+    double ground_truth_rot_drift_, ground_truth_vel_drift_, ground_truth_angular_drift_;
+    double ground_truth_rot_drift_frequency_, ground_truth_vel_drift_frequency_, ground_truth_angular_drift_frequency_;
+    double joint_state_pub_rate_ = 0.02;
+
+    ros::Time last_mocap_time_;
+  };
+}

aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_attitude_controller.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <aerial_robot_estimation/state_estimation.h>
+#include <aerial_robot_simulation/mujoco/mujoco_spinal_interface.h>
+#include <flight_control/flight_control.h>
+#include <controller_interface/controller.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace flight_controllers
+{
+  class MujocoAttitudeController : public controller_interface::Controller<hardware_interface::MujocoSpinalInterface>
+  {
+  public:
+    MujocoAttitudeController();
+    ~MujocoAttitudeController() {}
+
+    bool init(hardware_interface::MujocoSpinalInterface *robot, ros::NodeHandle &n);
+
+    void starting(const ros::Time& time);
+    void update(const ros::Time& time, const ros::Duration& period);
+
+  private:
+    hardware_interface::MujocoSpinalInterface* spinal_interface_;
+    boost::shared_ptr<FlightControl> controller_core_;
+    int motor_num_;
+
+  };
+}

aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_spinal_interface.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <hardware_interface/internal/hardware_resource_manager.h>
+#include <hardware_interface/hardware_interface.h>
+#include <aerial_robot_estimation/state_estimation.h>
+#include <state_estimate/state_estimate.h>
+#include <tf/LinearMath/Transform.h>
+#include <cassert>
+#include <limits>
+
+namespace hardware_interface
+{
+  class MujocoSpinalInterface : public HardwareInterface
+  {
+  public:
+    MujocoSpinalInterface();
+    bool init(ros::NodeHandle& nh, int motor_num);
+
+    void setImuValue(double acc_x, double acc_y, double acc_z, double gyro_x, double gyro_y, double gyro_z);
+    void setMagValue(double mag_x, double mag_y, double mag_z);
+    void setTrueBaselinkOrientation(double q_x, double q_y, double q_z, double q_w);
+    void setTrueBaselinkAngular(double w_x, double w_y, double w_z);
+
+    tf::Vector3 getTrueBaselinkRPY();
+    inline tf::Vector3 getTrueBaselinkAngular() { return baselink_angular_;}
+    tf::Vector3 getTrueCogRPY();
+    tf::Vector3 getTrueCogAngular();
+
+    void stateEstimate();
+    inline void onGround(bool flag) { on_ground_ = flag; }
+    StateEstimate* getEstimatorPtr() {return &spinal_state_estimator_;}
+    std::string getName() const{return "";}
+    int getMotorNum() {return motor_num_;}
+    double getForce(int index){return force_.at(index);}
+    void setForce(int index, double force) {force_.at(index) = force;}
+
+  private:
+    /* attitude estimator */
+    bool on_ground_;
+
+    int motor_num_;
+    std::vector<double> force_;
+    tf::Matrix3x3 baselink_rot_;
+    tf::Vector3 baselink_angular_;
+    StateEstimate spinal_state_estimator_;
+  };
+}

aerial_robot_simulation/launch/simulation.launch → aerial_robot_simulation/launch/gazebo.launch

@@ -21,12 +21,12 @@
   </include>
 
   <!-- basic parameter -->
-  <rosparam file="$(find aerial_robot_simulation)/config/Simulation.yaml" command="load" ns="$(arg robot_ns)" />
+  <rosparam file="$(find aerial_robot_simulation)/config/Gazebo.yaml" command="load" ns="$(arg robot_ns)" />
 
   <node name="$(anon urdf_spawner)" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
         args="-urdf -model $(arg robot_ns) -x $(arg spawn_x) -y $(arg spawn_y) -z $(arg spawn_z) -Y $(arg spawn_yaw) -param $(arg robot_ns)/robot_description"/>
 
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-        output="screen" ns="$(arg robot_ns)" args="joint_state_controller attitude_controller">
-  </node>
+        output="screen" ns="$(arg robot_ns)" args="joint_state_controller attitude_controller" />
+
 </launch>

aerial_robot_simulation/launch/mujoco.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+  <arg name="robot_ns" default="" />
+  <arg name="headless" default="false" />
+  <arg name="mujoco_model" default="" />
+
+  <!-- mujoco ros control -->
+  <include file="$(find mujoco_ros_control)/launch/mujoco.launch" >
+    <arg name="robot_ns" value="$(arg robot_ns)" />
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="mujoco_model" value="$(arg mujoco_model)"/>
+  </include>
+
+  <!-- basic parameter -->
+  <rosparam file="$(find aerial_robot_simulation)/config/Mujoco.yaml" command="load" ns="$(arg robot_ns)" />
+
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+        output="screen" ns="$(arg robot_ns)" args="attitude_controller" />
+
+</launch>

aerial_robot_simulation/mujoco_robot_hw_sim_plugin.xml

@@ -0,0 +1,10 @@
+<library path="lib/libmujoco_aerial_robot_hw_sim">
+  <class
+      name="mujoco_ros_control/AerialRobotHWSim"
+      type="mujoco_ros_control::AerialRobotHWSim"
+      base_class_type="mujoco_ros_control::RobotHWSim">
+    <description>
+       simulation interface for aerial robot in MuJoCo.
+    </description>
+  </class>
+</library>