diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h index 6dd2f36..bc48ad6 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h +++ b/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 &target_pos, + double kp, + double kd + ); void enter() override; @@ -22,15 +26,12 @@ public: FSMStateName checkChange() override; private: - 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 target_pos_[12] = {}; double start_pos_[12] = {}; rclcpp::Time start_time_; + double kp_, kd_; + double duration_ = 600; // steps double percent_ = 0; //% double phase = 0.0; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h index 0e6f5c3..090b47c 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h +++ b/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 &target_pos, + double kp, + double kd); void enter() override; @@ -22,16 +25,12 @@ public: FSMStateName checkChange() override; private: - 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 target_pos_[12] = {}; double start_pos_[12] = {}; rclcpp::Time start_time_; + double kp_, kd_; + double duration_ = 600; // steps double percent_ = 0; //% double phase = 0.0; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index cc34e06..f23cf5c 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -86,6 +86,24 @@ namespace unitree_guide_controller { std::vector imu_interface_types_; std::vector feet_names_; + // FR FL RR RL + std::vector 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 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::SharedPtr control_input_subscription_; rclcpp::Subscription::SharedPtr robot_description_subscription_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp index 442085c..a361fa3 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp @@ -6,9 +6,16 @@ #include -StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState( - FSMStateName::FIXEDDOWN, "fixed down", ctrlComp) { +StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp, + const std::vector &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); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index d80910e..6b2a604 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp @@ -6,15 +6,27 @@ #include -StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState( - FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) { +StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector &target_pos, + 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); } } diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index e3d7773..e7907cb 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -97,6 +97,12 @@ namespace unitree_guide_controller { feet_names_ = auto_declare >("feet_names", feet_names_); + // pose parameters + down_pos_ = auto_declare >("down_pos", down_pos_); + stand_pos_ = auto_declare >("stand_pos", stand_pos_); + stand_kp_ = auto_declare("stand_kp", stand_kp_); + stand_kd_ = auto_declare("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(ctrl_comp_); - state_list_.fixedDown = std::make_shared(ctrl_comp_); - state_list_.fixedStand = std::make_shared(ctrl_comp_); + state_list_.fixedDown = std::make_shared(ctrl_comp_, down_pos_, stand_kp_, stand_kd_); + state_list_.fixedStand = std::make_shared(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_); state_list_.swingTest = std::make_shared(ctrl_comp_); state_list_.freeStand = std::make_shared(ctrl_comp_); state_list_.balanceTest = std::make_shared(ctrl_comp_); diff --git a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml index efec54f..0c850d4 100644 --- a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml +++ b/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 diff --git a/descriptions/deep_robotics/x30_description/config/robot_control.yaml b/descriptions/deep_robotics/x30_description/config/robot_control.yaml index 7f63839..7364a32 100644 --- a/descriptions/deep_robotics/x30_description/config/robot_control.yaml +++ b/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 diff --git a/descriptions/unitree/aliengo_description/config/robot_control.yaml b/descriptions/unitree/aliengo_description/config/robot_control.yaml index fd150a0..691eb6e 100644 --- a/descriptions/unitree/aliengo_description/config/robot_control.yaml +++ b/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 diff --git a/descriptions/unitree/b2_description/config/robot_control.yaml b/descriptions/unitree/b2_description/config/robot_control.yaml index e7ff6fb..c2f0224 100644 --- a/descriptions/unitree/b2_description/config/robot_control.yaml +++ b/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 diff --git a/descriptions/unitree/go1_description/config/robot_control.yaml b/descriptions/unitree/go1_description/config/robot_control.yaml index d6f4b1f..d8e3494 100644 --- a/descriptions/unitree/go1_description/config/robot_control.yaml +++ b/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 diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 082dec5..fd37db0 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/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 diff --git a/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml b/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml index 4fc93a7..700db87 100644 --- a/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml +++ b/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: -# type: leg_pd_controller/LegPdController -# -# unitree_guide_controller: -# 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: -# ros__parameters: -# joints: -# - FR_abad_joint -# - FR_hip_joint -# - FR_knee_joint -# - FL_abad_joint -# - FL_hip_joint -# - FL_knee_joint -# - RR_abad_joint -# - RR_hip_joint -# - RR_knee_joint -# - RL_abad_joint -# - RL_hip_joint -# - RL_knee_joint -# -# command_interfaces: -# - effort -# -# state_interfaces: -# - position -# - velocity -# -#unitree_guide_controller: -# ros__parameters: -# command_prefix: "leg_pd_controller" -# joints: -# - FR_abad_joint -# - FR_hip_joint -# - FR_knee_joint -# - FL_abad_joint -# - FL_hip_joint -# - FL_knee_joint -# - RR_abad_joint -# - RR_hip_joint -# - RR_knee_joint -# - RL_abad_joint -# - RL_hip_joint -# - RL_knee_joint -# -# command_interfaces: -# - effort -# - position -# - velocity -# - kp -# - kd -# -# state_interfaces: -# - effort -# - 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 +unitree_guide_controller: + ros__parameters: + update_rate: 500 # Hz + joints: + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - FR_foot + - FL_foot + - RR_foot + - RL_foot + + imu_name: "imu_sensor" + base_name: "base" + + 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: