EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

rollback to ros2 humble

This commit is contained in:
Huang Zhenbiao 2025-03-28 10:02:39 +08:00
parent ff2d9e1928
commit 7868336380
10 changed files with 54 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp
View File

@ -79,11 +79,11 @@ namespace ocs2::legged_robot
for (int i = 0; i < 12; i++) for (int i = 0; i < 12; i++)
{ {
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque(i)); 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)); 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)); 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_); 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_kd_command_interface_[i].get().set_value(default_kd_);
} }
// Visualization // Visualization

10
controllers/rl_quadruped_controller/src/FSM/StateRL.cpp
View File

@ -391,16 +391,16 @@ void StateRL::setCommand() const
{ {
for (int i = 0; i < 12; i++) 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( set_value(
robot_command_.motor_command.q[i]); 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]); 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]); 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]); 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( set_value(
robot_command_.motor_command.tau[i]); robot_command_.motor_command.tau[i]);
} }

2
controllers/unitree_guide_controller/launch/gazebo_classic.launch.py
View File

@ -102,7 +102,7 @@ def launch_setup(context, *args, **kwargs):
def generate_launch_description(): def generate_launch_description():
pkg_description = DeclareLaunchArgument( pkg_description = DeclareLaunchArgument(
'pkg_description', 'pkg_description',
default_value='go2_description', default_value='go1_description',
description='package for robot description' description='package for robot description'
) )

8
controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
View File

@ -51,8 +51,8 @@ void StateBalanceTest::run(const rclcpp::Time &/*time*/, const rclcpp::Duration
Rd_ = rotz(yaw) * init_rotation_; Rd_ = rotz(yaw) * init_rotation_;
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.8); 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_kd_command_interface_[i].get().set_value(0.8);
} }
calcTorque(); calcTorque();
@ -97,8 +97,8 @@ void StateBalanceTest::calcTorque() {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i); KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i);
for (int j = 0; j < 3; j++) { 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));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + j].get().set_value( ctrl_interfaces_.joint_position_command_interface_[i * 3 + j].get().set_value(
current_joints[i](j)); current_joints[i](j));
} }
} }

10
controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
View File

@ -26,8 +26,8 @@ StateFreeStand::StateFreeStand(CtrlInterfaces &ctrl_interfaces,
void StateFreeStand::enter() { void StateFreeStand::enter() {
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(100); 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_kd_command_interface_[i].get().set_value(5);
} }
init_joint_pos_ = robot_model_->current_joint_pos_; 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); target_joint_pos_ = robot_model_->getQ(goal_pos);
for (int i = 0; i < 4; i++) { 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)); 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)); 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)); target_joint_pos_[i](2));
} }
} }

16
controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
View File

@ -25,12 +25,12 @@ StateSwingTest::StateSwingTest(CtrlInterfaces &ctrl_interfaces,
void StateSwingTest::enter() { void StateSwingTest::enter() {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(3); 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_kd_command_interface_[i].get().set_value(2);
} }
for (int i = 3; i < 12; i++) { for (int i = 3; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(180); 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_kd_command_interface_[i].get().set_value(5);
} }
Kp = KDL::Vector(20, 20, 50); Kp = KDL::Vector(20, 20, 50);
@ -89,9 +89,9 @@ void StateSwingTest::positionCtrl() {
target_foot_pos_[0] = fr_goal_pos_; target_foot_pos_[0] = fr_goal_pos_;
target_joint_pos_ = robot_model_->getQ(target_foot_pos_); target_joint_pos_ = robot_model_->getQ(target_foot_pos_);
for (int i = 0; i < 4; i++) { 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)); 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)); 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 + 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); KDL::JntArray torque0 = robot_model_->getTorque(force0, 0);
for (int i = 0; i < 3; i++) { 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));
} }
} }

14
controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
View File

@ -155,7 +155,7 @@ void StateTrotting::calcTau() {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i); KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i);
for (int j = 0; j < 3; j++) { 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 q_goal = robot_model_->getQ(pos_feet_target);
Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target); Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target);
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(q_goal(i)); 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_velocity_command_interface_[i].get().set_value(qd_goal(i));
} }
} }
@ -182,14 +182,14 @@ void StateTrotting::calcGain() const {
if (wave_generator_->contact_(i) == 0) { if (wave_generator_->contact_(i) == 0) {
// swing gain // swing gain
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(3); 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_kd_command_interface_[i * 3 + j].get().set_value(2);
} }
} else { } else {
// stable gain // stable gain
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8); 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_kd_command_interface_[i * 3 + j].get().set_value(0.8);
} }
} }
} }

14
libraries/controller_common/src/FSM/BaseFixedStand.cpp
View File

@ -23,15 +23,15 @@ void BaseFixedStand::enter()
{ {
for (int i = 0; i < 12; i++) 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++) for (int i = 0; i < 12; i++)
{ {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); 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); ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_); 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_kd_command_interface_[i].get().set_value(kd_);
} }
ctrl_interfaces_.control_inputs_.command = 0; 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_); phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) 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]); phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
} }
} }

14
libraries/controller_common/src/FSM/StateFixedDown.cpp
View File

@ -24,16 +24,16 @@ void StateFixedDown::enter()
{ {
for (int i = 0; i < 12; i++) 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; ctrl_interfaces_.control_inputs_.command = 0;
for (int i = 0; i < 12; i++) for (int i = 0; i < 12; i++)
{ {
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); 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); ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_); 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_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_); phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) 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]); phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
} }
} }

10
libraries/controller_common/src/FSM/StatePassive.cpp
View File

@ -15,23 +15,23 @@ void StatePassive::enter()
{ {
for (auto i : ctrl_interfaces_.joint_torque_command_interface_) 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_) 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_) 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_) 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_) 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; ctrl_interfaces_.control_inputs_.command = 0;
} }