如何在 Drake Toolbox 中设置 IK 轨迹优化？答案

【问题标题】：How to set up IK Trajectory Optimization in Drake Toolbox?如何在 Drake Toolbox 中设置 IK 轨迹优化？
【发布时间】：2021-10-15 20:10:57
【问题描述】：

我阅读了多个资源，说 Drake 工具箱的 InverseKinematics 类能够以两种方式解决 IK：单次 IK 和使用三次多项式轨迹的 IK 轨迹优化。 (Link1 Section 4.1, Link2 Section II.B and II.C)
我已经在一个瞬间实现了单次 IK，如下所示并且正在工作，现在我该如何使用 dircol 或其他东西为整个轨迹执行它？有什么文档可以参考吗？

// Building a floating-base plant for Ik
drake::multibody::MultibodyPlant<mjtNum> plant{0.0005};
drake::multibody::Parser parser(&plant);
std::string full_name = "model.urdf";
parser.AddModelFromFile(full_name);
plant.Finalize();

drake::multibody::InverseKinematics ik(plant,true);

// Constraints

Eigen::Matrix<mjtNum,33,1> q_ik_guess = Eigen::Matrix<mjtNum,33,1>::Zero();
ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
const auto result = Solve(ik.prog());
const auto q_sol = result.GetSolution(ik.q());

【问题讨论】：

标签： c++ robotics drake

【解决方案1】：

IK 三次多项式在 Drake 的过时版本中。您可以查看https://github.com/RobotLocomotion/drake/releases/tag/last_sha_with_original_matlab。在文件夹 drake/matlab/systems/plants@RigidBodyManipulator/inverseKinTraj.m

【讨论】：

InverseKinematicsTrajectory.m 类似吗？
对不起，我迟来的回复。是的 InverseKinematicsTrajectory 也可以。不幸的是，它也不在最新的 Drake 中。