diff --git a/bindings/pydrake/examples/acrobot_py.cc b/bindings/pydrake/examples/acrobot_py.cc index b2d5911c88e8..290b352d891e 100644 --- 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*>(&AcrobotPlant::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::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 index 1ba370d6d635..f0fa8c7866f8 100644 --- 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 index 2976ed837355..ebdd14098e46 100644 --- 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 index 07b9dd0c222a..2f9effd32d1b 100644 --- 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& context, Context* robot_context) const { diff --git a/manipulation/planner/differential_inverse_kinematics_integrator.h b/manipulation/planner/differential_inverse_kinematics_integrator.h index 96a223e530bb..98dbfd98b6e1 100644 --- 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& 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 index 21341faacd92..aa1aa7a97419 100644 --- 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& 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