39 #include <pybind11/operators.h>
43 namespace bind_planning_scene
46 moveit_msgs::msg::CollisionObject& collision_object_msg,
47 std::optional<moveit_msgs::msg::ObjectColor> color_msg)
53 if (color_msg.has_value())
56 planning_scene->setObjectColor(color_msg.value().id, color_msg.value().color);
63 return transformation.matrix();
68 moveit_msgs::msg::PlanningScene planning_scene_msg;
70 return planning_scene_msg;
77 py::class_<planning_scene::PlanningScene, std::shared_ptr<planning_scene::PlanningScene>>(
planning_scene,
80 Representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
83 .def(py::init<const moveit::core::RobotModelConstPtr&, const collision_detection::WorldPtr&>(),
84 py::arg("robot_model"), py::arg(
"world") = std::make_shared<collision_detection::World>())
88 str: The name of the planning scene.
92 py::return_value_policy::move,
94 :py:class:`moveit_py.core.RobotModel`: The robot model associated to this planning scene.
98 py::return_value_policy::move,
100 str: The frame in which planning is performed.
105 py::return_value_policy::reference_internal,
107 :py:class:`moveit_py.core.RobotState`: The current state of the robot.
111 py::return_value_policy::move)
116 py::return_value_policy::reference_internal)
126 .def(
"knows_frame_transform",
127 py::overload_cast<const moveit::core::RobotState&, const std::string&>(
129 py::arg(
"robot_state"), py::arg(
"frame_id"),
131 Check if a transform to the frame id is known. This will be known if id is a link name, an attached body id or a collision object.
134 robot_state (:py:class:`moveit_py.core.RobotState`): The robot state to check.
135 frame_id (str): The frame id to check.
138 bool: True if the transform is known, false otherwise.
141 .def("knows_frame_transform",
145 Check if a transform to the frame id is known. This will be known if id is a link name, an attached body id or a collision object.
148 frame_id (str): The frame id to check.
151 bool: True if the transform is known, false otherwise.
156 Get the transform corresponding to the frame id.
157 This will be known if id is a link name, an attached body id or a collision object. Return identity when no transform is available.
160 frame_id (str): The frame id to get the transform for.
163 :py:class:`numpy.ndarray`: The transform corresponding to the frame id.
169 Process a planning scene world message.
172 msg (:py:class:`moveit_msgs.msg.PlanningSceneWorld`): The planning scene world message.
176 py::arg(
"collision_object_msg"), py::arg(
"color_msg") =
nullptr,
178 Apply a collision object to the planning scene.
181 object (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene.
182 color (moveit_msgs.msg.ObjectColor, optional): The color of the collision object. Defaults to None if not specified.
186 py::arg(
"color_msg"),
188 Set the color of a collision object.
191 object_id (str): The id of the collision object to set the color of.
192 color (std_msgs.msg.ObjectColor): The color of the collision object.
198 Apply an attached collision object to the planning scene.
201 object (moveit_msgs.msg.AttachedCollisionObject): The attached collision object to apply to the planning scene.
204 .def("process_octomap",
208 Apply an octomap to the planning scene.
211 octomap (moveit_msgs.msg.Octomap): The octomap to apply to the planning scene.
216 Removes collision objects from the planning scene.
217 This method will remove all collision object from the scene except for attached collision objects.
221 .def(
"is_state_valid",
222 py::overload_cast<const moveit::core::RobotState&, const std::string&, bool>(
224 py::arg(
"robot_state"), py::arg(
"joint_model_group_name"), py::arg(
"verbose") =
false)
225 .def(
"is_state_colliding",
227 py::arg(
"joint_model_group_name"), py::arg(
"verbose") =
false,
229 Check if the robot state is in collision.
232 joint_model_group_name (str): The name of the group to check collision for.
233 verbose (bool): If true, print the link names of the links in collision.
235 bool: True if the robot state is in collision, false otherwise.
238 .def("is_state_colliding",
239 py::overload_cast<const moveit::core::RobotState&, const std::string&, bool>(
241 py::arg(
"robot_state"), py::arg(
"joint_model_group_name"), py::arg(
"verbose") =
false,
243 Check if the robot state is in collision.
246 robot_state (:py:class:`moveit_py.core.RobotState`): The robot state to check collision for.
247 joint_model_group_name (str): The name of the group to check collision for.
248 verbose (bool): If true, print the link names of the links in collision.
250 bool: True if the robot state is in collision, false otherwise.
253 .def("is_state_constrained",
254 py::overload_cast<const moveit::core::RobotState&, const moveit_msgs::msg::Constraints&, bool>(
256 py::arg(
"state"), py::arg(
"constraints"), py::arg(
"verbose") =
false,
258 Check if the robot state fulfills the passed constraints
261 state (moveit_py.core.RobotState): The robot state to check constraints for.
262 constraints (moveit_msgs.msg.Constraints): The constraints to check.
265 bool: true if state is constrained otherwise false.
268 .def("is_path_valid",
271 py::arg(
"trajectory"), py::arg(
"joint_model_group_name"), py::arg(
"verbose") =
false,
272 py::arg(
"invalid_index") =
nullptr,
274 Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibility)
277 trajectory (:py:class:`moveit_py.core.RobotTrajectory`): The trajectory to check.
278 joint_model_group_name (str): The joint model group to check the path against.
280 invalid_index (list):
283 bool: true if the path is valid otherwise false.
288 .def(
"check_collision",
289 py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
291 py::arg(
"collision_request"), py::arg(
"collision_result"),
293 Check whether the current state is in collision, and if needed, updates the collision transforms of the current state before the computation.
296 collision_request (:py:class:`moveit_py.core.CollisionRequest`): The collision request to use.
297 collision_result (:py:class:`moveit_py.core.CollisionResult`): The collision result to update
300 bool: true if state is in collision otherwise false.
303 .def("check_collision",
306 py::arg(
"collision_request"), py::arg(
"collision_result"), py::arg(
"state"),
308 Check if the robot state is in collision.
311 collision_request ():
316 bool: true if state is in collision otherwise false.
319 .def("check_collision",
323 py::arg(
"collision_request"), py::arg(
"collision_result"), py::arg(
"state"), py::arg(
"acm"),
325 Check if the robot state is in collision.
328 collision_request ():
334 bool: true if state is in collision otherwise false.
337 .def("check_collision_unpadded",
338 py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
340 py::arg(
"req"), py::arg(
"result"),
342 Check if the robot state is in collision.
345 collision_request ():
349 bool: true if state is in collision otherwise false.
352 .def("check_collision_unpadded",
356 py::arg(
"collision_request"), py::arg(
"collision_result"), py::arg(
"state"),
358 Check if the robot state is in collision.
361 collision_request ():
366 bool: true if state is in collision otherwise false.
369 .def("check_collision_unpadded",
373 py::arg(
"collision_request"), py::arg(
"collision_result"), py::arg(
"state"), py::arg(
"acm"),
375 Check if the robot state is in collision.
378 collision_request ():
384 bool: true if state is in collision otherwise false.
387 .def("check_self_collision",
388 py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
390 py::arg(
"collision_request"), py::arg(
"collision_result"),
392 Check if the robot state is in collision.
395 collision_request ():
399 bool: true if state is in collision otherwise false.
402 .def("check_self_collision",
405 py::arg(
"collision_request"), py::arg(
"collision_result"), py::arg(
"state"),
407 Check if the robot state is in collision.
410 collision request ():
415 bool: true if state is in self collision otherwise false.
418 .def("check_self_collision",
422 py::arg(
"collision_request"), py::arg(
"collision_result"), py::arg(
"state"), py::arg(
"acm"),
424 Check if the robot state is in collision.
427 collision request ():
433 bool: true if state is in self collision otherwise false.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
This class maintains the representation of the environment as seen by a planning instance....
void setName(const std::string &name)
Set the name of the planning scene.
void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix &acm)
Set the allowed collision matrix.
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not.
void setCurrentState(const moveit_msgs::msg::RobotState &state)
Set the current robot state to be state. If not all joint values are specified, the previously mainta...
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
bool isPathValid(const moveit_msgs::msg::RobotState &start_state, const moveit_msgs::msg::RobotTrajectory &trajectory, const std::string &group="", bool verbose=false, std::vector< std::size_t > *invalid_index=nullptr) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, and if needed, updates the collision transforms of t...
bool knowsFrameTransform(const std::string &id) const
Check if a transform to the frame id is known. This will be known if id is a link name,...
bool isStateValid(const moveit_msgs::msg::RobotState &state, const std::string &group="", bool verbose=false) const
Check if a given state is valid. This means checking for collisions and feasibility....
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, but use a collision_detection::CollisionRobot instan...
void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose &map)
bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject &object)
bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld &world)
void setObjectColor(const std::string &id, const std_msgs::msg::ColorRGBA &color)
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
const moveit::core::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
bool isStateColliding(const std::string &group="", bool verbose=false)
Check if the current state is in collision (with the environment or self collision)....
const moveit::core::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
bool isStateConstrained(const moveit_msgs::msg::RobotState &state, const moveit_msgs::msg::Constraints &constr, bool verbose=false) const
Check if a given state satisfies a set of constraints.
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
Maintain a sequence of waypoints and the time durations between these waypoints.
void initPlanningScene(py::module &m)
Eigen::MatrixXd getFrameTransform(std::shared_ptr< planning_scene::PlanningScene > &planning_scene, const std::string &id)
moveit_msgs::msg::PlanningScene getPlanningSceneMsg(std::shared_ptr< planning_scene::PlanningScene > &planning_scene)
void applyCollisionObject(std::shared_ptr< planning_scene::PlanningScene > &planning_scene, moveit_msgs::msg::CollisionObject &collision_object_msg, std::optional< moveit_msgs::msg::ObjectColor > color_msg)
This namespace includes the central class for representing planning contexts.
Representation of a collision checking request.
Representation of a collision checking result.