feat: [Destructive Update]del submodules unitree_ros

README.md

@@ -6,17 +6,10 @@ Simulation verification and physical deployment of the quadruped robot's reinfor
 
 ## Preparation
 
-Clone the code (sync submodules)
+Clone the code
 
 ```bash
-git clone --recursive https://github.com/fan-ziqi/rl_sar.git
-```
-
-If there are updates:
-
-```bash
-git pull
-git submodule update --remote --recursive
+git clone https://github.com/fan-ziqi/rl_sar.git
 ```
 
 ## Dependency
@@ -182,12 +175,10 @@ In the following, let ROBOT represent the name of your robot.
 4. Add a new launch file in the rl_sar/launch folder. Refer to other launch files for guidance on modification.
 5. Change ROBOT_NAME to ROBOT in rl_xxx.cpp.
 6. Compile and run.
-7. If the torque of your robot's joints exceeds 50Nm, you need to modify line 180 in `rl_sar/src/unitree_ros/unitree_legged_control/src/joint_controller.cpp` to:
-   ```cpp
-   // calcTorque = computeTorque(currentPos, currentVel, servoCmd);      
-   calcTorque = servoCmd.posStiffness * (servoCmd.pos - currentPos) + servoCmd.velStiffness * (servoCmd.vel - currentVel) + servoCmd.torque;
-   ```
-   This will remove the 50Nm limit.
+
+## Reference
+
+[unitree_ros](https://github.com/unitreerobotics/unitree_ros)
 
 ## Citation
 

README_CN.md

@@ -6,17 +6,10 @@
 
 ## 准备
 
-拉取代码（同步拉取子模块）
+拉取代码
 
 ```bash
-git clone --recursive https://github.com/fan-ziqi/rl_sar.git
-```
-
-如有更新：
-
-```bash
-git pull
-git submodule update --remote --recursive
+git clone https://github.com/fan-ziqi/rl_sar.git
 ```
 
 ## 依赖
@@ -172,7 +165,6 @@ rosrun rl_sar rl_real_a1
     bash kill_cyberdog.sh
     ```
 
-
 ## 添加你的机器人
 
 下文中将ROBOT代表机器人名称
@@ -183,12 +175,10 @@ rosrun rl_sar rl_real_a1
 4. 在rl_sar/launch文件夹中添加一个新的launch文件，请参考其他launch文件自行修改
 5. 修改rl_xxx.cpp中的ROBOT_NAME为ROBOT
 6. 编译运行
-7. 若您的机器人关节力矩大于50Nm，则需要修改`rl_sar/src/unitree_ros/unitree_legged_control/src/joint_controller.cpp`中180行为：
-   ```cpp
-   // calcTorque = computeTorque(currentPos, currentVel, servoCmd);      
-   calcTorque = servoCmd.posStiffness * (servoCmd.pos - currentPos) + servoCmd.velStiffness * (servoCmd.vel - currentVel) + servoCmd.torque;
-   ```
-   这样会解除50Nm的限制
+
+## 参考
+
+[unitree_ros](https://github.com/unitreerobotics/unitree_ros)
 
 ## 引用
 

src/rl_sar/CMakeLists.txt

@@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
     tf
     geometry_msgs
     robot_msgs
-    unitree_legged_msgs
+    robot_joint_controller
 )
 
 find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
@@ -36,7 +36,7 @@ include_directories(${YAML_CPP_INCLUDE_DIR})
 
 catkin_package(
     CATKIN_DEPENDS 
-    unitree_legged_msgs 
+    robot_joint_controller
 )
 
 include_directories(library/unitree_legged_sdk_3.2/include)
@@ -50,7 +50,6 @@ include_directories(
     include
     ${catkin_INCLUDE_DIRS}
     ${unitree_legged_sdk_INCLUDE_DIRS}
-    ../unitree_controller/include
     library/matplotlibcpp
     library/observation_buffer
     library/rl_sdk

src/rl_sar/include/rl_real_a1.hpp

@@ -1,13 +1,8 @@
 #ifndef RL_REAL_HPP
 #define RL_REAL_HPP
 
-// #include "../library/rl_sdk/rl_sdk.hpp"
 #include "rl_sdk.hpp"
 #include "observation_buffer.hpp"
-#include <unitree_legged_msgs/LowCmd.h>
-#include "unitree_legged_msgs/LowState.h"
-#include <unitree_legged_msgs/MotorCmd.h>
-#include <unitree_legged_msgs/MotorState.h>
 #include "unitree_legged_sdk/unitree_legged_sdk.h"
 #include "unitree_legged_sdk/unitree_joystick.h"
 #include <csignal>

src/rl_sar/include/rl_sim.hpp

@@ -7,11 +7,10 @@
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/JointState.h>
 #include <geometry_msgs/Twist.h>
-#include "unitree_legged_msgs/MotorCmd.h"
 #include "unitree_legged_sdk/loop.h"
 #include <csignal>
 
-// #include "robot_msgs/MotorCommand.h"
+#include "robot_msgs/MotorCommand.h"
 // #include "robot_msgs/RobotState.h"
 // #include "robot_msgs/RobotCommand.h"
 
@@ -53,9 +52,8 @@ class RL_Sim : public RL
     ros::Subscriber cmd_vel_subscriber_;
 
     std::map<std::string, ros::Publisher> torque_publishers;
-    std::vector<unitree_legged_msgs::MotorCmd> motor_commands;
+    std::vector<robot_msgs::MotorCommand> motor_commands;
 
-    // std::vector<robot_msgs::MotorCommand> motor_commands;
     // robot_msgs::RobotState robot_state;
     // robot_msgs::RobotCommand robot_command;
 

src/rl_sar/launch/start_a1.launch

@@ -51,11 +51,4 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
-    <!-- <node pkg="unitree_gazebo" type="servo" name="servo" required="true" output="screen"/> -->
-
-    <!-- load the parameter unitree_controller -->
-    <include file="$(find unitree_controller)/launch/set_ctrl.launch">
-        <arg name="rname" value="$(arg rname)"/>
-    </include>
-
 </launch>

src/rl_sar/launch/start_cyberdog.launch

@@ -49,11 +49,4 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
-    <!-- <node pkg="unitree_gazebo" type="servo" name="servo" required="true" output="screen"/> -->
-
-    <!-- load the parameter unitree_controller -->
-    <include file="$(find unitree_controller)/launch/set_ctrl.launch">
-        <arg name="rname" value="$(arg rname)"/>
-    </include>
-
 </launch>

src/rl_sar/package.xml

@@ -26,7 +26,7 @@
     <exec_depend>roscpp</exec_depend>
     <exec_depend>std_msgs</exec_depend>
     <depend>robot_msgs</depend>
-    <depend>unitree_legged_msgs</depend>  
+    <depend>robot_joint_controller</depend>
 
     <export>
 

src/rl_sar/src/rl_sim.cpp

@@ -54,7 +54,7 @@ RL_Sim::RL_Sim()
 
     for (int i = 0; i < params.num_of_dofs; ++i)
     {
-        torque_publishers[params.joint_names[i]] = nh.advertise<unitree_legged_msgs::MotorCmd>(
+        torque_publishers[params.joint_names[i]] = nh.advertise<robot_msgs::MotorCommand>(
             ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10);
     }
 
@@ -94,13 +94,12 @@ void RL_Sim::RobotControl()
     motiontime++;
     for (int i = 0; i < params.num_of_dofs; ++i)
     {
-        motor_commands[i].mode = 0x0A;
         motor_commands[i].q = output_dof_pos[0][i].item<double>();
         motor_commands[i].dq = 0;
         // motor_commands[i].Kp = params.stiffness;
         // motor_commands[i].Kd = params.damping;
-        motor_commands[i].Kp = params.p_gains[0][i].item<double>();
-        motor_commands[i].Kd = params.d_gains[0][i].item<double>();
+        motor_commands[i].kp = params.p_gains[0][i].item<double>();
+        motor_commands[i].kd = params.d_gains[0][i].item<double>();
         // motor_commands[i].tau = output_torques[0][i].item<double>();
         motor_commands[i].tau = 0;
 

src/robot_joint_controller/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_joint_controller)
+
+find_package(catkin REQUIRED COMPONENTS
+    controller_interface
+    hardware_interface
+    pluginlib
+    roscpp
+    realtime_tools
+    robot_msgs
+)
+
+catkin_package(
+    CATKIN_DEPENDS
+    robot_msgs
+    controller_interface
+    hardware_interface
+    pluginlib
+    roscpp
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR})
+
+link_directories(${catkin_LIB_DIRS} lib)
+
+add_library(robot_joint_controller 
+    src/robot_joint_controller.cpp
+)
+target_link_libraries(robot_joint_controller ${catkin_LIBRARIES})

src/robot_joint_controller/include/robot_joint_controller.h

@@ -0,0 +1,75 @@
+#ifndef ROBOT_JOINT_CONTROLLER_H
+#define ROBOT_JOINT_CONTROLLER_H
+
+#include <ros/node_handle.h>
+#include <urdf/model.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include "robot_msgs/MotorCommand.h"
+#include "robot_msgs/MotorState.h"
+#include <geometry_msgs/WrenchStamped.h>
+
+#include <stdio.h>
+#include <stdint.h>
+#include <algorithm>
+#include <math.h>
+
+#define PosStopF  (2.146E+9f)
+#define VelStopF  (16000.0f)
+
+typedef struct 
+{
+    uint8_t mode;
+    double pos;
+    double posStiffness;
+    double vel;
+    double velStiffness;
+    double torque;
+} ServoCommand;
+
+namespace robot_joint_controller
+{
+    class RobotJointController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+    {
+private:
+        hardware_interface::JointHandle joint;
+        ros::Subscriber sub_command, sub_ft;
+        control_toolbox::Pid pid_controller_;
+        boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_msgs::MotorState> > controller_state_publisher_ ;
+
+public:
+        std::string name_space;
+        std::string joint_name;
+        urdf::JointConstSharedPtr joint_urdf;
+        realtime_tools::RealtimeBuffer<robot_msgs::MotorCommand> command;
+        robot_msgs::MotorCommand lastCommand;
+        robot_msgs::MotorState lastState;
+        ServoCommand servoCommand;
+
+        RobotJointController();
+        ~RobotJointController();
+        virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+        virtual void starting(const ros::Time& time);
+        virtual void update(const ros::Time& time, const ros::Duration& period);
+        virtual void stopping();
+        void setCommandCB(const robot_msgs::MotorCommandConstPtr& msg);
+        void positionLimits(double &position);
+        void velocityLimits(double &velocity);
+        void effortLimits(double &effort);
+
+        void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
+        void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
+        void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+    };
+}
+
+#endif

src/robot_joint_controller/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>robot_joint_controller</name>
+    <version>0.0.0</version>
+    <description>The robot_joint_controller package</description>
+    <maintainer email="[email protected]">Ziqi Fan</maintainer>
+    <license>Apache</license>
+    <buildtool_depend>catkin</buildtool_depend>
+    <build_depend>controller_interface</build_depend>
+    <build_depend>hardware_interface</build_depend>
+    <build_depend>pluginlib</build_depend>
+    <build_depend>roscpp</build_depend>
+    <build_export_depend>controller_interface</build_export_depend>
+    <build_export_depend>hardware_interface</build_export_depend>
+    <build_export_depend>pluginlib</build_export_depend>
+    <build_export_depend>roscpp</build_export_depend>
+    <exec_depend>controller_interface</exec_depend>
+    <exec_depend>hardware_interface</exec_depend>
+    <exec_depend>pluginlib</exec_depend>
+    <exec_depend>roscpp</exec_depend>
+    <depend>robot_msgs</depend>
+    <!-- The export tag contains other, unspecified, tags -->
+    <export>
+        <!-- Other tools can request additional information be placed here -->
+        <controller_interface plugin="${prefix}/robot_joint_controller_plugins.xml"/>
+    </export>
+</package>

src/robot_joint_controller/robot_joint_controller_plugins.xml

@@ -0,0 +1,8 @@
+<library path="lib/librobot_joint_controller">
+    <class name="robot_joint_controller/RobotJointController" 
+           type="robot_joint_controller::RobotJointController"           
+           base_class_type="controller_interface::ControllerBase"/>
+        <description>
+            The robot joint controller.
+        </description>
+</library>