Initial condition of pydrake simulation not what I expect

43 views Asked by At

I'm running into an issue with simulation in pydrake where I'm trying to initialize my robot at a certain configuration, and what I see at t = 0 in meshcat is close to, but not quite that.

I'm solving an inverse kinematics problem to initialize my robot on a ladder. I expect it to look like this (Sorry, I don't have enough reputation to post images):

Correct behavior

Instead, it looks like this:

Incorrect behavior

It's most visible in the end-effectors (see purple arrows). This will be a problem when I re-enable collision, it looks like the simulation wills tart in collision.

I think the problem is related to how I'm connecting up the controller:

# connect up the controller
    controller = builder.AddSystem(BasicController(plant))
    builder.Connect(controller.GetOutputPort("torques"),
                    plant.get_actuation_input_port(parcli))
    builder.Connect(plant.get_state_output_port(),
                    controller.GetInputPort("state"))

If I run with the above, I get the undesired behavior. When I comment out the first builder.Connect() line, it behaves correctly. Also, when I leave that line uncommented, but inside the controller, I just set it to always output zero torque, I also get the right behavior.

I kind of feel like meshcat/visualizer is considering t = 0 to be a time that's already several timesteps in. I'm doing the usual thing for simulation/visualization:

# Run the simulation 
    meshcat.StartRecording()
    finish_time = 1
    simulator.AdvanceTo(finish_time)
    meshcat.PublishRecording()
0

There are 0 answers