From ac6e2278a174bd2c63492c2a7b2b705c8178babd Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Wed, 18 Sep 2024 22:19:37 +0800 Subject: [PATCH] fix joystick direction --- commands/joystick_input/readme.md | 2 +- commands/joystick_input/src/JoystickInput.cpp | 4 ++-- .../unitree_guide_controller/src/FSM/StateBalanceTest.cpp | 8 +++++--- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/commands/joystick_input/readme.md b/commands/joystick_input/readme.md index a82e1ca..123ea36 100644 --- a/commands/joystick_input/readme.md +++ b/commands/joystick_input/readme.md @@ -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 diff --git a/commands/joystick_input/src/JoystickInput.cpp b/commands/joystick_input/src/JoystickInput.cpp index 8a6d8ad..5a28926 100644 --- a/commands/joystick_input/src/JoystickInput.cpp +++ b/commands/joystick_input/src/JoystickInput.cpp @@ -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_); diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 7ec83dd..6885399 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -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 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));