mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Provide ComputeIK.ik_frame as full PoseStamped
Released MoveIt doesn't (yet) provide a conversion from string to PoseStamped.
This commit is contained in:
parent
885ac49ffb
commit
7926f69e97
@ -7,7 +7,7 @@ import rostest
|
|||||||
from moveit_commander.roscpp_initializer import roscpp_initialize
|
from moveit_commander.roscpp_initializer import roscpp_initialize
|
||||||
from moveit.task_constructor import core, stages
|
from moveit.task_constructor import core, stages
|
||||||
from moveit.core.planning_scene import PlanningScene
|
from moveit.core.planning_scene import PlanningScene
|
||||||
from geometry_msgs.msg import Vector3Stamped, Vector3
|
from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped
|
||||||
from std_msgs.msg import Header
|
from std_msgs.msg import Header
|
||||||
|
|
||||||
PLANNING_GROUP = "manipulator"
|
PLANNING_GROUP = "manipulator"
|
||||||
@ -75,7 +75,7 @@ class PyMoveRelX(stages.MoveRelative):
|
|||||||
def __init__(self, x, planner, name="Move ±x"):
|
def __init__(self, x, planner, name="Move ±x"):
|
||||||
stages.MoveRelative.__init__(self, name, planner)
|
stages.MoveRelative.__init__(self, name, planner)
|
||||||
self.group = PLANNING_GROUP
|
self.group = PLANNING_GROUP
|
||||||
self.ik_frame = "tool0"
|
self.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
|
||||||
self.setDirection(
|
self.setDirection(
|
||||||
Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0))
|
Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0))
|
||||||
)
|
)
|
||||||
|
|||||||
@ -40,7 +40,8 @@ generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose)
|
|||||||
# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose
|
# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose
|
||||||
computeIK = stages.ComputeIK("compute IK", generator)
|
computeIK = stages.ComputeIK("compute IK", generator)
|
||||||
computeIK.group = group # Use the group-specific IK solver
|
computeIK.group = group # Use the group-specific IK solver
|
||||||
computeIK.ik_frame = "panda_link8" # Which end-effector frame should reach the target?
|
# 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
|
computeIK.max_ik_solutions = 4 # Limit the number of IK solutions
|
||||||
# [propertyTut14]
|
# [propertyTut14]
|
||||||
props = computeIK.properties
|
props = computeIK.properties
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user