diff --git a/core/doc/conf.py b/core/doc/conf.py index 8915635e..d127a1af 100644 --- a/core/doc/conf.py +++ b/core/doc/conf.py @@ -226,5 +226,5 @@ intersphinx_mapping = {"https://docs.python.org/3": None} # Default options for generating documentation. autodoc_default_options = { - "exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward" + "exclude-members": "ContainerBase, InitStageError, ParallelContainerBase, PlannerInterface, WrapperBase, PropagatingForward, PropagatingBackward, MergeMode" } diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index 54303ac8..5457c78b 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -54,6 +54,7 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveRelative) PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect) PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects) PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose) PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose) PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick) PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place) @@ -80,7 +81,7 @@ std::vector elementOrList(const py::object& arg) { void export_stages(pybind11::module& m) { // clang-format off properties::class_(m, "ModifyPlanningScene", R"pbdoc( - ModifyPlanningScene(self, name) + ModifyPlanningScene(name) Allows modification of the planning scene. This stage takes the incoming planning scene and applies previously scheduled changes to it, for example: @@ -91,26 +92,13 @@ void export_stages(pybind11::module& m) { Args: name (str): Name of the stage. - :: - - task = core.Task() - - # Specify a grasp object by the known name - graspObject = "sampleObject" - - # allow collisions - allowCollision = stages.ModifyPlanningScene("allow object - hand collision") - allowCollision.allowCollisions( - graspObject, - "right_fingers", - True - ) - task.add(allowCollision) + .. literalinclude:: ./../../../demo/scripts/modify_planning_scene.py + :language: python )pbdoc") .def(py::init(), py::arg("name") = std::string("modify planning scene")) .def("attachObject", &ModifyPlanningScene::attachObject, R"pbdoc( - attachObject(self, name, link) + attachObject(name, link) Args: name (str): Name of the object @@ -120,7 +108,7 @@ void export_stages(pybind11::module& m) { None )pbdoc") .def("detachObject", &ModifyPlanningScene::detachObject, R"pbdoc( - detachObject(self, name, link) + detachObject(name, link) Detach an object from a robot link. @@ -134,7 +122,7 @@ void export_stages(pybind11::module& m) { const std::string& attach_link, bool attach) { self.attachObjects(elementOrList(names), attach_link, attach); }, py::arg("names"), py::arg("attach_link"), py::arg("attach") = true, R"pbdoc( - attachObjects(self, attach_link, attach) + attachObjects(attach_link, attach) Attach multiple objects to a robot link. @@ -149,7 +137,7 @@ void export_stages(pybind11::module& m) { const std::string& attach_link) { self.attachObjects(elementOrList(names), attach_link, false); }, py::arg("names"), py::arg("attach_link"), R"pbdoc( - detachObjects(self, attach_link) + detachObjects(attach_link) Detach multiple objects from a robot link. @@ -163,7 +151,7 @@ void export_stages(pybind11::module& m) { const py::object& first, const py::object& second, bool enable_collision) { self.allowCollisions(elementOrList(first), elementOrList(second), enable_collision); }, py::arg("first"), py::arg("second"), py::arg("enable_collision") = true, R"pbdoc( - allowCollisions(self, first, second, enable_collision) + allowCollisions(first, second, enable_collision) Allow or disable collisions between links and objects. @@ -171,48 +159,52 @@ void export_stages(pybind11::module& m) { first (str): Name of the first object or link. second (str): Name of the second object or link. enable_collision (bool): Set to true to enable collisions checks; - set to false to disable collision checks. + set to false to disable collision checks. Returns: None + )pbdoc") + .def("addObject", &ModifyPlanningScene::addObject, R"pbdoc( + addObject(collision_object) + + Add an object to the planning scene + + Args: + collision_object (CollisionObject_): Object to be added. Must be + in the appropriate message format. + + .. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html + )pbdoc"); // clang-format on properties::class_(m, "CurrentState", R"pbdoc( - CurrentState(self, name) + CurrentState(name) - Fetch the current PlanningScene state via get_planning_scene service. + Fetch the current PlanningScene state via the ``get_planning_scene`` service. Args: name (str): Name of the stage. - :: + .. literalinclude:: ./../../../demo/scripts/current_state.py + :language: python - task = core.Task() - - # create a stage instance - currentState = CurrentState('current state') - task.add(currentState) )pbdoc") .def(py::init(), py::arg("name") = std::string("current state")); properties::class_(m, "FixedState", R"pbdoc( - FixedState(self, name) + FixedState(name) Spawn a pre-defined PlanningScene state. Args: name (str): Name of the stage. - :: + .. literalinclude:: ./../../../demo/scripts/fixed_state.py + :language: python - task = core.Task() - - # create a stage instance - fixedState = FixedState('fixed state') - task.add(fixedState) )pbdoc") .def("setState", &FixedState::setState, R"pbdoc( - setState(self, scene) + setState(scene) Use a planning scene pointer to specify which state the Fixed State stage should have. @@ -233,7 +225,7 @@ void export_stages(pybind11::module& m) { ; properties::class_(m, "ComputeIK", R"pbdoc( - ComputeIK(self, name, stage) + ComputeIK(name, stage) Wrapper for any pose generator stage to compute the inverse kinematics for a pose in Cartesian space. @@ -255,17 +247,8 @@ void export_stages(pybind11::module& m) { name (str): Name of the stage. stage: Stage that contains the robot state for IK calculation. - :: - - # create a task - task = core.Task() - - # get the current robot state - currentState = stages.CurrentState("current state") - - # calculate the inverse kinematics for the current robot state - computeIK = stages.ComputeIK("compute IK", currentState) - task.add(computeIK) + .. literalinclude:: ./../../../demo/scripts/compute_ik.py + :language: python )pbdoc") .property("eef", R"pbdoc( @@ -289,15 +272,17 @@ void export_stages(pybind11::module& m) { the planning scene are allowed. )pbdoc") .property("ik_frame", R"pbdoc( - PoseStamped_ : Specify the frame with respect - to which the inverse kinematics should be calculated. + 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") .property("target_pose", R"pbdoc( - PoseStamped_ : Specify the pose on which + PoseStamped_: Specify the pose on which the inverse kinematics should be - calculated on. Since this property should almost always be set + calculated on. Since this property should + almost always be set in the Interface State which is sent by the child, if possible, avoid setting it manually. @@ -307,54 +292,38 @@ void export_stages(pybind11::module& m) { .def(py::init()); properties::class_>(m, "MoveTo", R"pbdoc( - MoveTo(self, name, planner) + MoveTo(name, planner) - Compute a trajectory between the robot state from the - interface state of the preceeding stage and a specified - goal. + Compute a trajectory between the robot state from the + interface state of the preceeding stage and a specified + goal. - Args: - name (str): Name of the stage. - planner (PlannerInterface): Planner that is used to compute the path of motion. + Args: + name (str): Name of the stage. + planner (PlannerInterface): Planner that is used to compute the path of motion. - :: + .. literalinclude:: ./../../../demo/scripts/cartesian.py + :language: python + :lines: 51-55 - # create a planner instance - jointspace = core.JointInterpolationPlanner() - - # specify planning group - group = "foo_robot" - - # create a task - task = core.Task() - - # get the current robot state - currentState = stages.CurrentState("current state") - task.add(currentState) - - # moveTo named posture, using joint-space interplation - move = stages.MoveTo("moveTo ready", jointspace) - move.group = group - move.setGoal("ready") - task.add(move) - )pbdoc") + )pbdoc") .property("group", R"pbdoc( str: Planning group which should be utilized for planning and execution. )pbdoc") .property("ik_frame", R"pbdoc( - PoseStamped_ : IK reference frame for the goal pose. + PoseStamped_: IK reference frame for the goal pose. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .property("path_constraints", R"pbdoc( - Constraints_ : Set path constraints via the corresponding moveit message type. + Constraints_: Set path constraints via the corresponding moveit message type. .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html )pbdoc") .def(py::init()) .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( - setGoal(self, goal) + setGoal(goal) Args: goal (PoseStamped_): Desired configuration. @@ -367,7 +336,7 @@ void export_stages(pybind11::module& m) { .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"pbdoc( - setGoal(self, goal) + setGoal(goal) Args: goal (PointStamped_): Desired configuration. @@ -410,77 +379,61 @@ void export_stages(pybind11::module& m) { )pbdoc"); properties::class_>(m, "MoveRelative", R"pbdoc( - MoveRelative(self, name, planner) + MoveRelative(name, planner) - Perform a Cartesian motion relative to some link . + Perform a Cartesian motion relative to some link. Args: name (str): Name of the stage. planner (PlannerInterface): Planner that is used to compute the path of motion. - :: - - # Planning group - group = "panda_arm" - - # Cartesian planner - cartesian = core.CartesianPath() - - task = core.Task() - - # start from current robot state - task.add(stages.CurrentState("current state")) - - # move along x - move = stages.MoveRelative("x +0.2", cartesian) - move.group = group - header = Header(frame_id="world") - move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0))) - task.add(move) + .. literalinclude:: ./../../../demo/scripts/cartesian.py + :language: python + :lines: 26-31 )pbdoc") .property("group", R"pbdoc( str: Planning group which should be utilized for planning and execution. )pbdoc") .property("ik_frame", R"pbdoc( - PoseStamped_ : IK reference frame for the goal pose. + PoseStamped_: IK reference frame for the goal pose. .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .property("min_distance", R"pbdoc( - double: Set the minimum distance to move. + float: Set the minimum distance to move. )pbdoc") .property("max_distance", R"pbdoc( - double: Set the maximum distance to move. + float: Set the maximum distance to move. )pbdoc") .property("path_constraints", R"pbdoc( - Constraints_ : These are the path constraints. + Constraints_: These are the path constraints. .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html )pbdoc") .def(py::init()) .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"pbdoc( - setDirection(self, twist) + setDirection(twist) - Perform twist motion on specified link. + Perform twist motion on specified link. - Args: - twist (Twist_): Use a Twist message as movement direction description. + Args: + twist (Twist_): Use a Twist message as movement direction description. - .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html )pbdoc") .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"pbdoc( - setDirection(self, direction) + setDirection(direction) - Translate link along given direction. + Translate link along given direction. )pbdoc") .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), R"pbdoc( - setDirection(self, joint_deltas) + setDirection(joint_deltas) - Move specified joint variables by given amount. + Move specified joint variables by given amount. )pbdoc"); py::enum_(m, "MergeMode", R"pbdoc( @@ -496,7 +449,7 @@ void export_stages(pybind11::module& m) { PropertyConverter(); properties::class_(m, "Connect", R"pbdoc( - Connect(self, name, planners) + Connect(name, planners) Connect arbitrary InterfaceStates by motion planning. You can specify the planning groups and the planners you @@ -514,23 +467,18 @@ void export_stages(pybind11::module& m) { Name (str): Name of the stage. Planners (list): List of the planner - group associations. - :: + The example below contains a snippet from the :ref:`pick pipeline example`. + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + :lines: 48-60 - # Create a planner instance - samplingPlanner = PipelinePlanner() - # Specify group-planner combinations - planners = [ - ('foo_group', samplingPlanner), - ('bar_group', samplingPlanner) - ] - # create a stage instance - connect = Connect('connect', planners) )pbdoc") .def(py::init(), py::arg("name") = std::string("connect"), py::arg("planners")); properties::class_(m, "FixCollisionObjects", R"pbdoc( - FixCollisionObjects(self, name) + FixCollisionObjects(name) Test for collisions and find a correction for applicable objects. Move the objects out of the way along the correction direction. @@ -538,13 +486,8 @@ void export_stages(pybind11::module& m) { Args: name (str): Name of the stage. - :: - - task = core.Task() - - # check for collisions and find corrections - fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") - task.add(fixCollisionObjects) + .. literalinclude:: ./../../../demo/scripts/fix_collision_objects.py + :language: python )pbdoc") .property("max_penetration", R"pbdoc( @@ -552,6 +495,34 @@ void export_stages(pybind11::module& m) { )pbdoc") .def(py::init(), py::arg("name") = std::string("fix collisions")); + properties::class_(m, "GeneratePlacePose", R"pbdoc( + GeneratePlacePose(name) + + GeneratePlacePose stage derives from monitoring generator and generates poses + for the place pipeline. Notice that whilst GenerateGraspPose spawns poses with an + ``angle_delta`` intervall, GeneratePlacePose samples a fixed amount, which is dependent + on the objects shape. + + Args: + name (str): Name of the stage. + + The example below contains a snippet from the :ref:`pick pipeline example`. + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + :lines: 115-122 + + )pbdoc") + .property("object", R"pbdoc( + str: Name of the object in the planning scene, + attached to the robot which should be placed. + )pbdoc") + .property("eef", R"pbdoc( + str: Name of the end effector that should be used for grasping. + )pbdoc") + .property("pose") + .def(py::init(), py::arg("name") = std::string("Generate Place Pose")); + properties::class_(m, "GenerateGraspPose", R"pbdoc( GenerateGraspPose(name) @@ -562,6 +533,12 @@ void export_stages(pybind11::module& m) { Args: name (str): Name of the stage. + The example below contains a snippet from the :ref:`pick pipeline example`. + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + :lines: 62-68 + )pbdoc") .property("object", R"pbdoc( str: Name of the Object in the planning scene, which should be grasped. @@ -576,36 +553,42 @@ void export_stages(pybind11::module& m) { str: Name of the grasp pose. )pbdoc") .property("angle_delta", R"pbdoc( - double: Angular step distance in rad with which positions around the object are sampled. + float: Angular step distance in rad with which positions around the object are sampled. )pbdoc") .def(py::init(), py::arg("name") = std::string("Generate Grasp Pose")); properties::class_(m, "GeneratePose", R"pbdoc( - GeneratePose(self, name) + GeneratePose(name) - Monitoring generator stage which can be used to generate a pose, based on solutions provided - by the monitored stage. + Monitoring generator stage which can be used to generate a pose, based on solutions provided + by the monitored stage. - Args: - name (str): Name of the stage. + Args: + name (str): Name of the stage. - :: - - task = core.Task() + .. literalinclude:: ./../../../demo/scripts/generate_pose.py + :language: python + :lines: 35-48 )pbdoc") .property("pose", R"pbdoc( + PoseStamped_: Set the pose, which should be spawned + on each new solution of the monitored stage. + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .def(py::init()); properties::class_(m, "Pick", R"pbdoc( - Pick(self, grasp_generator, name) + Pick(grasp_generator, name) Args: - grasp_generator (): + grasp_generator (SimpleGrasp): Simple Grasp stage to provide + poses and inverse kinematics solutions. name (str): Name of the stage. + .. _pick: + The Pick stage is a specialization of the PickPlaceBase class, which wraps the pipeline to pick or place an object with a given end effector. @@ -618,40 +601,77 @@ void export_stages(pybind11::module& m) { The end effector postures corresponding to pre-grasp and grasp as well as the end effector's cartesian pose needs to be provided by an external grasp stage. + + The example below combines pick and place routines in a single pickplace pipeline. + Please refer to the demonstration scripts of the moveit task constructor repository + for reference. + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + )pbdoc") .property("object", R"pbdoc( - Name of object to pick. + str: Name of object to pick. )pbdoc") .property("eef", R"pbdoc( - End effector name. + str: The End effector name. )pbdoc") .property("eef_frame", R"pbdoc( - Name of the end effector frame. + str: Name of the end effector frame. )pbdoc") .property("eef_group", R"pbdoc( - Joint model group of the end effector. + str: Joint model group of the end effector. )pbdoc") .property("eef_parent_group", R"pbdoc( - Joint model group of the eef's parent. + str: Joint model group of the eef's parent. )pbdoc") .def(py::init(), py::arg("grasp_generator"), py::arg("name") = std::string("pick")) .def("setApproachMotion", &Pick::setApproachMotion, R"pbdoc( + setApproachMotion(motion, min_distance, max_distance) + Args: + motion (Twist_): The twist, which represents the + approach motion. + min_distance (float): Minimum allowed distance. + max_distance (float): Maximum allowed distance. + + The approaching motion towards the grasping state is represented + by a twist message. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html )pbdoc") .def("setLiftMotion", py::overload_cast(&Pick::setLiftMotion), R"pbdoc( + setLiftMotion(motion, min_distance, max_distance) + Args: + motion (Twist_): The twist, which represents the + lift motion. + min_distance (float): Minimum allowed distance. + max_distance (float): Maximum allowed distance. + + The lifting motion away from the grasping state is represented + by a twist message. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html )pbdoc") .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion), R"pbdoc( + setLiftMotion(place) + Args: + place (dict): The place where the object should be lifted to, + given as joint-value pairs. + + The lifting motion away from the grasping state is represented + by its destination as joint-value pairs. )pbdoc"); properties::class_(m, "Place", R"pbdoc( - Place(self, place_generator, name) + Place(place_generator, name) Args: - place_generator (): + place_generator (SimpleUnGrasp): SimpleUnGrasp Wrapper for pose generation name (str): Name of the stage. The Place stage is a specialization of the PickPlaceBase class, which @@ -666,54 +686,224 @@ void export_stages(pybind11::module& m) { The end effector postures corresponding to pre-grasp and grasp as well as the end effector's Cartesian pose needs to be provided by an external grasp stage. + + For a working example, please consider the Pick_ Stage. + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + :lines: 131-135 )pbdoc") .property("object", R"pbdoc( - Name of object to pick. + str: Name of object to pick. )pbdoc") .property("eef", R"pbdoc( - End effector name. + str: The End effector name. )pbdoc") .property("eef_frame", R"pbdoc( - Name of the end effector frame. + str: Name of the end effector frame. )pbdoc") .property("eef_group", R"pbdoc( - Joint model group of the end effector. + str: Joint model group of the end effector. )pbdoc") .property("eef_parent_group", R"pbdoc( - Joint model group of the eef's parent. + str: Joint model group of the eef's parent. + )pbdoc") + .def("setRetractMotion", &Place::setRetractMotion, R"pbdoc( + setRetractMotion(motion, min_distance, max_distance) + + Args: + motion (Twist_): The twist, which represents the + retract motion. + min_distance (float): Minimum allowed distance. + max_distance (float): Maximum allowed distance. + + The retract motion towards the final state is represented + by a twist message. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )pbdoc") + .def("setPlaceMotion", + py::overload_cast(&Place::setPlaceMotion), R"pbdoc( + setPlaceMotion(motion, min_distance, max_distance) + + Args: + motion (Twist_): The twist, which represents the + place motion. + min_distance (float): Minimum allowed distance. + max_distance (float): Maximum allowed distance. + + The object-placing motion towards the final state is represented + by a twist message. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + + )pbdoc") + .def("setPlaceMotion", py::overload_cast&>(&Place::setPlaceMotion), R"pbdoc( + setPlaceMotion(joints) + + Args: + joints (dict): The place where the object should be placed at, + given as joint-value pairs. + + The placing motion to the final state is represented + by its destination as joint-value pairs. )pbdoc") .def(py::init(), py::arg("place_generator"), py::arg("name") = std::string("place")); properties::class_(m, "SimpleGrasp", R"pbdoc( - Specialization of SimpleGraspBase to realize grasping + SimpleGrasp(pose_generator, name) + + Args: + pose_generator (GenerateGraspPose): Generator stage to + sample possible grasp poses. + name (str): Name of the stage. + + Specialization of SimpleGraspBase to realize grasping. + Refer to the pick_ stage for a minimum code example: + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + :lines: 70-79 )pbdoc") .property("eef", R"pbdoc( - + str: The end effector of the robot. )pbdoc") .property("object", R"pbdoc( + str: The object to grasp (Must be present in the planning scene). + )pbdoc") + .property("ik_frame", R"pbdoc( + PoseStamped_: Set the frame for which + the inverse kinematics are calculated + with respect to each pose generated by + the pose_generator. + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .def(py::init(), py::arg("pose_generator"), py::arg("name") = std::string("grasp generator")) .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( + setIKFrame(transform) + Args: + transform (PoseStamped_): Transform to the IK Frame. + + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html )pbdoc") .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( + setIKFrame(pose, link) + Args: + pose (PoseStamped_): Transform from the given link to the IK frame. + link (str): Base link for pose transform to IK frame. + + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. )pbdoc") .def("setIKFrame", &SimpleGrasp::setIKFrame, R"pbdoc( + setIKFrame(link) + + Args: + link (str): IK Frame will be placed at the base frame of this link. + + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + )pbdoc") + .def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"pbdoc( + setMaxIKSolutions(max_ik_solutions) + + Args: + max_ik_solutions (int): + + Set the maximum number of inverse kinematics solutions that + should be computed. )pbdoc"); properties::class_(m, "SimpleUnGrasp", R"pbdoc( + SimpleUnGrasp(pose_generator, name) + + Args: + pose_generator (GeneratePlacePose): Generator stage to + sample possible Place + poses. + name (str): Name of the stage. + Specialization of SimpleGraspBase to realize ungrasping - )pbdoc") - .property("eef", R"pbdoc( + Refer to the place_ stage for a minimum code example. + Make shure to set the ``grasp`` and ``pregrasp`` properties + here again since they are not forwarded through the stage + hierarchy. + + .. literalinclude:: ./../../../demo/scripts/pickplace.py + :language: python + :lines: 124-129 )pbdoc") + .property("eef", R"pbdoc( + str: The end effector of the robot. + )pbdoc") .property("object", R"pbdoc( + str: The object to grasp (Must be present in the planning scene). + )pbdoc") + .property("pregrasp", R"pbdoc( + str: Name of the pre-grasp pose. + )pbdoc") + .property("grasp", R"pbdoc( + str: Name of the grasp pose. + )pbdoc") + .property("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("setIKFrame", &SimpleUnGrasp::setIKFrame, + R"pbdoc( + setIKFrame(transform) + + Args: + transform (PoseStamped_): Transform to the IK Frame. + + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )pbdoc") + .def("setIKFrame", + &SimpleUnGrasp::setIKFrame, R"pbdoc( + setIKFrame(pose, link) + + Args: + pose (PoseStamped_): Transform from the given link to the IK frame. + link (str): Base link for pose transform to IK frame. + + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + )pbdoc") + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"pbdoc( + setIKFrame(link) + + Args: + link (str): IK Frame will be placed at the base frame of this link. + + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + )pbdoc") + .def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"pbdoc( + setMaxIKSolutions(max_ik_solutions) + + Args: + max_ik_solutions (int): + + Set the maximum number of inverse kinematics solutions that + should be computed. )pbdoc") .def(py::init(), py::arg("pose_generator"), diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index 68686202..f97e0a77 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -4,7 +4,6 @@ from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped -from std_msgs.msg import Header import rospy import time