mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
59 lines
1.9 KiB
Python
Executable File
59 lines
1.9 KiB
Python
Executable File
#! /usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
from moveit.task_constructor import core, stages
|
|
from geometry_msgs.msg import PoseStamped, Pose, Vector3
|
|
from std_msgs.msg import Header
|
|
import time
|
|
|
|
from moveit.python_tools import roscpp_init
|
|
|
|
roscpp_init("mtc_tutorial_compute_ik")
|
|
|
|
# Specify the planning group
|
|
group = "panda_arm"
|
|
|
|
# Create a task
|
|
task = core.Task()
|
|
|
|
# Add a stage to retrieve the current state
|
|
task.add(stages.CurrentState("current state"))
|
|
|
|
# Add a planning stage connecting the generator stages
|
|
planner = core.PipelinePlanner() # create default planning pipeline
|
|
task.add(stages.Connect("connect", [(group, planner)])) # operate on group
|
|
del planner # Delete PipelinePlanner when not explicitly needed anymore
|
|
|
|
# [propertyTut12]
|
|
# Add a Cartesian pose generator
|
|
generator = stages.GeneratePose("cartesian pose")
|
|
# [propertyTut12]
|
|
# Inherit PlanningScene state from "current state" stage
|
|
generator.setMonitoredStage(task["current state"])
|
|
# Configure target pose
|
|
# [propertyTut13]
|
|
pose = Pose(position=Vector3(z=0.2))
|
|
generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose)
|
|
# [propertyTut13]
|
|
|
|
# [initAndConfigComputeIk]
|
|
# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose
|
|
computeIK = stages.ComputeIK("compute IK", generator)
|
|
computeIK.group = group # Use the group-specific IK solver
|
|
computeIK.ik_frame = "panda_link8" # Which end-effector frame should reach the target?
|
|
computeIK.max_ik_solutions = 4 # Limit the number of IK solutions
|
|
# [propertyTut14]
|
|
props = computeIK.properties
|
|
# derive target_pose from child's solution
|
|
props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
|
|
# [propertyTut14]
|
|
|
|
# Add the stage to the task hierarchy
|
|
task.add(computeIK)
|
|
# [initAndConfigComputeIk]
|
|
|
|
if task.plan():
|
|
task.publish(task.solutions[0])
|
|
|
|
time.sleep(1) # sleep some time to allow C++ threads to publish their messages
|