0

我试图使用 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？

4

1 回答 1

0

您是否尝试过将该行更改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())，我建议给它一个初始猜测，否则 Drake 将使用零向量作为默认初始猜测。机器人很可能处于具有零向量的奇异配置（雅可比行列式的某些行变为零）。对于许多基于梯度的非线性优化求解器来说，这不是一个好的开始。我建议将其更改为

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

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

于 2019-12-03T03:12:31.130 回答