修改Drake源码的最优工作流咨询：Python绑定还是全C++实现？

一、所需关键信息的获取方式 #

1. 机器人与物体的最近点对、距离及导数 #

• 虽然CalcDistanceDerivatives是内部函数，但可通过Drake公开API组合实现需求：
  • 调用SceneGraph::ComputeSignedDistancePairwiseClosestPoints获取最近点对与最小距离；
  • 距离对机器人状态q的导数：利用MultibodyPlant::CalcJacobianTranslationalVelocity分别计算两个最近点相对于q的雅可比矩阵，再结合两点间距离向量的单位方向推导得出；最近点自身对q的导数直接取对应点的雅可比即可。
  • 若需直接复用内部逻辑，可在本地源码中临时暴露CalcDistanceDerivatives，或提交PR请求官方公开该接口。

2. 末端执行器姿态相对于q的导数 #

• 无需依赖OrientationConstraint，直接通过MultibodyPlant的公开API实现：
  • 先通过GetBodyByName或GetFrameByName获取末端执行器对应的帧；
  • 调用CalcJacobianAngularVelocity计算姿态角速度相对于q的雅可比矩阵，这就是姿态对q的导数映射；若需要欧拉角、四元数等具体姿态参数的导数，可基于该雅可比结合姿态参数的微分关系进一步推导。

3. 未暴露API的处理 #

• 先查阅Drake官方文档与示例，确认是否存在间接实现路径；
• 若确实无替代方案，再考虑修改源码或向官方提交功能请求。

二、修改Drake源码的最优工作流 #

• 优先方案：为C++代码添加Python绑定，继续使用Python开发

  1. 在Drake的C++源码中，将需要的内部函数/类添加DRAKE_EXPORT标记，确保其可被外部调用；
  2. 在pydrake/bindings目录下的对应模块中添加Python绑定代码，将C++接口暴露给Python；
  3. 本地编译Drake，替换现有环境中的pydrake；
  4. 继续在Python代码中调用修改后的API，保留原有开发流程的便利性。

• 全C++实现的适用场景
  仅当控制器对性能要求极高（如微秒级响应），或逻辑过于复杂导致Python绑定难以实现时，才考虑将核心算法迁移到C++，再通过绑定、ROS节点等方式与原有Python代码交互。

• 通用修改流程

  • Fork Drake官方仓库，创建独立开发分支；
  • 修改C++源码，添加公开接口并补充注释；
  • 编写对应Python绑定代码；
  • 编译并验证修改后的功能；
  • 若修改具备通用性，可提交PR到官方仓库，贡献给社区。

内容的提问来源于stack exchange，提问作者Liqian