三自由度腿反运动学C++
我试图在arduino控制的四足动物上实现反向运动学,但我得到了一些不准确的计算结果。为了检查错误,我把算法移植到C++上,并在PC上运行。我的问题是,我假设坐标改变会给出相同的结果,不管我改变哪个方向的坐标。但是,x轴和y轴上的过渡会根据方向产生几度的差异。我的数学知识有点脏,所以在检查算法时可能遗漏了什么。腿的配置如下 什么会导致结果的差异三自由度腿反运动学C++,c++,arduino,inverse-kinematics,C++,Arduino,Inverse Kinematics,我试图在arduino控制的四足动物上实现反向运动学,但我得到了一些不准确的计算结果。为了检查错误,我把算法移植到C++上,并在PC上运行。我的问题是,我假设坐标改变会给出相同的结果,不管我改变哪个方向的坐标。但是,x轴和y轴上的过渡会根据方向产生几度的差异。我的数学知识有点脏,所以在检查算法时可能遗漏了什么。腿的配置如下 什么会导致结果的差异 #include <cmath> #include <iostream> #define CONNECTED_SERVOS 1
#include <cmath>
#include <iostream>
#define CONNECTED_SERVOS 12
#define PI 3.1415
#define coxa 55
#define femur 40
#define tibia 47
int nogo_buffer[CONNECTED_SERVOS];
int end_position[CONNECTED_SERVOS];
int cur_pw[CONNECTED_SERVOS];
int calibrate[] = { 1500, 1520, 1500, 1560, 1400, 1410, 1520, 1550, 1500, 1560, 1510, 1620 }; //calibrate servos to simmetrical positions
int floatToInt(double num) {
//Rounding a number avoiding truncation:
return (int)(num < 0 ? (num - 0.5) : (num + 0.5));
}
int radToMicro(double rad, int ref) {
//Make sure rad isn't negative:
if (rad < 0) {
while (rad < -PI) {
rad += PI;
}
}
else {
while (rad > PI) {
rad -= PI;
}
}
//edit 2400
return ref - floatToInt(572.958 * rad);
}
void IkLeg(int x, int y, int z, int nLeg)
{
double L1 = sqrt(pow(x,2) + pow(z,2));
double L = sqrt(pow(L1 - coxa,2) + pow(y,2));
double t = acos((pow(tibia,2) + pow(femur,2) - pow(L,2)) / (2 * tibia * femur)) / PI * 180;
double f1 = acos(y / L) / PI * 180;
double f2 = acos((pow(femur,2) + pow(L,2) - pow(tibia,2)) / (2 * femur * L)) / PI * 180;
double f = f1 + f2;
double c = atan2(z, x) / PI * 180;
std::cout << c << std::endl;
std::cout << f-90 << std::endl;
std::cout << t-90 << std::endl;
//coxa angle
nogo_buffer[3 * nLeg - 1] = radToMicro(c/180*PI, calibrate[3 * nLeg - 1]);
std::cout << nogo_buffer[3 * nLeg - 1] << std::endl;
//femur angle
nogo_buffer[3 * nLeg - 2] = radToMicro(f / 180 * PI, calibrate[3 * nLeg - 2]+900);
std::cout << nogo_buffer[3 * nLeg - 2] << std::endl;
//tibia angle
nogo_buffer[3 * nLeg - 3] = radToMicro(t / 180 * PI, calibrate[3 * nLeg - 3]+900);
std::cout << nogo_buffer[3 * nLeg - 3] << std::endl;
}
int main() {
IkLeg(95,47,0,1);
std::cout << "x transition" << std::endl;
IkLeg(105, 47, 0, 1);
IkLeg(85, 47, 0, 1);
std::cout << "y transition" << std::endl;
IkLeg(95, 57, 0, 1);
IkLeg(95, 37, 0, 1);
std::cout << "z transition" << std::endl;
IkLeg(95, 47, 30, 1);
IkLeg(95, 47, -30, 1);
return 0;
}
您可以共享原始arduino代码的访问权限吗?我不知道arduino是如何配置的,但我想知道这是否是一个精度问题,因为浮点的大小-双精度通常是64位,而浮点通常是32位-我怀疑arduino将只支持后者,而您显然在这里使用前者。这通常不重要,所以这可能不是您的问题,但如果您在长时间的集成后看到漂移,这可能就是问题所在。如果你不能共享arduino代码,你能举一个两者输出的例子吗?正如你所猜测的,arduino上运行的代码的唯一区别是使用float而不是double。这就是我把它移植到C++的原因之一,结果完全一样,我使用的伺服系统。1度的理论精度。漂移也存在于小的运动中,并因坐标的较大变化而放大。这是我上面贴的代码输出啊,好吧,我误解了你的问题;我以为你在你的ARDUNO代码和C++代码之间有了差异。我想我对预期的产出感到困惑;您认为您可以编辑问题以包含预期输出与粘贴箱中的内容吗?