Merge pull request #1 from tongtybj/develop/mujoco

[Mujoco] refactor files about mujoco

aerial_robot_simulation/CMakeLists.txt

@@ -56,51 +56,43 @@ include_directories(
 add_definitions(-DSIMULATION)
 
 add_library(flight_controllers
-  src/mujoco_attitude_controller.cpp
+  src/mujoco/mujoco_attitude_controller.cpp
   src/simulation_attitude_controller.cpp)
 add_dependencies(flight_controllers spinal_generate_messages_cpp ${MUJOCO})
 target_link_libraries(flight_controllers ${catkin_LIBRARIES} ${MUJOCO_LIBRARY})
 
 add_library(spinal_interface src/spinal_interface.cpp)
 target_link_libraries(spinal_interface ${catkin_LIBRARIES})
 
-add_library(mujoco_spinal_interface src/mujoco_spinal_interface.cpp)
+add_library(mujoco_spinal_interface src/mujoco/mujoco_spinal_interface.cpp)
 add_dependencies(mujoco_spinal_interface ${MUJOCO})
 target_link_libraries(mujoco_spinal_interface ${catkin_LIBRARIES} ${MUJOCO_LIBRARY})
 
 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_robot_hw_sim src/mujoco_robot_hw_sim.cpp)
+add_library(mujoco_robot_hw_sim
+  src/mujoco/mujoco_default_robot_hw_sim.cpp
+  src/mujoco/mujoco_aerial_robot_hw_sim.cpp
+  )
 add_dependencies(mujoco_robot_hw_sim ${MUJOCO})
-target_link_libraries(mujoco_robot_hw_sim ${catkin_LIBRARIES} ${MUJOCO_LIBRARY})
-
-add_library(mujoco_visualization_utils src/mujoco_visualization_utils.cpp)
-add_dependencies(mujoco_visualization_utils ${MUJOCO})
-target_link_libraries(mujoco_visualization_utils ${catkin_LIBRARIES} ${MUJOCO_LIBRARY})
-
-add_library(mujoco_attitude_controller src/mujoco_attitude_controller.cpp)
-add_dependencies(mujoco_attitude_controller ${MUJOCO})
-target_link_libraries(mujoco_attitude_controller ${catkin_LIBRARIES} ${MUJOCO_LIBRARY})
-
+target_link_libraries(mujoco_robot_hw_sim ${catkin_LIBRARIES} ${MUJOCO_LIBRARY}  mujoco_spinal_interface)
 
-add_executable(mujoco_ros_control src/mujoco_ros_control.cpp)
+add_executable(mujoco_ros_control
+  src/mujoco/mujoco_ros_control.cpp
+  src/mujoco/mujoco_visualization_utils.cpp)
 add_dependencies(mujoco_ros_control ${MUJOCO})
 target_link_libraries(mujoco_ros_control
   ${catkin_LIBRARIES}
   glfw
-  mujoco_visualization_utils
-  mujoco_attitude_controller
-  mujoco_spinal_interface
-  mujoco_robot_hw_sim
   ${MUJOCO_LIBRARY}
   )
 
 install(DIRECTORY include/${PROJECT_NAME}/ ${MUJOCO_INCLUDE_DIRS}
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
 
-install(TARGETS aerial_robot_hw_sim spinal_interface flight_controllers mujoco_ros_control mujoco_visualization_utils mujoco_attitude_controller mujoco_spinal_interface mujoco_robot_hw_sim
+install(TARGETS aerial_robot_hw_sim spinal_interface flight_controllers mujoco_ros_control mujoco_spinal_interface mujoco_robot_hw_sim
   DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 

aerial_robot_simulation/config/Mujoco.yaml

@@ -6,10 +6,12 @@ attitude_controller:
 # 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/include/aerial_robot_simulation/mujoco/mujoco_aerial_robot_hw_sim.h

@@ -0,0 +1,44 @@
+#pragma once
+
+
+#include <aerial_robot_simulation/mujoco/mujoco_default_robot_hw_sim.h>
+#include <aerial_robot_simulation/mujoco/mujoco_spinal_interface.h>
+#include <aerial_robot_simulation/noise_model.h>
+#include <geometry_msgs/PoseStamped.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_attitude_controller.h → aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_attitude_controller.h

@@ -2,7 +2,7 @@
 
 #include <ros/ros.h>
 #include <aerial_robot_estimation/state_estimation.h>
-#include <aerial_robot_simulation/mujoco_spinal_interface.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>

aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_default_robot_hw_sim.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <urdf/model.h>
+#include <pluginlib/class_list_macros.h>
+#include <aerial_robot_simulation/mujoco/mujoco_robot_hw_sim.h>
+#include <sensor_msgs/JointState.h>
+
+namespace mujoco_ros_control
+{
+  class DefaultRobotHWSim : public mujoco_ros_control::RobotHWSim
+  {
+  public:
+    DefaultRobotHWSim() {};
+    ~DefaultRobotHWSim() {}
+
+    bool virtual init(const std::string& robot_namespace,
+              ros::NodeHandle model_nh,
+              mjModel* mujoco_model,
+              mjData* mujoco_data
+              );
+
+    void virtual read(const ros::Time& time, const ros::Duration& period);
+
+    void virtual write(const ros::Time& time, const ros::Duration& period);
+
+    void controlInputCallback(const sensor_msgs::JointState & msg);
+
+
+  protected:
+    mjModel* mujoco_model_;
+    mjData* mujoco_data_;
+
+    std::vector<std::string> joint_list_;
+    ros::Publisher joint_state_pub_;
+    ros::Subscriber control_input_sub_;
+    double joint_state_pub_rate_ = 0.02;
+    std::vector<double> control_input_;
+
+    ros::Time last_joint_state_time_;
+
+  };
+}

aerial_robot_simulation/include/aerial_robot_simulation/mujoco_robot_hw_sim_plugin.h → aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_robot_hw_sim.h

@@ -2,19 +2,17 @@
 
 #include <ros/ros.h>
 #include <hardware_interface/robot_hw.h>
-#include <urdf/model.h>
-#include <transmission_interface/transmission_info.h>
 
 #include <mujoco/mujoco.h>
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
 
 namespace mujoco_ros_control
 {
-  class MujocoRobotHWSimPlugin : public hardware_interface::RobotHW
+  class RobotHWSim : public hardware_interface::RobotHW
   {
   public:
-    virtual ~MujocoRobotHWSimPlugin() {}
+    virtual ~RobotHWSim() {}
 
     virtual bool init(const std::string& robot_namespace,
                       ros::NodeHandle model_nh,

aerial_robot_simulation/include/aerial_robot_simulation/mujoco_ros_control.h → aerial_robot_simulation/include/aerial_robot_simulation/mujoco/mujoco_ros_control.h

@@ -1,21 +1,12 @@
 #pragma once
 
 #include <ros/ros.h>
-#include <ros/callback_queue.h>
 #include <rosgraph_msgs/Clock.h>
 #include <pluginlib/class_loader.h>
-#include <ros/package.h>
-#include <controller_manager/controller_manager.h>
-
 #include <boost/shared_ptr.hpp>
-
-#include <aerial_robot_model/model/aerial_robot_model.h>
-#include <aerial_robot_simulation/mujoco_spinal_interface.h>
-#include <aerial_robot_simulation/mujoco_attitude_controller.h>
-#include <aerial_robot_simulation/mujoco_visualization_utils.h>
-#include <aerial_robot_simulation/mujoco_robot_hw_sim_plugin.h>
-#include <aerial_robot_simulation/mujoco_robot_hw_sim.h>
-
+#include <controller_manager/controller_manager.h>
+#include <aerial_robot_simulation/mujoco/mujoco_visualization_utils.h>
+#include <aerial_robot_simulation/mujoco/mujoco_robot_hw_sim.h>
 
 #include <mujoco/mujoco.h>
 
@@ -35,9 +26,8 @@ namespace mujoco_ros_control
     bool headless_;
 
   protected:
-    boost::shared_ptr<pluginlib::ClassLoader<mujoco_ros_control::MujocoRobotHWSimPlugin> > robot_hw_sim_loader_;
-    boost::shared_ptr<mujoco_ros_control::MujocoRobotHWSimPlugin> robot_hw_sim_;
-    hardware_interface::MujocoSpinalInterface spinal_interface_;
+    boost::shared_ptr<pluginlib::ClassLoader<mujoco_ros_control::RobotHWSim> > robot_hw_sim_loader_;
+    boost::shared_ptr<mujoco_ros_control::RobotHWSim> robot_hw_sim_;
 
   private:
     ros::NodeHandle &nh_;

aerial_robot_simulation/mujoco_robot_hw_sim_plugin.xml

@@ -1,10 +1,18 @@
 <library path="lib/libmujoco_robot_hw_sim">
   <class
-      name="mujoco_ros_control/MujocoRobotHWSim"
-      type="mujoco_ros_control::MujocoRobotHWSim"
-      base_class_type="mujoco_ros_control::MujocoRobotHWSimPlugin">
+      name="mujoco_ros_control/DefaultRobotHWSim"
+      type="mujoco_ros_control::DefaultRobotHWSim"
+      base_class_type="mujoco_ros_control::RobotHWSim">
     <description>
-      A aerial robot simulation interface in MuJoCo.
+      Default simulation interface in MuJoCo.
+    </description>
+  </class>
+  <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>