Skip to content

Commit af7d60d

Browse files
committed
Example of constrained orientation planning
1 parent edea20b commit af7d60d

File tree

1 file changed

+62
-0
lines changed

1 file changed

+62
-0
lines changed

demo/scripts/constrained.py

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
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

Comments
 (0)