|
| 1 | +#! /usr/bin/env python |
| 2 | +# -*- coding: utf-8 -*- |
| 3 | + |
| 4 | +from std_msgs.msg import Header |
| 5 | +from geometry_msgs.msg import Vector3Stamped, Vector3, Pose |
| 6 | +from moveit_msgs.msg import CollisionObject, Constraints, OrientationConstraint |
| 7 | +from shape_msgs.msg import SolidPrimitive |
| 8 | +from moveit.task_constructor import core, stages |
| 9 | +import time |
| 10 | + |
| 11 | +from py_binding_tools import roscpp_init |
| 12 | + |
| 13 | +roscpp_init("mtc_tutorial") |
| 14 | + |
| 15 | +group = "panda_arm" |
| 16 | +planner = core.PipelinePlanner() |
| 17 | + |
| 18 | +task = core.Task() |
| 19 | +task.add(stages.CurrentState("current state")) |
| 20 | + |
| 21 | +co = CollisionObject() |
| 22 | +co.header.frame_id = "world" |
| 23 | +co.id = "obstacle" |
| 24 | +sphere = SolidPrimitive() |
| 25 | +sphere.type = sphere.SPHERE |
| 26 | +sphere.dimensions.insert(sphere.SPHERE_RADIUS, 0.1) |
| 27 | + |
| 28 | +pose = Pose() |
| 29 | +pose.position.x = 0.3 |
| 30 | +pose.position.y = 0.2 |
| 31 | +pose.position.z = 0.5 |
| 32 | +pose.orientation.w = 1.0 |
| 33 | +co.primitives.append(sphere) |
| 34 | +co.primitive_poses.append(pose) |
| 35 | +co.operation = co.ADD |
| 36 | + |
| 37 | +mps = stages.ModifyPlanningScene("modify planning scene") |
| 38 | +mps.addObject(co) |
| 39 | +task.add(mps) |
| 40 | + |
| 41 | +move = stages.MoveRelative("y +0.4", planner) |
| 42 | +move.group = group |
| 43 | +header = Header(frame_id="world") |
| 44 | +move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0))) |
| 45 | + |
| 46 | +constraints = Constraints() |
| 47 | +oc = OrientationConstraint() |
| 48 | +oc.header.frame_id = "world" |
| 49 | +oc.link_name = "panda_hand" |
| 50 | +oc.orientation.x = 1.0 |
| 51 | +oc.absolute_x_axis_tolerance = 0.01 |
| 52 | +oc.absolute_y_axis_tolerance = 0.01 |
| 53 | +oc.absolute_z_axis_tolerance = 0.01 |
| 54 | +oc.weight = 1.0 |
| 55 | +constraints.orientation_constraints.append(oc) |
| 56 | +move.path_constraints = constraints |
| 57 | + |
| 58 | +task.add(move) |
| 59 | + |
| 60 | +if task.plan(): |
| 61 | + task.publish(task.solutions[0]) |
| 62 | +time.sleep(100) |
0 commit comments