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++)
{
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

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++)
{
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]);
}

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():
pkg_description = DeclareLaunchArgument(
'pkg_description',
default_value='go2_description',
default_value='go1_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_;
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));
}
}

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

@ -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));
}
}

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

@ -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));
}
}

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++) {
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);
}
}
}

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

@ -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]);
}
}

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

@ -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]);
}
}

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_)
{
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;
}