From 7a56d355b24efcb2bc3b55c617d38cdecde44713 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Sat, 25 Mar 2023 16:09:07 +0100 Subject: [PATCH 1/3] Added set_robot_trajectory_msg to opython bindings --- .../robot_trajectory/robot_trajectory.cpp | 21 +++++++++++++++++++ .../robot_trajectory/robot_trajectory.h | 4 ++++ 2 files changed, 25 insertions(+) diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index 57d00fa68b..74c8de54f8 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -50,6 +50,13 @@ get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_ return msg; } +robot_trajectory::RobotTrajectory set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, + const moveit_msgs::msg::RobotTrajectory& msg) +{ + return robot_trajectory->setRobotTrajectoryMsg(robot_state, msg); +} + void init_robot_trajectory(py::module& m) { py::module robot_trajectory = m.def_submodule("robot_trajectory"); @@ -60,6 +67,14 @@ void init_robot_trajectory(py::module& m) Maintains a sequence of waypoints and the durations between these waypoints. )") + .def(py::init&>(), + R"( + Initializes an empty robot trajectory from a robot model. + + Args: + :py:class:`moveit_py.core.RobotModel`: The robot model associated to the instantiated robot state. + + )") .def("__getitem__", &robot_trajectory::RobotTrajectory::getWayPoint, py::arg("idx"), R"( Get the waypoint at the specified index in the trajectory. @@ -124,6 +139,12 @@ void init_robot_trajectory(py::module& m) Get the trajectory as a `moveit_msgs.msg.RobotTrajectory` message. Returns: moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. + )") + .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg, + py::arg("robot_state"), + py::arg("msg"), + R"( + Set the trajectory from a `moveit_msgs.msg.RobotTrajectory` message. )"); // TODO (peterdavidfagan): support other methods such as appending trajectories } diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h index a8bdb91eb0..d3887ac11e 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h @@ -51,6 +51,10 @@ namespace bind_robot_trajectory moveit_msgs::msg::RobotTrajectory get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, const std::vector& joint_filter); +robot_trajectory::RobotTrajectory +set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, + const moveit_msgs::msg::RobotTrajectory& msg); void init_robot_trajectory(py::module& m); } // namespace bind_robot_trajectory From 32c5f55bceb8e4b08a12123c791448f6c01bf6ad Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Sat, 25 Mar 2023 15:58:12 +0000 Subject: [PATCH 2/3] Updated formatting --- .../robot_trajectory/robot_trajectory.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index 74c8de54f8..9879769216 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -50,11 +50,11 @@ get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_ return msg; } -robot_trajectory::RobotTrajectory set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, - const moveit::core::RobotState& robot_state, - const moveit_msgs::msg::RobotTrajectory& msg) +robot_trajectory::RobotTrajectory +set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg) { - return robot_trajectory->setRobotTrajectoryMsg(robot_state, msg); + return robot_trajectory->setRobotTrajectoryMsg(robot_state, msg); } void init_robot_trajectory(py::module& m) @@ -140,9 +140,8 @@ void init_robot_trajectory(py::module& m) Returns: moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. )") - .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg, - py::arg("robot_state"), - py::arg("msg"), + .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::set_robot_trajectory_msg, + py::arg("robot_state"), py::arg("msg"), R"( Set the trajectory from a `moveit_msgs.msg.RobotTrajectory` message. )"); From 20c7e94af93294e2ca96b6077b13add47a595850 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Sat, 25 Mar 2023 15:59:05 +0000 Subject: [PATCH 3/3] Formatting of header files --- .../src/moveit/moveit_core/robot_trajectory/robot_trajectory.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h index d3887ac11e..6b8f3b7e8b 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h @@ -53,8 +53,7 @@ get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_ const std::vector& joint_filter); robot_trajectory::RobotTrajectory set_robot_trajectory_msg(const std::shared_ptr& robot_trajectory, - const moveit::core::RobotState& robot_state, - const moveit_msgs::msg::RobotTrajectory& msg); + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg); void init_robot_trajectory(py::module& m); } // namespace bind_robot_trajectory