moveit_task_constructor/demo/scripts/constrained.py
2025-10-04 18:17:56 +02:00

85 lines
2.3 KiB
Python
Executable File

#! /usr/bin/env python3
# -*- coding: utf-8 -*-
from std_msgs.msg import Header
from geometry_msgs.msg import Vector3Stamped, Vector3, Pose
from moveit_msgs.msg import CollisionObject, Constraints, OrientationConstraint
from shape_msgs.msg import SolidPrimitive
from moveit.task_constructor import core, stages
import time
import rclcpp
rclcpp.init()
node_options = rclcpp.NodeOptions()
node_options.automatically_declare_parameters_from_overrides = True
node = rclcpp.Node("mtc_tutorial", node_options)
group = "panda_arm"
planner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
task = core.Task()
task.name = "constrained"
task.loadRobotModel(node)
task.add(stages.CurrentState("current state"))
co = CollisionObject()
co.header.frame_id = "world"
co.id = "obstacle"
sphere = SolidPrimitive()
sphere.type = sphere.SPHERE
sphere.dimensions.insert(sphere.SPHERE_RADIUS, 0.1)
pose = Pose()
pose.position.x = 0.3
pose.position.y = 0.2
pose.position.z = 0.5
pose.orientation.w = 1.0
co.primitives.append(sphere)
co.primitive_poses.append(pose)
co.operation = co.ADD
mps = stages.ModifyPlanningScene("modify planning scene")
mps.addObject(co)
co.id = "object"
co.primitives[0].type = SolidPrimitive.BOX
co.primitives[0].dimensions = [0.1, 0.05, 0.03]
pose = co.primitive_poses[0]
pose.position.x = 0.30702
pose.position.y = 0.0
pose.position.z = 0.485
pose.orientation.x = pose.orientation.w = 0.70711 # 90° about x
mps.addObject(co)
mps.attachObjects(["object"], "panda_link8")
mps.allowCollisions("object", ["panda_leftfinger", "panda_rightfinger"], True)
task.add(mps)
move = stages.MoveRelative("y +0.4", planner)
move.timeout = 5
move.group = group
header = Header(frame_id="world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(x=0.0, y=0.4, z=0.0)))
constraints = Constraints()
oc = OrientationConstraint()
oc.parameterization = oc.ROTATION_VECTOR
oc.header.frame_id = "world"
oc.link_name = "object"
oc.orientation.x = oc.orientation.w = 0.70711 # 90° about x
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
constraints.orientation_constraints.append(oc)
move.path_constraints = constraints
task.add(move)
if task.plan():
task.publish(task.solutions[0])
time.sleep(500)