add pos parameter for unitree guide controller

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h

@@ -11,7 +11,11 @@
 
 class StateFixedDown final : public FSMState {
 public:
-    explicit StateFixedDown(CtrlComponent &ctrlComp);
+    explicit StateFixedDown(CtrlComponent &ctrlComp,
+                            const std::vector<double> &target_pos,
+                            double kp,
+                            double kd
+    );
 
     void enter() override;
 
@@ -22,15 +26,12 @@ public:
     FSMStateName checkChange() override;
 
 private:
-    double target_pos_[12] = {
+    double target_pos_[12] = {};
-        0.0473455, 1.22187, -2.44375, -0.0473455,
-        1.22187, -2.44375, 0.0473455, 1.22187,
-        -2.44375, -0.0473455, 1.22187, -2.44375
-    };
-
     double start_pos_[12] = {};
     rclcpp::Time start_time_;
 
+    double kp_, kd_;
+
     double duration_ = 600; // steps
     double percent_ = 0; //%
     double phase = 0.0;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h

@@ -11,7 +11,10 @@
 
 class StateFixedStand final : public FSMState {
 public:
-    explicit StateFixedStand(CtrlComponent &ctrlComp);
+    explicit StateFixedStand(CtrlComponent &ctrlComp,
+                             const std::vector<double> &target_pos,
+                             double kp,
+                             double kd);
 
     void enter() override;
 
@@ -22,16 +25,12 @@ public:
     FSMStateName checkChange() override;
 
 private:
-    double target_pos_[12] = {
+    double target_pos_[12] = {};
-        0.00571868, 0.608813, -1.21763,
-        -0.00571868, 0.608813, -1.21763,
-        0.00571868, 0.608813, -1.21763,
-        -0.00571868, 0.608813, -1.21763
-    };
-
     double start_pos_[12] = {};
     rclcpp::Time start_time_;
 
+    double kp_, kd_;
+
     double duration_ = 600; // steps
     double percent_ = 0; //%
     double phase = 0.0;

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -86,6 +86,24 @@ namespace unitree_guide_controller {
         std::vector<std::string> imu_interface_types_;
         std::vector<std::string> feet_names_;
 
+        // FR FL RR RL
+        std::vector<double> stand_pos_ = {
+            0.0, 0.67, -1.3,
+            0.0, 0.67, -1.3,
+            0.0, 0.67, -1.3,
+            0.0, 0.67, -1.3
+        };
+
+        std::vector<double> down_pos_ = {
+            0.0, 1.3, -2.4,
+            0.0, 1.3, -2.4,
+            0.0, 1.3, -2.4,
+            0.0, 1.3, -2.4
+        };
+
+        double stand_kp_ = 80.0;
+        double stand_kd_ = 3.5;
+
         rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
         rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
 

controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp

@@ -6,9 +6,16 @@
 
 #include <cmath>
 
-StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState(
+StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp,
-    FSMStateName::FIXEDDOWN, "fixed down", ctrlComp) {
+                               const std::vector<double> &target_pos,
+                               const double kp,
+                               const double kd)
+    : FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrlComp),
+      kp_(kp), kd_(kd) {
     duration_ = ctrl_comp_.frequency_ * 1.2;
+    for (int i = 0; i < 12; i++) {
+        target_pos_[i] = target_pos[i];
+    }
 }
 
 void StateFixedDown::enter() {
@@ -16,6 +23,12 @@ void StateFixedDown::enter() {
         start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
     }
     ctrl_comp_.control_inputs_.command = 0;
+    for (int i = 0; i < 12; i++) {
+        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
+    }
 }
 
 void StateFixedDown::run() {
@@ -24,10 +37,6 @@ void StateFixedDown::run() {
     for (int i = 0; i < 12; i++) {
         ctrl_comp_.joint_position_command_interface_[i].get().set_value(
             phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
-        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(30.0);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.5);
     }
 }
 

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -6,15 +6,27 @@
 
 #include <cmath>
 
-StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState(
+StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &target_pos,
-    FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) {
+                                 const double kp,
+                                 const double kd)
+    : FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp),
+      kp_(kp), kd_(kd) {
     duration_ = ctrl_comp_.frequency_ * 1.2;
+    for (int i = 0; i < 12; i++) {
+        target_pos_[i] = target_pos[i];
+    }
 }
 
 void StateFixedStand::enter() {
     for (int i = 0; i < 12; i++) {
         start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
     }
+    for (int i = 0; i < 12; i++) {
+        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
+    }
     ctrl_comp_.control_inputs_.command = 0;
 }
 
@@ -24,11 +36,6 @@ void StateFixedStand::run() {
     for (int i = 0; i < 12; i++) {
         ctrl_comp_.joint_position_command_interface_[i].get().set_value(
             phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
-        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(
-            phase * 60.0 + (1 - phase) * 20.0);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
     }
 }
 

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -97,6 +97,12 @@ namespace unitree_guide_controller {
             feet_names_ =
                     auto_declare<std::vector<std::string> >("feet_names", feet_names_);
 
+            // pose parameters
+            down_pos_ = auto_declare<std::vector<double> >("down_pos", down_pos_);
+            stand_pos_ = auto_declare<std::vector<double> >("stand_pos", stand_pos_);
+            stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
+            stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
+
             get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
             RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
 
@@ -160,8 +166,8 @@ namespace unitree_guide_controller {
 
         // Create FSM List
         state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
-        state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);
+        state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_);
-        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
+        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_);
         state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
         state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
         state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);

descriptions/deep_robotics/lite3_description/config/robot_control.yaml

@@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
 
     # Define the available controllers
     joint_state_broadcaster:
@@ -27,7 +27,7 @@ imu_sensor_broadcaster:
     
 unitree_guide_controller:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
     joints:
       - FR_HipX
       - FR_HipY
@@ -42,6 +42,34 @@ unitree_guide_controller:
       - HL_HipY
       - HL_Knee
 
+    down_pos:
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+
+    stand_pos:
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+
     command_interfaces:
       - effort
       - position

descriptions/deep_robotics/x30_description/config/robot_control.yaml

@@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
 
     # Define the available controllers
     joint_state_broadcaster:
@@ -27,7 +27,7 @@ imu_sensor_broadcaster:
     
 unitree_guide_controller:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
     joints:
       - FR_HipX
       - FR_HipY
@@ -42,6 +42,37 @@ unitree_guide_controller:
       - HL_HipY
       - HL_Knee
 
+    stand_kp: 500.0
+    stand_kd: 20.0
+
+    down_pos:
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+
+    stand_pos:
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+
     command_interfaces:
       - effort
       - position

descriptions/unitree/aliengo_description/config/robot_control.yaml

@@ -24,6 +24,8 @@ imu_sensor_broadcaster:
 unitree_guide_controller:
   ros__parameters:
     update_rate: 500  # Hz
+    stand_kp: 100.0
+    stand_kd: 8.0
     joints:
       - FR_hip_joint
       - FR_thigh_joint

descriptions/unitree/b2_description/config/robot_control.yaml

@@ -24,6 +24,8 @@ imu_sensor_broadcaster:
 unitree_guide_controller:
   ros__parameters:
     update_rate: 500  # Hz
+    stand_kp: 500.0
+    stand_kd: 60.0
     joints:
       - FR_hip_joint
       - FR_thigh_joint

descriptions/unitree/go1_description/config/robot_control.yaml

@@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
 
     # Define the available controllers
     joint_state_broadcaster:
@@ -23,7 +23,7 @@ imu_sensor_broadcaster:
 
 unitree_guide_controller:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint

descriptions/unitree/go2_description/config/robot_control.yaml

@@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 1000  # Hz
+    update_rate: 200  # Hz
 
     # Define the available controllers
     joint_state_broadcaster:
@@ -26,7 +26,7 @@ imu_sensor_broadcaster:
 
 unitree_guide_controller:
   ros__parameters:
-    update_rate: 500  # Hz
+    update_rate: 200  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint

descriptions/xiaomi/cyberdog_description/config/robot_control.yaml

@@ -10,11 +10,8 @@ controller_manager:
     imu_sensor_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
-#    leg_pd_controller:
+    unitree_guide_controller:
-#      type: leg_pd_controller/LegPdController
+      type: unitree_guide_controller/UnitreeGuideController
-#
-#    unitree_guide_controller:
-#      type: unitree_guide_controller/UnitreeGuideController
 
     ocs2_quadruped_controller:
       type: ocs2_quadruped_controller/Ocs2QuadrupedController
@@ -24,78 +21,55 @@ imu_sensor_broadcaster:
     sensor_name: "imu_sensor"
     frame_id: "imu_link"
 
-#leg_pd_controller:
+unitree_guide_controller:
-#  ros__parameters:
+  ros__parameters:
-#    joints:
+    update_rate: 500  # Hz
-#      - FR_abad_joint
+    joints:
-#      - FR_hip_joint
+      - FR_hip_joint
-#      - FR_knee_joint
+      - FR_thigh_joint
-#      - FL_abad_joint
+      - FR_calf_joint
-#      - FL_hip_joint
+      - FL_hip_joint
-#      - FL_knee_joint
+      - FL_thigh_joint
-#      - RR_abad_joint
+      - FL_calf_joint
-#      - RR_hip_joint
+      - RR_hip_joint
-#      - RR_knee_joint
+      - RR_thigh_joint
-#      - RL_abad_joint
+      - RR_calf_joint
-#      - RL_hip_joint
+      - RL_hip_joint
-#      - RL_knee_joint
+      - RL_thigh_joint
-#
+      - RL_calf_joint
-#    command_interfaces:
+
-#      - effort
+    command_interfaces:
-#
+      - effort
-#    state_interfaces:
+      - position
-#      - position
+      - velocity
-#      - velocity
+      - kp
-#
+      - kd
-#unitree_guide_controller:
+
-#  ros__parameters:
+    state_interfaces:
-#    command_prefix: "leg_pd_controller"
+      - effort
-#    joints:
+      - position
-#      - FR_abad_joint
+      - velocity
-#      - FR_hip_joint
+
-#      - FR_knee_joint
+    feet_names:
-#      - FL_abad_joint
+      - FR_foot
-#      - FL_hip_joint
+      - FL_foot
-#      - FL_knee_joint
+      - RR_foot
-#      - RR_abad_joint
+      - RL_foot
-#      - RR_hip_joint
+
-#      - RR_knee_joint
+    imu_name: "imu_sensor"
-#      - RL_abad_joint
+    base_name: "base"
-#      - RL_hip_joint
+
-#      - RL_knee_joint
+    imu_interfaces:
-#
+      - orientation.w
-#    command_interfaces:
+      - orientation.x
-#      - effort
+      - orientation.y
-#      - position
+      - orientation.z
-#      - velocity
+      - angular_velocity.x
-#      - kp
+      - angular_velocity.y
-#      - kd
+      - angular_velocity.z
-#
+      - linear_acceleration.x
-#    state_interfaces:
+      - linear_acceleration.y
-#      - effort
+      - linear_acceleration.z
-#      - position
-#      - velocity
-#
-#    feet_names:
-#      - FR_foot
-#      - FL_foot
-#      - RR_foot
-#      - RL_foot
-#
-#    imu_name: "imu_sensor"
-#    base_name: "base_link"
-#
-#    imu_interfaces:
-#      - orientation.w
-#      - orientation.x
-#      - orientation.y
-#      - orientation.z
-#      - angular_velocity.x
-#      - angular_velocity.y
-#      - angular_velocity.z
-#      - linear_acceleration.x
-#      - linear_acceleration.y
-#      - linear_acceleration.z
 
 
 ocs2_quadruped_controller: