关于cos / sin的矩阵数学
Matrix math with cosin / sin
我有一个程序,它将接受联合参数d, theta, a和alpha作为输入,并将产生相应的4x4齐次矩阵作为输出。我已经测试了我的矩阵乘法,它确实工作得很好。我将从输入中得到5个矩阵它们相乘得到$t^0_5$。测试用例在这里。我的输出与结果完全不同。下面是我的代码:
首先是我们将数据输入到使用DH参数表的矩阵中的矩阵:
//Kinematics
Matrix44 Matrix44::kinematics(double d, double theta, double a, double alpha)const {
Matrix44 result;
double pi = 3.14159;
double radstheta = theta*(pi/180);
double radsalpha = alpha*(pi/180);
//row1
result.element[0][0] = cos(radstheta);
result.element[0][3] = -cos(radsalpha)*sin(radstheta);
result.element[0][4] = sin(radsalpha)*sin(radstheta);
result.element[0][3] = a*cos(radstheta);
//row2
result.element[1][0] = sin(radstheta);
result.element[1][5] = cos(radsalpha)*cos(radstheta);
result.element[1][6] = -sin(radsalpha)*cos(radstheta);
result.element[1][3] = a*sin(radstheta);
//row3
result.element[2][0] = 0;
result.element[2][7] = sin(radsalpha);
result.element[2][8] = cos(radsalpha);
result.element[2][3] = d;
//row4
result.element[3][0] = 0;
result.element[3][9] = 0;
result.element[3][10] = 0;
result.element[3][3] = 1;
return result;
}
main中我得到结果的部分,数据来自这个表:
Matrix44 a,b,c,d,e;
//in order (d,theta,a,alpha)
//all data is static and given except for theta which changes, see link for cases
a = a.kinematics(27.2,0, 0, 90);
b = b.kinematics(0,0,19.2,180);
c = c.kinematics(0,0,19.2,0);
d = d.kinematics(0,0+90,0,90);
e = e.kinematics(10.5,0,0,0);
//anyone know how to format this nicely? The operator is overload to print a matrix
//
cout << left <<setw(20) << a*b*c*d*e;
为角度,D和A为距离。
输出/输入的代码: //User Input
istream& operator>> (istream& s, Matrix44& t) {
for (int i=0; i<4; i++)
for (int j=0; j<4; j++)
s >> t.element[i][j];
if (!s) { cerr << "Error reading Matrix from stream"; exit(0); }
return s;
}
//User Output
ostream& operator<< (ostream& s, const Matrix44& t) {
for (int i=0; i<4; i++) {
for (int j=0; j<4; j++)
s << t.element[i][j] << " ";
s << endl;
}
if (!s) { cerr << "Error writing Matrix to stream"; exit(0); }
return s;
}
矩阵: class Matrix44 {
private:
double element[4][4];
friend class Point;
public:
Matrix44(void);
Matrix44 transpose(void) const;
Matrix44 inverse(Matrix44 x) const;
Matrix44 kinematics(double d, double theta, double a, double alpha) const;
friend istream& operator>>(istream& s, Matrix44& t);
friend ostream& operator<<(ostream& s, const Matrix44& t);
Matrix44 operator *(Matrix44 b);
Point operator*(const Point & P);
};
有几个问题,一个在您展示的代码中,另一个不在。
问题1:这可能不是问题的根源,但double pi = 3.14159;
对浮点数很好,但对双精度数不适用。这里应该有至少16位。更好的是,使用<math.h>
中的M_PI
,这是许多编译器中常见的扩展名。如果你的编译器没有定义M_PI
,使用类似于3.14159265358979323846264338327950288。
问题2:您没有在代码中显示进行乘法运算的位置,因此这也可能不是问题的根源。问题是旋转群是不可交换的:A*B不等于B*A。你必须非常小心这里的乘法约定。举个例子:拿起一本书,把它放平,让封面朝上,书脊朝左。将书绕着指向一行文本方向的轴旋转90度。现在绕着从页面底部到顶部的轴旋转90度。你的书应该有一个方向,这样你就可以把它放在书架上(书脊垂直朝向你)。现在把书放回原来的方向,重复旋转,但顺序相反。这次你会看到一个非常不同的画面。
你的索引肯定是错误的?而不是
//row1
result.element[0][0] = cos(radstheta);
result.element[0][3] = -cos(radsalpha)*sin(radstheta);
result.element[0][4] = sin(radsalpha)*sin(radstheta);
result.element[0][3] = a*cos(radstheta);
应该是
//row1
result.element[0][0] = cos(radstheta);
result.element[0][1] = -cos(radsalpha)*sin(radstheta);
result.element[0][2] = sin(radsalpha)*sin(radstheta);
result.element[0][3] = a*cos(radstheta);
相关文章:
- 如何在 Gnuplot 中分别绘制 2 个文件数据?我有一个文件"sin.txt",另一个文件"cos.txt",我想将它们分别绘制在一个图表上
- MacOS 上的 Xcode 11 项目不在一个函数中使用 sin 和 cos:未定义的符号"___sincosf_stret"
- 未解析的外部符号 fabs, cos, sin.
- sin cos 函数在大于 90 的度数时给出错误的结果
- 用C 创建SIN/COS图
- 使用SIN和COS函数迭代时,NAN会导致
- 我的神经网络学习sin x,但不学习cos x
- C 程序创建SIN COS和切线值
- cos 和 sin 不提供预期的输出
- 将 cos 和 sin 存储到一个向量中并获得奇怪的值
- 泰勒级数 在sin(90)和cos(120)之后得到nan
- 简单的三角函数算法,用于计算定点数的 sin() 和 cos()
- 在表中预先计算一次cos()和sin()
- sin() cos() ring formula
- 在 glm::rotate 中使用 "custom" sin 和 cos
- 关于cos / sin的矩阵数学
- 为什么在启用优化时sin/cos会变慢
- Sin和cos图表绘图仪…c++的一些算法错误
- 对于已知的角度Sin和Cos给出了意想不到的结果
- 求解cos()和sin()中的变量