From 78683363804827b6210cfe61c86ff6430c3f1077 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Fri, 28 Mar 2025 10:02:39 +0800 Subject: [PATCH] rollback to ros2 humble --- .../src/FSM/StateOCS2.cpp | 10 +++++----- .../rl_quadruped_controller/src/FSM/StateRL.cpp | 10 +++++----- .../launch/gazebo_classic.launch.py | 2 +- .../src/FSM/StateBalanceTest.cpp | 8 ++++---- .../src/FSM/StateFreeStand.cpp | 10 +++++----- .../src/FSM/StateSwingTest.cpp | 16 ++++++++-------- .../src/FSM/StateTrotting.cpp | 14 +++++++------- .../controller_common/src/FSM/BaseFixedStand.cpp | 14 +++++++------- .../controller_common/src/FSM/StateFixedDown.cpp | 14 +++++++------- .../controller_common/src/FSM/StatePassive.cpp | 10 +++++----- 10 files changed, 54 insertions(+), 54 deletions(-) diff --git a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp index f60df3c..550e91f 100644 --- a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp +++ b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp @@ -79,11 +79,11 @@ namespace ocs2::legged_robot for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque(i)); - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(pos_des(i)); - std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(vel_des(i)); - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(default_kp_); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(default_kd_); + ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque(i)); + ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(pos_des(i)); + ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(vel_des(i)); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(default_kp_); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(default_kd_); } // Visualization diff --git a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp index ccee726..b7bf2fb 100644 --- a/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp +++ b/controllers/rl_quadruped_controller/src/FSM/StateRL.cpp @@ -391,16 +391,16 @@ void StateRL::setCommand() const { for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get(). + ctrl_interfaces_.joint_position_command_interface_[i].get(). set_value( robot_command_.motor_command.q[i]); - std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value( + ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value( robot_command_.motor_command.dq[i]); - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value( + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value( robot_command_.motor_command.kp[i]); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value( + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value( robot_command_.motor_command.kd[i]); - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get(). + ctrl_interfaces_.joint_torque_command_interface_[i].get(). set_value( robot_command_.motor_command.tau[i]); } diff --git a/controllers/unitree_guide_controller/launch/gazebo_classic.launch.py b/controllers/unitree_guide_controller/launch/gazebo_classic.launch.py index 7b6259e..de496d5 100644 --- a/controllers/unitree_guide_controller/launch/gazebo_classic.launch.py +++ b/controllers/unitree_guide_controller/launch/gazebo_classic.launch.py @@ -102,7 +102,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): pkg_description = DeclareLaunchArgument( 'pkg_description', - default_value='go2_description', + default_value='go1_description', description='package for robot description' ) diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 9cafb49..1ee3dbd 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -51,8 +51,8 @@ void StateBalanceTest::run(const rclcpp::Time &/*time*/, const rclcpp::Duration Rd_ = rotz(yaw) * init_rotation_; for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.8); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(0.8); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.8); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(0.8); } calcTorque(); @@ -97,8 +97,8 @@ void StateBalanceTest::calcTorque() { for (int i = 0; i < 4; i++) { KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i); for (int j = 0; j < 3; j++) { - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j)); - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + j].get().set_value( + ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j)); + ctrl_interfaces_.joint_position_command_interface_[i * 3 + j].get().set_value( current_joints[i](j)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index 9359277..3071ee1 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp @@ -26,8 +26,8 @@ StateFreeStand::StateFreeStand(CtrlInterfaces &ctrl_interfaces, void StateFreeStand::enter() { for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(100); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(100); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5); } init_joint_pos_ = robot_model_->current_joint_pos_; @@ -78,11 +78,11 @@ void StateFreeStand::calc_body_target(const float row, const float pitch, target_joint_pos_ = robot_model_->getQ(goal_pos); for (int i = 0; i < 4; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value( + ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value( target_joint_pos_[i](0)); - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value( + ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value( target_joint_pos_[i](1)); - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value( + ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value( target_joint_pos_[i](2)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index 949ad47..4241017 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -25,12 +25,12 @@ StateSwingTest::StateSwingTest(CtrlInterfaces &ctrl_interfaces, void StateSwingTest::enter() { for (int i = 0; i < 3; i++) { - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(3); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(2); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(3); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(2); } for (int i = 3; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(180); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(180); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5); } Kp = KDL::Vector(20, 20, 50); @@ -89,9 +89,9 @@ void StateSwingTest::positionCtrl() { target_foot_pos_[0] = fr_goal_pos_; target_joint_pos_ = robot_model_->getQ(target_foot_pos_); for (int i = 0; i < 4; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2)); + ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); + ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); + ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2)); } } @@ -106,6 +106,6 @@ void StateSwingTest::torqueCtrl() const { KDL::JntArray torque0 = robot_model_->getTorque(force0, 0); for (int i = 0; i < 3; i++) { - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque0(i)); + ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque0(i)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index 0602dec..8e0ef03 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp @@ -155,7 +155,7 @@ void StateTrotting::calcTau() { for (int i = 0; i < 4; i++) { KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i); for (int j = 0; j < 3; j++) { - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j)); + ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j)); } } } @@ -172,8 +172,8 @@ void StateTrotting::calcQQd() { Vec12 q_goal = robot_model_->getQ(pos_feet_target); Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target); for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(q_goal(i)); - std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i)); + ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(q_goal(i)); + ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i)); } } @@ -182,14 +182,14 @@ void StateTrotting::calcGain() const { if (wave_generator_->contact_(i) == 0) { // swing gain for (int j = 0; j < 3; j++) { - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(3); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(2); + ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(3); + ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(2); } } else { // stable gain for (int j = 0; j < 3; j++) { - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8); + ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8); + ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8); } } } diff --git a/libraries/controller_common/src/FSM/BaseFixedStand.cpp b/libraries/controller_common/src/FSM/BaseFixedStand.cpp index c5c653e..6761b1d 100644 --- a/libraries/controller_common/src/FSM/BaseFixedStand.cpp +++ b/libraries/controller_common/src/FSM/BaseFixedStand.cpp @@ -23,15 +23,15 @@ void BaseFixedStand::enter() { for (int i = 0; i < 12; i++) { - start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().value(); + start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value(); } for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); - std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0); - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_); + ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); + ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0); + ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_); } ctrl_interfaces_.control_inputs_.command = 0; } @@ -42,7 +42,7 @@ void BaseFixedStand::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*p phase = std::tanh(percent_); for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value( + ctrl_interfaces_.joint_position_command_interface_[i].get().set_value( phase * target_pos_[i] + (1 - phase) * start_pos_[i]); } } diff --git a/libraries/controller_common/src/FSM/StateFixedDown.cpp b/libraries/controller_common/src/FSM/StateFixedDown.cpp index 7faa476..e979dd3 100644 --- a/libraries/controller_common/src/FSM/StateFixedDown.cpp +++ b/libraries/controller_common/src/FSM/StateFixedDown.cpp @@ -24,16 +24,16 @@ void StateFixedDown::enter() { for (int i = 0; i < 12; i++) { - start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().value(); + start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value(); } ctrl_interfaces_.control_inputs_.command = 0; for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); - std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0); - std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); - std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_); - std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_); + ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); + ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0); + ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); + ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_); + ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_); } } @@ -43,7 +43,7 @@ void StateFixedDown::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*p phase = std::tanh(percent_); for (int i = 0; i < 12; i++) { - std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value( + ctrl_interfaces_.joint_position_command_interface_[i].get().set_value( phase * target_pos_[i] + (1 - phase) * start_pos_[i]); } } diff --git a/libraries/controller_common/src/FSM/StatePassive.cpp b/libraries/controller_common/src/FSM/StatePassive.cpp index ede8c5f..0c8580a 100644 --- a/libraries/controller_common/src/FSM/StatePassive.cpp +++ b/libraries/controller_common/src/FSM/StatePassive.cpp @@ -15,23 +15,23 @@ void StatePassive::enter() { for (auto i : ctrl_interfaces_.joint_torque_command_interface_) { - std::ignore = i.get().set_value(0); + i.get().set_value(0); } for (auto i : ctrl_interfaces_.joint_position_command_interface_) { - std::ignore = i.get().set_value(0); + i.get().set_value(0); } for (auto i : ctrl_interfaces_.joint_velocity_command_interface_) { - std::ignore = i.get().set_value(0); + i.get().set_value(0); } for (auto i : ctrl_interfaces_.joint_kp_command_interface_) { - std::ignore = i.get().set_value(0); + i.get().set_value(0); } for (auto i : ctrl_interfaces_.joint_kd_command_interface_) { - std::ignore = i.get().set_value(1); + i.get().set_value(1); } ctrl_interfaces_.control_inputs_.command = 0; }