CODEWITHSUNDEEP
drake
Chen Wang
Chen Wang

Reputation: 528

Initialize `ManipulationStationHardwareInterface` with lcm message

I am trying to use drake to control a kuka iiwa robot and use the ManipulationStationHardwareInterface for Lcm communication. Before deployment on the real robot, I use mock_station_simulation to test. One thing that I find is that after initializing the simulator (which I think should trigger the initialization event?), Eval HardwareInterface's output will give the default values instead of the current lcm message values. For example,

drake::systems::DiagramBuilder<double> builder;
  auto *interface = builder.AddSystem<ManipulationStationHardwareInterface>();
  interface->Connect();
  auto diagram = builder.Build();

  drake::systems::Simulator<double> simulator(*diagram);
  auto &simulator_context = simulator.get_mutable_context();
  auto &interface_context = interface->GetMyMutableContextFromRoot(&simulator_context);
  interface->GetInputPort("iiwa_position").FixValue(&interface_context, current_position);

  simulator.set_publish_every_time_step(false);
  simulator.set_target_realtime_rate(1.0);
  simulator.Initialize();
  auto q = interface->GetOutputPort("iiwa_position_measured").Eval(interface_context);
  std::cout << "after initialization, interface think that the position of robot is" << q << std::endl;

q will be a zero vector. This behavior bothers me when I try to use the robot_state input port of DifferentialInverseKinematicsIntegrator. DifferentialInverseKinematicsIntegrator will use this q to initialize its internal state rather than the robot's real position. The robot will move violently. As a workaround, I need to read the robot's position first and use the SetPositions method of DifferentialInverseKinematicsIntegrator and do not connect the robot_state input port. Another thing is that LogVectorOutput will always have the default value as the first entry, which is of little use.

I think this problem should be related with the LcmSubscriberSystem. My question is that is it possible to use the Lcm message to initialize the system rather than using the default value?

Thank you.

Upvotes: 1

Views: 74

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

This is an interesting (and very reasonable) question. I could imagine having an initialization event for the LcmSubscriber that blocks until the first message to arrive. But currently I don't believe that we guarantee the order of the initialization events in a Diagram (likely the order is determined by something like the order the system was added to the Diagram, and we don't have a nice mechanism for setting it). It's possible that the diff IK block could initialize before the LcmSubscriber.

In this case, I think it might be better to capture the first LcmSubscriber message yourself outside the simulation loop, and manually set the diff IK integrator initial state. Then start the simulation.

I'll see if I can get some of the other Drake developers to weigh in.

Upvotes: 1

Related Questions