Skip to content

Commit bb5c02e

Browse files
RussTedrakeRussTedrake
committed
use gcs derivative constraints
1 parent c4735df commit bb5c02e

10 files changed

+580
-277
lines changed

‎book/pick.html

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1321,7 +1321,7 @@ <h1><a href="index.html" style="text-decoration:none;">Robotic Manipulation</a><
13211321
coordinates of x, y, z, roll, pitch, and yaw, typically specified in the robot's
13221322
base frame. When you're trying to slide the robot end-effector along the table
13231323
top, the velocity component going into the table will saturate, but the components
1324-
orthogonal the table need not saturate. For example, you won't ever see the
1324+
orthogonal the table need not saturate. Importantly, you won't ever see the
13251325
end-effector twist when you are commanding pure translation, and won't see the
13261326
end-effector translate when you are commanding a pure rotation.</p>
13271327

‎book/trajectories/BUILD.bazel

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,15 @@ rt_ipynb_test(
2727
],
2828
)
2929

30+
# Waiting on https://github.com/RobotLocomotion/drake/pull/21504
31+
#rt_ipynb_test(
32+
# name = "gcs_derivatives",
33+
# srcs = ["gcs_derivatives.ipynb"],
34+
# deps = [
35+
# requirement("drake"),
36+
# ],
37+
#)
38+
3039
rt_ipynb_test(
3140
name = "gcs_simple_2d_cspace",
3241
srcs = ["gcs_simple_2d_cspace.ipynb"],

‎book/trajectories/gcs_derivatives.ipynb

Lines changed: 286 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,286 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": null,
6+
"id": "c8b13b5b",
7+
"metadata": {
8+
"lines_to_end_of_cell_marker": 2
9+
},
10+
"outputs": [],
11+
"source": [
12+
"import numpy as np\n",
13+
"import pydot\n",
14+
"from IPython.display import SVG, display\n",
15+
"from matplotlib import gridspec\n",
16+
"from matplotlib import pyplot as plt\n",
17+
"from pydrake.geometry.optimization import GraphOfConvexSetsOptions, HPolyhedron, Point\n",
18+
"from pydrake.planning import GcsTrajectoryOptimization"
19+
]
20+
},
21+
{
22+
"cell_type": "markdown",
23+
"id": "dbc32a9c",
24+
"metadata": {},
25+
"source": [
26+
"# GCS Trajectory Optimization with Derivative Constraints\n",
27+
"\n",
28+
"GCS Trajectory Optimization provides a powerful tool for obtaining globally-optimal solution. With the more powerful solver, you might have be a bit more careful what you wish for! In particular, the interplay between shortest paths and derivative constraints can be quite subtle. This simple notebook tries to make that point.\n",
29+
"\n",
30+
"Consider a very simple GCSTrajOpt problem with (effectively) two regions -- one to the left of the origin (in light blue), and one to the right of the origin (light green). We'll put the start at the bottom left, the goal at the bottom right, and use an edge constraint to ensure that the trajectory passes through the origin."
31+
]
32+
},
33+
{
34+
"cell_type": "code",
35+
"execution_count": null,
36+
"id": "7f18fb61",
37+
"metadata": {},
38+
"outputs": [],
39+
"source": [
40+
"def PlotEnvironment():\n",
41+
" plt.axis(\"square\")\n",
42+
" plt.fill([-1, 0, 0, -1], [-1, -1, 1, 1], \"lightblue\", alpha=0.5)\n",
43+
" plt.fill([0, 1, 1, 0], [-1, -1, 1, 1], \"lightgreen\", alpha=0.5)\n",
44+
" plt.plot([-1, 0, 1], [-1, 0, -1], \"r*\")\n",
45+
" plt.xlim([-1.25, 1.25])\n",
46+
" plt.ylim([-1.25, 1.25])\n",
47+
" plt.xlabel(\"x\")\n",
48+
" plt.ylabel(\"y\")\n",
49+
" plt.xticks()\n",
50+
" plt.yticks()\n",
51+
" plt.grid(1)\n",
52+
"\n",
53+
"\n",
54+
"def PlotSolution(traj, result):\n",
55+
" assert result.is_success()\n",
56+
"\n",
57+
" gs = gridspec.GridSpec(2, 1, height_ratios=[4, 1]) # 4:1 ratio for height\n",
58+
" plt.subplot(gs[0])\n",
59+
" PlotEnvironment()\n",
60+
"\n",
61+
" plt.plot(*traj.value(traj.start_time()), \"kx\")\n",
62+
" plt.plot(*traj.value(traj.end_time()), \"kx\")\n",
63+
" times = np.linspace(traj.start_time(), traj.end_time(), 1000)\n",
64+
" waypoints = traj.vector_values(times)\n",
65+
" plt.plot(*waypoints, \"b\", zorder=5)\n",
66+
" for seg in [traj.segment(i) for i in range(traj.get_number_of_segments())]:\n",
67+
" plt.plot(seg.control_points()[0], seg.control_points()[1], \"ro\")\n",
68+
"\n",
69+
" plt.subplot(gs[1])\n",
70+
" plt.plot(times, waypoints.T)\n",
71+
" plt.xlabel(\"time (s)\")\n",
72+
" plt.legend([\"x\", \"y\"])\n",
73+
"\n",
74+
"\n",
75+
"PlotEnvironment()\n",
76+
"\n",
77+
"\n",
78+
"def AddRegionsAndEdges(trajopt, order=4):\n",
79+
" left = trajopt.AddRegions(\n",
80+
" [HPolyhedron.MakeBox([-1, -1], [0, 1])], order=order, name=\"left\"\n",
81+
" )\n",
82+
" right = trajopt.AddRegions(\n",
83+
" [HPolyhedron.MakeBox([0, -1], [1, 1])], order=order, name=\"right\"\n",
84+
" )\n",
85+
" source = trajopt.AddRegions([Point([-1, -1])], order=0, name=\"source\")\n",
86+
" target = trajopt.AddRegions([Point([1, -1])], order=0, name=\"target\")\n",
87+
" trajopt.AddEdges(source, left)\n",
88+
" trajopt.AddEdges(right, target)\n",
89+
" e = trajopt.AddEdges(left, right).Edges()[0]\n",
90+
" e.AddConstraint(e.xu()[-2] == 0).evaluator().set_description(\n",
91+
" \"left-right edge: y = 0\"\n",
92+
" )\n",
93+
" return source, left, right, target\n",
94+
"\n",
95+
"\n",
96+
"trajopt = GcsTrajectoryOptimization(2)\n",
97+
"source, left, right, target = AddRegionsAndEdges(trajopt, order=4)\n",
98+
"\n",
99+
"display(\n",
100+
" SVG(\n",
101+
" pydot.graph_from_dot_data(trajopt.graph_of_convex_sets().GetGraphvizString())[\n",
102+
" 0\n",
103+
" ].create_svg()\n",
104+
" )\n",
105+
")"
106+
]
107+
},
108+
{
109+
"cell_type": "markdown",
110+
"id": "9ab2e4cb",
111+
"metadata": {},
112+
"source": [
113+
"## Minimum distance, no derivative constraints\n",
114+
"\n",
115+
"Naturally, the shortest path given this setup is the straight line from the start to the origin, then the origin to the goal. Solving GcsTrajOpt without any derivative constraints recovers this solution."
116+
]
117+
},
118+
{
119+
"cell_type": "code",
120+
"execution_count": null,
121+
"id": "b2c67211",
122+
"metadata": {},
123+
"outputs": [],
124+
"source": [
125+
"trajopt = GcsTrajectoryOptimization(2)\n",
126+
"source, left, right, target = AddRegionsAndEdges(trajopt, order=4)\n",
127+
"\n",
128+
"trajopt.AddPathLengthCost()\n",
129+
"[traj, result] = trajopt.SolvePath(source, target)\n",
130+
"\n",
131+
"PlotSolution(traj, result)"
132+
]
133+
},
134+
{
135+
"cell_type": "markdown",
136+
"id": "9931b57d",
137+
"metadata": {},
138+
"source": [
139+
"Notice the time duration in the solution. Why did it choose that for the time duration? Our problem formulation so far has underspecified the timing. It could be infinitely fast or infinitely slow. This can lead to strange numerical artifacts. An alternative formulation would be to solve for minimum time, subject to some velocity bounds.\n",
140+
"\n",
141+
"## Minimum distance, with velocity limits"
142+
]
143+
},
144+
{
145+
"cell_type": "code",
146+
"execution_count": null,
147+
"id": "e31da5d6",
148+
"metadata": {},
149+
"outputs": [],
150+
"source": [
151+
"trajopt = GcsTrajectoryOptimization(2)\n",
152+
"source, left, right, target = AddRegionsAndEdges(trajopt, order=4)\n",
153+
"\n",
154+
"trajopt.AddPathLengthCost()\n",
155+
"trajopt.AddVelocityBounds([-1, -1], [1, 1])\n",
156+
"[traj, result] = trajopt.SolvePath(source, target)\n",
157+
"\n",
158+
"PlotSolution(traj, result)"
159+
]
160+
},
161+
{
162+
"cell_type": "markdown",
163+
"id": "6bd03129",
164+
"metadata": {},
165+
"source": [
166+
"## Minimum time, with velocity limits"
167+
]
168+
},
169+
{
170+
"cell_type": "code",
171+
"execution_count": null,
172+
"id": "3c6d7f60",
173+
"metadata": {},
174+
"outputs": [],
175+
"source": [
176+
"trajopt = GcsTrajectoryOptimization(2)\n",
177+
"source, left, right, target = AddRegionsAndEdges(trajopt, order=4)\n",
178+
"\n",
179+
"trajopt.AddVelocityBounds([-1, -1], [1, 1])\n",
180+
"trajopt.AddTimeCost()\n",
181+
"[traj, result] = trajopt.SolvePath(source, target)\n",
182+
"\n",
183+
"PlotSolution(traj, result)"
184+
]
185+
},
186+
{
187+
"cell_type": "markdown",
188+
"id": "14d3b762",
189+
"metadata": {},
190+
"source": [
191+
"## Minimum time, velocity limits and C(1) continuity"
192+
]
193+
},
194+
{
195+
"cell_type": "code",
196+
"execution_count": null,
197+
"id": "b68f8f5b",
198+
"metadata": {},
199+
"outputs": [],
200+
"source": [
201+
"trajopt = GcsTrajectoryOptimization(2)\n",
202+
"source, left, right, target = AddRegionsAndEdges(trajopt, order=4)\n",
203+
"\n",
204+
"trajopt.AddVelocityBounds([-1, -1], [1, 1])\n",
205+
"trajopt.AddContinuityConstraints(continuity_order=1)\n",
206+
"trajopt.AddTimeCost()\n",
207+
"[traj, result] = trajopt.SolvePath(source, target)\n",
208+
"\n",
209+
"PlotSolution(traj, result)"
210+
]
211+
},
212+
{
213+
"cell_type": "markdown",
214+
"id": "355f30ce",
215+
"metadata": {},
216+
"source": [
217+
"Now here's the tricky one. If you ask for minimum distance + continuity constraints, you might be surprised with what you get.\n",
218+
"\n",
219+
"## Minimum distance with C(1) continuity"
220+
]
221+
},
222+
{
223+
"cell_type": "code",
224+
"execution_count": null,
225+
"id": "e9341a84",
226+
"metadata": {},
227+
"outputs": [],
228+
"source": [
229+
"trajopt = GcsTrajectoryOptimization(2)\n",
230+
"source, left, right, target = AddRegionsAndEdges(trajopt, order=4)\n",
231+
"\n",
232+
"trajopt.AddVelocityBounds([-1, -1], [1, 1])\n",
233+
"trajopt.AddContinuityConstraints(continuity_order=1)\n",
234+
"trajopt.AddPathLengthCost()\n",
235+
"options = GraphOfConvexSetsOptions()\n",
236+
"# NOTE: I have to disable rounding... otherwise SNOPT will fail.\n",
237+
"options.max_rounded_paths = 0\n",
238+
"[traj, result] = trajopt.SolvePath(source, target, options)\n",
239+
"\n",
240+
"PlotSolution(traj, result)"
241+
]
242+
},
243+
{
244+
"cell_type": "markdown",
245+
"id": "5bd29899",
246+
"metadata": {},
247+
"source": [
248+
"The solution in time looks reasonable. The solution in x-y looks a little surprising... that doesn't look like a smooth curve? What's going on here?\n",
249+
"The optimal solution puts multiple control points immediately on top of each other. So the velocity is continuous in time, but could change very rapidly in space. Adding higher derivative limits does not help... the trajectory can slow down in time, but still change direction very rapidly in space in order minimize the distance.\n",
250+
"\n",
251+
"This feels like a bad formulation... it drives the solver towards a solution that is numerically bad. In fact, SNOPT fails to solve it... so I've actually had to disable the rounding step in the code cell above.\n",
252+
"\n",
253+
"## Takeaways\n",
254+
"\n",
255+
"Be careful with your formulation. If your optimal solution is arbitrarily bad numerically, then you might need to rethink your formulation."
256+
]
257+
},
258+
{
259+
"cell_type": "markdown",
260+
"id": "a609b884",
261+
"metadata": {},
262+
"source": []
263+
}
264+
],
265+
"metadata": {
266+
"kernelspec": {
267+
"display_name": "Python 3",
268+
"language": "python",
269+
"name": "python3"
270+
},
271+
"language_info": {
272+
"codemirror_mode": {
273+
"name": "ipython",
274+
"version": 3
275+
},
276+
"file_extension": ".py",
277+
"mimetype": "text/x-python",
278+
"name": "python",
279+
"nbconvert_exporter": "python",
280+
"pygments_lexer": "ipython3",
281+
"version": "3.10.12"
282+
}
283+
},
284+
"nbformat": 4,
285+
"nbformat_minor": 5
286+
}

0 commit comments

Comments
 (0)