diff --git a/python/moveit_py/src/moveit_py/core.cpp b/python/moveit_py/src/moveit_py/core.cpp index b0a380e5f0f..a2eb32262a3 100644 --- a/python/moveit_py/src/moveit_py/core.cpp +++ b/python/moveit_py/src/moveit_py/core.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -74,8 +75,8 @@ PYBIND11_MODULE(core, m) // JointModelGroup (no direct initialization for now) py::class_(m, "JointModelGroup", R"mydelimiter( - Representation of a group of joints that are part of a robot model. - )mydelimiter") + Representation of a group of joints that are part of a robot model. + )mydelimiter") .def_property("name", &moveit::core::JointModelGroup::getName, nullptr, R"mydelimiter( @@ -90,8 +91,8 @@ PYBIND11_MODULE(core, m) // RobotModel py::class_>(m, "RobotModel", R"mydelimiter( - Representation of a kinematic model. - )mydelimiter") + Representation of a kinematic model. + )mydelimiter") .def(py::init([](std::string& urdf_xml_path, std::string& srdf_xml_path) { // Read in URDF @@ -201,12 +202,12 @@ PYBIND11_MODULE(core, m) // RobotState py::class_>(m, "RobotState", R"mydelimiter( - Representation of a robot's state. + Representation of a robot's state. - At the lowest level, a state is a collection of variables. Each variable has a name and can have position, velocity, acceleration and effort associated to it. Effort and acceleration share the memory area for efficiency reasons (one should not set both acceleration and effort in the same state and expect things to work). Often variables correspond to joint names as well (joints with one degree of freedom have one variable), but joints with multiple degrees of freedom have more variables. Operations are allowed at variable level, joint level (see JointModel) and joint group level (see JointModelGroup). + At the lowest level, a state is a collection of variables. Each variable has a name and can have position, velocity, acceleration and effort associated to it. Effort and acceleration share the memory area for efficiency reasons (one should not set both acceleration and effort in the same state and expect things to work). Often variables correspond to joint names as well (joints with one degree of freedom have one variable), but joints with multiple degrees of freedom have more variables. Operations are allowed at variable level, joint level (see JointModel) and joint group level (see JointModelGroup). For efficiency reasons a state computes forward kinematics in a lazy fashion. This can sometimes lead to problems if the update() function was not called on the state. - )mydelimiter") + )mydelimiter") .def(py::init&>(), R"mydelimiter( @@ -224,6 +225,11 @@ PYBIND11_MODULE(core, m) :py:class:`moveit_py.core.RobotModel`: The robot model instance associated to this robot state. )mydelimiter") + .def_property("dirty", &moveit::core::RobotState::dirty, nullptr, + R"mydelimiter( + bool: True if the robot state is dirty. + )mydelimiter") + .def("get_frame_transform", &moveit_py::bind_robot_state::get_frame_transform, py::arg("frame_id"), py::return_value_policy::move, R"mydelimiter( @@ -231,6 +237,8 @@ PYBIND11_MODULE(core, m) If frame_id was not found, frame_found is set to false and an identity transform is returned. + This method is restricted to frames defined within the robot state and doesn't include collision object present in the collision world. Please use the PlanningScene.get_frame_transform method for collision world objects. + Args: frame_id (str): The id of the frame to get the transform for. @@ -279,7 +287,7 @@ PYBIND11_MODULE(core, m) joint_model_group_name (str): The name of the joint model group to compute the Jacobian for. link_name (str): The name of the link model to compute the Jacobian for. reference_point_position (:py:class:`numpy.ndarray`): The position of the reference point in the link frame. - use_quaternion_representation (bool): If true, the Jacobian will be represented as a quaternion. + use_quaternion_representation (bool): If true, the Jacobian will be represented as a quaternion. Returns: :py:class:`numpy.ndarray`: The Jacobian of the specified group with respect to the reference point. @@ -332,10 +340,10 @@ PYBIND11_MODULE(core, m) position_values (:py:class:`numpy.ndarray`): The positions of the joints in the joint model group. )mydelimiter") - .def("copy_joint_group_positions", &moveit_py::bind_robot_state::copy_joint_group_positions, + .def("get_joint_group_positions", &moveit_py::bind_robot_state::copy_joint_group_positions, py::arg("joint_model_group_name"), R"mydelimiter( - For a given group, copy the position values of the variables that make up the group. + For a given group, get the position values of the variables that make up the group. Args: joint_model_group_name (str): The name of the joint model group to copy the positions for. @@ -356,10 +364,10 @@ PYBIND11_MODULE(core, m) velocity_values (:py:class:`numpy.ndarray`): The velocities of the joints in the joint model group. )mydelimiter") - .def("copy_joint_group_velocities", &moveit_py::bind_robot_state::copy_joint_group_velocities, + .def("get_joint_group_velocities", &moveit_py::bind_robot_state::copy_joint_group_velocities, py::arg("joint_model_group_name"), R"mydelimiter( - For a given group, copy the velocity values of the variables that make up the group. + For a given group, get the velocity values of the variables that make up the group. Args: joint_model_group_name (str): The name of the joint model group to copy the velocities for. @@ -380,10 +388,10 @@ PYBIND11_MODULE(core, m) acceleration_values (:py:class:`numpy.ndarray`): The accelerations of the joints in the joint model group. )mydelimiter") - .def("copy_joint_group_accelerations", &moveit_py::bind_robot_state::copy_joint_group_accelerations, + .def("get_joint_group_accelerations", &moveit_py::bind_robot_state::copy_joint_group_accelerations, py::arg("joint_model_group_name"), R"mydelimiter( - For a given group, copy the acceleration values of the variables that make up the group. + For a given group, get the acceleration values of the variables that make up the group. Args: joint_model_group_name (str): The name of the joint model group to copy the accelerations for. @@ -393,17 +401,16 @@ PYBIND11_MODULE(core, m) )mydelimiter") // Forward kinematics - .def("get_global_link_transform", &moveit_py::bind_robot_state::get_global_link_transform, - py::arg("link_name"), - R"mydelimiter( - Returns the transform of the specified link in the global frame. + .def("get_global_link_transform", &moveit_py::bind_robot_state::get_global_link_transform, py::arg("link_name"), + R"mydelimiter( + Returns the transform of the specified link in the global frame. - Args: - link_name (str): The name of the link to get the transform for. + Args: + link_name (str): The name of the link to get the transform for. - Returns: - :py:class:`numpy.ndarray`: The transform of the specified link in the global frame. - )mydelimiter") + Returns: + :py:class:`numpy.ndarray`: The transform of the specified link in the global frame. + )mydelimiter") // Setting state from inverse kinematics .def("set_from_ik", &moveit_py::bind_robot_state::set_from_ik, py::arg("joint_model_group_name"), @@ -465,20 +472,48 @@ PYBIND11_MODULE(core, m) joint_model_group (:py:class:`moveit_py.core.JointModelGroup`): The joint model group to set the random values for. )mydelimiter") - .def("update", &moveit::core::RobotState::update, py::arg("force") = false, + .def("clear_attached_bodies", py::overload_cast<>(&moveit::core::RobotState::clearAttachedBodies), R"mydelimiter( - Update all transforms. + Clear all attached bodies. + + We only allow for attaching of objects via the PlanningScene instance. This method allows any attached objects that are associated to this RobotState instance to be removed. + )mydelimiter") - Args: - force (bool): - )mydelimiter"); + .def("update", &moveit_py::bind_robot_state::update, py::arg("force") = false, py::arg("type") = "all", + R"mydelimiter( + Update state transforms. + + Args: + force (bool): + category (str): specifies the category to update. All indicates updating all transforms while "links_only" + and "collisions_only" ensure that only links or collision transforms are updated. )mydelimiter"); // RobotTrajectory py::class_>(m, "RobotTrajectory", R"mydelimiter( - Maintains a sequence of waypoints and the durations between these waypoints. - )mydelimiter") + Maintains a sequence of waypoints and the durations between these waypoints. + )mydelimiter") + + .def("__getitem__", &robot_trajectory::RobotTrajectory::getWayPoint, py::arg("idx"), + R"mydelimiter( + Get the waypoint at the specified index in the trajectory. + + Returns: + :py:class:`moveit_py.core.RobotState`: The robot state corresponding to a waypoint at the specified index in the trajectory. + )mydelimiter") + + .def("__len__", &robot_trajectory::RobotTrajectory::getWayPointCount, + R"mydelimiter( + Returns: + int: The number of waypoints in the trajectory. + )mydelimiter") + + .def("__reverse__", &robot_trajectory::RobotTrajectory::reverse, + R"mydelimiter( + Reverse the trajectory. + )mydelimiter") + .def_property("joint_model_group_name", &robot_trajectory::RobotTrajectory::getGroupName, &robot_trajectory::RobotTrajectory::setGroupName, R"mydelimiter( @@ -490,11 +525,6 @@ PYBIND11_MODULE(core, m) :py:class:`moveit_py.core.RobotModel`: The robot model that this trajectory is for. )mydelimiter") - .def_property("num_waypoints", &robot_trajectory::RobotTrajectory::getWayPointCount, nullptr, - R"mydelimiter( - int: The number of waypoints in the trajectory. - )mydelimiter") - .def_property("duration", &robot_trajectory::RobotTrajectory::getDuration, nullptr, R"mydelimiter( float: The duration of the trajectory. @@ -505,23 +535,17 @@ PYBIND11_MODULE(core, m) float: The average duration of the segments in the trajectory. )mydelimiter") - .def("get_waypoint", &robot_trajectory::RobotTrajectory::getWayPoint, py::arg("idx"), + .def("unwind", py::overload_cast<>(&robot_trajectory::RobotTrajectory::unwind), R"mydelimiter( - Get the waypoint at the specified index in the trajectory. - - Args: - idx (int): The index of the waypoint to get. - - Returns: - :py:class:`moveit_py.core.RobotState`: The robot state corresponding to a waypoint at the specified index in the trajectory. - )mydelimiter") + Unwind the trajectory. + )mydelimiter") .def("get_waypoint_durations", &robot_trajectory::RobotTrajectory::getWayPointDurations, R"mydelimiter( - Get the duration of each waypoint in the trajectory. + Get the durations from the previous waypoint in the trajectory. Returns: - list of float: The duration of each waypoint in the trajectory. + list of float: The duration from previous of each waypoint in the trajectory. )mydelimiter"); // TODO (peterdavidfagan): support other methods such as appending trajectories @@ -607,6 +631,22 @@ PYBIND11_MODULE(core, m) dict: The individual cost sources from computed costs. )mydelimiter"); + // Allowed Collision Matrix + py::class_>( + m, "AllowedCollisionMatrix", + R"mydelimiter( + Definition of a structure for the allowed collision matrix. All elements in the collision world are referred to by their names. This class represents which collisions are allowed to happen and which are not. + )mydelimiter") + .def(py::init&, bool>(), + R"mydelimiter( + Initialize the allowed collision matrix using a list of names of collision objects. + + Args: + names (list of str): A list of names of the objects in the collision world (corresponding to object IDs in the collision world). + allowed (bool): If false, indicates that collisions between all elements must be checked for and no collisions will be ignored. + )mydelimiter", + py::arg("names"), py::arg("default_entry") = false); + // Planning Scene py::class_>(m, "PlanningScene", R"mydelimiter( @@ -637,6 +677,8 @@ PYBIND11_MODULE(core, m) :py:class:`moveit_py.core.RobotState`: The current state of the robot. )mydelimiter") + .def_property("planning_scene_message", &moveit_py::bind_planning_scene::get_planning_scene_msg, nullptr, + py::return_value_policy::move) // TODO (peterdavidfagan): requires binding of transform object. //.def_property("transforms", &planning_scene::PlanningScene::getTransforms, nullptr) @@ -687,24 +729,42 @@ PYBIND11_MODULE(core, m) :py:class:`numpy.ndarray`: The transform corresponding to the frame id. )mydelimiter") - .def("get_current_state_updated", &moveit_py::bind_planning_scene::get_current_state_updated, py::arg("update"), + //.def("get_current_state_updated", &moveit_py::bind_planning_scene::get_current_state_updated, py::arg("update"), + // R"mydelimiter( + // Get a copy of the current state with components overwritten by the state message update. + // + // Args: + // update (:py:class:`moveit_msgs.msg.RobotState`): The update to apply to the current state. + // + // Returns: + // :py:class:`moveit_py.core.RobotState`: The current robot state after applying the update. + // )mydelimiter") + + // writing to the planning scene + .def("apply_planning_scene_world", &moveit_py::bind_planning_scene::apply_planning_scene_world, py::arg("scene"), R"mydelimiter( - Get a copy of the current state with components overwritten by the state message update. + Apply a planning scene world message to the current planning scene. Args: - update (:py:class:`moveit_msgs.msg.RobotState`): The update to apply to the current state. + scene (:py:class:`moveit_msgs.msg.PlanningSceneWorld`): The planning scene world message to apply. + )mydelimiter") - Returns: - :py:class:`moveit_py.core.RobotState`: The current robot state after applying the update. - )mydelimiter") + //.def("apply_collision_object", py::overload_cast&, + // py::object&>(&moveit_py::bind_planning_scene::apply_collision_object), py::arg("object"), R"mydelimiter( Apply + // a collision object to the planning scene. + // + // Args: + // object (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene. + //)mydelimiter") - // writing to the planning scene - .def("apply_collision_object", &moveit_py::bind_planning_scene::apply_collision_object, py::arg("object"), + .def("apply_collision_object", &moveit_py::bind_planning_scene::apply_collision_object, + py::arg("collision_object_msg"), py::arg("color_msg") = py::none(), R"mydelimiter( Apply a collision object to the planning scene. Args: object (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene. + color (moveit_msgs.msg.ObjectColor, optional): The color of the collision object. Defaults to None if not specified. )mydelimiter") .def("apply_attached_collision_object", &moveit_py::bind_planning_scene::apply_attached_collision_object, @@ -726,7 +786,9 @@ PYBIND11_MODULE(core, m) .def("remove_all_collision_objects", &planning_scene::PlanningScene::removeAllCollisionObjects, R"mydelimiter( - Remove all collision objects from the planning scene. + Removes collision objects from the planning scene. + + This method will remove all collision object from the scene except for attached collision objects. )mydelimiter") // checking state validity @@ -764,7 +826,7 @@ PYBIND11_MODULE(core, m) .def("is_state_constrained", &moveit_py::bind_planning_scene::is_state_constrained, py::arg("state"), py::arg("constraints"), py::arg("verbose") = false, R"mydelimiter( - Check if the robot state is constrained. + Check if the robot state fulfills the passed constraints Args: state (moveit_py.core.RobotState): The robot state to check constraints for. @@ -824,6 +886,24 @@ PYBIND11_MODULE(core, m) bool: true if state is in collision otherwise false. )mydelimiter") + .def("check_collision", + py::overload_cast( + &planning_scene::PlanningScene::checkCollision, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), py::arg("acm"), + R"mydelimiter( + Check if the robot state is in collision. + + Args: + collision_request (): + collision_result (): + state (): + acm (): + + Returns: + bool: true if state is in collision otherwise false. + )mydelimiter") + .def("check_collision_unpadded", py::overload_cast( &planning_scene::PlanningScene::checkCollisionUnpadded), @@ -856,6 +936,24 @@ PYBIND11_MODULE(core, m) bool: true if state is in collision otherwise false. )mydelimiter") + .def("check_collision_unpadded", + py::overload_cast( + &planning_scene::PlanningScene::checkCollisionUnpadded, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), py::arg("acm"), + R"mydelimiter( + Check if the robot state is in collision. + + Args: + collision_request (): + collision_result (): + state (): + acm (): + + Returns: + bool: true if state is in collision otherwise false. + )mydelimiter") + .def("check_self_collision", py::overload_cast( &planning_scene::PlanningScene::checkSelfCollision), @@ -883,7 +981,26 @@ PYBIND11_MODULE(core, m) collision_result (): state (): + Returns: + bool: true if state is in self collision otherwise false. + )mydelimiter") + + .def("check_self_collision", + py::overload_cast( + &planning_scene::PlanningScene::checkSelfCollision, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), py::arg("acm"), + R"mydelimiter( + Check if the robot state is in collision. + + Args: + collision request (): + collision_result (): + state (): + acm(): + Returns: bool: true if state is in self collision otherwise false. )mydelimiter"); } + diff --git a/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.cpp b/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.cpp index a39bab75d81..2ed080bb8c4 100644 --- a/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.cpp +++ b/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.cpp @@ -44,10 +44,48 @@ namespace moveit_py namespace bind_planning_scene { -void apply_collision_object(std::shared_ptr& planning_scene, py::object& collision_object) +void apply_planning_scene_world(std::shared_ptr& planning_scene, + py::object& planning_scene_world_msg) { - moveit_msgs::msg::CollisionObject collision_object_cpp = CollisionObjectToCpp(collision_object); - planning_scene->processCollisionObjectMsg(collision_object_cpp); + moveit_msgs::msg::PlanningSceneWorld planning_scene_world_cpp; + py::module_ rclpy_serialization = py::module::import("rclpy.serialization"); + py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(planning_scene_world_msg); + deserializeMsg(serialized_msg, planning_scene_world_cpp); + planning_scene->processPlanningSceneWorldMsg(planning_scene_world_cpp); +} + +// void apply_collision_object(std::shared_ptr& planning_scene, py::object& collision_object) +//{ +// moveit_msgs::msg::CollisionObject collision_object_cpp = CollisionObjectToCpp(collision_object); +// planning_scene->processCollisionObjectMsg(collision_object_cpp); +// } + +void apply_collision_object(std::shared_ptr& planning_scene, + py::object& collision_object_msg, std::optional& color_msg) +{ + // convert collision object message to cpp equivalent + moveit_msgs::msg::CollisionObject collision_object_cpp = CollisionObjectToCpp(collision_object_msg); + + // check if color message is provided + if (color_msg.has_value()) + { + // convert color message to cpp equivalent (serialization method used for now) + moveit_msgs::msg::ObjectColor color_cpp; + py::module_ rclpy_serialization = py::module::import("rclpy.serialization"); + py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(color_msg); + deserializeMsg(serialized_msg, color_cpp); + + // apply collision object + planning_scene->processCollisionObjectMsg(collision_object_cpp); + + // set object color + planning_scene->setObjectColor(color_cpp.id, color_cpp.color); + } + else + { + // apply collision object + planning_scene->processCollisionObjectMsg(collision_object_cpp); + } } void apply_attached_collision_object(std::shared_ptr& planning_scene, @@ -76,6 +114,18 @@ Eigen::MatrixXd get_frame_transform(std::shared_ptr& planning_scene) +{ + moveit_msgs::msg::PlanningScene planning_scene_msg; + planning_scene->getPlanningSceneMsg(planning_scene_msg); + py::module_ rclpy_serialization = py::module::import("rclpy.serialization"); + py::bytes serialized_msg = serializeMsg(planning_scene_msg); + + py::module_ moveit_msgs_planning_scene = py::module::import("moveit_msgs.msg._planning_scene"); + py::object planning_scene_msg_py = moveit_msgs_planning_scene.attr("PlanningScene")(); + return rclpy_serialization.attr("deserialize_message")(serialized_msg, planning_scene_msg_py); +} + bool is_path_valid(std::shared_ptr& planning_scene, robot_trajectory::RobotTrajectory& robot_trajectory, std::string& group, bool verbose) { @@ -105,16 +155,16 @@ moveit::core::RobotState& get_current_state(std::shared_ptrgetCurrentStateNonConst(); } -moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr& planning_scene, - py::object update) -{ - moveit_msgs::msg::RobotState robot_state_cpp; - py::module_ rclpy_serialization = py::module::import("rclpy.serialization"); - py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(update); - deserializeMsg(serialized_msg, robot_state_cpp); - - return planning_scene->getCurrentStateUpdated(robot_state_cpp); -} +// moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr& planning_scene, +// py::object update) +//{ +// moveit_msgs::msg::RobotState robot_state_cpp; +// py::module_ rclpy_serialization = py::module::import("rclpy.serialization"); +// py::bytes serialized_msg = rclpy_serialization.attr("serialize_message")(update); +// deserializeMsg(serialized_msg, robot_state_cpp); +// +// return planning_scene->getCurrentStateUpdated(robot_state_cpp); +// } void set_current_state(std::shared_ptr& planning_scene, py::object& robot_state) { diff --git a/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.h b/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.h index 07628265865..f5e32768e72 100644 --- a/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.h +++ b/python/moveit_py/src/moveit_py/moveit_core/planning_scene/planning_scene.h @@ -43,9 +43,17 @@ namespace moveit_py { namespace bind_planning_scene { +void apply_planning_scene_world(std::shared_ptr& planning_scene, + py::object& planning_scene_world_msg); + +// void apply_collision_object(std::shared_ptr& planning_scene, +// py::object& collision_object); void apply_collision_object(std::shared_ptr& planning_scene, - py::object& collision_object); + py::object& collision_object_msg, std::optional& color_msg); + +// void apply_collision_object(std::shared_ptr& planning_scene, py::object& +// collision_object_msg, py::object& color_msg); void apply_attached_collision_object(std::shared_ptr& planning_scene, py::object& attached_collision_object); @@ -55,6 +63,8 @@ void apply_octomap(std::shared_ptr& planning_scen Eigen::MatrixXd get_frame_transform(std::shared_ptr& planning_scene, const std::string& id); +py::object get_planning_scene_msg(std::shared_ptr& planning_scene); + bool is_path_valid(std::shared_ptr& planning_scene, robot_trajectory::RobotTrajectory& robot_trajectory, std::string& group, bool verbose); @@ -68,8 +78,8 @@ bool is_state_constrained(std::shared_ptr& planni moveit::core::RobotState& get_current_state(std::shared_ptr& planning_scene); -moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr& planning_scene, - py::object update); +// moveit::core::RobotStatePtr get_current_state_updated(std::shared_ptr& planning_scene, +// py::object update); void set_current_state(std::shared_ptr& planning_scene, py::object& robot_state); } // namespace bind_planning_scene diff --git a/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp b/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp index ee072264a54..1bb22e0c4a7 100644 --- a/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp +++ b/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp @@ -43,6 +43,27 @@ namespace moveit_py { namespace bind_robot_state { + +void update(std::shared_ptr& robot_state, bool force, std::string& category) +{ + if (category == "all") + { + robot_state->update(force); + } + else if (category == "links_only") + { + robot_state->updateLinkTransforms(); + } + else if (category == "collisions_only") + { + robot_state->updateCollisionBodyTransforms(); + } + else + { + throw std::invalid_argument("Invalid category"); + } +} + Eigen::MatrixXd get_frame_transform(std::shared_ptr& robot_state, std::string& frame_id) { bool* frame_found; @@ -50,7 +71,8 @@ Eigen::MatrixXd get_frame_transform(std::shared_ptr& r return transformation.matrix(); } -std::vector get_global_link_transform(std::shared_ptr& robot_state, std::string& link_name) +std::vector get_global_link_transform(std::shared_ptr& robot_state, + std::string& link_name) { auto transformation = robot_state->getGlobalLinkTransform(link_name); std::vector transforms; @@ -96,7 +118,7 @@ bool set_from_ik(std::shared_ptr& robot_state, const s const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(joint_model_group_name); geometry_msgs::msg::Pose pose_cpp = PoseToCpp(geometry_pose); - + return robot_state->setFromIK(joint_model_group, pose_cpp, tip_name, timeout); } diff --git a/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h b/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h index 6503425b390..0458fdab732 100644 --- a/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h +++ b/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h @@ -45,10 +45,12 @@ namespace moveit_py { namespace bind_robot_state { +void update(std::shared_ptr& robot_state, bool force, std::string& category); Eigen::MatrixXd get_frame_transform(std::shared_ptr& robot_state, std::string& frame_id); -std::vector get_global_link_transform(std::shared_ptr& robot_state, std::string& link_name); +std::vector get_global_link_transform(std::shared_ptr& robot_state, + std::string& link_name); py::object get_pose(std::shared_ptr& robot_state, std::string link_name); diff --git a/python/moveit_py/src/moveit_py/planning.cpp b/python/moveit_py/src/moveit_py/planning.cpp index 04d125dc727..7cf6508e95f 100644 --- a/python/moveit_py/src/moveit_py/planning.cpp +++ b/python/moveit_py/src/moveit_py/planning.cpp @@ -191,6 +191,21 @@ PYBIND11_MODULE(planning, m) Stops the scene monitor. )mydelimiter") + .def("start_state_monitor", &planning_scene_monitor::PlanningSceneMonitor::startStateMonitor, + R"mydelimiter( + Starts the state monitor. + )mydelimiter") + + .def("stop_state_monitor", &planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor, + R"mydelimiter( + Stops the state monitor. + )mydelimiter") + + .def("wait_for_current_robot_state", &planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState, + R"mydelimiter( + Waits for the current robot state to be received. + )mydelimiter") + .def("clear_octomap", &planning_scene_monitor::PlanningSceneMonitor::clearOctomap, R"mydelimiter( Clears the octomap. @@ -370,7 +385,7 @@ PYBIND11_MODULE(planning, m) R"mydelimiter( Specify the workspace bounding box. - The box is specified in the planning frame (i.e. relative to the robot root link start position). This is useful when the planning group contains the root joint of the robot – i.e. when planning motion for the robot relative to the world. + The box is specified in the planning frame (i.e. relative to the robot root link start position). The workspace applies only to the root joint of a mobile robot (driving base, quadrotor) and does not limit the workspace of a robot arm. Args: min_x (float): The minimum x value of the workspace.