can visualize and control gazebo robots

controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h

@@ -9,7 +9,7 @@
 
 
 namespace leg_pd_controller {
-    class LegPdController : public controller_interface::ChainableControllerInterface {
+    class LegPdController final : public controller_interface::ChainableControllerInterface {
     public:
         controller_interface::CallbackReturn on_init() override;
 

controllers/leg_pd_controller/src/LegPdController.cpp

@@ -18,6 +18,14 @@ namespace leg_pd_controller {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
         }
+
+        const size_t joint_num = joint_names_.size();
+        joint_effort_command_.assign(joint_num, 0);
+        joint_position_command_.assign(joint_num, 0);
+        joint_velocities_command_.assign(joint_num, 0);
+        joint_kp_command_.assign(joint_num, 0);
+        joint_kd_command_.assign(joint_num, 0);
+
         return CallbackReturn::SUCCESS;
     }
 
@@ -50,6 +58,11 @@ namespace leg_pd_controller {
 
     controller_interface::CallbackReturn LegPdController::on_activate(
         const rclcpp_lifecycle::State & /*previous_state*/) {
+
+        joint_effort_command_interface_.clear();
+        joint_position_state_interface_.clear();
+        joint_velocity_state_interface_.clear();
+
         // assign effort command interface
         for (auto &interface: command_interfaces_) {
             joint_effort_command_interface_.emplace_back(interface);
@@ -75,6 +88,24 @@ namespace leg_pd_controller {
 
     controller_interface::return_type LegPdController::update_and_write_commands(
         const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        if (joint_names_.size() != joint_effort_command_.size() ||
+            joint_names_.size() != joint_kp_command_.size() ||
+            joint_names_.size() != joint_position_command_.size() ||
+            joint_names_.size() != joint_position_state_interface_.size() ||
+            joint_names_.size() != joint_velocity_state_interface_.size() ||
+            joint_names_.size() != joint_effort_command_interface_.size()) {
+
+            std::cout << "joint_names_.size() = " << joint_names_.size() << std::endl;
+            std::cout << "joint_effort_command_.size() = " << joint_effort_command_.size() << std::endl;
+            std::cout << "joint_kp_command_.size() = " << joint_kp_command_.size() << std::endl;
+            std::cout << "joint_position_command_.size() = " << joint_position_command_.size() << std::endl;
+            std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() << std::endl;
+            std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << std::endl;
+            std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << std::endl;
+
+            throw std::runtime_error("Mismatch in vector sizes in update_and_write_commands");
+        }
+
         for (size_t i = 0; i < joint_names_.size(); ++i) {
             // PD Controller
             const double torque = joint_effort_command_[i] + joint_kp_command_[i] * (
@@ -98,7 +129,7 @@ namespace leg_pd_controller {
             reference_interfaces.emplace_back(controller_name, joint_name + "/position", &joint_position_command_[ind]);
             reference_interfaces.emplace_back(controller_name, joint_name + "/velocity",
                                               &joint_velocities_command_[ind]);
-            reference_interfaces.emplace_back(controller_name, joint_name + "/torque", &joint_effort_command_[ind]);
+            reference_interfaces.emplace_back(controller_name, joint_name + "/effort", &joint_effort_command_[ind]);
             reference_interfaces.emplace_back(controller_name, joint_name + "/kp", &joint_kp_command_[ind]);
             reference_interfaces.emplace_back(controller_name, joint_name + "/kd", &joint_kd_command_[ind]);
             ind++;

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -81,7 +81,9 @@ namespace unitree_guide_controller {
         std::vector<std::string> state_interface_types_;
 
         std::string imu_name_;
+        std::string command_prefix_;
         std::vector<std::string> imu_interface_types_;
+        std::vector<std::string> feet_names_;
 
         rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
         rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
@@ -89,7 +91,7 @@ namespace unitree_guide_controller {
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
         command_interface_map_ = {
-            {"torque", &ctrl_comp_.joint_torque_command_interface_},
+            {"effort", &ctrl_comp_.joint_torque_command_interface_},
             {"position", &ctrl_comp_.joint_position_command_interface_},
             {"velocity", &ctrl_comp_.joint_velocity_command_interface_},
             {"kp", &ctrl_comp_.joint_kp_command_interface_},

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h

@@ -21,7 +21,7 @@ public:
 
     ~QuadrupedRobot() = default;
 
-    void init(const std::string &robot_description);
+    void init(const std::string &robot_description, const std::vector<std::string> &feet_names);
 
     /**
      * Calculate the joint positions based on the foot end position

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -15,9 +15,13 @@ namespace unitree_guide_controller {
         conf.names.reserve(joint_names_.size() * command_interface_types_.size());
         for (const auto &joint_name: joint_names_) {
             for (const auto &interface_type: command_interface_types_) {
+                if (!command_prefix_.empty()) {
+                    conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
+                } else {
                     conf.names.push_back(joint_name + "/" += interface_type);
                 }
             }
+        }
 
         return conf;
     }
@@ -84,6 +88,9 @@ namespace unitree_guide_controller {
             // imu sensor
             imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
             imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
+            command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
+            feet_names_ =
+                    auto_declare<std::vector<std::string> >("feet_names", feet_names_);
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
@@ -105,9 +112,9 @@ namespace unitree_guide_controller {
             });
 
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
-            "~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
+            "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
             [this](const std_msgs::msg::String::SharedPtr msg) {
-                ctrl_comp_.robot_model_.init(msg->data);
+                ctrl_comp_.robot_model_.init(msg->data, feet_names_);
                 ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
             });
 
@@ -126,7 +133,12 @@ namespace unitree_guide_controller {
 
         // assign command interfaces
         for (auto &interface: command_interfaces_) {
-            command_interface_map_[interface.get_interface_name()]->push_back(interface);
+            std::string interface_name = interface.get_interface_name();
+            if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
+                command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
+            } else {
+                command_interface_map_[interface_name]->push_back(interface);
+            }
         }
 
         // assign state interfaces

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -6,14 +6,15 @@
 #include "unitree_guide_controller/control/CtrlComponent.h"
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
-void QuadrupedRobot::init(const std::string &robot_description) {
+void QuadrupedRobot::init(const std::string &robot_description, const std::vector<std::string> &feet_names) {
     KDL::Tree robot_tree;
     kdl_parser::treeFromString(robot_description, robot_tree);
 
-    robot_tree.getChain("base", "FR_foot", fr_chain_);
+    robot_tree.getChain("base", feet_names[0], fr_chain_);
-    robot_tree.getChain("base", "FL_foot", fl_chain_);
+    robot_tree.getChain("base", feet_names[1], fl_chain_);
-    robot_tree.getChain("base", "RR_foot", rr_chain_);
+    robot_tree.getChain("base", feet_names[2], rr_chain_);
-    robot_tree.getChain("base", "RL_foot", rl_chain_);
+    robot_tree.getChain("base", feet_names[3], rl_chain_);
+
 
     robot_legs_.resize(4);
     robot_legs_[0] = std::make_shared<RobotLeg>(fr_chain_);
@@ -70,6 +71,7 @@ std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
     result.resize(4);
     for (int i = 0; i < 4; i++) {
         result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]);
+        result[i].M = KDL::Rotation::Identity();
     }
     return result;
 }

controllers/unitree_guide_controller/src/robot/RobotLeg.cpp

@@ -18,7 +18,6 @@ RobotLeg::RobotLeg(const KDL::Chain &chain) {
 KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const {
     KDL::Frame pEe;
     fk_pose_solver_->JntToCart(joint_positions, pEe);
-    pEe.M = KDL::Rotation::Identity();
     return pEe;
 }
 

descriptions/go2_description/config/robot_control.yaml

@@ -36,7 +36,7 @@ unitree_guide_controller:
       - RL_calf_joint
 
     command_interfaces:
-      - torque
+      - effort
       - position
       - velocity
       - kp
@@ -47,6 +47,12 @@ unitree_guide_controller:
       - position
       - velocity
 
+    feet_names:
+      - FR_FOOT
+      - FL_FOOT
+      - RR_FOOT
+      - RL_FOOT
+
     imu_name: "imu_sensor"
 
     imu_interfaces:

descriptions/go2_description/launch/hardware.launch.py

@@ -48,9 +48,6 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_controllers],
-        remappings=[
-            ("~/robot_description", "/robot_description"),
-        ],
         output="both",
     )
 

descriptions/go2_description/xacro/ros2_control.xacro

@@ -10,7 +10,7 @@
         <joint name="FR_hip_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -22,7 +22,7 @@
         <joint name="FR_thigh_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -34,7 +34,7 @@
         <joint name="FR_calf_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -46,7 +46,7 @@
         <joint name="FL_hip_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -58,7 +58,7 @@
         <joint name="FL_thigh_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -70,7 +70,7 @@
         <joint name="FL_calf_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -82,7 +82,7 @@
         <joint name="RR_hip_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -94,7 +94,7 @@
         <joint name="RR_thigh_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -106,7 +106,7 @@
         <joint name="RR_calf_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -118,7 +118,7 @@
         <joint name="RL_hip_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -130,7 +130,7 @@
         <joint name="RL_thigh_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 
@@ -142,7 +142,7 @@
         <joint name="RL_calf_joint">
             <command_interface name="position"/>
             <command_interface name="velocity"/>
-            <command_interface name="torque"/>
+            <command_interface name="effort"/>
             <command_interface name="kp"/>
             <command_interface name="kd"/>
 

descriptions/quadruped_gazebo/config/jtc.yaml

@@ -13,6 +13,9 @@ controller_manager:
     leg_pd_controller:
       type: leg_pd_controller/LegPdController
 
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
 imu_sensor_broadcaster:
   ros__parameters:
     sensor_name: "imu_sensor"
@@ -20,10 +23,9 @@ imu_sensor_broadcaster:
 
 leg_pd_controller:
   ros__parameters:
-    update_rate: 500  # Hz
     joints:
       - RF_HAA
-      - RH_HFE
+      - RF_HFE
       - RF_KFE
       - LF_HAA
       - LF_HFE
@@ -44,9 +46,10 @@ leg_pd_controller:
 
 unitree_guide_controller:
   ros__parameters:
+    command_prefix: "leg_pd_controller"
     joints:
       - RF_HAA
-      - RH_HFE
+      - RF_HFE
       - RF_KFE
       - LF_HAA
       - LF_HFE
@@ -59,7 +62,7 @@ unitree_guide_controller:
       - LH_KFE
 
     command_interfaces:
-      - torque
+      - effort
       - position
       - velocity
       - kp
@@ -70,6 +73,12 @@ unitree_guide_controller:
       - position
       - velocity
 
+    feet_names:
+      - RF_FOOT
+      - LF_FOOT
+      - RH_FOOT
+      - LH_FOOT
+
     imu_name: "imu_sensor"
 
     imu_interfaces:

descriptions/quadruped_gazebo/launch/gazebo.launch.py

@@ -66,6 +66,11 @@ def launch_setup(context, *args, **kwargs):
                    "--controller-manager", "/controller_manager"],
     )
 
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
+    )
 
     return [
         robot_state_publisher,
@@ -83,6 +88,12 @@ def launch_setup(context, *args, **kwargs):
                 on_exit=[leg_pd_controller],
             )
         ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=leg_pd_controller,
+                on_exit=[unitree_guide_controller],
+            )
+        ),
     ]
 
 

descriptions/quadruped_gazebo/urdf/common/leg.xacro

@@ -8,7 +8,7 @@
 
         <joint name="${prefix}_HAA" type="revolute">
             <xacro:insert_block name="origin"/>
-            <parent link="base"/>
+            <parent link="trunk"/>
             <child link="${prefix}_hip"/>
             <axis xyz="1 0 0"/>
             <dynamics damping="${damping}" friction="${friction}"/>
@@ -40,7 +40,7 @@
                     <mesh filename="${mesh_path}/hip.dae"
                           scale="1 1 1"/>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="${pi/2.0} 0 0" xyz="0 0 0"/>
@@ -81,7 +81,7 @@
                               scale="1 1 1"/>
                     </xacro:if>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="0 ${pi/2.0+0.1} 0" xyz="${thigh_x_offset} 0 ${-thigh_length/2.0}"/>
@@ -121,7 +121,7 @@
                     <mesh filename="${mesh_path}/calf.dae"
                           scale="1 1 1"/>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="0 ${pi/2.0} 0" xyz="${calf_x_offset} 0 ${-calf_length/2.0}"/>
@@ -151,7 +151,7 @@
                 <geometry>
                     <sphere radius="${foot_radius-0.01}"/>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -178,26 +178,45 @@
         <gazebo reference="${prefix}_hip">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
-            <material>Gazebo/DarkGrey</material>
+            <material>
+                <ambient>.175 .175 .175  1.0</ambient>
+                <diffuse>.175 .175 .175  1.0</diffuse>
+                <specular>.175 .175 .175 1.000000 1.500000</specular>
+            </material>
         </gazebo>
         <gazebo reference="${prefix}_thigh">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
             <self_collide>0</self_collide>
-            <material>Gazebo/DarkGrey</material>
+            <material>
+                <ambient>.175 .175 .175  1.0</ambient>
+                <diffuse>.175 .175 .175  1.0</diffuse>
+                <specular>.175 .175 .175 1.000000 1.500000</specular>
+            </material>
             <kp value="1000000.0"/>
             <kd value="100.0"/>
         </gazebo>
+
         <gazebo reference="${prefix}_calf">
             <mu1>0.6</mu1>
             <mu2>0.6</mu2>
             <self_collide>1</self_collide>
+            <material>
+                <ambient>.175 .175 .175  1.0</ambient>
+                <diffuse>.175 .175 .175  1.0</diffuse>
+                <specular>.175 .175 .175 1.000000 1.500000</specular>
+            </material>
         </gazebo>
+
         <gazebo reference="${prefix}_FOOT">
             <mu1>0.6</mu1>
             <mu2>0.6</mu2>
             <self_collide>1</self_collide>
-            <material>Gazebo/DarkGrey</material>
+            <material>
+                <ambient>.175 .175 .175  1.0</ambient>
+                <diffuse>.175 .175 .175  1.0</diffuse>
+                <specular>.175 .175 .175 1.000000 1.500000</specular>
+            </material>
             <kp value="1000000.0"/>
             <kd value="100.0"/>
 

descriptions/quadruped_gazebo/urdf/go1/const.xacro

@@ -128,4 +128,12 @@
     <!-- foot -->
     <xacro:property name="foot_mass" value="0.06"/>
 
+    <gazebo reference="trunk">
+        <material>
+            <ambient>.175 .175 .175  1.0</ambient>
+            <diffuse>.175 .175 .175  1.0</diffuse>
+            <specular>.175 .175 .175 1.000000 1.500000</specular>
+        </material>
+    </gazebo>
+
 </robot>

descriptions/quadruped_gazebo/urdf/robot.xacro

@@ -13,6 +13,21 @@
     <xacro:property name="mesh_path" value="file://$(find quadruped_gazebo)/meshes/$(arg robot_type)"/>
 
     <link name="base">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="floating_base" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="base"/>
+        <child link="trunk"/>
+    </joint>
+
+    <link name="trunk">
         <visual>
             <origin rpy="0 0 0" xyz="0 0 0"/>
             <geometry>
@@ -35,7 +50,7 @@
         </inertial>
     </link>
 
-    <xacro:IMU connected_to="base" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>
+    <xacro:IMU connected_to="trunk" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>
 
 
     <xacro:leg prefix="LF" mesh_path="${mesh_path}" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">

hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp

@@ -100,7 +100,7 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
 
     ind = 0;
     for (const auto &joint_name: joint_interfaces["effort"]) {
-        command_interfaces.emplace_back(joint_name, "torque", &joint_torque_command_[ind]);
+        command_interfaces.emplace_back(joint_name, "effort", &joint_torque_command_[ind]);
         command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]);
         command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]);
         ind++;