运动学解算正确
This commit is contained in:
parent
cca513ae0b
commit
1ec5910f1c
4
.vscode/settings.json
vendored
4
.vscode/settings.json
vendored
@ -2,6 +2,8 @@
|
||||
"files.associations": {
|
||||
"deque": "cpp",
|
||||
"string": "cpp",
|
||||
"vector": "cpp"
|
||||
"vector": "cpp",
|
||||
"chrono": "cpp",
|
||||
"array": "cpp"
|
||||
}
|
||||
}
|
||||
@ -19,9 +19,9 @@ void StateBalance::enter(MdogSimpleController* ctrl) {
|
||||
void StateBalance::run(MdogSimpleController* ctrl) {
|
||||
// 贝塞尔控制点
|
||||
std::vector<std::array<double, 3>> 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<double, 3> 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;
|
||||
|
||||
@ -4,41 +4,48 @@
|
||||
namespace mdog_simple_controller {
|
||||
namespace kinematics {
|
||||
|
||||
// 正运动学：已知关节角，求末端位置
|
||||
void forward_kinematics(const std::array<double, 3>& theta,
|
||||
const std::array<double, 3>& link,
|
||||
std::array<double, 3>& 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<double, 3>& pos,
|
||||
const std::array<double, 3>& link,
|
||||
std::array<double, 3>& 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;
|
||||
}
|
||||
|
||||
|
||||
@ -18,7 +18,7 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
|
||||
joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
|
||||
|
||||
if_no_hardware_ = this->declare_parameter<bool>("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",
|
||||
|
||||
@ -35,8 +35,8 @@
|
||||
|
||||
<!-- 定义thigh_joint -->
|
||||
<joint name="${name}_thigh_joint" type="revolute">
|
||||
<!-- <origin rpy="0 -0.7854 0" xyz="0 ${mirror * 0.05} 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 ${mirror * 0.05} 0"/>
|
||||
<origin rpy="0 -1.5708 0" xyz="0 ${mirror * 0.05} 0"/>
|
||||
<!-- <origin rpy="0 0 0" xyz="0 ${mirror * 0.05} 0"/> -->
|
||||
<parent link="${name}_hip"/>
|
||||
<child link="${name}_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user