如何使用逆运动学在多体工厂上求解 IK?

How to use InverseKinematics to solve IK on MultibodyPlant?

本文关键字：IK 工厂 何使用 运动学      更新时间：2023-10-16

我试图使用InverseKinematics来解决MultibodyPlant的IK。我试图向它添加一些位置约束，然后做求解器。

drake::multibody::InverseKinematics kinsol(plant);
auto world = plant.world_frame();
Eigen::Ref<const Eigen::Vector3d> p_BQ;
Eigen::Ref<const Eigen::Vector3d> p_AQ_lower;
Eigen::Ref<const Eigen::Vector3d> p_AQ_upper;
auto COM = plant.GetFrameByName("hip_mass");
p_BQ << ...
p_AQ_lower << ...
p_AQ_upper << ...
auto pos1 = kinsol.AddPositionConstraint(COM, p_BQ, world, p_AQ_lower, p_AQ_upper);
drake::solvers::MathematicalProgramResult result = drake::solvers::Solve(kinsol.prog());
auto result_vec = result.GetSolution();

但是编译器给出了错误

error: call to deleted constructor of 'drake::multibody::BodyFrame<double>'
auto world = plant.world_frame();
^       ~~~~~~~~~~~~~~~~~
bazel-out/k8-py2-opt/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/body.h:68:35: note: 'BodyFrame' has been explicitly marked deleted here
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BodyFrame)

error: no matching constructor for initialization of 'Eigen::Ref<const Eigen::Vector3d>' (aka 'Ref<const Matrix<double, 3, 1> >')
Eigen::Ref<const Eigen::Vector3d> p_BQ;
error: variable type 'drake::multibody::Frame<double>' is an abstract class
auto COM = plant.GetFrameByName("hip_mass");

我不确定我是否正确使用了反向运动学。有人对此有解决方案吗？或者也许还有其他方法可以为RigidbodyTree做IK？

您是否尝试过将该行auto world = plant.world_frame()更改为

const auto& world = plant.world_frame();

当你使用auto时，编译器可能会尝试推断类型为BodyFrame，但我们没有BodyFrame的复制构造函数。如果将其更改为const auto&，则world的类型为const BodyFrame&，构造此 const 引用不需要调用复制构造函数。

同样，对于行auto COM = plant.GetFrameByName("hip_mass");，此行应更改为

const auto& COM = plant.GetFrameByName("hip_mass");

顺便说一句，臀部质量框架很可能不完全是质心 (COM( 的位置。

当你调用Solve(kinsol.prog())时，我建议给它一个初步猜测，否则德雷克将使用零向量作为默认的初始猜测。机器人很可能处于零向量的奇异配置中(雅可比的某些行变为零(。对于许多基于梯度的非线性优化求解器来说，这不是一个好的初始开始。我建议将其更改为

const Eigen::VectorXd q_guess = Eigen::VectorXd::Constant(plant.num_positions(), 0.1);
const auto result = Solve(kinsol.prog(), q_guess);

您使用非零向量q_guess作为初始猜测。

相关文章：