Support relative frames in DifferentialInverseKinematicsIntegrator (R…

…obotLocomotion#20973)

bindings/pydrake/multibody/inverse_kinematics_py_differential.cc

@@ -201,6 +201,17 @@ void DefineIkDifferential(py::module m) {
     constexpr auto& cls_doc = doc.DifferentialInverseKinematicsIntegrator;
     py::class_<Class, LeafSystem<double>>(
         m, "DifferentialInverseKinematicsIntegrator", cls_doc.doc)
+        .def(py::init<const multibody::MultibodyPlant<double>&,
+                 const multibody::Frame<double>&,
+                 const multibody::Frame<double>&, double,
+                 const DifferentialInverseKinematicsParameters&,
+                 const systems::Context<double>*, bool>(),
+            // Keep alive, reference: `self` keeps `robot` alive.
+            py::keep_alive<1, 2>(), py::arg("robot"), py::arg("frame_A"),
+            py::arg("frame_E"), py::arg("time_step"), py::arg("parameters"),
+            py::arg("robot_context") = nullptr,
+            py::arg("log_only_when_result_state_changes") = true,
+            cls_doc.ctor.doc_7args)
         .def(py::init<const multibody::MultibodyPlant<double>&,
                  const multibody::Frame<double>&, double,
                  const DifferentialInverseKinematicsParameters&,
@@ -210,7 +221,7 @@ void DefineIkDifferential(py::module m) {
             py::arg("time_step"), py::arg("parameters"),
             py::arg("robot_context") = nullptr,
             py::arg("log_only_when_result_state_changes") = true,
-            cls_doc.ctor.doc)
+            cls_doc.ctor.doc_6args)
         .def("SetPositions", &Class::SetPositions, py::arg("context"),
             py::arg("positions"), cls_doc.SetPositions.doc)
         .def("ForwardKinematics", &Class::ForwardKinematics, py::arg("context"),

bindings/pydrake/multibody/test/inverse_kinematics_differential_test.py

@@ -139,18 +139,34 @@ def test_diff_ik_integrator(self):
         Parser(plant).AddModels(file_name)
         plant.Finalize()
 
-        context = plant.CreateDefaultContext()
+        robot_context = plant.CreateDefaultContext()
         frame = plant.GetFrameByName("Link2")
         time_step = 0.1
         parameters = mut.DifferentialInverseKinematicsParameters(2, 2)
 
         integrator = mut.DifferentialInverseKinematicsIntegrator(
             robot=plant,
+            frame_A=plant.world_frame(),
             frame_E=frame,
             time_step=time_step,
             parameters=parameters,
-            robot_context=context,
+            robot_context=robot_context,
             log_only_when_result_state_changes=True)
 
+        context = integrator.CreateDefaultContext()
+        X_AE = integrator.ForwardKinematics(context=context)
+
         integrator.get_mutable_parameters().set_time_step(0.2)
         self.assertEqual(integrator.get_parameters().get_time_step(), 0.2)
+
+        integrator2 = mut.DifferentialInverseKinematicsIntegrator(
+            robot=plant,
+            frame_E=frame,
+            time_step=time_step,
+            parameters=parameters,
+            robot_context=robot_context,
+            log_only_when_result_state_changes=True)
+
+        context2 = integrator2.CreateDefaultContext()
+        X_WE = integrator2.ForwardKinematics(context2)
+        self.assertTrue(X_AE.IsExactlyEqualTo(X_WE))

multibody/inverse_kinematics/differential_inverse_kinematics_integrator.cc

@@ -11,20 +11,28 @@ using systems::Context;
 
 DifferentialInverseKinematicsIntegrator::
     DifferentialInverseKinematicsIntegrator(
-        const MultibodyPlant<double>& robot,
+        const MultibodyPlant<double>& robot, const Frame<double>& frame_A,
         const Frame<double>& frame_E, double time_step,
         const DifferentialInverseKinematicsParameters& parameters,
         const Context<double>* context, bool log_only_when_result_state_changes)
     : robot_(robot),
+      frame_A_(frame_A),
       frame_E_(frame_E),
       parameters_(parameters),
       time_step_(time_step) {
+  DRAKE_DEMAND(frame_A.index() != frame_E.index());
   parameters_.set_time_step(time_step);
 
-  X_WE_desired_index_ = this->DeclareAbstractInputPort(
-                                "X_WE_desired", Value<math::RigidTransformd>{})
+  X_AE_desired_index_ = this->DeclareAbstractInputPort(
+                                "X_AE_desired", Value<math::RigidTransformd>{})
                             .get_index();
 
+  systems::InputPort<double>& X_WE_desired = this->DeclareAbstractInputPort(
+                                "X_WE_desired", Value<math::RigidTransformd>{});
+  this->DeprecateInputPort(X_WE_desired,
+                           "Use the `X_AE_desired` input port instead.");
+  X_WE_desired_index_ = X_WE_desired.get_index();
+
   robot_state_index_ =
       this->DeclareVectorInputPort("robot_state", robot.num_multibody_states())
           .get_index();
@@ -61,6 +69,16 @@ DifferentialInverseKinematicsIntegrator::
       &DifferentialInverseKinematicsIntegrator::UpdateRobotContext);
 }
 
+DifferentialInverseKinematicsIntegrator::
+    DifferentialInverseKinematicsIntegrator(
+        const MultibodyPlant<double>& robot, const Frame<double>& frame_E,
+        double time_step,
+        const DifferentialInverseKinematicsParameters& parameters,
+        const Context<double>* context, bool log_only_when_result_state_changes)
+    : DifferentialInverseKinematicsIntegrator(
+          robot, robot.world_frame(), frame_E, time_step, parameters, context,
+          log_only_when_result_state_changes) {}
+
 void DifferentialInverseKinematicsIntegrator::SetPositions(
     Context<double>* context,
     const Eigen::Ref<const Eigen::VectorXd>& positions) const {
@@ -71,11 +89,11 @@ void DifferentialInverseKinematicsIntegrator::SetPositions(
 math::RigidTransformd
 DifferentialInverseKinematicsIntegrator::ForwardKinematics(
     const Context<double>& context) const {
+  this->ValidateContext(context);
   const Context<double>& robot_context =
       robot_context_cache_entry_->Eval<Context<double>>(context);
 
-  return robot_.EvalBodyPoseInWorld(robot_context, frame_E_.body()) *
-         frame_E_.CalcPoseInBodyFrame(robot_context);
+  return frame_E_.CalcPose(robot_context, frame_A_);
 }
 
 const DifferentialInverseKinematicsParameters&
@@ -107,16 +125,21 @@ systems::EventStatus DifferentialInverseKinematicsIntegrator::Integrate(
     const Context<double>& context,
     systems::DiscreteValues<double>* discrete_state) const {
   const AbstractValue* input =
-      this->EvalAbstractInput(context, X_WE_desired_index_);
-  DRAKE_DEMAND(input != nullptr);
+      this->EvalAbstractInput(context, X_AE_desired_index_);
+  if (!input) {
+    // Continue to support the deprecated input port until removal.
+    DRAKE_THROW_UNLESS(frame_A_.index() == robot_.world_frame().index());
+    input = this->EvalAbstractInput(context, X_WE_desired_index_);
+    DRAKE_DEMAND(input != nullptr);
+  }
   DRAKE_THROW_UNLESS(parameters_.get_time_step() == time_step_);
-  const math::RigidTransformd& X_WE_desired =
+  const math::RigidTransformd& X_AE_desired =
       input->get_value<math::RigidTransformd>();
 
   const Context<double>& robot_context =
       robot_context_cache_entry_->Eval<Context<double>>(context);
   DifferentialInverseKinematicsResult result = DoDifferentialInverseKinematics(
-      robot_, robot_context, X_WE_desired, frame_E_, parameters_);
+      robot_, robot_context, X_AE_desired, frame_A_, frame_E_, parameters_);
 
   const auto& positions = robot_.GetPositions(robot_context);
   if (result.status == DifferentialInverseKinematicsStatus::kNoSolutionFound) {
@@ -127,8 +150,11 @@ systems::EventStatus DifferentialInverseKinematicsIntegrator::Integrate(
     }
     discrete_state->set_value(0, positions);
   } else {
+    Eigen::VectorXd qdot(robot_.num_positions());
+    robot_.MapVelocityToQDot(robot_context, result.joint_velocities.value(),
+                             &qdot);
     discrete_state->set_value(
-        0, positions + time_step_ * result.joint_velocities.value());
+        0, positions + time_step_ * qdot);
   }
 
   if (this->num_discrete_state_groups() > 1) {

multibody/inverse_kinematics/differential_inverse_kinematics_integrator.h

@@ -55,6 +55,7 @@ class DifferentialInverseKinematicsIntegrator
   /** Constructs the system.
 
   @param robot A MultibodyPlant describing the robot.
+  @param frame_A Reference frame (inertial or non-inertial).
   @param frame_E End-effector frame.
   @param time_step the discrete time step of the (Euler) integration.
   @param parameters Collection of various problem specific constraints and
@@ -71,6 +72,40 @@ class DifferentialInverseKinematicsIntegrator
   IsDifferenceEquationSystem() to return `true`.
 
   Note: All references must remain valid for the lifetime of this system.
+  @pre frame_E != frame_A.
+  */
+  DifferentialInverseKinematicsIntegrator(
+      const MultibodyPlant<double>& robot,
+      const Frame<double>& frame_A,
+      const Frame<double>& frame_E,
+      double time_step,
+      // Note: parameters last so they could be optional in the future.
+      const DifferentialInverseKinematicsParameters& parameters,
+      const systems::Context<double>* robot_context = nullptr,
+      bool log_only_when_result_state_changes = true);
+
+  /** Constructs the system.
+
+  @param robot A MultibodyPlant describing the robot.
+  @param frame_E End-effector frame.
+  @param time_step the discrete time step of the (Euler) integration.
+  @param parameters Collection of various problem specific constraints and
+  constants.  The `time_step` parameter will be set to @p time_step.
+  @param robot_context Optional Context of the MultibodyPlant.  The position
+  values of this context will be overwritten during integration; you only need
+  to pass this in if the robot has any non-default parameters.  @default
+  `robot.CreateDefaultContext()`.
+  @param log_only_when_result_state_changes is a boolean that determines whether
+  the system will log on every differential IK failure, or only when the failure
+  state changes.  When the value is `true`, it will cause the system to have an
+  additional discrete state variable to store the most recent
+  DifferentialInverseKinematicsStatus.  Set this to `false` if you want
+  IsDifferenceEquationSystem() to return `true`.
+
+  In this overload, the reference frame, A, is taken to be the world frame.
+
+  Note: All references must remain valid for the lifetime of this system.
+  @pre frame_E != robot.world_frame().
   */
   DifferentialInverseKinematicsIntegrator(
       const MultibodyPlant<double>& robot,
@@ -86,7 +121,7 @@ class DifferentialInverseKinematicsIntegrator
   void SetPositions(systems::Context<double>* context,
                     const Eigen::Ref<const Eigen::VectorXd>& positions) const;
 
-  /** Provides X_WE as a function of the joint position set in `context`. */
+  /** Provides X_AE as a function of the joint position set in `context`. */
   math::RigidTransformd ForwardKinematics(
       const systems::Context<double>& context) const;
 
@@ -120,10 +155,12 @@ class DifferentialInverseKinematicsIntegrator
       systems::DiscreteValues<double>* values) const;
 
   const MultibodyPlant<double>& robot_;
+  const Frame<double>& frame_A_;
   const Frame<double>& frame_E_;
   DifferentialInverseKinematicsParameters parameters_;
   const double time_step_{0.0};
   const systems::CacheEntry* robot_context_cache_entry_{};
+  systems::InputPortIndex X_AE_desired_index_{};
   systems::InputPortIndex X_WE_desired_index_{};
   systems::InputPortIndex robot_state_index_{};
   systems::InputPortIndex use_robot_state_index_{};

multibody/inverse_kinematics/test/differential_inverse_kinematics_integrator_test.cc

@@ -102,6 +102,151 @@ GTEST_TEST(DifferentialInverseKinematicsIntegratorTest, BasicTest) {
   EXPECT_FALSE(CompareMatrices(discrete_state->get_value(0), last_q, 1e-12));
 }
 
+// Sets frame_A to be iiwa_link_0 instead of the world frame.
+GTEST_TEST(DifferentialInverseKinematicsIntegratorTest, FrameATest) {
+  // This time the iiwa will _not_ be welded to the world.
+  auto robot = std::make_unique<multibody::MultibodyPlant<double>>(0.01);
+  multibody::Parser parser(robot.get());
+  const std::string filename = FindResourceOrThrow(
+      "drake/manipulation/models/"
+      "iiwa_description/sdf/iiwa14_no_collision.sdf");
+  parser.AddModels(filename);
+  robot->Finalize();
+
+  auto robot_context = robot->CreateDefaultContext();
+  const multibody::Frame<double>& frame_A =
+      robot->GetFrameByName("iiwa_link_0");
+  const multibody::Frame<double>& frame_E =
+      robot->GetFrameByName("iiwa_link_7");
+
+  DifferentialInverseKinematicsParameters params(robot->num_positions(),
+                                                 robot->num_velocities());
+
+  const double time_step = 0.1;
+  DifferentialInverseKinematicsIntegrator diff_ik(
+      *robot, frame_A, frame_E, time_step, params);
+  auto diff_ik_context = diff_ik.CreateDefaultContext();
+
+  // Set the results status state to a failure state.
+  diff_ik_context->get_mutable_discrete_state(1).SetAtIndex(
+      0, static_cast<double>(
+             DifferentialInverseKinematicsStatus::kNoSolutionFound));
+
+  Eigen::VectorXd q(14);
+  q << 0.1, 0.2,  -.15, 0.43, -0.32, -0.21, 0.11,
+       0.4, 0.21, -.25, 0.67, -0.21, -0.53, 0.21;
+
+  // Test SetPosition and ForwardKinematics.
+  robot->SetPositions(robot_context.get(), q);
+  diff_ik.SetPositions(diff_ik_context.get(), q);
+  EXPECT_TRUE(diff_ik.ForwardKinematics(*diff_ik_context)
+                  .IsExactlyEqualTo(frame_E.CalcPose(*robot_context, frame_A)));
+
+  // Test that discrete update is equivalent to calling diff IK directly.
+  math::RigidTransformd X_AE_desired(Eigen::Vector3d(0.2, 0.0, 0.2));
+  diff_ik.GetInputPort("X_AE_desired")
+      .FixValue(diff_ik_context.get(), X_AE_desired);
+  auto discrete_state = diff_ik.AllocateDiscreteVariables();
+  for (const auto& [data, events] : diff_ik.MapPeriodicEventsByTiming()) {
+    dynamic_cast<const systems::DiscreteUpdateEvent<double>*>(events[0])
+        ->handle(diff_ik, *diff_ik_context, discrete_state.get());
+  }
+
+  // Confirm that the result status state was updated properly.
+  EXPECT_EQ(
+      discrete_state->get_vector(1).GetAtIndex(0),
+      static_cast<double>(DifferentialInverseKinematicsStatus::kSolutionFound));
+
+  params.set_time_step(time_step);  // intentionally set this after diff_ik call
+  DifferentialInverseKinematicsResult result = DoDifferentialInverseKinematics(
+      *robot, *robot_context, X_AE_desired, frame_A, frame_E, params);
+
+  EXPECT_EQ(result.status, DifferentialInverseKinematicsStatus::kSolutionFound);
+  Eigen::VectorXd qdot(robot->num_positions());
+  robot->MapVelocityToQDot(*robot_context, result.joint_velocities.value(),
+                           &qdot);
+  EXPECT_TRUE(CompareMatrices(q + time_step * qdot,
+                              discrete_state->get_vector(0).get_value()));
+}
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+GTEST_TEST(DeprecatedTest, BasicTest) {
+  auto robot = MakeIiwa();
+  auto robot_context = robot->CreateDefaultContext();
+  const multibody::Frame<double>& frame_E =
+      robot->GetFrameByName("iiwa_link_7");
+
+  DifferentialInverseKinematicsParameters params(robot->num_positions(),
+                                                 robot->num_velocities());
+
+  const double time_step = 0.1;
+  DifferentialInverseKinematicsIntegrator diff_ik(
+      *robot, frame_E, time_step, params);
+  auto diff_ik_context = diff_ik.CreateDefaultContext();
+
+  // Set the results status state to a failure state.
+  diff_ik_context->get_mutable_discrete_state(1).SetAtIndex(
+      0, static_cast<double>(
+             DifferentialInverseKinematicsStatus::kNoSolutionFound));
+
+  Eigen::VectorXd q(7);
+  q << 0.1, 0.2, -.15, 0.43, -0.32, -0.21, 0.11;
+
+  // Test SetPosition and ForwardKinematics.
+  robot->SetPositions(robot_context.get(), q);
+  diff_ik.SetPositions(diff_ik_context.get(), q);
+  // Note: BodyPoseInWorld == FramePoseInWorld because this is a body frame.
+  EXPECT_TRUE(diff_ik.ForwardKinematics(*diff_ik_context)
+                  .IsExactlyEqualTo(robot->EvalBodyPoseInWorld(
+                      *robot_context, frame_E.body())));
+
+  // Test that discrete update is equivalent to calling diff IK directly.
+  math::RigidTransformd X_WE_desired(Eigen::Vector3d(0.2, 0.0, 0.2));
+  diff_ik.GetInputPort("X_WE_desired")
+      .FixValue(diff_ik_context.get(), X_WE_desired);
+  auto discrete_state = diff_ik.AllocateDiscreteVariables();
+  for (const auto& [data, events] : diff_ik.MapPeriodicEventsByTiming()) {
+    dynamic_cast<const systems::DiscreteUpdateEvent<double>*>(events[0])
+        ->handle(diff_ik, *diff_ik_context, discrete_state.get());
+  }
+
+  // Confirm that the result status state was updated properly.
+  EXPECT_EQ(
+      discrete_state->get_vector(1).GetAtIndex(0),
+      static_cast<double>(DifferentialInverseKinematicsStatus::kSolutionFound));
+
+  params.set_time_step(time_step);  // intentionally set this after diff_ik call
+  DifferentialInverseKinematicsResult result = DoDifferentialInverseKinematics(
+      *robot, *robot_context, X_WE_desired, frame_E, params);
+
+  EXPECT_EQ(result.status, DifferentialInverseKinematicsStatus::kSolutionFound);
+  EXPECT_TRUE(CompareMatrices(q + time_step * result.joint_velocities.value(),
+                              discrete_state->get_vector(0).get_value()));
+
+  // Add infeasible constraints, and confirm the result state.
+  // clang-format off
+  const auto A = (MatrixX<double>(2, 7) <<
+      1, -1,  0, 0, 0, 0, 0,
+      0,  1, -1, 0, 0, 0, 0).finished();
+  // clang-format on
+  const auto b = Eigen::VectorXd::Zero(A.rows());
+  diff_ik.get_mutable_parameters().AddLinearVelocityConstraint(
+      std::make_shared<solvers::LinearConstraint>(A, b, b));
+  Eigen::VectorXd last_q = robot->GetPositions(*robot_context);
+  for (const auto& [data, events] : diff_ik.MapPeriodicEventsByTiming()) {
+    dynamic_cast<const systems::DiscreteUpdateEvent<double>*>(events[0])
+        ->handle(diff_ik, *diff_ik_context, discrete_state.get());
+  }
+  EXPECT_EQ(discrete_state->get_vector(1).GetAtIndex(0),
+            static_cast<double>(
+                DifferentialInverseKinematicsStatus::kStuck));
+  // kStuck does *not* imply that the positions will not advance.
+  EXPECT_FALSE(CompareMatrices(discrete_state->get_value(0), last_q, 1e-12));
+}
+#pragma GCC diagnostic pop
+
+
 GTEST_TEST(DifferentialInverseKinematicsIntegratorTest, ParametersTest) {
   auto robot = MakeIiwa();
   auto robot_context = robot->CreateDefaultContext();