mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
constrained.py: constrain orientation of attached object pickplace.py: keep object upright during transport
79 lines
2.0 KiB
Python
Executable File
79 lines
2.0 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
|
|
|
|
from py_binding_tools import roscpp_init
|
|
|
|
roscpp_init("mtc_tutorial")
|
|
|
|
group = "panda_arm"
|
|
planner = core.PipelinePlanner()
|
|
|
|
task = core.Task()
|
|
task.name = "constrained"
|
|
|
|
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_hand")
|
|
|
|
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(0, 0.4, 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(100)
|