moveit2
The MoveIt Motion Planning Framework for ROS 2.
pyplanning_scene.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021, Peter Mitrano
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * The name of Peter Mitrano may not be used to endorse or promote
18  * products derived from this software without specific prior
19  * written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Peter Mitrano */
36 
37 #include <memory>
38 #include <pybind11/pybind11.h>
39 #include <pybind11/stl.h>
42 
43 namespace py = pybind11;
44 using namespace planning_scene;
45 
46 void def_planning_scene_bindings(py::module& m)
47 {
48  m.doc() = "The planning scene represents the state of the world and the robot, "
49  "and can be used for collision checking";
50 
51  py::class_<PlanningScene, PlanningScenePtr>(m, "PlanningScene")
52  .def(py::init<const moveit::core::RobotModelConstPtr&, const collision_detection::WorldPtr&>(),
53  py::arg("robot_model"), py::arg("world") = std::make_shared<collision_detection::World>())
54  .def("checkSelfCollision",
55  py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
57  .def("checkSelfCollision",
59  const robot_state::RobotState&, const collision_detection::AllowedCollisionMatrix&>(
61  .def("checkCollision",
62  py::overload_cast<const collision_detection::CollisionRequest&, collision_detection::CollisionResult&>(
63  &PlanningScene::checkCollision, py::const_))
64  .def("checkCollision",
66  const robot_state::RobotState&, const collision_detection::AllowedCollisionMatrix&>(
67  &PlanningScene::checkCollision, py::const_))
68  .def("getCurrentStateNonConst", &PlanningScene::getCurrentStateNonConst)
69  .def("getCurrentState", &PlanningScene::getCurrentState)
70  .def("getAllowedCollisionMatrix", &PlanningScene::getAllowedCollisionMatrix)
71  .def("isStateConstrained",
72  py::overload_cast<const moveit_msgs::RobotState&, const kinematic_constraints::KinematicConstraintSet&, bool>(
74  py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
75  .def(
76  "isStateConstrained",
77  py::overload_cast<const moveit::core::RobotState&, const kinematic_constraints::KinematicConstraintSet&, bool>(
79  py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
80  .def("isStateConstrained",
81  py::overload_cast<const moveit_msgs::RobotState&, const moveit_msgs::Constraints&, bool>(
83  py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
84  .def("isStateConstrained",
85  py::overload_cast<const moveit::core::RobotState&, const moveit_msgs::Constraints&, bool>(
87  py::arg("state"), py::arg("constr"), py::arg("verbose") = false)
88  .def("getTransforms", py::overload_cast<>(&PlanningScene::getTransforms, py::const_),
89  py::return_value_policy::reference)
90  // .def("setStateFeasibilityPredicate", &PlanningScene::setStateFeasibilityPredicate)
91  .def("isStateValid",
92  py::overload_cast<const moveit_msgs::RobotState&, const std::string&, bool>(&PlanningScene::isStateValid,
93  py::const_),
94  py::arg("state"), py::arg("group"), py::arg("verbose") = false)
95  .def("isStateValid",
96  py::overload_cast<const moveit::core::RobotState&, const std::string&, bool>(&PlanningScene::isStateValid,
97  py::const_),
98  py::arg("state"), py::arg("group"), py::arg("verbose") = false)
99  .def("isStateValid",
100  py::overload_cast<const moveit_msgs::RobotState&, const moveit_msgs::Constraints&, const std::string&, bool>(
101  &PlanningScene::isStateValid, py::const_),
102  py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false)
103  .def("isStateValid",
104  py::overload_cast<const moveit::core::RobotState&, const moveit_msgs::Constraints&, const std::string&, bool>(
105  &PlanningScene::isStateValid, py::const_),
106  py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false)
107  .def("isStateValid",
109  const std::string&, bool>(&PlanningScene::isStateValid, py::const_),
110  py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false)
111  .def("setCurrentState", py::overload_cast<const moveit_msgs::RobotState&>(&PlanningScene::setCurrentState))
112  .def("setCurrentState", py::overload_cast<const robot_state::RobotState&>(&PlanningScene::setCurrentState))
113  //
114  ;
115 }
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
A class that contains many different constraints, and can check RobotState *versus the full set....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
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...
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...
moveit::core::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
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.
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.
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.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
This namespace includes the central class for representing planning contexts.
void def_planning_scene_bindings(py::module &m)
Representation of a collision checking request.
Representation of a collision checking result.