From 1ec5910f1c0d2f8595dbff7bb22d3e6623ab8c3b Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Sun, 18 May 2025 01:14:08 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BF=90=E5=8A=A8=E5=AD=A6=E8=A7=A3=E7=AE=97?= =?UTF-8?q?=E6=AD=A3=E7=A1=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 ++- .../src/FSM/state_balance.cpp | 10 +++++-- .../src/common/kinematics.cpp | 29 ++++++++++++------- .../src/mdog_simple_controller.cpp | 9 ++++-- .../qut/mdog_description/xacro/leg.xacro | 4 +-- 5 files changed, 36 insertions(+), 20 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 23e9642..a92bb05 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,6 +2,8 @@ "files.associations": { "deque": "cpp", "string": "cpp", - "vector": "cpp" + "vector": "cpp", + "chrono": "cpp", + "array": "cpp" } } \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp index 4530634..b33b079 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp @@ -19,9 +19,9 @@ void StateBalance::enter(MdogSimpleController* ctrl) { void StateBalance::run(MdogSimpleController* ctrl) { // 贝塞尔控制点 std::vector> control_points = { - {0.0, 0.0, -0.30}, // 起点 - {0.05, 0.0, 0.05}, // 抬腿中点 - {0.10, 0.0, -0.30} // 落点 + {0.30, 0.0, 0.15}, // 起点 + {0.10, 0.0, 0.0}, // 抬腿中点 + {0.30, 0.0, -0.15}, // 落点 }; // 起点和终点 const auto& p0 = control_points.front(); @@ -59,6 +59,10 @@ void StateBalance::run(MdogSimpleController* ctrl) { std::array link = {0.08, 0.25, 0.25}; // 根据实际参数 bool ok = kinematics::inverse_kinematics(foot_pos, link, theta); if (ok) { + // 左右腿hip方向相反，FR(0) RR(2)为右腿，FL(1) RL(3)为左腿 + if (leg == 1 || leg == 3) { // 右腿 + theta[0] = -theta[0]; + } for (int j = 0; j < 3; ++j) { ctrl->data_.command_real[leg].pos_des[j] = theta[j]; ctrl->data_.command_real[leg].speed_des[j] = 0; diff --git a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp index fc9e6fe..f476ddb 100644 --- a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp +++ b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp @@ -4,41 +4,48 @@ namespace mdog_simple_controller { namespace kinematics { +// 正运动学：已知关节角，求末端位置 void forward_kinematics(const std::array& theta, const std::array& link, std::array& pos) { + // theta: [hip_yaw, hip_pitch, knee_pitch] + // link: [hip_offset(y方向), thigh_length(x方向), shank_length(x方向)] double t1 = theta[0], t2 = theta[1], t3 = theta[2]; double l1 = link[0], l2 = link[1], l3 = link[2]; double x = (l2 * cos(t2) + l3 * cos(t2 + t3)) * cos(t1); - double y = (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1); - double z = l1 + l2 * sin(t2) + l3 * sin(t2 + t3); + double y = l1 + (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1); + double z = l2 * sin(t2) + l3 * sin(t2 + t3); pos[0] = x; pos[1] = y; pos[2] = z; } +// 逆运动学：已知末端位置，求关节角 bool inverse_kinematics(const std::array& pos, const std::array& link, std::array& theta) { double x = pos[0], y = pos[1], z = pos[2]; double l1 = link[0], l2 = link[1], l3 = link[2]; - theta[0] = atan2(y, x); + double y1 = y - l1; + double t1 = atan2(y1, x); - double d = sqrt(x * x + y * y); - double z1 = z - l1; + double d = sqrt(x * x + y1 * y1); + double z1 = z; double D = (d * d + z1 * z1 - l2 * l2 - l3 * l3) / (2 * l2 * l3); - if (D < -1.0 || D > 1.0) return false; // 不可达 + if (D < -1.0 || D > 1.0) return false; - theta[2] = atan2(-sqrt(1 - D * D), D); - - double phi = atan2(z1, d); - double psi = atan2(l3 * sin(theta[2]), l2 + l3 * cos(theta[2])); - theta[1] = phi - psi; + double t3 = atan2(-sqrt(1 - D * D), D); + double k1 = l2 + l3 * cos(t3); + double k2 = l3 * sin(t3); + double t2 = atan2(z1, d) - atan2(k2, k1); + theta[0] = t1; + theta[1] = t2; + theta[2] = t3; return true; } diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp index 7898781..f83c942 100644 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp @@ -18,7 +18,7 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") { joint_state_pub_ = this->create_publisher("joint_states", 10); if_no_hardware_ = this->declare_parameter("if_no_hardware", false); - + memset(&data_, 0, sizeof(data_)); current_state_ = &state_zero_; current_state_->enter(this); timer_ = this->create_wall_timer( @@ -77,9 +77,11 @@ void MdogSimpleController::control_loop() { data_.feedback[leg].torque[j] = data_.command[leg].torque_des[j]; data_.feedback[leg].speed[j] = data_.command[leg].speed_des[j]; data_.feedback[leg].pos[j] = data_.command[leg].pos_des[j]; + // data_.feedback_real[leg].torque[j] = data_.command[leg].torque_des[j]; + // data_.feedback_real[leg].speed[j] = data_.command[leg].speed_des[j]; + // data_.feedback_real[leg].pos[j] = data_.command[leg].pos_des[j]; } } - // 若有feedback_to_real处理，也可调用 feedback_to_real(data_, data_); } @@ -113,7 +115,8 @@ void MdogSimpleController::control_loop() { } RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d", input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status); - sensor_msgs::msg::JointState js_msg; + + sensor_msgs::msg::JointState js_msg; js_msg.header.stamp = this->now(); js_msg.name = { "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", diff --git a/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro b/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro index 090ece8..0859c21 100644 --- a/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro +++ b/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro @@ -35,8 +35,8 @@ - - + +