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

add pos parameter for unitree guide controller

This commit is contained in:
Huang Zhenbiao 2024-10-21 12:43:17 +08:00
parent 4575779eb5
commit ca616a8c87
13 changed files with 192 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h
View File

@ -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] = {
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;

15
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
View File

@ -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] = {
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;

18
controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
View File

@ -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_;

21
controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
View File

@ -6,9 +6,16 @@
#include <cmath>
StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState(
FSMStateName::FIXEDDOWN, "fixed down", ctrlComp) {
StateFixedDown::StateFixedDown(CtrlComponent &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);
}
}

21
controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
View File

@ -6,15 +6,27 @@
#include <cmath>
StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState(
FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) {
StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &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);
}
}

10
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -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_.fixedStand = std::make_shared<StateFixedStand>(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_, 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_);

32
descriptions/deep_robotics/lite3_description/config/robot_control.yaml
View File

@ -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

35
descriptions/deep_robotics/x30_description/config/robot_control.yaml
View File

@ -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

2
descriptions/unitree/aliengo_description/config/robot_control.yaml
View File

@ -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

2
descriptions/unitree/b2_description/config/robot_control.yaml
View File

@ -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

4
descriptions/unitree/go1_description/config/robot_control.yaml
View File

@ -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

4
descriptions/unitree/go2_description/config/robot_control.yaml
View File

@ -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

128
descriptions/xiaomi/cyberdog_description/config/robot_control.yaml
View File

@ -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: