如何使用 InverseKinematics 解决 MultibodyPlant 上的 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&，构造这个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 作为初始猜测。

如何使用 InverseKinematics 解决 MultibodyPlant 上的 IK？

How to use InverseKinematics to solve IK on MultibodyPlant?

c++

drake