mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Fix demo/scripts/fixed_state.py
This commit is contained in:
parent
0fbdfbc818
commit
d732bfe3eb
@ -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)
|
||||
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user