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

fix joystick direction

This commit is contained in:
Huang Zhenbiao 2024-09-18 22:19:37 +08:00
parent af6599e904
commit ac6e2278a1
3 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
commands/joystick_input/readme.md
View File

@ -9,7 +9,7 @@ colcon build --packages-up-to joystick_input
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch jotstick_input joystick.launch.py
ros2 launch joystick_input joystick.launch.py
```
## 1. Use Instructions

4
commands/joystick_input/src/JoystickInput.cpp
View File

@ -29,9 +29,9 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
inputs_.command = 8; // LT + Y
} else {
inputs_.command = 0;
inputs_.lx = msg->axes[0];
inputs_.lx = -msg->axes[0];
inputs_.ly = msg->axes[1];
inputs_.rx = msg->axes[3];
inputs_.rx = -msg->axes[3];
inputs_.ry = msg->axes[4];
}
publisher_->publish(inputs_);

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

@ -80,12 +80,14 @@ void StateBalanceTest::calcTorque() {
Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGlobalGyro());
// calculate foot force
const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
estimator_.getFeetPos2Body(), wave_generator_.contact_);
const Vec34 pos_feet_2_body_global = estimator_.getFeetPos2Body();
const Vec34 force_feet_global = -balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
pos_feet_2_body_global, wave_generator_.contact_);
const Vec34 force_feet_body = G2B_Rotation * force_feet_global;
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
for (int i = 0; i < 4; i++) {
KDL::JntArray torque = robot_model_.getTorque(-foot_force.col(i), i);
KDL::JntArray torque = robot_model_.getTorque(force_feet_body.col(i), i);
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));