How to use InverseKinematics to solve IK on MultibodyPlant?

Question

I was trying to use InverseKinematics to solve the IK for a MultibodyPlant. I was trying to add some position constraints to it and then do the solver.

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();

But the compiler gives the errors

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");

I am not sure if I used the InverseKinematics correctly. Does anyone have solutions for this? Or maybe other ways to do the IK for RigidbodyTree?

Hongkai Dai · Accepted Answer · 2019-12-03T18:19:32.700

Have you tried to change that line auto world = plant.world_frame() to

const auto& world = plant.world_frame();

When you use auto, the compiler probably tries to infer the type as BodyFrame, but we don't have a copy constructor for BodyFrame. If you change it to const auto&, the type of world is const BodyFrame&, constructing this const reference doesn't need to call the copy constructor.

Similarly for the line auto COM = plant.GetFrameByName("hip_mass");, this line should be changed to

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

BTW, the hip mass frame is very likely not exactly where the center of mass (COM) is.

And when you call Solve(kinsol.prog()), I would suggest to give it an initial guess, otherwise Drake will use a zero vector as the default initial guess. Very likely the robot is in a singular configuration with zero vector (some rows of your Jacobian become zero). That is not a good initial start for many gradient-based nonlinear optimization solvers. I would recommend to change it to

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

that you use a non-zero vector q_guess as the initial guess.

How to use InverseKinematics to solve IK on MultibodyPlant?

1 Answers1