Fix ComputeIK demo

This commit is contained in:
Robert Haschke 2022-01-11 21:08:35 +01:00
parent 4b9e85395d
commit d408d73018

View File

@ -2,42 +2,49 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from geometry_msgs.msg import PoseStamped
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 frame at the origin of link8
ikFrame = PoseStamped()
ikFrame.header.frame_id = "panda_link8"
# Create a task
task = core.Task()
# Get the current robot state
currentState = stages.CurrentState("current state")
# Add a stage to retrieve the current state
task.add(stages.CurrentState("current state"))
# Calculate the inverse kinematics for the current robot state
computeIK = stages.ComputeIK("compute IK", currentState)
# 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
# Specify the planning group that should be used for ik calculation
computeIK.group = group
# Add a Cartesian pose generator
generator = stages.GeneratePose("cartesian pose")
# Inherit PlanningScene state from "current state" stage
generator.setMonitoredStage(task["current state"])
# Configure target pose
pose = Pose(position=Vector3(z=0.2))
generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose)
# ik_frame references possible tip frames of the kinematic tree of
# the chosen planning group
computeIK.ik_frame = ikFrame
computeIK.target_pose = ikFrame
# Limit the number of inverse kinematics solutions to 4
computeIK.max_ik_solutions = 4
# 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
props = computeIK.properties
# derive target_pose from child's solution
props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
# Add the stage to the task hierarchy
task.add(computeIK)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)
time.sleep(1) # sleep some time to allow C++ threads to publish their messages