From f3e99ffffa032838e8f61430ae4d57de82f18a4c Mon Sep 17 00:00:00 2001 From: Akhil Raju Date: Tue, 10 Sep 2024 10:30:16 -0700 Subject: [PATCH] Add Python bindings for the joint-velocity-to-joint-velocity LSQP mapper. PiperOrigin-RevId: 673016410 Change-Id: Id990ce78fca1e748a28559459048824214a26566 --- cpp/controllers_py/CMakeLists.txt | 31 +++ cpp/controllers_py/joint_velocity_filter.cc | 252 ++++++++++++++++++ .../joint_velocity_filter_example.py | 138 ++++++++++ .../joint_velocity_filter_python_helpers.cc | 133 +++++++++ .../joint_velocity_filter_python_helpers.h | 177 ++++++++++++ .../joint_velocity_filter_test.py | 234 ++++++++++++++++ 6 files changed, 965 insertions(+) create mode 100644 cpp/controllers_py/joint_velocity_filter.cc create mode 100644 cpp/controllers_py/joint_velocity_filter_example.py create mode 100644 cpp/controllers_py/joint_velocity_filter_python_helpers.cc create mode 100644 cpp/controllers_py/joint_velocity_filter_python_helpers.h create mode 100644 cpp/controllers_py/joint_velocity_filter_test.py diff --git a/cpp/controllers_py/CMakeLists.txt b/cpp/controllers_py/CMakeLists.txt index 7e3a6ac..cec6a50 100644 --- a/cpp/controllers_py/CMakeLists.txt +++ b/cpp/controllers_py/CMakeLists.txt @@ -105,9 +105,14 @@ set(SRCS ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_6d_to_joint_velocity_mapper.cc ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_6d_to_joint_velocity_mapper_python_helpers.h ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_6d_to_joint_velocity_mapper_python_helpers.cc + ${CMAKE_CURRENT_SOURCE_DIR}/joint_velocity_filter.cc + ${CMAKE_CURRENT_SOURCE_DIR}/joint_velocity_filter_python_helpers.h + ${CMAKE_CURRENT_SOURCE_DIR}/joint_velocity_filter_python_helpers.cc ${CMAKE_CURRENT_SOURCE_DIR}/pybind_utils.h ${CMAKE_CURRENT_SOURCE_DIR}/pybind_utils.cc ) + +# Cartesian 6d mapper pybind11_add_module(cartesian_6d_to_joint_velocity_mapper_pybind ${SRCS} ) @@ -132,3 +137,29 @@ target_link_libraries(cartesian_6d_to_joint_velocity_mapper_pybind dmr_lsqp_core dmr_controllers_lsqp ) + +# Joint velocity mapper +pybind11_add_module(joint_velocity_filter_pybind + ${SRCS} +) +set_target_properties(joint_velocity_filter_pybind + PROPERTIES + OUTPUT_NAME "joint_velocity_filter" +) +target_include_directories(joint_velocity_filter_pybind + PRIVATE + $ +) +target_link_libraries(joint_velocity_filter_pybind + PRIVATE + absl::span + absl::hash + absl::btree + absl::flat_hash_map + absl::hashtablez_sampler + absl::strings + absl::status + dmr_mujoco + dmr_lsqp_core + dmr_controllers_lsqp +) diff --git a/cpp/controllers_py/joint_velocity_filter.cc b/cpp/controllers_py/joint_velocity_filter.cc new file mode 100644 index 0000000..89512e5 --- /dev/null +++ b/cpp/controllers_py/joint_velocity_filter.cc @@ -0,0 +1,252 @@ +// Copyright 2022 DeepMind Technologies Limited. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include "joint_velocity_filter_python_helpers.h" // controller +#include "pybind11/pybind11.h" // pybind +#include "pybind11/stl.h" // pybind +// Internal status include + +namespace dm_robotics { +namespace { + +namespace py = pybind11; + +constexpr char kFilterDocstring[] = R"delim( +Filters joint velocities subject to constraints. + +Filters joint velocities through the use of an LSQP Stack-of-tasks problem +solved at every call to `FilterJointVelocities`. The target joint velocities +are specified as a single vector. + +In its most basic configuration, it computes the joint velocities that are as +close as possible to the desired joint velocities while also supporting the +following functionality: +* Collision avoidance can be enabled for a set of CollisionPair objects, + which defines which geoms should avoid each other. +* Limits on the joint positions and velocities can be defined to ensure that + the computed joint velocities do not result in limit violations. +An error is returned if the filter failed to compute the velocities. +)delim"; + +constexpr char kParametersDocstring[] = R"delim( +Initialization parameters for joint_velocity_filter.JointVelocityFilter. + +Attributes: + model: (`dm_control.MjModel`) MuJoCo model describing the scene. + joint_ids: (`Sequence[int]`) MuJoCo joint IDs of the joints to be controlled. + Only 1 DoF joints are allowed at the moment. + integration_timestep: (`float` or `datetime.timedelta`) amount of time that + the joint velocities will be executed for, A.K.A. 'dt'. If unsure, higher + values are more conservative. This timestep will be used when integrating + the joint velocities to ensure that safety constraints are not violated. + If a `float` is provided, it will be interpreted in seconds. + enable_joint_position_limits: (`bool`) whether to enable joint limit + avoidance. Joint limits are deduced from the MuJoCo model. + joint_position_limit_velocity_scale: (`float`) value (0,1] that defines how + fast each joint is allowed to move towards the joint limits in each + iteration. Values lower than 1 are safer but may make the joints move + slowly. 0.95 is usually enough since it is not affected by Jacobian + linearization. Ignored if `enable_joint_position_limits` is false. + minimum_distance_from_joint_position_limit: (`float`) offset in meters + (slide joints) or radians (hinge joints) to be added to the limits. + Positive values decrease the range of motion, negative values increase it + (i.e. negative values allow penetration). Ignored if + `enable_joint_position_limits` is false. + enable_joint_velocity_limits: (`bool`) whether to enable joint velocity + limits. + joint_velocity_magnitude_limits: array of maximum allowed magnitudes of + joint velocities for each joint, in m/s (slide joints) or rad/s (hinge + joints). Must be ordered according to the `joint_ids` field. Ignored if + `enable_joint_velocity_limits` is false. + enable_collision_avoidance: (`bool`) whether to enable active collision + avoidance. + use_minimum_distance_contacts_only: ('bool') if true, it will only create one + inequality constraint per geom pair, corresponding to the MuJoCo contact + with the minimum distance. Otherwise, it will create one inequality + constraint for each of the MuJoCo contacts detected per geom pair. + Ignored if `enable_collision_avoidance` is `false`. In problems where many + geoms are avoiding each other, setting this option to `true` will + considerably speed up solve times, but the solution is more likely to + result in penetration at high speeds. + collision_avoidance_normal_velocity_scale: (`float`) value between (0, 1] that + defines how fast each geom is allowed to move towards another in each + iteration. Values lower than 1 are safer but may make the geoms move + slower towards each other. In the literature, a common starting value is + 0.85. Ignored if `enable_collision_avoidance` is false. + minimum_distance_from_collisions: (`float`) defines the minimum distance that + the solver will attempt to leave between any two geoms. A negative distance + would allow the geoms to penetrate by the specified amount. Ignored if + `enable_collision_avoidance` is false. + collision_detection_distance: (`float`) defines the distance between two geoms + at which the active collision avoidance behaviour will start. A large value + will cause collisions to be detected early, but may incure high + computational costs. A negative value will cause the geoms to be detected + only after they penetrate by the specified amount. + collision_pairs: (`Sequence[Tuple[Sequence[str], Sequence[str]]]`) set of + collision pairs in which to perform active collision avoidance. A collision + pair is defined as a pair of geom groups. A geom group is a set of geom + names. For each collision pair, the mapper will attempt to compute joint + velocities that avoid collisions between every geom in the first geom group + with every geom in the second geom group. Self collision is achieved by + adding a collision pair with the same geom group in both pair fields. + check_solution_validity: (`bool`) if true, an extra validity check will be + performed on the computed velocities to ensure it does not violate any + constraints. At the moment, this checks whether the computed velocities + would result in increased penetration when integrated. If the computed + velocities are found to be invalid, an exception will be thrown with a + description of why the validity check failed. + max_qp_solver_iterations: (`int`) maximum number of iterations that the + internal LSQP solver is allowed to spend on the joint velocity optimization + problem. If the internal solver is unable to find a feasible solution within + the specified number of iterations, it will throw an exception. + solution_tolerance: (`float`) absolute tolerance for the internal LSQP solver. + A smaller tolerance may be more accurate but may require a large number of + iterations. This tolerance affects the optimality and validity (i.e. + constraint violation) of the joint velocity control optimization problem. + The physical interpretation of the tolerance is different depending on the + task or constraint being considered. For example, when considering the + validity of the solution with respect to the collision avoidance constraint, + this value represents the tolerance of the maximum normal velocity between + any two geoms, in m/s. + use_adaptive_step_size: (`bool`) if true, the internal LSQP solver will use + an adaptive step size when solving the resultant QP problem. Note that + setting this to true can greatly speed up the convergence of the algorithm, + but the solution will no longer be numerically deterministic. + log_collision_warnings: (`bool`) If true, a warning will be logged during + a call to `FilterJointVelocities` if `data` describes a joint configuration + in which one or more collision pairs are closer than the + `minimum_normal_distance` threshold. Ignored if `enable_collision_avoidance` + is false. + +)delim"; + +constexpr char kFilterJointVelocitiesDocstring[] = R"delim( +Filters joint velocities subject to the configured constraints. + +The computed velocities are guaranteed to be valid and respect the +constraints defined by the user, e.g. collision avoidance constraint and +joint limits. + +Returns an error if: +* The internal solver was unable to find a valid solution to the control + optimization problem. +* The internal solver found a solution to the optimization problem(s), but + the computed velocities violate the constraints defined by the user. + This may happen if, for example, MuJoCo collision detection was not + accurate or if errors due to the local Jacobian linearization about the + current configuration cause the computed velocities to violate the + constraints when integrated over the user-defined integration timestep. + +Args: + data: (`dm_control.MjData`) MuJoCo data with the current scene configuration. + target_joint_velocities: (`Sequence[float]`) Array representing the target + joint velocities. +)delim"; + +} // namespace + +PYBIND11_MODULE(joint_velocity_filter, m) { + // Internal status module placeholder. + using dm_robotics::internal::PyJointVelocityFilter; + using dm_robotics::internal::PyJointVelocityFilterParameters; + + // JointVelocityFilter::Parameters. + py::class_(m, "Parameters", + kParametersDocstring) + .def(py::init<>()) + .def_property("model", &PyJointVelocityFilterParameters::GetModel, + &PyJointVelocityFilterParameters::SetModel) + .def_property("joint_ids", &PyJointVelocityFilterParameters::GetJointIds, + &PyJointVelocityFilterParameters::SetJointIds) + .def_property("integration_timestep", + &PyJointVelocityFilterParameters::GetIntegrationTimestep, + &PyJointVelocityFilterParameters::SetIntegrationTimestep) + + .def_property( + "enable_joint_position_limits", + &PyJointVelocityFilterParameters::GetEnableJointPositionLimits, + &PyJointVelocityFilterParameters::SetEnableJointPositionLimits) + .def_property( + "joint_position_limit_velocity_scale", + &PyJointVelocityFilterParameters::GetJointPositionLimitVelocityScale, + &PyJointVelocityFilterParameters::SetJointPositionLimitVelocityScale) + .def_property("minimum_distance_from_joint_position_limit", + &PyJointVelocityFilterParameters:: + GetMinimumDistanceFromJointPositionLimit, + &PyJointVelocityFilterParameters:: + SetMinimumDistanceFromJointPositionLimit) + + .def_property( + "enable_joint_velocity_limits", + &PyJointVelocityFilterParameters::GetEnableJointVelocityLimits, + &PyJointVelocityFilterParameters::SetEnableJointVelocityLimits) + .def_property( + "joint_velocity_magnitude_limits", + &PyJointVelocityFilterParameters::GetJointVelocityMagnitudeLimits, + &PyJointVelocityFilterParameters::SetJointVelocityMagnitudeLimits) + + .def_property( + "enable_collision_avoidance", + &PyJointVelocityFilterParameters::GetEnableCollisionAvoidance, + &PyJointVelocityFilterParameters::SetEnableCollisionAvoidance) + .def_property( + "use_minimum_distance_contacts_only", + &PyJointVelocityFilterParameters::GetUseMinimumDistanceContactsOnly, + &PyJointVelocityFilterParameters::SetUseMinimumDistanceContactsOnly) + .def_property("collision_avoidance_normal_velocity_scale", + &PyJointVelocityFilterParameters:: + GetCollisionAvoidanceNormalVelocityScale, + &PyJointVelocityFilterParameters:: + SetCollisionAvoidanceNormalVelocityScale) + .def_property( + "minimum_distance_from_collisions", + &PyJointVelocityFilterParameters::GetMinimumDistanceFromCollisions, + &PyJointVelocityFilterParameters::SetMinimumDistanceFromCollisions) + .def_property( + "collision_detection_distance", + &PyJointVelocityFilterParameters::GetCollisionDetectionDistance, + &PyJointVelocityFilterParameters::SetCollisionDetectionDistance) + .def_property("collision_pairs", + &PyJointVelocityFilterParameters::GetCollisionPairs, + &PyJointVelocityFilterParameters::SetCollisionPairs) + + .def_property("check_solution_validity", + &PyJointVelocityFilterParameters::GetCheckSolutionValidity, + &PyJointVelocityFilterParameters::SetCheckSolutionValidity) + .def_property("max_qp_solver_iterations", + &PyJointVelocityFilterParameters::GetMaxQpSolverIterations, + &PyJointVelocityFilterParameters::SetMaxQpSolverIterations) + .def_property("solution_tolerance", + &PyJointVelocityFilterParameters::GetSolutionTolerance, + &PyJointVelocityFilterParameters::SetSolutionTolerance) + + .def_property("use_adaptive_step_size", + &PyJointVelocityFilterParameters::GetUseAdaptiveStepSize, + &PyJointVelocityFilterParameters::SetUseAdaptiveStepSize) + .def_property("log_collision_warnings", + &PyJointVelocityFilterParameters::GetLogCollisionWarnings, + &PyJointVelocityFilterParameters::SetLogCollisionWarnings); + + // JointVelocityFilter. + py::class_(m, "JointVelocityFilter", kFilterDocstring) + .def(py::init(), + py::arg("params"), "Initializes a joint velocity filter.") + .def("filter_joint_velocities", + py::overload_cast&>( + &PyJointVelocityFilter::FilterJointVelocities), + py::arg("data"), py::arg("target_joint_velocities"), + kFilterJointVelocitiesDocstring); +} +} // namespace dm_robotics diff --git a/cpp/controllers_py/joint_velocity_filter_example.py b/cpp/controllers_py/joint_velocity_filter_example.py new file mode 100644 index 0000000..b336b40 --- /dev/null +++ b/cpp/controllers_py/joint_velocity_filter_example.py @@ -0,0 +1,138 @@ +# Copyright 2020 DeepMind Technologies Limited. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Example usage for joint_velocity_filter module.""" + +import threading +import time +from typing import Sequence + +from absl import app +from dm_control import mujoco +from dm_control.suite import humanoid +from dm_control.viewer import gui +from dm_robotics.controllers import joint_velocity_filter +import numpy as np + +mjlib = mujoco.wrapper.mjbindings.mjlib + +_WINDOW_HEIGHT = 480 +_WINDOW_WIDTH = 640 + + +def _create_filter_params( + physics: mujoco.Physics, +) -> joint_velocity_filter.Parameters: + """Creates the parameters for the joint velocity filter.""" + # Set params for controlling the left hand, and enabling active collision + # avoidance between the left arm and the floor. + params = joint_velocity_filter.Parameters() + params.model = physics.model + params.joint_ids = [19, 20, 21] + params.integration_timestep = 0.005 # 5ms + + params.enable_joint_position_limits = False + params.joint_position_limit_velocity_scale = 0.95 + params.minimum_distance_from_joint_position_limit = 0.01 # ~0.5deg. + + params.enable_joint_velocity_limits = True + params.joint_velocity_magnitude_limits = [0.5, 0.5, 0.5] + + params.enable_collision_avoidance = True + params.collision_avoidance_normal_velocity_scale = 0.01 + params.minimum_distance_from_collisions = 0.005 + params.collision_detection_distance = 10.0 + params.collision_pairs = [ + (["left_upper_arm", "left_lower_arm", "left_hand"], ["floor"]) + ] + + params.check_solution_validity = True + params.solution_tolerance = 1e-3 + + return params + + +def _control_humanoid(physics: mujoco.Physics, physics_lock: threading.Lock): + """Controls the humanoid's left arm to move towards the floor.""" + + params = _create_filter_params(physics) + joint_vel_filter = joint_velocity_filter.JointVelocityFilter(params) + + # Move the left arm down towards the floor. + # These values were chosen to produce an initial downward motion that later + # isn't purely downwards but would still hit the floor. + target_velocity = [-0.02, 0.02, -0.02] + + while True: + with physics_lock: + # Compute joint velocities. + solution = joint_vel_filter.filter_joint_velocities( + physics.data, target_velocity + ) + + # Set joint velocities. Note that `solution` is already sorted in + # ascending order of `params.joint_ids`. + physics.data.qvel[:] = [0.0] * physics.model.nv + for joint_id, velocity in zip(sorted(params.joint_ids), solution): + dof_adr = physics.model.jnt_dofadr[joint_id] + physics.data.qvel[dof_adr] = velocity + + # Integrate, run MuJoCo kinematics, and render. + mjlib.mj_integratePos( + physics.model.ptr, + physics.data.qpos, + physics.data.qvel, + params.integration_timestep.total_seconds(), + ) + mjlib.mj_fwdPosition(physics.model.ptr, physics.data.ptr) + time.sleep(params.integration_timestep.total_seconds()) + + +def _render_image( + physics: mujoco.Physics, physics_lock: threading.Lock +) -> np.ndarray: + """Returns a view of the scene as a numpy array.""" + with physics_lock: + return physics.render(height=_WINDOW_HEIGHT, width=_WINDOW_WIDTH) + + +def main(argv: Sequence[str]): + del argv + + physics = humanoid.Physics.from_xml_string(*humanoid.get_model_and_assets()) + physics_lock = threading.Lock() + + # Place the humanoid in a position where the left hand can collide with the + # floor if it moves down. + physics.data.qpos[2] = 0.3 + mjlib.mj_fwdPosition(physics.model.ptr, physics.data.ptr) + + # Start control thread to compute velocities and integrate. + control_thread = threading.Thread( + target=lambda: _control_humanoid(physics, physics_lock) + ) + control_thread.start() + + # Show the rendered image. Note how the left arm avoids collisions with the + # floor. + while True: + window = gui.RenderWindow( + width=_WINDOW_WIDTH, + height=_WINDOW_HEIGHT, + title="JointVelocityFilterExample", + ) + window.update(lambda: _render_image(physics, physics_lock)) + + +if __name__ == "__main__": + app.run(main) diff --git a/cpp/controllers_py/joint_velocity_filter_python_helpers.cc b/cpp/controllers_py/joint_velocity_filter_python_helpers.cc new file mode 100644 index 0000000..1775b11 --- /dev/null +++ b/cpp/controllers_py/joint_velocity_filter_python_helpers.cc @@ -0,0 +1,133 @@ +#include "joint_velocity_filter_python_helpers.h" // controller + +#include // NOLINT(build/c++11) +#include +#include + +#include "absl/container/btree_set.h" +#include "absl/status/status.h" +#include "absl/status/statusor.h" +#include "absl/time/time.h" +#include "absl/types/span.h" +#include "dm_robotics/controllers/lsqp/joint_velocity_filter.h" +#include "dm_robotics/least_squares_qp/core/utils.h" +#include "dm_robotics/mujoco/types.h" +#include "mujoco/mujoco.h" +#include "pybind_utils.h" // controller +#include "pybind11/cast.h" // pybind +#include "pybind11/pytypes.h" // pybind + +namespace dm_robotics::internal { +namespace { +namespace py = ::pybind11; + +// Converts a non-ok `status` into a Python exception. +// +// Internal comment. +void RaiseIfNotOk(const absl::Status& status) { + if (status.ok()) return; + internal::RaiseRuntimeErrorWithMessage(status.message()); +} + +// Helper function for throwing if `ValidateParameters` fails, or returning a +// reference to the parameter otherwise. +const JointVelocityFilter::Parameters& RaiseIfNotValid( + const JointVelocityFilter::Parameters& params) { + RaiseIfNotOk(JointVelocityFilter::ValidateParameters(params)); + return params; +} + +} // namespace + +PyJointVelocityFilterParameters::PyJointVelocityFilterParameters() + : py_model_(py::cast(nullptr)) { + params_.model = nullptr; +} + +const JointVelocityFilter::Parameters& +PyJointVelocityFilterParameters::RawParameters() const { + return params_; +} + +PyJointVelocityFilterParameters +PyJointVelocityFilterParameters::FromRawParameters( + const JointVelocityFilter::Parameters& params) { + PyJointVelocityFilterParameters py_params; + py_params.params_ = params; + // Lib and model will not be valid. Leave them as they have been constructed. + return py_params; +} + +py::object PyJointVelocityFilterParameters::GetModel() { return py_model_; } + +void PyJointVelocityFilterParameters::SetModel(py::object model) { + py_model_ = model; + params_.model = internal::GetmjModelOrRaise(model); +} + +std::vector PyJointVelocityFilterParameters::GetJointIds() { + return std::vector(params_.joint_ids.begin(), params_.joint_ids.end()); +} + +void PyJointVelocityFilterParameters::SetJointIds(const std::vector& val) { + params_.joint_ids = absl::btree_set(val.begin(), val.end()); +} + +std::chrono::microseconds +PyJointVelocityFilterParameters::GetIntegrationTimestep() { + return absl::ToChronoMicroseconds(params_.integration_timestep); +} + +void PyJointVelocityFilterParameters::SetIntegrationTimestep( + std::chrono::microseconds val) { + params_.integration_timestep = absl::FromChrono(val); +} + +std::vector +PyJointVelocityFilterParameters::GetCollisionPairs() { + std::vector collision_pairs; + for (const auto& collision_pair : params_.collision_pairs) { + PybindGeomGroup first(collision_pair.first.begin(), + collision_pair.first.end()); + PybindGeomGroup second(collision_pair.second.begin(), + collision_pair.second.end()); + collision_pairs.push_back(PybindCollisionPair(first, second)); + } + return collision_pairs; +} + +void PyJointVelocityFilterParameters::SetCollisionPairs( + const std::vector& collision_pairs) { + for (const auto& collision_pair : collision_pairs) { + GeomGroup first(collision_pair.first.begin(), collision_pair.first.end()); + GeomGroup second(collision_pair.second.begin(), + collision_pair.second.end()); + params_.collision_pairs.insert(CollisionPair(first, second)); + } +} + +PyJointVelocityFilter::PyJointVelocityFilter( + const internal::PyJointVelocityFilterParameters& py_params) + : py_model_(py_params.py_model_), + filter_(RaiseIfNotValid(py_params.params_)), + num_dof_(py_params.params_.joint_ids.size()) {} + +std::vector PyJointVelocityFilter::FilterJointVelocities( + py::handle data, const std::vector& target_joint_velocities) { + // Get solution. + const mjData& data_ref = *internal::GetmjDataOrRaise(data); + absl::StatusOr> solution_or = + filter_.FilterJointVelocities(data_ref, target_joint_velocities); + + // Re-raise SIGINT if it was interrupted. This will propagate SIGINT to + // Python. + if (solution_or.status().code() == absl::StatusCode::kCancelled) { + std::raise(SIGINT); + } + + // Handle error, if any, or return a copy of the solution. + RaiseIfNotOk(solution_or.status()); + return AsCopy(*solution_or); +} + +} // namespace dm_robotics::internal diff --git a/cpp/controllers_py/joint_velocity_filter_python_helpers.h b/cpp/controllers_py/joint_velocity_filter_python_helpers.h new file mode 100644 index 0000000..59f0a5f --- /dev/null +++ b/cpp/controllers_py/joint_velocity_filter_python_helpers.h @@ -0,0 +1,177 @@ +#ifndef DM_ROBOTICS_PY_CONTROLLERS_JOINT_VELOCITY_FILTER_PYTHON_HELPERS_H_ +#define DM_ROBOTICS_PY_CONTROLLERS_JOINT_VELOCITY_FILTER_PYTHON_HELPERS_H_ + +#include // NOLINT(build/c++11) +#include +#include +#include + +#include "dm_robotics/controllers/lsqp/joint_velocity_filter.h" +#include "pybind11/chrono.h" // pybind +#include "pybind11/pybind11.h" // pybind +#include "pybind11/pytypes.h" // pybind +#include "pybind11/stl.h" // pybind +// Internal status include + +namespace dm_robotics::internal { + +using PybindGeomGroup = std::vector; +using PybindCollisionPair = std::pair; + +// Helper class for binding JointVelocityFilter::Parameters that also allows us +// to keep a Python mjModel object alive as long as the instance is alive. +class PyJointVelocityFilterParameters { + public: + PyJointVelocityFilterParameters(); + + // `model` property. + pybind11::object GetModel(); + + void SetModel(pybind11::object model); + + // `joint_ids` property. + std::vector GetJointIds(); + + void SetJointIds(const std::vector& val); + + // `integration_timestep` property. + // Note that in Pybind: + // * `float`s will be interpreted in seconds and casted to a + // `std::chrono::duration`; + // * `datetime.timedelta`s will be casted to a `std::chrono::duration` with + // microsecond precision. + // For more information refer to: + // https://pybind11.readthedocs.io/en/stable/advanced/cast/chrono.html + std::chrono::microseconds GetIntegrationTimestep(); + + void SetIntegrationTimestep(std::chrono::microseconds val); + + // `collision_pairs` property. + std::vector GetCollisionPairs(); + + void SetCollisionPairs( + const std::vector& collision_pairs); + + // MACRO-based property function definitions. +#define PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(var_name, getter_name, \ + setter_name) \ + decltype(JointVelocityFilter::Parameters::var_name) getter_name() { \ + return params_.var_name; \ + } \ + void setter_name( \ + const decltype(JointVelocityFilter::Parameters::var_name)& val) { \ + params_.var_name = val; \ + } + + // `enable_joint_position_limits` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(enable_joint_position_limits, + GetEnableJointPositionLimits, + SetEnableJointPositionLimits) + + // `joint_position_limit_velocity_scale` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY( + joint_position_limit_velocity_scale, GetJointPositionLimitVelocityScale, + SetJointPositionLimitVelocityScale) + + // `minimum_distance_from_joint_position_limit` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY( + minimum_distance_from_joint_position_limit, + GetMinimumDistanceFromJointPositionLimit, + SetMinimumDistanceFromJointPositionLimit) + + // `enable_joint_velocity_limits` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(enable_joint_velocity_limits, + GetEnableJointVelocityLimits, + SetEnableJointVelocityLimits) + + // `joint_velocity_magnitude_limits` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(joint_velocity_magnitude_limits, + GetJointVelocityMagnitudeLimits, + SetJointVelocityMagnitudeLimits) + + // `enable_collision_avoidance` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(enable_collision_avoidance, + GetEnableCollisionAvoidance, + SetEnableCollisionAvoidance) + + // `use_minimum_distance_contacts_only' property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY( + use_minimum_distance_contacts_only, GetUseMinimumDistanceContactsOnly, + SetUseMinimumDistanceContactsOnly) + + // `collision_avoidance_normal_velocity_scale` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY( + collision_avoidance_normal_velocity_scale, + GetCollisionAvoidanceNormalVelocityScale, + SetCollisionAvoidanceNormalVelocityScale) + + // `minimum_distance_from_collisions` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(minimum_distance_from_collisions, + GetMinimumDistanceFromCollisions, + SetMinimumDistanceFromCollisions) + + // `collision_detection_distance` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(collision_detection_distance, + GetCollisionDetectionDistance, + SetCollisionDetectionDistance) + + // `check_solution_validity` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(check_solution_validity, + GetCheckSolutionValidity, + SetCheckSolutionValidity) + + // `max_qp_solver_iterations` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(max_qp_solver_iterations, + GetMaxQpSolverIterations, + SetMaxQpSolverIterations) + + // `solution_tolerance` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(solution_tolerance, + GetSolutionTolerance, + SetSolutionTolerance) + + // `use_adaptive_step_size` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(use_adaptive_step_size, + GetUseAdaptiveStepSize, + SetUseAdaptiveStepSize) + + // `log_collision_warnings` property. + PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY(log_collision_warnings, + GetLogCollisionWarnings, + SetLogCollisionWarnings) +#undef PYBIND_JOINT_VEL_FILTER_PARAMETERS_PROPERTY + + const JointVelocityFilter::Parameters& RawParameters() const; + + static PyJointVelocityFilterParameters FromRawParameters( + const JointVelocityFilter::Parameters& params); + + private: + friend class PyJointVelocityFilter; + pybind11::object py_model_; + JointVelocityFilter::Parameters params_; +}; + +// Helper class for binding JointVelocityFilter that also allows +// us to keep a Python mjModel object alive as long as the instance is alive. +class PyJointVelocityFilter { + public: + explicit PyJointVelocityFilter( + const internal::PyJointVelocityFilterParameters& py_params); + + std::vector FilterJointVelocities( + pybind11::handle data, + const std::vector& target_joint_velocities); + + private: + pybind11::object py_model_; + JointVelocityFilter filter_; + + // We keep a copy of these to detect failed preconditions, which allows us to + // throw an exception on precondition violations. + int num_dof_; +}; + +} // namespace dm_robotics::internal + +#endif // DM_ROBOTICS_PY_CONTROLLERS_JOINT_VELOCITY_FILTER_PYTHON_HELPERS_H_ diff --git a/cpp/controllers_py/joint_velocity_filter_test.py b/cpp/controllers_py/joint_velocity_filter_test.py new file mode 100644 index 0000000..269b1c7 --- /dev/null +++ b/cpp/controllers_py/joint_velocity_filter_test.py @@ -0,0 +1,234 @@ +# Copyright 2020 DeepMind Technologies Limited. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Tests for joint_velocity_filter PyBind11 module.""" + +from absl.testing import absltest +from absl.testing import parameterized +from dm_control import mujoco +from dm_control.suite import humanoid +from dm_robotics.controllers import joint_velocity_filter +import numpy as np + +_MjGeom = mujoco.wrapper.mjbindings.enums.mjtObj.mjOBJ_GEOM +_MjBody = mujoco.wrapper.mjbindings.enums.mjtObj.mjOBJ_BODY +_MjSite = mujoco.wrapper.mjbindings.enums.mjtObj.mjOBJ_SITE +mjlib = mujoco.wrapper.mjbindings.mjlib + + +def _set_joint_velocities(physics, joint_ids, joint_velocities): + """Sets the joint velocities in physics for a subset of joints.""" + for i in range(0, physics.model.nv): + physics.data.qvel[i] = 0.0 + for joint_id, velocity in zip(sorted(joint_ids), joint_velocities): + dof_adr = physics.model.jnt_dofadr[joint_id] + physics.data.qvel[dof_adr] = velocity + + +class JointVelocityFilterTest(absltest.TestCase): + + def test_parameters_attributes(self): + params = joint_velocity_filter.Parameters() + + attributes = sorted( + [attr for attr in dir(params) if not attr.startswith("_")] + ) + expected_attributes = sorted([ + "model", + "joint_ids", + "integration_timestep", + "enable_joint_position_limits", + "joint_position_limit_velocity_scale", + "minimum_distance_from_joint_position_limit", + "enable_joint_velocity_limits", + "joint_velocity_magnitude_limits", + "enable_collision_avoidance", + "use_minimum_distance_contacts_only", + "collision_avoidance_normal_velocity_scale", + "minimum_distance_from_collisions", + "collision_detection_distance", + "collision_pairs", + "check_solution_validity", + "max_qp_solver_iterations", + "solution_tolerance", + "use_adaptive_step_size", + "log_collision_warnings", + ]) + self.assertEqual(expected_attributes, attributes) + + +@parameterized.named_parameters( + ("use_adaptive_step_size", True), + ("do_not_use_adaptive_step_size", False), +) +class JointVelocityFilterParameterizedTest(absltest.TestCase): + + def test_filter_attributes(self, use_adaptive_step_size): + physics = humanoid.Physics.from_xml_string(*humanoid.get_model_and_assets()) + + params = joint_velocity_filter.Parameters() + params.model = physics.model + params.joint_ids = [19, 20, 21] + params.integration_timestep = 1.0 + params.use_adaptive_step_size = use_adaptive_step_size + joint_vel_filter = joint_velocity_filter.JointVelocityFilter(params) + + self.assertTrue(hasattr(joint_vel_filter, "filter_joint_velocities")) + + def test_solution_realizes_target(self, use_adaptive_step_size): + physics = humanoid.Physics.from_xml_string(*humanoid.get_model_and_assets()) + + params = joint_velocity_filter.Parameters() + params.model = physics.model + params.joint_ids = [16, 17, 18] + params.integration_timestep = 1.0 + params.solution_tolerance = 1.0e-15 + params.max_qp_solver_iterations = 300 + params.use_adaptive_step_size = use_adaptive_step_size + joint_vel_filter = joint_velocity_filter.JointVelocityFilter(params) + + # Set target to a realizable velocity and solve. + target_velocity = [ + 0.0450566, + 0.0199436, + 0.0199436, + ] + solution = joint_vel_filter.filter_joint_velocities( + physics.data, target_velocity + ) + _set_joint_velocities(physics, params.joint_ids, solution) + + # Realized joint velocity must be within the specified tolerance of the + # target velocity. + diff_norm = np.linalg.norm(solution - np.array(target_velocity)) + self.assertLess(diff_norm, params.solution_tolerance) + + def test_solution_with_all_constraints_not_in_collision( + self, use_adaptive_step_size + ): + physics = humanoid.Physics.from_xml_string(*humanoid.get_model_and_assets()) + + # Increase collision detection margin for all geoms. + for i in range(0, physics.model.ngeom): + physics.model.geom_margin[i] = 0.01 + + # Place the humanoid in a position where the left hand can collide with the + # floor if it moves down. + physics.data.qpos[2] = 0.3 + mjlib.mj_fwdPosition(physics.model.ptr, physics.data.ptr) + + # Set params with collision avoidance and full list of constraints. + params = joint_velocity_filter.Parameters() + params.model = physics.model + params.joint_ids = [19, 20, 21] + params.integration_timestep = 0.005 # 5ms + + params.enable_joint_position_limits = True + params.joint_position_limit_velocity_scale = 0.95 + params.minimum_distance_from_joint_position_limit = 0.01 # ~0.5deg. + + params.enable_joint_velocity_limits = True + params.joint_velocity_magnitude_limits = [0.5, 0.5, 0.5] + + params.enable_collision_avoidance = True + params.collision_avoidance_normal_velocity_scale = 0.01 + params.minimum_distance_from_collisions = 0.005 + params.collision_detection_distance = 10.0 + params.collision_pairs = [( + ["left_upper_arm", "left_lower_arm", "left_hand"], + ["floor"], + )] + + params.check_solution_validity = True + params.solution_tolerance = 1e-6 + params.max_qp_solver_iterations = 300 + params.use_adaptive_step_size = use_adaptive_step_size + joint_vel_filter = joint_velocity_filter.JointVelocityFilter(params) + + # Approximate the distance of the left hand and floor geoms by the + # difference in Z components minus the radius. + lhand_radius = physics.named.model.geom_size["left_hand"][0] + lhand_floor_dist = ( + physics.named.data.geom_xpos["left_hand"][2] + - physics.named.data.geom_xpos["floor"][2] + - lhand_radius + ) + + # We make the left hand move down towards the plane. + # These values were chosen to produce an initial downward motion that later + # isn't purely downwards but would still hit the floor. + target_velocity = [-0.02, 0.02, -0.02] + + # Compute velocities and integrate, for 5000 steps. + for _ in range(0, 5000): + # Compute joint velocities. + solution = joint_vel_filter.filter_joint_velocities( + physics.data, target_velocity + ) + + # Set joint velocities, integrate, and run MuJoCo kinematics. + _set_joint_velocities(physics, params.joint_ids, solution) + mjlib.mj_integratePos( + physics.model.ptr, + physics.data.qpos, + physics.data.qvel, + params.integration_timestep.total_seconds(), + ) + mjlib.mj_fwdPosition(physics.model.ptr, physics.data.ptr) + + # Compute the new distance between the floor and the left hand. + # We expect the left hand site to get closer to the floor and settle at + # around <0.006. + new_lhand_floor_dist = ( + physics.named.data.geom_xpos["left_hand"][2] + - physics.named.data.geom_xpos["floor"][2] + - lhand_radius + ) + self.assertLess(new_lhand_floor_dist, max(0.006, lhand_floor_dist)) + lhand_floor_dist = new_lhand_floor_dist + + # Ensure there is no contact between any left arm geom and the floor. + for contact in physics.data.contact: + geom1_name = physics.model.id2name(contact.geom1, _MjGeom) + geom2_name = physics.model.id2name(contact.geom2, _MjGeom) + if contact.dist < params.minimum_distance_from_collisions: + is_any_left_hand = ( + geom1_name == "left_hand" or geom2_name == "left_hand" + ) + is_any_left_upperarm = ( + geom1_name == "left_upper_arm" or geom2_name == "left_upper_arm" + ) + is_any_left_lowerarm = ( + geom1_name == "left_lower_arm" or geom2_name == "left_lower_arm" + ) + is_any_left_arm = ( + is_any_left_hand or is_any_left_upperarm or is_any_left_lowerarm + ) + is_any_floor = geom1_name == "floor" or geom2_name == "floor" + + self.assertFalse(is_any_left_arm and is_any_floor) + + def test_invalid_parameters_throws(self, use_adaptive_step_size): + physics = humanoid.Physics.from_xml_string(*humanoid.get_model_and_assets()) + + params = joint_velocity_filter.Parameters() + params.model = physics.model + params.joint_ids = [19, 20, 21] + params.integration_timestep = -1.0 # Not valid. + params.use_adaptive_step_size = use_adaptive_step_size + with self.assertRaises(Exception): + _ = joint_velocity_filter.JointVelocityFilter(params) + + +if __name__ == "__main__": + absltest.main()