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):
Instead, it looks like this:
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()