[sig.johnnson]

我有一个带有导入对象的工作站，它由曲面、曲线和点组成。Item.GetPoints ()可以很好地获取每个点的位置。然而，与这些点相关的旋转是在IJK坐标系中。我怎样才能以一种在曲线对象之外有效的方式解释IJK坐标呢?具体来说，我想把IJK坐标转换成欧拉角。

我最终想要完成的整个过程是:
1.导入曲面/曲线/点到RoboDK工作站
2.获取点坐标(6 DoF)
3.调整6个DoF点坐标，使其垂直于表面(PROJECTION_RECALC对这个很有用)
4.调整6个DoF点坐标使其与曲线相切
5.导出调整后的6自由度坐标供其他用途

为了得到表面法线，我现在唯一的方法就是用numpy-stl解析我的网格并从那里提取最近的表面法线，但这似乎很难(而且很慢)，所以可能有更好的方法。

为了得到曲线的切线，我过去已经计算了每个维度上的曲线导数以允许从ijk到欧拉角的转换，但这是痛苦和复杂的，我不想再做一次。与上述类似，肯定有一种更简单的方法，只是我没有意识到。

任何提示将非常感激。

[山姆]

看看poinutilities和CurveUtilities插件的源代码:
https://github.com/RoboDK/Plug-In-Interf...oTarget.py
https://github.com/RoboDK/Plug-In-Interf...Targets.py

它展示了如何将XYZ+IJK转换为姿势。然后你可以把姿势转换成另一种格式，比如欧拉角:
//www.sinclairbody.com/doc/en/PythonAPI/robo...ose_2_KUKA

[sig.johnnson]

这工作完美，谢谢山姆!

[sig.johnnson]

嗨,山姆,

我现在意识到，这并没有解决切线问题。如何使目标点的X轴在该点与曲线相切?

(我总是可以在这个问题上做数学计算，这似乎是一个不必要的障碍。)

[山姆]

IJK不提供沿轴旋转。

我链接的脚本使用point_Zaxis_2_pose与首选的Y方向。
//www.sinclairbody.com/doc/en/PythonAPI/robo...xis_2_pose

您可以为Y提供一个固定的值，或者通过计算指向下一个点的向量来动态地计算它(向前看)。
我知道，这并不完美。但如果你的曲线步长足够小，应该没问题。
不幸的是，我没有为此准备好的脚本。

请随时在这里提供您的实现，也许我们可以将其添加为应用程序的功能。

[sig.johnnson]

这在我测试的路径上是有效的。如果相邻航路点之间的表面法线变化很大，我预计这可能是不可预测的。请让我知道，如果你把这个代码的一个版本到API发布，这样我就可以采用标准版本。

代码:

#给定:target_handles是robolink类型的目标列表。项
给定:每个目标的Z轴垂直于最近的表面
给出:函数定义在单独的代码块中，如下所示
断言len(target_handles) >= 2， "MustHaveAtLeastTwoTargetsToCalculateTangency"
对于I在range(len(target_handles)):

#第一个目标只:向前差分
如果I == 0:
angle_about_z = checkRotationSign(forwardDifference(target_handles[i]， target_handles[i+1])， target_handles[i]， next_target=target_handles[i+1]))
target_handles[i].setPose(target_handles[i].Pose() * robomath.rotz(angle_about_z))

#所有中间目标:中心差值是向前和向后差值的平均值
if I < (len(target_handles) - 1):
forward_rotation = checkRotationSign(forwardDifference(target_handles[i]， target_handles[i+1])， target_handles[i]， next_target=target_handles[i+1]))
backward_rotation = checkRotationSign(backwardDifference(target_handles[i]， target_handles[i+1])， target_handles[i]， prev_target=target_handles[i-1]))
target_handles[i]. setpose (target_handles[i]. pose () * robomath。Rotz ((forward_rotation + backward_rotation) /2))

#最终目标只:向后差
其他:
angle_about_z = checkRotationSign(backwardDifference(target_handles[i]， target_handles[i-1])， target_handles[i]， prev_target=target_handles[i-1])
target_handles[i].setPose(target_handles[i].Pose() * robomath.rotz(angle_about_z))

代码:

向前和向后差分从离散微分方法中获得命名灵感，但不使用相同的底层数学。
#方法:
# 1。获取从目标到其邻居的向量
# 2。投影向量(s)到平面
# 3。计算矢量(s)与当前X轴之间的旋转差(旋转约为Z)
# 4。检查结果，如有必要，翻转旋转或偏移180度的标志
# 5。应用计算旋转目标，平均向前和向后偏移，如果两者都存在
defforwarddifference (base_target: robolink。Item, next_target: robolink.Item) -> float:

#计算在基本目标的框架内完成
Tangent_vector = np。asarray (pose_wrt (next_target base_target) [0:3 3] .tolist ())
Plane_normal = [0,0,1]
向量u在平面上的投影是通过从u中减去u与平面正交的分量得到的
Tangent_orthog = np。乘(plane_normal np。点(tangent_vector plane_normal))
Tangent_vector = Tangent_vector - tangent_orthog
Angle_about_z = np.arccos(np。Dot (tangent_vector， [1,0,0]) / np. linalg_norm (tangent_vector))

返回浮动(angle_about_z)

def backwardDifference(base_target: robolink。Item, prev_target: robolink.Item) -> float:

#计算在基本目标的框架内完成
Tangent_vector = np。asarray (pose_wrt (prev_target base_target) [0:3 3] .tolist ())
Plane_normal = [0,0,1]
向量u在平面上的投影是通过从u中减去u与平面正交的分量得到的
Tangent_orthog = np。乘(plane_normal np。点(tangent_vector plane_normal))
Tangent_vector = Tangent_vector - tangent_orthog
# a * b = |a||b|cos(θ)
Angle_about_z = np.arccos(np。Dot (tangent_vector， [-1,0,0]) / np. linalg_norm (tangent_vector))

返回浮点数(2*3.14 - angle_about_z)

由于cos()是偶函数，因此可能会在错误的方向上旋转。
检查旋转目标时Y误差减少。否则，旋转方向相反。
另外，由于180度的误差在Y上看起来很好，必须单独检查X误差的符号
#以确保结果不是180度偏移。
def checkRotationSign(angle_about_z, base_target, prev_target=None, next_target=None):

assert (prev_target == None) ^ (next_target == None)， "MustCheckExactlyOneTargetRelativeToBaseTarget"

如果prev_target != None:
Target = prev_target
其他:
Target = next_target
Original_error = pose_wrt(target, base_target)[1,3]

#翻转目标，检查剩余错误，然后放回去
base_target.setPose(base_target.Pose() * robomath.rotz(angle_about_z))
Residual_error = pose_wrt(target, base_target)[0:3,3].tolist()
base_target.setPose(base_target.Pose() * robomath.rotz(-angle_about_z))

如果我们在右象限，但旋转方向错误，Y上的残差不会接近0
如果abs(residual_error[1]) > 0.9 * abs(original_error):
Angle_about_z = - Angle_about_z
base_target.setPose(base_target.Pose() * robomath.rotz(angle_about_z))
Residual_error = pose_wrt(target, base_target)[0:3,3].tolist()
base_target.setPose(base_target.Pose() * robomath.rotz(-angle_about_z))

如果X定位错误，则X的残差对于正向差为负，对于反向差为正
if (prev_target == None and residual_error[0] < 0) or (next_target == None and residual_error[0] > 0):
Angle_about_z = Angle_about_z + 3.14

返回angle_about_z % (2*3.14)

#内置PoseWrt是不可靠的，当机器人存在于工作站。使用黑客，直到修复。
Def pose_wrt(child: robolink.)项, parent: robolink.Item) -> robomath.Mat:
childPose = child.PoseAbs()
parentPose = parent.PoseAbs()
return parentPose.inv() * childPose

[sig.johnnson]

对于那些带着同样问题来到这里的人:上面的代码有很多我在发布后发现的bug。当我对它更有信心时，我可能会发布一个更新的版本。