概述

geometry.py 提供了处理场景和机器人几何形状的原语。

主要概念

几何类型

• Pose：物体的6D位置和方向。构造方法

Pose(position=[x, y, z], quaternion=[w, i, j, k])
Pose.from_hmat(homogeneous_matrix)
Pose.from_poseuler(poseuler=[x,y,z, p,r,w], ordering="XYZ")

• Wrench 和 WrenchStamped 6D力矩。
• Twist 和 TwistStamped 6D速度。
• Accel 和 AccelStamped 6D加速度。

为了调试和可视化，可以使用 name 参数构造 Pose 和 PoseStamped 对象。

框架和标记类型

此处所有数量均为普通和“盖章”种类。盖章数量（例如 TwistStamped）始终定义为相对于一个frame的普通类型（例如 Twist）。frame是一种可以评估为世界姿态的东西——要么是一个PoseStamped，要么是一个Grounding。Grounding是可选的（即可以使用None来表示世界坐标）。此库支持不同的grounding后端，例如mujoco-elements或observation-dicts。

例如，机器人抓取器中插头的姿态可以用PoseStamped来表示，这是通过组合抓取器在世界坐标系的Pose和插头相对于抓取器的姿态来实现的。

盖章数量的frame可以是：

• 另一个盖章数量，
• 一个“grounding”（用户选择类型），
• 用户提供的Physics实例必须能够提供grounding的世界Pose。库定义的MujocoPhysics为mjcf.Element实例执行此操作。
• None，在这种情况下，假设世界坐标。

因此，在上面的例子中，第一个数量可能来自mujoco模型（可能同步于现实世界），第二个数量可能是先验知识或由视觉估计。

gripper_in_world = PoseStamped(pose=None, frame=synced_gripper_mjcf_body)
plug_in_gripper = Pose(...)  # From vision pose model.
plug_pose = PoseStamped(plug_in_gripper, frame=gripper_in_world)

（一个更现实的例子将具有插头在相机帧中的姿态，然后是相机在机器人的运动学树中（连接到机器人）或者它们都在世界坐标中）

然后，可以计算出插头在世界世界坐标中的姿态：

plug_in_world: Pose = plug_pose.get_world_pose()

不可变性

geometry.py中的所有类型都是不可变的。您可以使用replace和with_*方法创建现有值的修改副本。replace与collections.namedtuple._replace类似，这些方法返回新的对象。

PoseStamped

这是Pose的盖章版本。

• 可以通过to_world将其frame层次结构扁平化，它返回世界坐标中的PoseStamped。
• 另一个frame的相对Pose由get_relative_pose返回。
• 世界坐标的相对Pose由get_world_pose返回。

因此，在相机帧中的抓取器不连接到机器人的情况下，我们可以对抓取器或插头的PoseStamped执行to_world()，给出一个新的frame，然后使用get_relative_pose计算它们的相对姿态（插头相对于机器人）。

HybridPoseStamped

一个可以覆盖位置或方向的PoseStamped。这对于表达（例如）具有世界方向的抓取器位置的想法非常有用，以便允许更直观的操作员控制抓取器。