CODEWITHSUNDEEP
pythondrake
L Wall
L Wall

Reputation: 59

Drake: Faking attachment between floating objects with locking joints - is it possible?

ubuntu os 22.04 | pydrake - 1.19.0 | ros humble | pycharm

I'm trying to simulate a pick-n-place task with a kuka iiwa arm and a large box. The multibody plant is first constructed by the sdf included below. I'd like to use the Lock feature of joints to 'fake' attaching and detaching the box from the end effector. My plan was to add a QuaternionFloatingJoint between the box and the end of arm tool to the plant, and then use the Lock and Unlock features of the joint to make it look like the robot 'picks' the box and eventually 'places' it. I have the start of my code below, up to the point where it fails.

SDF model: world sdf file containing an iiwa and a box

Code-Snippet Example:

    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)

Error Code Example:

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.

Things I've Tried: (none of these have resulted in any change to the outcome)

(a) creating the floating joint via body frames vs link frames
(b) utilizing a simple context instead of a collision context (the same error appears when running IK on the sample Cartesian start pose)
(c) including and removing the start pose for the rso in the sdf
(d) upgrading to pydrake 1.20.0 (upgrading further would require some deprecation handling, which I can do but this looks like a 'me' problem, not a source code problem)`

Upvotes: 0

Views: 114

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

Yes, it is possible, but it is admittedly not very ergonomic yet. I've added an updated version of my response to this drake issue.

The suction example I linked there accomplishes this in a few steps:

  • before simulation, it adds floating joints between the gripper and the manipulands
  • rather than calling simulator.AdvanceTo(duration), it implements a loop calling simulator.AdvanceTo(small_number) and between those calls it
  • updates the status of the joint locks by calling joint.Lock(...) and joint.Unlock(...)

Upvotes: 0

Related Questions