Adds DifferentialInverseKinematicsIntegrator::get_parameters() (#13920

)
RobotLocomotion · Aug 22, 2020 · 3da9e75 · 3da9e75
1 parent 7751155
commit 3da9e75
Show file tree

Hide file tree

Showing 6 changed files with 44 additions and 2 deletions.
diff --git a/bindings/pydrake/examples/acrobot_py.cc b/bindings/pydrake/examples/acrobot_py.cc
@@ -51,7 +51,7 @@ PYBIND11_MODULE(acrobot, m) {
       .def_static("get_mutable_state",
           py::overload_cast<Context<T>*>(&AcrobotPlant<T>::get_mutable_state),
           py::arg("context"),
-          // Keey alive, ownership: `return` keeps `context` alive
+          // Keep alive, ownership: `return` keeps `context` alive
           py::keep_alive<0, 1>(), doc.AcrobotPlant.get_mutable_state.doc)
       .def("get_parameters", &AcrobotPlant<T>::get_parameters,
           py_rvp::reference_internal, py::arg("context"),

diff --git a/bindings/pydrake/manipulation/planner_py.cc b/bindings/pydrake/manipulation/planner_py.cc
@@ -159,7 +159,11 @@ PYBIND11_MODULE(planner, m) {
         .def("SetPositions", &Class::SetPositions, py::arg("context"),
             py::arg("positions"), cls_doc.SetPositions.doc)
         .def("ForwardKinematics", &Class::ForwardKinematics, py::arg("context"),
-            cls_doc.ForwardKinematics.doc);
+            cls_doc.ForwardKinematics.doc)
+        .def("get_parameters", &Class::get_parameters,
+            py_rvp::reference_internal, cls_doc.get_parameters.doc)
+        .def("get_mutable_parameters", &Class::get_mutable_parameters,
+            py_rvp::reference_internal, cls_doc.get_mutable_parameters.doc);
   }
 }
 

diff --git a/bindings/pydrake/manipulation/test/planner_test.py b/bindings/pydrake/manipulation/test/planner_test.py
@@ -97,3 +97,6 @@ def test_diff_ik_integrator(self):
             time_step=time_step,
             parameters=parameters,
             robot_context=context)
+
+        integrator.get_mutable_parameters().set_timestep(0.2)
+        self.assertEqual(integrator.get_parameters().get_timestep(), 0.2)
diff --git a/manipulation/planner/differential_inverse_kinematics_integrator.cc b/manipulation/planner/differential_inverse_kinematics_integrator.cc
@@ -63,6 +63,16 @@ DifferentialInverseKinematicsIntegrator::ForwardKinematics(
          frame_E_.CalcPoseInBodyFrame(robot_context);
 }
 
+const DifferentialInverseKinematicsParameters&
+DifferentialInverseKinematicsIntegrator::get_parameters() const {
+  return parameters_;
+}
+
+DifferentialInverseKinematicsParameters&
+DifferentialInverseKinematicsIntegrator::get_mutable_parameters() {
+  return parameters_;
+}
+
 void DifferentialInverseKinematicsIntegrator::UpdateRobotContext(
     const Context<double>& context,
     Context<double>* robot_context) const {

diff --git a/manipulation/planner/differential_inverse_kinematics_integrator.h b/manipulation/planner/differential_inverse_kinematics_integrator.h
@@ -68,6 +68,14 @@ class DifferentialInverseKinematicsIntegrator
   math::RigidTransformd ForwardKinematics(
       const systems::Context<double>& context) const;
 
+  /** Returns a const reference to the differential IK parameters owned by this
+  system. */
+  const DifferentialInverseKinematicsParameters& get_parameters() const;
+
+  /** Returns a mutable reference to the  differential IK parameters owned by
+  this system. */
+  DifferentialInverseKinematicsParameters& get_mutable_parameters();
+
  private:
   // Updates the position in the cached Context for the robot to match the
   // internal state of the integrator.

diff --git a/manipulation/planner/test/differential_inverse_kinematics_integrator_test.cc b/manipulation/planner/test/differential_inverse_kinematics_integrator_test.cc
@@ -69,6 +69,23 @@ GTEST_TEST(DifferentialInverseKinematicsIntegatorTest, BasicTest) {
                               discrete_state->get_vector(0).get_value()));
 }
 
+GTEST_TEST(DifferentialInverseKinematicsIntegatorTest, ParametersTest) {
+  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);
+
+  EXPECT_EQ(diff_ik.get_parameters().get_timestep(), time_step);
+  diff_ik.get_mutable_parameters().set_timestep(0.2);
+  EXPECT_EQ(diff_ik.get_parameters().get_timestep(), 0.2);
+}
+
 }  // namespace
 }  // namespace planner
 }  // namespace manipulation