Drake：用锁定关节伪造漂浮物体间的连接 - 可能吗？

Question

ubuntu操作系统 22.04 | pydrake - 1.19.0 | ros humble | pycharm

我正在尝试模拟一个用Kuka iiwa机械臂和一个大箱子进行的抓取和放置任务。首先，我通过下面的sdf文件构建了一个多体植物模型。我想利用关节的锁定功能来“假装”把箱子从末端执行器上固定和拆卸。我的计划是在箱子和机械臂末端工具之间添加一个QuaternionFloatingJoint，然后使用这个关节的锁定和解锁功能，让机器人看起来像是“抓取”了箱子，最后再“放置”它。

以下是我代码的开头部分，直到出现错误的地方。

SDF模型：

代码示例：

    rdb = RobotDiagramBuilder(time_step=0)
    parser = rdb.parser()
    self.models = parser.AddModels(resource_file)
    self.model = get_model_by_name(rdb.plant(), 'robot')
    robot_tip_frame_name = 'iiwa_link_7'
    box_target_frame_name = 'base'
    box_lock = QuaternionFloatingJoint('box_lock',rdb.plant().GetFrameByName(robot_tip_frame_name),rdb.plant().GetFrameByName(box_target_frame_name))
    self.box_lock = rdb.plant().AddJoint(box_lock)
    floating_joint_placement = RigidTransform(RollPitchYaw(np.array([np.pi, 0.0, 0.0])), np.array([0.6, 0.0, 0.05]))
    self.box_lock.SetDefaultPose(floating_joint_placement)
    rdb.plant().Finalize()
    self.robot = rdb.Build()
    cc = SceneGraphCollisionChecker(model=self.robot, robot_model_instances=[self.model],
                                configuration_distance_function=l2_dist, edge_step_size=0.01,
                                env_collision_padding=0.0, self_collision_padding=0.0)

错误代码示例：

Failure at bazel-out/k8-opt/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/body.h:293 in floating_positions_start(): condition 'is_floating()' failed.

我尝试过的事情：（这些都没有改变结果）

(a) 通过身体框架和链接框架创建浮动关节
(b) 使用简单上下文而不是碰撞上下文（在运行样本笛卡尔起始姿势时出现相同的错误）
(c) 在sdf中包含和移除rso的起始姿势
(d) 升级到pydrake 1.20.0（进一步升级需要处理一些弃用问题，我可以做到，但这看起来是我的问题，而不是源代码的问题）

Answer 1

是的，这个是可以做到的，不过现在的操作方式确实不太方便。我在这个drake问题上更新了我的回复。

我在那儿提到的吸力示例是通过几个步骤来实现的：

• 在模拟之前，它在夹具和操作物体之间添加了浮动关节
• 不是直接调用 simulator.AdvanceTo(duration)，而是实现了一个循环，调用 simulator.AdvanceTo(small_number)，并在这些调用之间
• 通过调用 joint.Lock(...) 和 joint.Unlock(...) 来更新关节锁的状态