在 Drake 模拟中实现空气动力并解析关节角度

Question

我正在尝试计算自定义空气动力，将其应用于 Drake 中的 Multibodyplant（从 SDF 解析的机器人），并使用 Meshcat 进行可视化。当计算速度和空气动力学计算在某些点变得不一致时就会出现问题（有时解析的速度与模拟中显示的速度截然不同）。通过将时间步长减少到 0.00001，这个问题得到了部分修复。

我如何设置力计算和应用是一个外部空气动力学叶系统，该系统的端口连接到多体工厂：

    builder.Connect(aerodynamics_system.get_output_port(), plant.get_applied_spatial_force_input_port());
    builder.Connect(plant.get_body_poses_output_port(), aerodynamics_system.get_input_port(0));
    builder.Connect(plant.get_body_spatial_velocities_output_port(), aerodynamics_system.get_input_port(1));

我想知道这是否是为敏感的小型扑动（50hz）机器人实现空气动力学的有效方法。我正在考虑是否由于具有单独的叶系统而导致系统内存在施力差异。

此构建带来的另一个问题是尝试访问关节角度，因为 multibodyplant 仅提供身体姿势和身体空间速度，我必须使用

right_wing_passive_joint_ = &plant_.GetJointByName<drake::multibody::RevoluteJoint>("joint_RW_J_Pitch");
std::court << right_wing_passive_joint_->get_angle(context) <<  std::endl;

这在我的空气动力学叶系统中出现错误，因为上下文参数需要多体植物上下文而不是空气动力学叶系统。考虑到这一点，我采用了一种可疑的方法，在空气动力学叶系统的 CalcOutput 函数中创建植物环境，并使用它来评估关节角度

    std::unique_ptr<drake::systems::Context<double>> plant_context = plant_.CreateDefaultContext();
    plant_context->SetTime(context.get_time());
    std::cout << "rw joint angle: " << right_wing_passive_joint_->get_angle(*plant_context) << std::endl;

这不起作用，只导致打印 0 值，所以我想知道是否要根据身体位置和方向计算角度。

我希望得到关于如何设计它以实现高效和优化的空气动力学实现以及能够在模拟的每个时间步访问关节角度的建议。如果需要更多代码，请告诉我。

Answer 1

您的模拟需要很小的时间步长这一事实可能是由于松散耦合造成的。目前，空气动力学机器人的空间力计算是滞后的，即空间力是根据前一个时间步的机器人姿态来计算的（请参阅https://github.com/RobotLocomotion/drake/issues/12786 为什么会这样）。这可能会导致高动态场景中的不稳定。

对于你的第二个问题：

std::unique_ptr<drake::systems::Context<double>> plant_context = plant_.CreateDefaultContext();
    plant_context->SetTime(context.get_time());
std::cout << "rw joint angle: " << right_wing_passive_joint_->get_angle(*plant_context) << std::endl;

是错误的做法。它的作用是创建 MultibodyPlant 的默认上下文，将其时间设置为某个值（不修改其他任何内容），并打印出关节角度值，默认情况下为零。

将关节位置提供给控制器的一种方法是

right_wing_passive_joint_ = &plant_.GetJointByName<drake::multibody::RevoluteJoint>("joint_RW_J_Pitch");
// The starting index of the joint in the vector of generalized positions q for the plant.
const int position_start = right_wing_passive_joint_->position_start();
const BasicVector<double>& state = get_state_input_port().Eval(context);
const double joint_angle = state.value()(position_start);

假设您已为控制器打开多体状态输入端口并将其连接到 MultibodyPlant 的 状态输出端口。