c++ - 雅可比矩阵的 5 DOF 逆运动学
问题描述
我正在尝试将 IK 应用于我的人体骨骼的手臂,肩部位置固定,只有手臂在移动。
手臂的FK是这样的
(x,y,z) = Root_T * Shoulder_T * Shoulder_Rx(q_0) * Shoulder_Ry(q_1) *
Shoulder_Rz(q_2) * Elbow_T * Elbow_Rx(q_3) * Hand_T * Hand_Rx(q_4) * Position
这是我的 IK 解算器功能
//Q_desired is a 5*1 Vector whose entries are initialized as 0.0
//TargetP is where I want the end effector to be
void BoneRig::solveIK(glm::vec3& targetP, Eigen::VectorXf& Q_desired) {
//Basic setups
float error = 10.0f;
glm::vec3 errorP;
Eigen::VectorXf deltaQ(5);
glm::vec3 endEffector = glm::vec3(0.0f,0.0f,0.0f);
Eigen::MatrixXf J(3,5);
Eigen::MatrixXf J_plus(5,3);
float deltaTime = 0.1;
int limit = 1000;
int limitCounter = 0;
while(error > 0.01){
//gets the current endEffector location and stores it in endEffector
getEndEffectorP(endEffector);
errorP = targetP - endEffector;
Eigen::VectorXf deltaX(3);
deltaX << (errorP * deltaTime).x, (errorP * deltaTime).y, (errorP * deltaTime).z;
getJacobian(J, targetP);
J_plus = J.transpose();
deltaQ = J_plus * deltaX;
Q_desired += deltaQ;
setOrientation(Q_desired);
error = glm::length(errorP);
if(limitCounter>= limit){
std::cout << "You could not solve this"<<std::endl;
break;
}
}
}
我认为问题出在setOrientation(Q_desired);
功能上。Q_desired = Q_desired + deltaQ
更新每个循环中的自由度角度,因此我相应地更新每个关节的旋转矩阵。这是setOrientation(Q_desired)
功能。
//Joints[15] is the Shoulder joint, Joints[16] is the Elbow joint.
//Joints[17] is the Hand joint
void BoneRig::setOrientation(const Eigen::VectorXf& ceta_d){
glm::mat4 temp = glm::mat4(1.0f);
Joints[15].setR(glm::rotate(temp,ceta_d(0),glm::vec3(1.0f,0.0f,0.0f)));
Joints[15].setR(glm::rotate(Joints[15].returnR(),ceta_d(1),glm::vec3(0.0f,1.0f,0.0f)));
Joints[15].setR(glm::rotate(Joints[15].returnR(),ceta_d(2),glm::vec3(0.0f,0.0f,1.0f)));
temp = glm::mat4(1.0f);
Joints[16].setR(glm::rotate(temp,ceta_d(3),glm::vec3(1.0f, 0.0f, 0.0f)));
temp = glm::mat4(1.0f);
Joints[17].setR(glm::rotate(temp,ceta_d(4),glm::vec3(1.0f, 0.0f, 0.0f)));
return;
}
我很好奇是否应该在那些旋转轴上应用角度旋转(应该是全局轴,而不是局部轴?)我没有发布 getJacobian() 方法,因为我认为它是正确的,但如果这段代码没有缺陷。任何帮助,将不胜感激。提前致谢!
编辑 这就是我获得手臂的雅可比矩阵的方式。请注意,只有肩部是球窝关节,其余只是基于局部 x 轴的旋转关节。
void BoneRig::getJacobian(Eigen::MatrixXf& J, glm::vec3& targetWorld){
glm::vec4 xAxis = glm::vec4(1.0f,0.0f,0.0f,0.0f);
glm::vec4 yAxis = glm::vec4(0.0f,1.0f,0.0f,0.0f);
glm::vec4 zAxis = glm::vec4(0.0f,0.0f,1.0f,0.0f);
glm::mat4 temp = Joints[0].returnT() * Joints[0].returnR() * Joints[11].returnT() * Joints[11].returnR() * Joints[14].returnT() * Joints[14].returnR() * Joints[15].returnT() * Joints[15].returnR();
glm::vec3 worldXAxis = glm::vec3(temp * xAxis);
glm::vec3 worldYAxis = glm::vec3(temp * yAxis);
glm::vec3 worldZAxis = glm::vec3(temp * zAxis);
glm::vec3 worldPos = glm::vec3(temp * glm::vec4(0.0f,0.0f,0.0f, 1.0f));
glm::vec3 p = targetWorld - worldPos;
glm::vec3 upperPart = glm::cross(worldXAxis,p);
J(0,0) = upperPart.x;
J(1,0) = upperPart.y;
J(2,0) = upperPart.z;
upperPart = glm::cross(worldYAxis,p);
J(0,1) = upperPart.x;
J(1,1) = upperPart.y;
J(2,1) = upperPart.z;
upperPart = glm::cross(worldZAxis,p);
J(0,2) = upperPart.x;
J(1,2) = upperPart.y;
J(2,2) = upperPart.z;
temp = temp * Joints[16].returnT() * Joints[16].returnR();
worldXAxis = glm::vec3(temp * xAxis);
worldPos = glm::vec3(temp * glm::vec4(0.0f,0.0f,0.0f, 1.0f));
p = targetWorld - worldPos;
upperPart = glm::cross(worldXAxis,p);
J(0,3) = upperPart.x;
J(1,3) = upperPart.y;
J(2,3) = upperPart.z;
temp = temp * Joints[17].returnT() * Joints[17].returnR();
worldXAxis = glm::vec3(temp * xAxis);
worldPos = glm::vec3(temp * glm::vec4(0.0f,0.0f,0.0f, 1.0f));
p = targetWorld - worldPos;
upperPart = glm::cross(worldXAxis,p);
J(0,4) = upperPart.x;
J(1,4) = upperPart.y;
J(2,4) = upperPart.z;
}
我对雅可比矩阵的每一列都使用了这个方程。轴应该在全局坐标中计算,所以这就是我在代码中所做的。
解决方案
舍特勒先生是对的。代码中的所有内容都是正确的,除了我不应该使用J_plus = J.transpose
. 相反,我应该使用 Moore-Penrose Pseudoinverse,它是
J_plus = (J.transpose() * J).inverse() * J.transpose() // This is for Overdetermined systems
J_plus = J.transpose * (J*J.transpose()).inverse() // This is for Underdetermined systems
在这种情况下,因为它是一个只有 3 行雅可比矩阵的 5DOF 系统,所以我应该使用第一个版本。
再次感谢@Nico Schertler!
推荐阅读
- javascript - 如何在 JS 中过滤 JSON 数据
- php - 在查询结果或查询比较中包含列名
- ruby-on-rails-5.2 - 按常用列关联关闭 3 个模型
- javascript - AngularJS ng-repeat orderBy / filter:在一种情况下调用后端的函数,在其他情况下在前端排序
- javascript - JavaScript 变量作用域
- arrays - 无法在 Vue 中响应实例
- java - NetBeans java.lang.ClassNotFoundException: com.sun.xml.internal.bind.v2.ContextFactory] 在 javax.xml.bind.JAXB.unmarshal(JAXB.java:171)
- microsoft-graph-api - 无法通过事件字段 $select 和 $filter 消息
- php - 检查一个数字是否是另一个数字的倍数
- javascript - 将货币紧凑表示法(例如 '$1.5k')转换为整数(例如 1500)