在这种情况下，Drake是否要遵守关节限制？我该如何检查？

如何解决在这种情况下，Drake是否要遵守关节限制？我该如何检查？

在这个简单的示例中，我试图了解Drake是否遵循关节极限。

我有这个URDF

<?xml version="1.0"?>
<robot name="SimpleDoublePendulum">
  <link name="base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin xyz="0 0.1 0"/>
      <geometry>
        <Box size="1 0.2 1"/>
      </geometry>
      <material name="Red"/>
    </visual>
  </link>
  <link name="upper_arm">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.5 0"/>
      <mass value="1"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0 -0.5 0"/>
      <geometry>
        <cylinder length="1.0" radius="0.1"/>
      </geometry>
      <material name="Green"/>
    </visual>
  </link>
  <link name="lower_arm">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.5 0"/>
      <mass value="1"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0 -0.5 0"/>
      <geometry>
        <cylinder length="1.0" radius="0.1"/>
      </geometry>
      <material name="Blue"/>
    </visual>
  </link>
  <joint name="joint1" type="revolute">
    <parent link="base"/>
    <child link="upper_arm"/>
    <origin rpy="0 0 0" xyz="0 0.0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="0" upper="0.2" effort="5" veLocity="4" />
    <dynamics damping="0.1"/>
  </joint>
  <joint name="joint2" type="revolute">
    <parent link="upper_arm"/>
    <child link="lower_arm"/>
    <origin rpy="0 0 0" xyz="0 -1.0 0"/>
    <axis xyz="0 0 1"/>
    <!-- <limit lower="-1.87" upper="1.87" /> -->
    <dynamics damping="0.1"/>
  </joint>
</robot>

这是我正在运行的代码

// ... 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<double> builder;

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

  // Load and parse double pendulum SDF from file into a tree.
  MultibodyPlant<double>* dp = builder.AddSystem<MultibodyPlant<double>>(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<WeldJoint>("weld_base",dp->world_body(),std::nullopt,root_link,RigidTransform<double>::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<PidController<double>>(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<ConstantVectorSource<double>>(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<Context<double>> diagram_context =
      diagram->CreateDefaultContext();

  // Create plant_context to set veLocity.
  Context<double>& 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<double> 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;
}

我的问题是双重的：

Drake是否正确遵守关节极限？
有没有一种方法可以使用API并检查屏幕上的内容。有没有办法从模拟中读取关节状态？

这是我看到的图像的GIF。但是，仅使用观察结果，似乎joint1并没有遵循我为此设置的联合限制。

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

解决方法

Drake是否正确遵守关节极限？

在仿真过程中，Drake并未严格遵守关节极限。它将接头极限视为弹簧阻尼器系统。当关节超出关节极限时，弹簧阻尼器系统会施加较大的恢复力，将关节推回到关节极限之内。

有没有一种方法可以使用API并在屏幕上进行检查。有没有办法从模拟中读取关节状态？

您可以使用SignalLogger系统，例如https://github.com/RobotLocomotion/drake/blob/9c30d5b73580616badd75905da7733ff64663bb8/examples/manipulation_station/joint_teleop.py#L118-L120中的一个示例，您可以构造一个SignalLogger，将其连接到机器人状态端口，在仿真之后，您可以读取记录的机器人状态，类似于https://github.com/RobotLocomotion/drake/blob/9c30d5b73580616badd75905da7733ff64663bb8/examples/manipulation_station/joint_teleop.py#L157-L163

与Sherm交谈后，我认为问题在于惯性ixx,iyy,izz都为零。我认为，如果将惯性更改为非零值，那么模拟将看起来是正确的。

原因是我们使用相邻链节的质量/惯性属性估计了关节极限的刚度。当惯性为零时，则关节刚度为零。