在这种情况下，德雷克是否遵守关节限制，我该如何检查？
Is Drake obeying joint limits in this case and how can I check?
我试图了解 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, std::nullopt,
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 和 cout 来检查屏幕。有没有办法从模拟中读取关节状态？
这是我看到的图像的 GIF。但是，仅使用观察结果，关节 joint1 似乎并没有遵守我为其设置的关节限制。
https://user-images.githubusercontent.com/4957157/92673069-130d1d00-f2cf-11ea-8af5-a97a9817e785.gif
Is Drake correctly obeying joint limits?
Drake 在模拟过程中没有严格遵守关节限制。它将关节限制视为一个 spring-damper 系统。当关节超过关节限制时，spring阻尼系统施加更大的恢复力将关节推回到关节限制内。
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?
你可以使用SignalLogger系统，一个例子在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
和Sherm谈过后，我觉得问题是惯性ixx, iyy, izz都为零。我认为如果您将惯性更改为 non-zero 值，那么模拟看起来是正确的。
原因是我们使用相邻连杆的 mass/inertia 属性 来估计关节极限的刚度。当惯性为零时，关节刚度为零。
我试图了解 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, std::nullopt,
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 和 cout 来检查屏幕。有没有办法从模拟中读取关节状态？
这是我看到的图像的 GIF。但是，仅使用观察结果，关节 joint1 似乎并没有遵守我为其设置的关节限制。
https://user-images.githubusercontent.com/4957157/92673069-130d1d00-f2cf-11ea-8af5-a97a9817e785.gif
Is Drake correctly obeying joint limits?
Drake 在模拟过程中没有严格遵守关节限制。它将关节限制视为一个 spring-damper 系统。当关节超过关节限制时，spring阻尼系统施加更大的恢复力将关节推回到关节限制内。
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?
你可以使用SignalLogger系统，一个例子在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
和Sherm谈过后，我觉得问题是惯性ixx, iyy, izz都为零。我认为如果您将惯性更改为 non-zero 值，那么模拟看起来是正确的。
原因是我们使用相邻连杆的 mass/inertia 属性 来估计关节极限的刚度。当惯性为零时，关节刚度为零。