CODEWITHSUNDEEP
pythoncollision-detectiondrake
Cameron Pittman
Cameron Pittman

Reputation: 51

Contact between two models causing IndexError: _Map_base::at

Newer Drake user here. We're setting up a ping pong playing iiwa, but we're getting the following error when the ball and paddle come in contact.

IndexError                                Traceback (most recent call last)
/tmp/ipykernel_3391/3824908623.py in <module>
     94 
     95 # simulator.AdvanceTo(0.9) # works
---> 96 simulator.AdvanceTo(1.0) # fails when the ball and paddle come in contact

IndexError: _Map_base::at

There isn't much of a traceback, so it's unclear what's causing the error. Our suspicion is that it's related to an optimization routine involved in contact.

If you can spot the issue in our demo code below, we'd love to hear it. If not, any pointers on debugging this issue would be appreciated. Thanks!

import numpy as np
import pydot
from IPython.display import display, Javascript, SVG
from pydrake.examples.manipulation_station import ManipulationStation
# manipulation lib from https://github.com/RussTedrake/manipulation
from manipulation.scenarios import AddIiwa, AddShape
from manipulation.meshcat_cpp_utils import StartMeshcat
from pydrake.all import (
    AddMultibodyPlantSceneGraph, BasicVector, DiagramBuilder, LeafSystem, MeshcatVisualizerCpp, MeshcatVisualizerParams, Parser, 
    RollPitchYaw, RigidTransform, SceneGraph, Sphere, Simulator, InverseDynamicsController, MultibodyPlant, BasicVector_,
    WitnessFunctionDirection, UnrestrictedUpdateEvent_
)

meshcat = StartMeshcat()

time_step = 0.001

builder = DiagramBuilder()

# iiwa plant
plant_iiwa, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
plant_iiwa.set_name("iiwa_plant")
parser_iiwa = Parser(plant_iiwa, scene_graph)
iiwa = AddIiwa(plant_iiwa)
paddle = parser_iiwa.AddModelFromFile("notebooks/models/paddle.sdf", model_name="paddle")
plant_iiwa.WeldFrames(
    plant_iiwa.GetFrameByName("iiwa_link_7"), 
    plant_iiwa.GetFrameByName("base_link"), 
    RigidTransform(RollPitchYaw(0, -np.pi/2, 0), [0, 0, 0.25]))
plant_iiwa.Finalize()

# ball plant
plant_ball = builder.AddSystem(MultibodyPlant(time_step=time_step))
plant_ball.set_name("ball_plant")
plant_ball.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(plant_ball.get_geometry_poses_output_port(), 
                scene_graph.get_source_pose_port(plant_ball.get_source_id())
               )
builder.Connect(scene_graph.get_query_output_port(), 
                plant_ball.get_geometry_query_input_port()
               )
parser_ball = Parser(plant_ball, scene_graph)
ball = parser_ball.AddModelFromFile("notebooks/models/ball.sdf", model_name="ball")
plant_ball.Finalize()

visualizer = MeshcatVisualizerCpp.AddToBuilder(
    builder, 
    scene_graph, 
    meshcat,
    MeshcatVisualizerParams(delete_prefix_initialization_event=False))

# inverse kinematics on the iiwa
Kp = np.full(plant_iiwa.num_positions(), 100)
Ki = 2 * np.sqrt(Kp)
Kd = np.full(plant_iiwa.num_positions(), 1)

iiwa_controller = builder.AddSystem(InverseDynamicsController(plant_iiwa, Kp, Ki, Kd, False))
iiwa_controller.set_name("iiwa_controller")

builder.Connect(plant_iiwa.get_state_output_port(iiwa),
                iiwa_controller.get_input_port_estimated_state())
builder.Connect(iiwa_controller.get_output_port_control(),
                plant_iiwa.get_actuation_input_port())

# setup the diagram
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()

ball_context = plant_ball.GetMyMutableContextFromRoot(diagram_context)
iiwa_plant_context = plant_iiwa.GetMyMutableContextFromRoot(diagram_context)
iiwa_controller_context = iiwa_controller.GetMyMutableContextFromRoot(diagram_context)

# initial positions
q0 = np.array([-1.57, 0, 0, -np.pi/2, 0, 0, 0])
x0 = np.hstack((q0, 0*q0))

plant_iiwa.SetPositions(iiwa_plant_context, iiwa, q0)

ball_q0 = [1, 1, 1, 1, 0, -0.75, 5] # land on the paddle
plant_ball.SetPositions(ball_context, ball, np.array(ball_q0))

iiwa_controller.GetInputPort('desired_state').FixValue(iiwa_controller_context, x0)

# simulate
diagram.Publish(diagram_context)

simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(0.25)

# simulator.AdvanceTo(0.9) # works
simulator.AdvanceTo(1.0) # fails when the ball and paddle come in contact

Models are borrowed from: https://github.com/mattbev/robot-juggler/tree/main/utils/models

Upvotes: 1

Views: 1027

Answers (1)

jwnimmer-tri
jwnimmer-tri

Reputation: 2449

I have only looked at the error here briefly, but the thing that stands out in the example file is that there are two MultibodyPlant systems connected to the same SceneGraph. Usually there is only 1 plant and 1 scene graph. I don't know whether using two plants is supposed to work in this case, but it seems like something worth exploring.

Possibly you should try adding the ball sdf and iiwa sdf to the same plant object, but still as two different models.

Upvotes: 1

Related Questions