AdjustPose 调整位姿

AdjustPose 算子用于调整位姿，包含 InvertPose 、AveragePose 、RotateAxis 、ReferencePose 、NormalizePose 。

类型 功能
InvertPose 对原 pose 取逆。
AveragePose 求取输入 pose 列表的平均 pose 。
RotateAxis /
ReferencePose 根据参考姿态调整 pose 。
NormalizePose 根据参考轴和角度调整pose的roll 、pitch 、yaw角

InvertPose

将 AdjustPose 算子的 类型 设置为 InvertPose ，用于对原 pose 取逆。

算子参数

  • 坐标/pose：设置逆 pose 在 3D 视图中的可视化属性。

    • icon_visOn 打开逆 pose 可视化。

    • icon_visOff 关闭逆 pose 可视化。

    • icon_size 设置逆 pose 的尺寸大小。取值范围：[ 0.001,10] 。默认值：0.1 。

  • 坐标列表/pose_list：设置逆 pose 列表在 3D 视图中的可视化属性。值描述与 坐标 一致。

数据信号输入输出

输入：

说明：根据需求选择其中一种数据信号输入即可。

  • pose：

    • 数据类型：Pose

    • 输入内容：pose 数据

  • pose_list：

    • 数据类型：PoseList

    • 输入内容：pose 列表数据

输出：

  • pose：

    • 数据类型：Pose

    • 输出内容：逆 pose 数据

  • pose_list：

    • 数据类型：PoseList

    • 输出内容：逆 pose 列表数据

功能演示

使用 AdjustPose 算子中 类型 属性 InvertPose 将加载的 pose 取逆。

步骤1：算子准备

添加 Trigger 、Load 、AdjustPose 算子至算子图。

步骤2：设置算子参数

  1. 设置 Load 算子参数：

    • 类型 → Pose

    • 文件 → icon_more →选择 pose 文件名( example_data/pose/tcp1.txt )

    • 坐标 → icon_visOn 可视

  2. 设置 AdjustPose 算子参数：

    • 类型 → InvertPose

    • 坐标 → icon_visOn 可视

步骤3：连接算子

3D_AdjustPose_InvertPose_node

步骤4：运行

点击 RVS 运行按钮，触发 Trigger 算子。

运行结果

如下图所示，3D 视图中显示 Load 算子的 pose 和 AdjustPose 的结果。分别为加载的 pose 及其逆 pose 。

3D_AdjustPose_InvertPose_result

AveragePose

将 AdjustPose 算子的 类型 属性设置为 AveragePose ，用于求取输入 pose 列表的平均 pose 。

项目中，主要用于对同一目标多次计算所得的相近位姿求取平均作为最终结果位姿。

其中的位置信息 xyz 数值直接求平均值。而姿态信息对应的三个旋转角计算过程相对复杂，仅在输入 pose 的旋转角彼此相差不大时具有实际意义，一般彼此之间的旋转角差值标准差不超过 0.3 弧度为佳。

算子参数

  • 坐标/pose：设置平均 pose 在 3D 视图中的可视化属性。

    • icon_visOn 打开平均 pose 可视化。

    • icon_visOff 关闭平均 pose 可视化。

    • icon_size 设置平均 pose 的尺寸大小。取值范围：[0.001,10] 。默认值：0.1 。

数据信号输入输出

输入：

  • pose_list：

    • 数据类型：Pose

    • 输入内容：pose 数据

输出：

  • average：

    • 数据类型：Pose

    • 输出内容：平均 pose 数据

功能演示

使用 AdjustPose 算子中 类型 属性 AveragePose 求取加载的三个 pose 的平均 pose 。

步骤1：算子准备

添加 Trigger 、Load 、AdjustPose 算子至算子图。

步骤2：设置算子参数

  1. 设置 Load 算子参数：

    • 类型 → Pose

    • 文件 → icon_more → 选择 pose 文件目录名( example_data/averagepose.txt )

    • 坐标列表 → icon_visOn 可视

  2. 设置 AdjustPose 算子参数：

    • 类型 → AveragePose

    • 坐标 → icon_visOn 可视

步骤3：连接算子

3D_Adjust_AveragePose_nodes

步骤4：运行

点击 RVS 的运行按钮，触发 Trigger 算子。

运行结果

如下图所示，3D 视图中显示 Load 算子和 AdjustPose 的结果。分别为加载的三个 pose 和平均 pose 后的结果。

3D_AdjustPose_Average_result1

ReferencePose

将 AdjustPose 算子的 类型 属性设置为 ReferencePose ，用于根据参考姿态调整 pose 姿态。

算子参数

  • 坐标/pose：设置调整姿态后的 pose 在3D视图中的可视化属性。

    • icon_visOn 打开 pose 可视化。

    • icon_visOff 关闭 pose 可视化。

    • icon_size 设置 pose 的尺寸大小。取值范围：[0.001,10] 。默认值：0.1 。

数据信号输入输出

输入：

  • pose：

    • 数据类型：Pose

    • 输入内容：pose 数据

  • ref_pose：

    • 数据类型：Pose

    • 输入内容：参考 pose 数据

输出：

  • pose：

    • 数据类型：Pose

    • 输出内容：调整姿态后 pose 数据

功能演示

使用AdjustPose算子中 类型 属性 ReferencePose 根据参考姿态调整 pose 。

步骤1：算子准备

添加 Trigger 、Emit（2个） 、AdjustPose 算子至算子图。

步骤2：设置算子参数

  1. 设置 Emit 算子参数：

    • 类型 → Pose

    • 坐标 → 0 0 0 0 0 0

    • 坐标 → icon_visOn 可视

  2. 设置 Emit_1 算子参数：

    • 类型 → Pose

    • 坐标 → 0.5 0. 1 0.1 1.57 1.57 -1.57

    • 坐标 → icon_visOn 可视

  3. 设置 AdjustPose 算子参数：

    • 类型 → ReferencePose

    • 坐标 → icon_visOn 可视 → icon_size 0.2

步骤3：连接算子

3D_AdjustPose_ReferencePose_nodes

步骤4：运行

点击 RVS 的运行按钮，触发 Trigger 算子。

运行结果

如下图所示，3D 视图中显示 Emit 算子以及 AdjustPose 算子的结果。

referenceoose

NormalizePose

将 AdjustPose 算子中将 类型 设置为 NormalizePose ，用于计算并输出 pose 的两种不同姿态角表达形式。通常来说，同一姿态角在 -π 到 π 的定义域区间内至少有两种表达形式，其中一种姿态角的表达形式无法用机器人直接移动，因此需要筛选某一旋转轴的最优旋转角度，通过参数 ref_rot_axis 和 ref_angle 进行判断。原理是判断两种表达形式的某一旋转轴（由 ref_rot_axis 设置）是否更接近于参考值 ref_angle ，分别输出为 pose_positive 和 pose_negative 。判断旋转角度是否更接近需符合旋转角度周期原则，即 -π 和 π 认为是同一个角度，若参考值为 -3，那么认为 π 比 0 更接近于 -3 。

算子参数

  • 参考旋转轴/ref_rot_axis：参考轴：X / Y / Z 。

  • 参考角度/ref_angle：参考弧度。

  • 正向坐标/pose_positive：设置pose 最优表达形式在 3D 视图中的可视化属性。

    • icon_visOn 打开调整后 pose 位姿可视化。

    • icon_visOff 关闭调整后 pose 位姿可视化。

    • icon_size 设置调整后 pose 位姿的尺寸大小。取值范围：[0.001,10] 。默认值：0.1 。

  • 负向坐标/pose_negative：设置 pose 另一种表达形式在 3D 视图中的可视化属性。参数值与 pose_positive 描述一致。

  • pose_positive_list：设置pose 列表最优表达形式在 3D 视图中的可视化属性。参数值与 pose_positive 描述一致。

  • pose_negative_list：设置 pose 列表另一种表达形式在 3D 视图中的可视化属性。参数值与 pose_positive 描述一致。

数据信号输入输出

输入：

  • pose：

    • 数据类型：Pose

    • 输入内容：pose 数据

  • pose_list：

    • 数据类型：PoseList

    • 输入内容：pose 列表数据

输出：

  • pose_positive：

    • 数据类型：Pose

    • 输出内容：姿态调整后与参考轴和弧度相近的 pose 数据

  • pose_negative：

    • 数据类型：Pose

    • 输出内容：姿态调整后负向 pose 数据

  • pose_positive_list：

    • 数据类型：PoseList

    • 输出内容：姿态调整后与参考轴和弧度相近的 pose 列表数据

  • pose_negative_list：

    • 数据类型：PoseList

    • 输出内容：姿态调整后负向 pose 列表数据

功能演示

使用 AdjustPose 算子中 类型 属性 NormalizePose 根据 x 轴 在参考角度 3(弧度值) 进行姿态调整。

步骤1：算子准备

添加 Trigger、Emit 、AdjustPose 算子至算子图。

步骤2：设置算子参数

  1. 设置 Emit 算子参数：

    • 类型 → Pose

    • 坐标 → 1 2 0.5 0.5 0.1 0.3

    • 坐标 → icon_visOn 可视

  2. 设置 AdjustPose 算子参数：

    • 类型 → NormalizePose

    • 参考旋转轴 → x

    • 参考角度 → 3

    • 正向坐标 → icon_visOn 可视

    • 负向坐标 → icon_visOn 可视

步骤3：连接算子

3D_AdjustPose_NormalizePose_nodes

步骤4：运行

  1. 点击打开 RVS 的运行按钮，触发 Trigger 算子。

  2. 分别将三个 pose 与交互面板中的输出工具“ pose 输出” 进行绑定。

运行结果

如下图所示，在交互面板中显示 Emit 算子和 AdjustPose 算子输出端口的结果。

3D_AdjustPose_NormalizePose_result