compute_ik.py

Go to the documentation of this file.

#! /usr/bin/env python3
# -*- coding: utf-8 -*-
 
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped, Pose, Point
from std_msgs.msg import Header
import time
 
from py_binding_tools import roscpp_init
 
roscpp_init("mtc_tutorial")
 
# Specify the planning group
group = "panda_arm"
 
# Create a task
task = core.Task()
task.name = "compute IK"
 
# 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=Point(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
# Which end-effector frame should reach the target?
computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_link8"))
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