如何在Drake工具箱中设置IK轨迹优化?

Question

我已经阅读了多个资源，说Drake工具箱的InverseKinematics类能够以两种方式解决IK:使用三次多项式轨迹的单次IK和IK轨迹优化。(Link1 Section 4.1, Link2 Section II。B和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());

Answer 1

IK立方多项式在过时版本的Drake中。你可以登录https://github.com/RobotLocomotion/drake/releases/tag/lastrongha_with_original_matlab。在文件夹drake/matlab/systems/plants@RigidBodyManipulator/inverseKinTraj.m