关于反向运动学算法的帮助
Help with Inverse Kinematics Algorithm
我正在尝试在2D 中实现CCD逆运动学
该函数应进行CCD 的1次迭代
现在作为一个测试案例,我从左脚开始,让它停在骨盆处。
每次调用此函数时,骨骼的骨骼都会更新。
我的骨骼工作方式是:getFrameX、Y、Angle返回骨骼/效应器末端的绝对位置。CCD的每一次迭代都会更新这些内容。getAngle,X,Y返回相对值。
二传手也是如此。
现在它从不停留在一个位置,每次我咯咯地笑鼠标时,它都会随机逆时针移动骨骼。
我想知道是否有什么明显的错误,可以为我的调试指明正确的方向。
void inverseKinematics(float targetX, float targetY, skl::Bone* targetBone)
{
std::string stopBone = "Pelvis";
//===
// Track the end effector position (the final bone)
double endX = targetBone->getFrameX();
double endY = targetBone->getFrameY();
//===
// Perform CCD on the bones by optimizing each bone in a loop
// from the final bone to the root bone
bool modifiedBones = false;
targetBone = targetBone->getParent();
while(targetBone->getName() != stopBone)
{
// Get the vector from the current bone to the end effector position.
double curToEndX = endX - targetBone->getFrameX();
double curToEndY = endY - targetBone->getFrameY();
double curToEndMag = sqrt( curToEndX*curToEndX + curToEndY*curToEndY );
// Get the vector from the current bone to the target position.
double curToTargetX = targetX - targetBone->getFrameX();
double curToTargetY = targetY - targetBone->getFrameY();
double curToTargetMag = sqrt( curToTargetX*curToTargetX
+ curToTargetY*curToTargetY );
// Get rotation to place the end effector on the line from the current
// joint position to the target position.
double cosRotAng;
double sinRotAng;
double endTargetMag = (curToEndMag*curToTargetMag);
if( endTargetMag <= 0.1f )
{
cosRotAng = 1.0f;
sinRotAng = 0.0f;
}
else
{
cosRotAng = (curToEndX*curToTargetX + curToEndY*curToTargetY) / endTargetMag;
sinRotAng = (curToEndX*curToTargetY - curToEndY*curToTargetX) / endTargetMag;
}
// Clamp the cosine into range when computing the angle (might be out of range
// due to floating point error).
double rotAng = acosf( max(-1.0f, min(1.0f,cosRotAng) ) );
if( sinRotAng < 0.0f )
rotAng = -rotAng;
// Rotate the end effector position.
endX = targetBone->getFrameX() + cosRotAng*curToEndX - sinRotAng*curToEndY;
endY = targetBone->getFrameY() + sinRotAng*curToEndX + cosRotAng*curToEndY;
// Rotate the current bone in local space (this value is output to the user)
targetBone->setAngle(SimplifyAngle(targetBone->getAngle() + rotAng));
// Check for termination
double endToTargetX = (targetX-endX);
double endToTargetY = (targetY-endY);
if( endToTargetX*endToTargetX + endToTargetY*endToTargetY <= 1.0f )
{
// We found a valid solution.
return;
}
// Track if the arc length that we moved the end effector was
// a nontrivial distance.
if( !modifiedBones && fabs(rotAng)*curToEndMag > 0.0001f )
{
modifiedBones = true;
}
targetBone = targetBone->getParent();
}
感谢
不,您给出的程序清单中没有任何明显的错误。您正在正确计算末端效应器的角度变化rotAng
和新位置(endX, endY)
。
您可以更简单地将rotAng
计算为
double rotAng =
atan2(curToTargetY, curToTargetX) - atan2(curToEndY, curToEndX);
这给出了相同的结果(假设向量是非零的)。
我怀疑这个错误在你给出的程序清单之外。inverseKinematics()
中假设的正向运动学与显示例程和其他地方中使用的实际正向运动学之间可能存在差异。尝试在过程结束时重新计算正向运动学,以查看系统的其余部分是否同意末端效应器处于(endX, endY)
。
相关文章:
- 为什么这个运算符<重载函数对 STL 算法不可见?
- 需要帮助设置在C++中使用的Potrace
- 基于ELO的团队匹配算法
- C++选择排序算法中的逻辑错误
- 在指针的帮助下,文本文件中单词的频率
- 有没有办法将谓词中的元素偏移量传递给 std 算法?
- 需要帮助为我的算法中的缺陷提供优雅的修复
- 合并排序算法编译错误 [帮助]
- 合并排序算法帮助
- 我需要一些关于这个C++算法的帮助
- 请比较两种常见的比较算法及其大O帮助
- 需要算法的帮助来找到DAG中的最大路径
- 需要帮助将 Kruskal 算法应用于使用 2D 结构存储数据的现有邻接矩阵程序
- A* *算法帮助c++
- 帮助我理解这个算法(简单)
- 由于指针指向父项,星形算法无法工作,请帮助修复它
- Tic-Tac-Toe C++算法调试帮助
- 关于反向运动学算法的帮助
- 需要帮助找出我在这个反转算法中做错了什么(基于合并排序)
- 需要帮助理解std::find算法