Fix demo/scripts/fixed_state.py

This commit is contained in:
Robert Haschke 2021-12-30 15:18:21 +01:00
parent 0fbdfbc818
commit d732bfe3eb
3 changed files with 32 additions and 33 deletions

View File

@ -568,7 +568,8 @@ void export_core(pybind11::module& m) {
.def_property("name", &Task::name, &Task::setName, R"pbdoc(
str: Set the name property of the task.
)pbdoc")
.def("loadRobotModel", &Task::loadRobotModel, R"pbdoc(
.def("loadRobotModel", &Task::loadRobotModel, py::arg("robot_description") = "robot_description",
R"pbdoc(
loadRobotModel(robot_description)
Args:
@ -578,6 +579,7 @@ void export_core(pybind11::module& m) {
Load robot model from given parameter.
)pbdoc")
.def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, py::arg("enabled") = true, R"pbdoc(
enableIntrospection(enable)

View File

@ -177,7 +177,6 @@ void export_stages(pybind11::module& m) {
.. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html
)pbdoc");
// clang-format on
properties::class_<CurrentState, Stage>(m, "CurrentState", R"pbdoc(
CurrentState(name)
@ -205,7 +204,7 @@ void export_stages(pybind11::module& m) {
:language: python
)pbdoc")
.def("setState", &FixedState::setState, R"pbdoc(
.def("setState", &FixedState::setState, R"pbdoc(
setState(scene)
Use a planning scene pointer to specify which state the Fixed State
@ -418,7 +417,7 @@ void export_stages(pybind11::module& m) {
)pbdoc")
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>())
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection),
R"pbdoc(
R"pbdoc(
setDirection(twist)
1. Perform twist motion on specified link.
@ -431,7 +430,7 @@ void export_stages(pybind11::module& m) {
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)pbdoc")
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection),
R"pbdoc(
R"pbdoc(
2. Translate link along given direction.
Args:
@ -442,7 +441,7 @@ void export_stages(pybind11::module& m) {
.. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html
)pbdoc")
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection),
R"pbdoc(
R"pbdoc(
3. Move specified joint variables by given amount.
Args:
@ -529,15 +528,16 @@ void export_stages(pybind11::module& m) {
:lines: 115-122
)pbdoc")
.property<std::string>("object", R"pbdoc(
.property<std::string>("object", R"pbdoc(
str: Name of the object in the planning scene,
attached to the robot which should be placed.
)pbdoc")
.property<std::string>("eef", R"pbdoc(
.property<std::string>("eef", R"pbdoc(
str: Name of the end effector that should be used for grasping.
)pbdoc")
.property<geometry_msgs::PoseStamped>("pose")
.def(py::init<const std::string&>(), py::arg("name") = std::string("Generate Place Pose"));
.property<geometry_msgs::PoseStamped>("pose")
.def(py::init<const std::string&>(), py::arg("name") = std::string("Generate Place Pose"));
properties::class_<GenerateGraspPose, MonitoringGenerator>(m, "GenerateGraspPose", R"pbdoc(
GenerateGraspPose(name)
@ -657,8 +657,7 @@ void export_stages(pybind11::module& m) {
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)pbdoc")
.def("setLiftMotion",
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion), R"pbdoc(
.def("setLiftMotion", py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion), R"pbdoc(
setLiftMotion(motion, min_distance, max_distance)
1. The lifting motion away from the grasping state is represented by a twist message.
@ -724,7 +723,7 @@ void export_stages(pybind11::module& m) {
.property<std::string>("eef_parent_group", R"pbdoc(
str: Joint model group of the eef's parent.
)pbdoc")
.def("setRetractMotion", &Place::setRetractMotion, R"pbdoc(
.def("setRetractMotion", &Place::setRetractMotion, R"pbdoc(
setRetractMotion(motion, min_distance, max_distance)
Args:
@ -740,8 +739,7 @@ void export_stages(pybind11::module& m) {
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)pbdoc")
.def("setPlaceMotion",
py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Place::setPlaceMotion), R"pbdoc(
.def("setPlaceMotion", py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Place::setPlaceMotion), R"pbdoc(
setPlaceMotion(motion, min_distance, max_distance)
1. The object-placing motion towards the final state is represented by a twist message.
@ -792,7 +790,7 @@ void export_stages(pybind11::module& m) {
.property<std::string>("object", R"pbdoc(
str: The object to grasp (Must be present in the planning scene).
)pbdoc")
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
PoseStamped_: Set the frame for which
the inverse kinematics are calculated
with respect to each pose generated by
@ -815,8 +813,7 @@ void export_stages(pybind11::module& m) {
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)pbdoc")
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame,
R"pbdoc(
.def<void (SimpleGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc(
2. Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
@ -835,7 +832,7 @@ void export_stages(pybind11::module& m) {
Returns:
None
)pbdoc")
.def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"pbdoc(
.def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"pbdoc(
setMaxIKSolutions(max_ik_solutions)
Args:
@ -876,21 +873,20 @@ void export_stages(pybind11::module& m) {
.property<std::string>("object", R"pbdoc(
str: The object to grasp (Must be present in the planning scene).
)pbdoc")
.property<std::string>("pregrasp", R"pbdoc(
.property<std::string>("pregrasp", R"pbdoc(
str: Name of the pre-grasp pose.
)pbdoc")
.property<std::string>("grasp", R"pbdoc(
.property<std::string>("grasp", R"pbdoc(
str: Name of the grasp pose.
)pbdoc")
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
.property<geometry_msgs::PoseStamped>("ik_frame", R"pbdoc(
PoseStamped_: Specify the frame with respect
to which the inverse kinematics
should be calculated.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)pbdoc")
.def<void (SimpleUnGrasp::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleUnGrasp::setIKFrame,
R"pbdoc(
.def<void (SimpleUnGrasp::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc(
setIKFrame(transform)
1. Set the frame for which the inverse kinematics are calculated
@ -903,8 +899,7 @@ void export_stages(pybind11::module& m) {
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)pbdoc")
.def<void (SimpleUnGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame",
&SimpleUnGrasp::setIKFrame, R"pbdoc(
.def<void (SimpleUnGrasp::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc(
2. Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
@ -923,7 +918,7 @@ void export_stages(pybind11::module& m) {
Returns:
None
)pbdoc")
.def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"pbdoc(
.def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"pbdoc(
setMaxIKSolutions(max_ik_solutions)
Args:

View File

@ -4,7 +4,7 @@
from moveit.core import planning_scene
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander import PlanningScene
from moveit.core.planning_scene import PlanningScene
import time
roscpp_init("mtc_tutorial_current_state")
@ -13,12 +13,12 @@ roscpp_init("mtc_tutorial_current_state")
# Create a task
task = core.Task()
# Get the current robot state
fixedState = stages.FixedState("fixed state")
# Initialize a PlanningScene for use in a FixedState stage
task.loadRobotModel() # load the robot model (usually done in init())
planningScene = PlanningScene(task.getRobotModel())
# create an empty planning scene and assign it to the
# fixed state
planningScene = PlanningScene()
# Create a FixedState stage and pass the created PlanningScene as its state
fixedState = stages.FixedState("fixed state")
fixedState.setState(planningScene)
# Add the stage to the task hierarchy
@ -26,4 +26,6 @@ task.add(fixedState)
if task.plan():
task.publish(task.solutions[0])
del planningScene # Avoid ClassLoader warning by destroying the RobotModel
time.sleep(1)