diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py new file mode 100644 index 000000000..c211efdfa --- /dev/null +++ b/demo/scripts/constrained.py @@ -0,0 +1,62 @@ +#! /usr/bin/env python +# -*- 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.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) +task.add(mps) + +move = stages.MoveRelative("y +0.4", planner) +move.group = group +header = Header(frame_id="world") +move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0))) + +constraints = Constraints() +oc = OrientationConstraint() +oc.header.frame_id = "world" +oc.link_name = "panda_hand" +oc.orientation.x = 1.0 +oc.absolute_x_axis_tolerance = 0.01 +oc.absolute_y_axis_tolerance = 0.01 +oc.absolute_z_axis_tolerance = 0.01 +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)