如何在MultibodyPlant上使用InverseKinematics解决IK问题？

Question

我正在尝试使用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");

我不确定我是否正确使用了InverseKinematics。有人对此有解决方案吗？或者可能是为RigidbodyTree做IK的其他方法？

Answer 1

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

const auto& world = plant.world_frame();

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

与auto COM = plant.GetFrameByName("hip_mass");行类似，应将此行更改为

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

顺便说一句，臀部质量框架很可能不是精确的质心(COM)。

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

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

你使用一个非零向量q_guess作为初始猜测。