Я функционирую в тусклой руке ⇐ C++

[Anonymous]

Я работаю над проектом Robotic Arm 6 Dof, но код обратной кинематики (IK) в моем файле `control.ino` не работает, как и ожидалось для управления рукой в плоскости 3D X-Y-Z. Код предназначен для 3R Planar Arm с тремя соединениями (длины ссылки L1 = 100 мм, L2 = 170 мм, L3 = 130 мм) и принимает x, y и phi (конечная эффекторная ориентация) в качестве входов. Тем не менее, моя физическая рука с 6 добычи требует позиционирования в трехмерном пространстве (x, y, z) с шестью углами сустава, а текущая функция IK вычисляет только три угла, что приводит к непредсказуемому движению соединений 3 и 6 (от 0 до ~ 0,9 и ~ 0,3 радиан, соответственно), когда установлено в домашнее положение 0. Кроме того, я могу контролировать линейное соединение (если в настоящем), потому что в полной мере, потому что я не могу ответить, потому что в полной мере. 6-dof Kinematics или 3D-конфигурация ARM. Я пытался увеличить сервопривод, но это не помогло, так как проблема, кажется, связана с алгоритмом IK, не соответствующим настройке моей руки. Может ли кто-нибудь просмотреть мой код и помочь исправить функцию IK, чтобы правильно обрабатывать движение 6-DOF в плоскости X-Y-Z, гарантируя, что все соединения перемещаются к правильным углам?#include
#include

Servo joint1Servo;
Servo joint2Servo;
Servo joint3Servo;

const float l1 = 100.0;
const float l2 = 170.0;
const float l3 = 130.0;

float joint1 = 0.0;
float joint2 = 0.0;
float joint3 = 0.0;

void setup() {

joint1Servo.attach(9); // Base rotation
joint2Servo.attach(10); // Second joint
joint3Servo.attach(11); // Third joint

// Initialize serial communication
Serial.begin(9600);
Serial.println("Enter target x,y,phi (in degrees):");

// Set initial servo positions
joint1Servo.write(joint1);
joint2Servo.write(joint2);
joint3Servo.write(joint3);
}

bool inverseKinematics(float x, float y, float phi, float &theta1, float &theta2, float &theta3, bool elbowDown = true) {
float phi_rad = phi * PI / 180.0;
float x_w = x - l3 * cos(phi_rad);
float y_w = y - l3 * sin(phi_rad);
float d2 = x_w * x_w + y_w * y_w;
if (d2 == 0) return false;
float d = sqrt(d2);
float cos_theta2 = (d2 - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
if (abs(cos_theta2) > 1) return false;

float theta2_rad = acos(cos_theta2);
if (!elbowDown) theta2_rad = -theta2_rad;

float sin_theta2 = sin(theta2_rad);
float cos_theta2 = cos(theta2_rad);
float theta1_rad = atan2(y_w, x_w) - atan2(l2 * sin_theta2, l1 + l2 * cos_theta2);

float theta3_rad = phi_rad - theta1_rad - theta2_rad;

theta1 = theta1_rad * 180.0 / PI;
theta2 = theta2_rad * 180.0 / PI;
theta3 = theta3_rad * 180.0 / PI;

if (theta1 < 0 || theta1 > 180 || theta2 < 0 || theta2 > 180 || theta3 < 0 || theta3 > 180) {
return false;
}

return true;
}

void loop() {

if (Serial.available() > 0) {

String input = Serial.readStringUntil('\n');
float x, y, phi;

int comma1 = input.indexOf(',');
int comma2 = input.indexOf(',', comma1 + 1);
if (comma1 != -1 && comma2 != -1) {
x = input.substring(0, comma1).toFloat();
y = input.substring(comma1 + 1, comma2).toFloat();
phi = input.substring(comma2 + 1).toFloat();

float newJoint1, newJoint2, newJoint3;
if (inverseKinematics(x, y, phi, newJoint1, newJoint2, newJoint3)) {

joint1 = newJoint1;
joint2 = newJoint2;
joint3 = newJoint3;

joint1Servo.write(joint1);
joint2Servo.write(joint2);
joint3Servo.write(joint3);

Serial.print("Moving to x=");
Serial.print(x);
Serial.print(", y=");
Serial.print(y);
Serial.print(", phi=");
Serial.print(phi);
Serial.print(" | Joint1=");
Serial.print(joint1);
Serial.print(", Joint2=");
Serial.print(joint2);
Serial.print(", Joint3=");
Serial.println(joint3);
} else {
Serial.println("No valid solution for given position and orientation!");
}
} else {
Serial.println("Invalid input format! Use: x,y,phi");
}
}
}


Подробнее здесь: https://stackoverflow.com/questions/797 ... in-dof-arm