Is Drake obeying joint limits in this case and how can I check?

Question

I'm trying to understand if Drake is following joint limits in this simple example.

I have this URDF

Here is the code I'm running

// ... includes and using

double target_realtime_rate = 1.0;
double simulation_time = 1000;
double max_time_step = 1.0e-4;
double Kp_ = 1.0;
double Ki_ = 0.0;
double Kd_ = 0.0;

// Fixed path to double pendulum URDF model.
static const char* const kDoublePendulumSdfPath = "double_pendulum/pendulum.urdf";


void DoMain() {
  DRAKE_DEMAND(simulation_time > 0);

  DiagramBuilder builder;

  SceneGraph& scene_graph = *builder.AddSystem();
  scene_graph.set_name("scene_graph");

  // Load and parse double pendulum SDF from file into a tree.
  MultibodyPlant* dp = builder.AddSystem>(max_time_step);
  dp->set_name("plant");
  dp->RegisterAsSourceForSceneGraph(&scene_graph);

  Parser parser(dp);
  parser.AddModelFromFile(kDoublePendulumSdfPath);

  // Weld the base link to world frame with no rotation.
  const auto& root_link = dp->GetBodyByName("base");
  dp->AddJoint("weld_base", dp->world_body(), std::nullopt,
                          root_link, std::nullopt,
                          RigidTransform::Identity());
  dp->AddJointActuator("a2", dp->GetJointByName("joint2"));
  dp->AddJointActuator("a1", dp->GetJointByName("joint1"));

  // Now the plant is complete.
  dp->Finalize();

  // Create PID Controller.
  const Eigen::VectorXd Kp = Eigen::Vector2d(1,1) * Kp_;
  const Eigen::VectorXd Ki = Eigen::Vector2d(1,1) * Ki_;
  const Eigen::VectorXd Kd = Eigen::Vector2d(1,1) * Kd_;
  const auto* const pid = builder.AddSystem>(Kp, Ki, Kd);
  builder.Connect(dp->get_state_output_port(),
                  pid->get_input_port_estimated_state());
  builder.Connect(pid->get_output_port_control(),
                  dp->get_actuation_input_port());
  // Set PID desired states.
  auto desired_base_source = builder.AddSystem>(Eigen::VectorXd::Zero(dp->num_multibody_states()));
  builder.Connect(desired_base_source->get_output_port(),
                  pid->get_input_port_desired_state());

  // Connect plant with scene_graph to get collision information.
  DRAKE_DEMAND(!!dp->get_source_id());
  builder.Connect(dp->get_geometry_poses_output_port(),
                  scene_graph.get_source_pose_port(dp->get_source_id().value()));
  builder.Connect(scene_graph.get_query_output_port(),
                  dp->get_geometry_query_input_port());

  ConnectDrakeVisualizer(&builder, scene_graph);

  auto diagram = builder.Build();
  std::unique_ptr> diagram_context =
      diagram->CreateDefaultContext();

  // Create plant_context to set velocity.
  Context& plant_context =
      diagram->GetMutableSubsystemContext(*dp, diagram_context.get());
  // Set init position.
  Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);
  positions[0] = 0.1;
  positions[1] = 0.4;
  dp->SetPositions(&plant_context, positions);

  Simulator simulator(*diagram, std::move(diagram_context));
  simulator.set_publish_every_time_step(true);
  simulator.set_target_realtime_rate(target_realtime_rate);
  simulator.Initialize();
  simulator.AdvanceTo(simulation_time);
}

int main(int argc, char** argv) {
  DoMain();
  return 0;
}

My question is twofold:

Is Drake correctly obeying joint limits?
Is there a way to check this using the API and cout to the screen. Is there a way to read joint state from the simulation?

This is a GIF of the images that I see. However, just using observations it doesn't look like the joint joint1 is obeying the joint limits I set for it.

https://user-images.githubusercontent.com/4957157/92673069-130d1d00-f2cf-11ea-8af5-a97a9817e785.gif

Hongkai Dai · Accepted Answer

Is Drake correctly obeying joint limits?

Drake doesn't strictly obey the joint limits during simulation. It treats the joint limits as if a spring-damper system. When the joint exceeds the joint limits, the spring damper system applies a larger restoration force to push the joint back to within the joint limits.

Is there a way to check this using the API and cout to the screen. Is there a way to read joint state from the simulation?

You could use SignalLogger system, one example is in https://github.com/RobotLocomotion/drake/blob/9c30d5b73580616badd75905da7733ff64663bb8/examples/manipulation_station/joint_teleop.py#L118-L120, you could construct a SignalLogger, connect it to the robot state port, and after the simulation, you could read the logged robot state, similar to https://github.com/RobotLocomotion/drake/blob/9c30d5b73580616badd75905da7733ff64663bb8/examples/manipulation_station/joint_teleop.py#L157-L163

Is Drake obeying joint limits in this case and how can I check?

Answers (2)

Related Questions