0

I have read multiple resources that say the InverseKinematics class of Drake toolbox is able to solve IK in two fashions: Single-shot IK and IK trajectory optimization using cubic polynomial trajectories. (Link1 Section 4.1, Link2 Section II.B and II.C)
I have already implemented the single-shot IK for a single instant as shown below and is working, Now how do I go about doing it for a whole trajectory using dircol or something? Any documentation to refer to?

// Building a floating-base plant for Ik
drake::multibody::MultibodyPlant<mjtNum> plant{0.0005};
drake::multibody::Parser parser(&plant);
std::string full_name = "model.urdf";
parser.AddModelFromFile(full_name);
plant.Finalize();

drake::multibody::InverseKinematics ik(plant,true);

// Constraints

Eigen::Matrix<mjtNum,33,1> q_ik_guess = Eigen::Matrix<mjtNum,33,1>::Zero();
ik.get_mutable_prog()->SetInitialGuess(ik.q(), q_ik_guess);
const auto result = Solve(ik.prog());
const auto q_sol = result.GetSolution(ik.q());
skywalker
  • 9
  • 1

1 Answers1

0

The IK cubic-polynomial is in an outdated version of Drake. You can check out https://github.com/RobotLocomotion/drake/releases/tag/last_sha_with_original_matlab. In the folder drake/matlab/systems/plants@RigidBodyManipulator/inverseKinTraj.m

Hongkai Dai
  • 2,546
  • 11
  • 12
  • [InverseKinematicsTrajectory.m](https://github.com/openhumanoids/drake/blob/master-oh/drake/solvers/trajectoryOptimization/InverseKinematicsTrajectory.m) is this something similar? – skywalker Oct 20 '21 at 20:15
  • Sorry for my belated reply. Yes InverseKinematicsTrajectory would also work. Unfortunately it is not in the newest Drake either. – Hongkai Dai Oct 20 '21 at 23:45