-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
gcs_trajectory_optimization.h
904 lines (760 loc) · 42.5 KB
/
gcs_trajectory_optimization.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
#pragma once
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
#include "drake/common/trajectories/bezier_curve.h"
#include "drake/common/trajectories/composite_trajectory.h"
#include "drake/geometry/optimization/convex_set.h"
#include "drake/geometry/optimization/graph_of_convex_sets.h"
#include "drake/multibody/plant/multibody_plant.h"
namespace drake {
namespace planning {
namespace trajectory_optimization {
/**
GcsTrajectoryOptimization implements a simplified motion planning optimization
problem introduced in the paper ["Motion Planning around Obstacles with Convex
Optimization"](https://arxiv.org/abs/2205.04422) by Tobia Marcucci, Mark
Petersen, David von Wrangel, Russ Tedrake.
Instead of using the full time-scaling curve, this problem uses a single
time-scaling variable for each region. This formulation yields continuous
trajectories, which are not differentiable at the transition times between the
regions since non-convex continuity constraints are not supported yet. However,
it supports continuity on the path r(s) for arbitrary degree. The path
r(s) can be reconstructed from the gcs solution q(t) with
`NormalizeSegmentTimes()` and post-processed with e.g. Toppra to enforce
acceleration bounds.
The ith piece of the composite trajectory is defined as q(t) = r((t - tᵢ) /
hᵢ). r : [0, 1] → ℝⁿ is a the path, parametrized as a Bézier curve with order
n. tᵢ and hᵢ are the initial time and duration of the ith sub-trajectory.
This class supports the notion of a Subgraph of regions. This has proven useful
to facilitate multi-modal motion planning such as: Subgraph A: find a
collision-free trajectory for the robot to a grasping posture, Subgraph B: find
a collision-free trajectory for the robot with the object in its hand to a
placing posture, etc.
@anchor continuous_revolute_joints
### Continuous Revolute Joints
This class also supports robots with continuous revolute joints (revolute joints
that don't have any joint limits) and mobile bases. Adding or subtracting 2π to
such a joint's angle leaves it unchanged; this logic is implemented behind the
scenes. To use it, one should specify the joint indices that don't have limits,
and ensure all sets satisfy the "convexity radius" property -- their width along
a dimension corresponding to a continuous revolute joint must be less than π.
This can be enforced when constructing the convex sets, or after the fact with
`geometry::optimization::PartitionConvexSet`. The `GcsTrajectoryOptimization`
methods `AddRegions` and `AddEdges` will handle all of the intersection checks
behind the scenes, including applying the appropriate logic to connect sets that
"wrap around" 2π.
*/
class GcsTrajectoryOptimization final {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GcsTrajectoryOptimization);
/** Constructs the motion planning problem.
@param num_positions is the dimension of the configuration space.
@param continuous_revolute_joints is a list of indices corresponding to
continuous revolute joints, i.e., revolute joints which don't have any joint
limits, and hence "wrap around" at 2π. Each entry in
continuous_revolute_joints must be non-negative, less than num_positions, and
unique. This feature is currently only supported within a single subgraph:
continuous revolute joints won't be taken into account when constructing edges
between subgraphs or checking if sets intersect through a subspace.
*/
explicit GcsTrajectoryOptimization(
int num_positions,
std::vector<int> continuous_revolute_joints = std::vector<int>());
~GcsTrajectoryOptimization();
/** A Subgraph is a subset of the larger graph. It is defined by a set of
regions and edges between them based on intersection. From an API standpoint,
a Subgraph is useful to define a multi-modal motion planning problem. Further,
it allows different constraints and objects to be added to different
subgraphs. Note that the the GraphOfConvexSets does not differentiate between
subgraphs and can't be mixed with other instances of
GcsTrajectoryOptimization.
*/
class Subgraph final {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Subgraph);
~Subgraph();
/** Returns the name of the subgraph. */
const std::string& name() const { return name_; }
/** Returns the order of the Bézier trajectory within the region. */
int order() const { return order_; }
/** Returns the number of vertices in the subgraph. */
int size() const { return vertices_.size(); }
/** Returns constant reference to a vector of mutable pointers to the
vertices stored in the subgraph. The order of the vertices is the same as
the order the regions were added.*/
const std::vector<geometry::optimization::GraphOfConvexSets::Vertex*>&
Vertices() {
return vertices_;
}
/** Returns pointers to the vertices stored in the subgraph.
The order of the vertices is the same as the order the regions were added.
@exclude_from_pydrake_mkdoc{This overload is not bound in pydrake.} */
std::vector<const geometry::optimization::GraphOfConvexSets::Vertex*>
Vertices() const;
/** Returns constant reference to a vector of mutable pointers to the
edges. */
const std::vector<geometry::optimization::GraphOfConvexSets::Edge*>&
Edges() {
return edges_;
}
/** Returns pointers to the edges stored in the subgraph.
@exclude_from_pydrake_mkdoc{This overload is not bound in pydrake.} */
std::vector<const geometry::optimization::GraphOfConvexSets::Edge*> Edges()
const;
/** Returns the regions associated with this subgraph before the
CartesianProduct. */
const geometry::optimization::ConvexSets& regions() const {
return regions_;
}
/** Adds a minimum time cost to all regions in the subgraph. The cost is the
sum of the time scaling variables.
@param weight is the relative weight of the cost.
*/
void AddTimeCost(double weight = 1.0);
/** Adds multiple L2Norm Costs on the upper bound of the path length.
Since we cannot directly compute the path length of a Bézier curve, we
minimize the upper bound of the path integral by minimizing the sum of
distances between control points. For Bézier curves, this is equivalent to
the sum of the L2Norm of the derivative control points of the curve divided
by the order.
@param weight_matrix is the relative weight of each component for the cost.
The diagonal of the matrix is the weight for each dimension. The
off-diagonal elements are the weight for the cross terms, which can be used
to penalize diagonal movement.
@pre weight_matrix must be of size num_positions() x num_positions().
*/
void AddPathLengthCost(const Eigen::MatrixXd& weight_matrix);
/** Adds multiple L2Norm Costs on the upper bound of the path length.
We upper bound the trajectory length by the sum of the distances between
control points. For Bézier curves, this is equivalent to the sum
of the L2Norm of the derivative control points of the curve divided by the
order.
@param weight is the relative weight of the cost.
*/
void AddPathLengthCost(double weight = 1.0);
/** Adds a linear velocity constraint to the subgraph `lb` ≤ q̇(t) ≤
`ub`.
@param lb is the lower bound of the velocity.
@param ub is the upper bound of the velocity.
@throws std::exception if subgraph order is zero, since the velocity is
defined as the derivative of the Bézier curve.
@throws std::exception if lb or ub are not of size num_positions().
*/
void AddVelocityBounds(const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub);
/** Adds a nonlinear derivative constraints to the subgraph `lb` ≤
dᴺq(t) / dtᴺ ≤ `ub`.
This adds a nonlinear constraint to the restriction and MIP
GraphOfConvexSets::Transcription, while adding a convex surrogate to the
relaxation. For more details, see @ref nonconvex_graph_of_convex_sets
"Guiding Non-convex Optimization with the GraphOfConvexSets".
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ
which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the
nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of
the set. For now, h₀ is set to 1.0 for all sets.
@param lb is the lower bound of the derivative.
@param ub is the upper bound of the derivative.
@param derivative_order is the order of the derivative to be constrained.
@throws std::exception if subgraph order is less than the derivative order.
@throws std::exception if the derivative order <= 1, since the linear
velocity bounds are preferred.
@throws std::exception if lb or ub are not of size num_positions().
*/
void AddNonlinearDerivativeBounds(
const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub, int derivative_order);
/** Enforces derivative continuity constraints on the subgraph.
Note that the constraints are on the control points of the
derivatives of r(s) and not q(t). This may result in discontinuities of the
trajectory return by `SolvePath()` since the r(s) will get rescaled by the
duration h to yield q(t). `NormalizeSegmentTimes()` will return r(s) with
valid continuity.
@param continuity_order is the order of the continuity constraint.
@throws std::exception if the continuity order is not equal or less than
the order the subgraphs.
@throws std::exception if the continuity order is less than one since path
continuity is enforced by default.
*/
void AddPathContinuityConstraints(int continuity_order);
/** Enforces derivative continuity constraints on the subgraph.
This adds a nonlinear constraint to the restriction and MIP
GraphOfConvexSets::Transcription, while adding a convex surrogate to the
relaxation. For more details, see @ref nonconvex_graph_of_convex_sets
"Guiding Non-convex Optimization with the GraphOfConvexSets".
The continuity is enforced on the control points of q(t), which appear as
nonlinear constraints.
<pre> (dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ </pre>
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are
replaced by the characteristic times of the respective sets:
<pre> (dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ </pre>.
For now, these are set to one, but future work may involve scaling them by
the size of the sets.
@param continuity_order is the order of the continuity constraint.
@throws std::exception if the continuity order is not equal or less than
the order the subgraphs.
@throws std::exception if the continuity order is less than one since path
continuity is enforced by default.
*/
void AddContinuityConstraints(int continuity_order);
private:
/* Constructs a new subgraph and copies the regions. */
Subgraph(const geometry::optimization::ConvexSets& regions,
const std::vector<std::pair<int, int>>& regions_to_connect,
int order, double h_min, double h_max, std::string name,
GcsTrajectoryOptimization* traj_opt,
std::optional<const std::vector<Eigen::VectorXd>> edge_offsets);
/* Convenience accessor, for brevity. */
int num_positions() const { return traj_opt_.num_positions(); }
/* Convenience accessor, for brevity. */
const std::vector<int>& continuous_revolute_joints() const {
return traj_opt_.continuous_revolute_joints();
}
/* Throw an error if any convex set in regions violates the convexity
radius. */
void ThrowsForInvalidConvexityRadius() const;
/* Extracts the control points variables from a vertex. */
Eigen::Map<const MatrixX<symbolic::Variable>> GetControlPoints(
const geometry::optimization::GraphOfConvexSets::Vertex& v) const;
/* Extracts the time scaling variable from a vertex. */
symbolic::Variable GetTimeScaling(
const geometry::optimization::GraphOfConvexSets::Vertex& v) const;
const geometry::optimization::ConvexSets regions_;
const int order_;
const double h_min_;
const std::string name_;
GcsTrajectoryOptimization& traj_opt_;
std::vector<geometry::optimization::GraphOfConvexSets::Vertex*> vertices_;
std::vector<geometry::optimization::GraphOfConvexSets::Edge*> edges_;
// r(s) is a BezierCurve of the right shape and order, which can be used to
// design costs and constraints for the underlying vertices and edges.
trajectories::BezierCurve<double> r_trajectory_;
friend class GcsTrajectoryOptimization;
};
/** EdgesBetweenSubgraphs are defined as the connecting edges between two
given subgraphs. These edges are a subset of the many other edges in the
larger graph. From an API standpoint, EdgesBetweenSubgraphs enable transitions
between Subgraphs, which can enable transitions between modes. Further, it
allows different constraints to be added in the transition between subgraphs.
Note that the EdgesBetweenSubgraphs can't be separated from the actual edges
in the GraphOfConvexSets framework, thus mixing it with other instances of
GCSTrajetoryOptimization is not supported.
*/
class EdgesBetweenSubgraphs final {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(EdgesBetweenSubgraphs);
~EdgesBetweenSubgraphs();
/** Adds a linear velocity constraint to the control point connecting the
subgraphs `lb` ≤ q̇(t) ≤ `ub`.
@param lb is the lower bound of the velocity.
@param ub is the upper bound of the velocity.
@throws std::exception if both subgraphs order is zero, since the velocity
is defined as the derivative of the Bézier curve. At least one of the
subgraphs must have an order of at least 1.
@throws std::exception if lb or ub are not of size num_positions().
*/
void AddVelocityBounds(const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub);
/** Adds a nonlinear derivative constraints to the control point connecting
the subgraphs `lb` ≤ dᴺq(t) / dtᴺ ≤ `ub`.
This adds a nonlinear constraint to the restriction and MIP
GraphOfConvexSets::Transcription, while adding a convex surrogate to the
relaxation. For more details, see @ref nonconvex_graph_of_convex_sets
"Guiding Non-convex Optimization with the GraphOfConvexSets".
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ
which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the
nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of
the set. For now, h₀ is set to 1.0 for all sets.
@param lb is the lower bound of the derivative.
@param ub is the upper bound of the derivative.
@param derivative_order is the order of the derivative to be constrained.
@throws std::exception if both subgraphs order is less than the desired
derivative order.
@throws std::exception if the derivative order <= 1, since the linear
velocity bounds are preferred.
@throws std::exception if lb or ub are not of size num_positions().
*/
void AddNonlinearDerivativeBounds(
const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub, int derivative_order);
/** Enforces zero derivatives on the control point connecting the subgraphs.
For velocity, acceleration, jerk, etc. enforcing zero-derivative on the
trajectory q(t) is equivalent to enforcing zero-derivative on the trajectory
r(s). Hence this constraint is convex.
@param derivative_order is the order of the derivative to be constrained.
@throws std::exception if the derivative order < 1.
@throws std::exception if both subgraphs order is less than the desired
derivative order.
*/
void AddZeroDerivativeConstraints(int derivative_order);
/** Enforces derivative continuity constraints on the edges between the
subgraphs.
Note that the constraints are on the control points of the
derivatives of r(s) and not q(t). This may result in discontinuities of the
trajectory return by `SolvePath()` since the r(s) will get rescaled by the
duration h to yield q(t). `NormalizeSegmentTimes()` will return r(s) with
valid continuity.
@param continuity_order is the order of the continuity constraint.
@throws std::exception if the continuity order is not equal or less than
the order of both subgraphs.
@throws std::exception if the continuity order is less than one since path
continuity is enforced by default.
*/
void AddPathContinuityConstraints(int continuity_order);
/** Enforces derivative continuity constraints on the edges between the
subgraphs.
This adds a nonlinear constraint to the restriction and MIP
GraphOfConvexSets::Transcription, while adding a convex surrogate to the
relaxation. For more details, see @ref nonconvex_graph_of_convex_sets
"Guiding Non-convex Optimization with the GraphOfConvexSets".
The continuity is enforced on the control points of q(t), which appear as
nonlinear constraints.
<pre> (dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ </pre>
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are
replaced by the characteristic times of the respective sets:
<pre> (dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ </pre>.
For now, these are set to one, but future work may involve scaling them by
the size of the sets.
@param continuity_order is the order of the continuity constraint.
@throws std::exception if the continuity order is not equal or less than
the order of both subgraphs.
@throws std::exception if the continuity order is less than one since path
continuity is enforced by default.
*/
void AddContinuityConstraints(int continuity_order);
/** Returns constant reference to a vector of mutable pointers to the
edges. */
const std::vector<geometry::optimization::GraphOfConvexSets::Edge*>&
Edges() {
return edges_;
}
/** Returns pointers to the edges.
@exclude_from_pydrake_mkdoc{This overload is not bound in pydrake.} */
std::vector<const geometry::optimization::GraphOfConvexSets::Edge*> Edges()
const;
private:
EdgesBetweenSubgraphs(
const Subgraph& from_subgraph, const Subgraph& to_subgraph,
const geometry::optimization::ConvexSet* subspace,
GcsTrajectoryOptimization* traj_opt,
const std::vector<std::pair<int, int>>& edges_between_regions = {},
std::optional<const std::vector<Eigen::VectorXd>> edge_offsets =
std::nullopt);
/* Convenience accessor, for brevity. */
int num_positions() const { return traj_opt_.num_positions(); }
/* Convenience accessor, for brevity. */
const std::vector<int>& continuous_revolute_joints() const {
return traj_opt_.continuous_revolute_joints();
}
bool RegionsConnectThroughSubspace(
const geometry::optimization::ConvexSet& A,
const geometry::optimization::ConvexSet& B,
const geometry::optimization::ConvexSet& subspace,
std::optional<const Eigen::VectorXd> maybe_set_B_offset = std::nullopt,
std::optional<const Eigen::VectorXd> maybe_subspace_offset =
std::nullopt);
/* Extracts the control points variables from an edge. */
Eigen::Map<const MatrixX<symbolic::Variable>> GetControlPointsU(
const geometry::optimization::GraphOfConvexSets::Edge& e) const;
/* Extracts the control points variables from an edge. */
Eigen::Map<const MatrixX<symbolic::Variable>> GetControlPointsV(
const geometry::optimization::GraphOfConvexSets::Edge& e) const;
/* Extracts the time scaling variable from a edge. */
symbolic::Variable GetTimeScalingU(
const geometry::optimization::GraphOfConvexSets::Edge& e) const;
/* Extracts the time scaling variable from a edge. */
symbolic::Variable GetTimeScalingV(
const geometry::optimization::GraphOfConvexSets::Edge& e) const;
GcsTrajectoryOptimization& traj_opt_;
const Subgraph& from_subgraph_;
const Subgraph& to_subgraph_;
trajectories::BezierCurve<double> ur_trajectory_;
trajectories::BezierCurve<double> vr_trajectory_;
std::vector<geometry::optimization::GraphOfConvexSets::Edge*> edges_;
friend class GcsTrajectoryOptimization;
};
/** Returns the number of position variables. */
int num_positions() const { return num_positions_; }
/** Returns a list of indices corresponding to continuous revolute joints. */
const std::vector<int>& continuous_revolute_joints() {
return continuous_revolute_joints_;
}
/** Returns a Graphviz string describing the graph vertices and edges. If
`results` is supplied, then the graph will be annotated with the solution
values.
@param show_slacks determines whether the values of the intermediate
(slack) variables are also displayed in the graph.
@param precision sets the floating point precision (how many digits are
generated) of the annotations.
@param scientific sets the floating point formatting to scientific (if true)
or fixed (if false).
*/
std::string GetGraphvizString(
const std::optional<solvers::MathematicalProgramResult>& result =
std::nullopt,
bool show_slack = true, int precision = 3,
bool scientific = false) const {
return gcs_.GetGraphvizString(result, show_slack, precision, scientific);
}
/** Creates a Subgraph with the given regions and indices.
@param regions represent the valid set a control point can be in. We retain a
copy of the regions since other functions may access them. If any of the
positions represent revolute joints without limits, each region has a maximum
width of strictly less than π along dimensions corresponding to those joints.
@param edges_between_regions is a list of pairs of indices into the regions
vector. For each pair representing an edge between two regions, an edge is
added within the subgraph. Note that the edges are directed so (i,j) will only
add an edge from region i to region j.
@param order is the order of the Bézier curve.
@param h_max is the maximum duration to spend in a region (seconds). Some
solvers struggle numerically with large values.
@param h_min is the minimum duration to spend in a region (seconds) if that
region is visited on the optimal path. Some cost and constraints are only
convex for h > 0. For example the perspective quadratic cost of the path
energy ||ṙ(s)||² / h becomes non-convex for h = 0. Otherwise h_min can be set
to 0.
@param name is the name of the subgraph. If the passed name is an empty
string, a default name will be provided.
@param edge_offsets is an optional list of vectors. If defined, the list must
contain the same number of entries as `edges_between_regions`. In other words,
if defined, there must be one edge offset for each specified edge. For each
pair of sets listed in `edges_between_regions`, the first set is translated
(in configuration space) by the corresponding vector in edge_offsets before
computing the constraints associated to that edge. This is used to add edges
between sets that "wrap around" 2π along some dimension, due to, e.g., a
continuous revolute joint. This edge offset corresponds to the translation
component of the affine map τ_uv in equation (11) of "Non-Euclidean Motion
Planning with Graphs of Geodesically-Convex Sets", and per the discussion in
Subsection VI A, τ_uv has no rotation component.
@throws std::exception if any index referenced in `edges_between_regions` is
outside the range [0, ssize(regions)).
*/
Subgraph& AddRegions(
const geometry::optimization::ConvexSets& regions,
const std::vector<std::pair<int, int>>& edges_between_regions, int order,
double h_min = 0, double h_max = 20, std::string name = "",
std::optional<const std::vector<Eigen::VectorXd>> edge_offsets =
std::nullopt);
/** Creates a Subgraph with the given regions.
This function will compute the edges between the regions based on the set
intersections.
@param regions represent the valid set a control point can be in. We retain a
copy of the regions since other functions may access them. If any of the
positions represent continuous revolute joints, each region must have a
maximum width of strictly less than π along dimensions corresponding to those
joints.
@param order is the order of the Bézier curve.
@param h_min is the minimum duration to spend in a region (seconds) if that
region is visited on the optimal path. Some cost and constraints are only
convex for h > 0. For example the perspective quadratic cost of the path
energy ||ṙ(s)||² / h becomes non-convex for h = 0. Otherwise h_min can be set
to 0.
@param h_max is the maximum duration to spend in a region (seconds). Some
solvers struggle numerically with large values.
@param name is the name of the subgraph. A default name will be provided.
@throws std::exception if any of the regions has a width of π or greater along
dimensions corresponding to continuous revolute joints.
*/
Subgraph& AddRegions(const geometry::optimization::ConvexSets& regions,
int order, double h_min = 0, double h_max = 20,
std::string name = "");
/** Remove a subgraph and all associated edges found in the subgraph and
to and from other subgraphs.
@pre The subgraph must have been created from a call to AddRegions() on this
object.
*/
void RemoveSubgraph(const Subgraph& subgraph);
/** Connects two subgraphs with directed edges.
@param from_subgraph is the subgraph to connect from. Must have been created
from a call to AddRegions() on this object, not some other optimization
program.
@param to_subgraph is the subgraph to connect to. Must have been created from
a call to AddRegions() on this object, not some other optimization program.
@param subspace is the subspace that the connecting control points must be in.
Subspace is optional. Only edges that connect through the subspace will be
added, and the subspace is added as a constraint on the connecting control
points. Subspaces of type point or HPolyhedron are supported since other sets
require constraints that are not yet supported by the GraphOfConvexSets::Edge
constraint, e.g., set containment of a Hyperellipsoid is formulated via
LorentzCone constraints. Workaround: Create a subgraph of zero order with the
subspace as the region and connect it between the two subgraphs. This works
because GraphOfConvexSet::Vertex supports arbitrary instances of ConvexSets.
@param edges_between_regions can be used to manually specify which edges
should be added, avoiding the intersection checks. It should be a list of
tuples `(i,j)`, where an edge will be added from the `i`th index region in
`from_subgraph` to the `j`th index region in `to_subgraph`. If an empty
vector is given for this argument, the edges are manually computed by checking
for pairwise intersections.
@param edge_offsets is an optional list of vectors. If defined, the list must
contain the same number of entries as `edges_between_regions`. In other words,
if defined, there must be one edge offset for each specified edge. For each
pair of sets listed in `edges_between_regions`, the first set is translated
(in configuration space) by the corresponding vector in edge_offsets before
computing the constraints associated to that edge. This is used to add edges
between sets that "wrap around" 2π along some dimension, due to, e.g., a
continuous revolute joint. This edge offset corresponds to the translation
component of the affine map τ_uv in equation (11) of "Non-Euclidean Motion
Planning with Graphs of Geodesically-Convex Sets", and per the discussion in
Subsection VI A, τ_uv has no rotation component.
@throws std::exception if `edge_offsets` is provided, but `edge_offsets.size()
!= edges_between_regions.size()`.
*/
EdgesBetweenSubgraphs& AddEdges(
const Subgraph& from_subgraph, const Subgraph& to_subgraph,
const geometry::optimization::ConvexSet* subspace = nullptr,
const std::vector<std::pair<int, int>>& edges_between_regions = {},
std::optional<const std::vector<Eigen::VectorXd>> edge_offsets =
std::nullopt);
/** Adds a minimum time cost to all regions in the whole graph. The cost is
the sum of the time scaling variables.
This cost will be added to the entire graph. Note that this cost
will be applied even to subgraphs added in the future.
@param weight is the relative weight of the cost.
*/
void AddTimeCost(double weight = 1.0);
/** Adds multiple L2Norm Costs on the upper bound of the path length.
Since we cannot directly compute the path length of a Bézier curve, we
minimize the upper bound of the path integral by minimizing the sum of
(weighted) distances between control points: ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂.
This cost will be added to the entire graph. Since the path length is only
defined for Bézier curves that have two or more control points, this cost will
only added to all subgraphs with order greater than zero. Note that this cost
will be applied even to subgraphs added in the future.
@param weight_matrix is the relative weight of each component for the cost.
The diagonal of the matrix is the weight for each dimension. The
off-diagonal elements are the weight for the cross terms, which can be used
to penalize diagonal movement.
@pre weight_matrix must be of size num_positions() x num_positions().
*/
void AddPathLengthCost(const Eigen::MatrixXd& weight_matrix);
/** Adds multiple L2Norm Costs on the upper bound of the path length.
Since we cannot directly compute the path length of a Bézier curve, we
minimize the upper bound of the path integral by minimizing the sum of
distances between control points. For Bézier curves, this is equivalent to the
sum of the L2Norm of the derivative control points of the curve divided by the
order.
This cost will be added to the entire graph. Since the path length is only
defined for Bézier curves that have two or more control points, this cost will
only added to all subgraphs with order greater than zero. Note that this cost
will be applied even to subgraphs added in the future.
@param weight is the relative weight of the cost.
*/
void AddPathLengthCost(double weight = 1.0);
/** Adds a linear velocity constraint to the entire graph `lb` ≤ q̇(t) ≤
`ub`.
@param lb is the lower bound of the velocity.
@param ub is the upper bound of the velocity.
This constraint will be added to the entire graph. Since the velocity requires
forming the derivative of the Bézier curve, this constraint will only added to
all subgraphs with order greater than zero. Note that this constraint will be
applied even to subgraphs added in the future.
@throws std::exception if lb or ub are not of size num_positions().
*/
void AddVelocityBounds(const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub);
/** Adds a nonlinear derivative constraints to the entire graph `lb` ≤
dᴺq(t) / dtᴺ ≤ `ub`.
This adds a nonlinear constraint to the restriction and MIP
GraphOfConvexSets::Transcription, while adding a convex surrogate to the
relaxation. For more details, see @ref nonconvex_graph_of_convex_sets
"Guiding Non-convex Optimization with the GraphOfConvexSets".
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ
which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the
nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of
the set. For now, h₀ is set to 1.0 for all sets.
@param lb is the lower bound of the derivative.
@param ub is the upper bound of the derivative.
@param derivative_order is the order of the derivative to be constrained.
@throws std::exception if lb or ub are not of size num_positions().
@throws std::exception if the derivative order <= 1, since the linear
velocity bounds are preferred.
*/
void AddNonlinearDerivativeBounds(const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub,
int derivative_order);
/** Enforces derivative continuity constraints on the entire graph.
Note that the constraints are on the control points of the
derivatives of r(s) and not q(t). This may result in discontinuities of the
trajectory return by `SolvePath()` since the r(s) will get rescaled by the
duration h to yield q(t). `NormalizeSegmentTimes()` will return r(s) with
valid continuity.
@param continuity_order is the order of the continuity constraint.
@throws std::exception if the continuity order is less than one since path
continuity is enforced by default.
*/
void AddPathContinuityConstraints(int continuity_order);
/** Enforces derivative continuity constraints on the entire graph.
This adds a nonlinear constraint to the restriction and MIP
GraphOfConvexSets::Transcription, while adding a convex surrogate to the
relaxation. For more details, see @ref nonconvex_graph_of_convex_sets
"Guiding Non-convex Optimization with the GraphOfConvexSets".
The continuity is enforced on the control points of q(t), which appear as
nonlinear constraints.
<pre> (dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ </pre>
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are
replaced by the characteristic times of the respective sets:
<pre> (dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ </pre>.
For now, these are set to one, but future work may involve scaling them by
the size of the sets.
@param continuity_order is the order of the continuity constraint.
@throws std::exception if the continuity order is less than one since path
continuity is enforced by default.
*/
void AddContinuityConstraints(int continuity_order);
/** Formulates and solves the mixed-integer convex formulation of the
shortest path problem on the whole graph. @see
`geometry::optimization::GraphOfConvexSets::SolveShortestPath()` for further
details.
@param source specifies the source subgraph. Must have been created from a
call to AddRegions() on this object, not some other optimization program. If
the source is a subgraph with more than one region, an empty set will be
added and optimizer will choose the best region to start in. To start in a
particular point, consider adding a subgraph of order zero with a single
region of type Point.
@param target specifies the target subgraph. Must have been created from a
call to AddRegions() on this object, not some other optimization program. If
the target is a subgraph with more than one region, an empty set will be
added and optimizer will choose the best region to end in. To end in a
particular point, consider adding a subgraph of order zero with a single
region of type Point.
@param options include all settings for solving the shortest path problem.
The following default options will be used if they are not provided in
`options`:
- `options.convex_relaxation = true`,
- `options.max_rounded_paths = 5`,
- `options.preprocessing = true`.
@see `geometry::optimization::GraphOfConvexSetsOptions` for further details.
*/
std::pair<trajectories::CompositeTrajectory<double>,
solvers::MathematicalProgramResult>
SolvePath(
const Subgraph& source, const Subgraph& target,
const geometry::optimization::GraphOfConvexSetsOptions& options = {});
/** Solves a trajectory optimization problem through specific vertices.
This method allows for targeted optimization by considering only selected
active vertices, reducing the problem's complexity.
See geometry::optimization::GraphOfConvexSets::SolveConvexRestriction().
This API prefers a sequence of vertices over edges, as a user may know which
regions the solution should pass through.
GcsTrajectoryOptimization::AddRegions() automatically manages edge creation
and intersection checks, which makes passing a sequence of edges less
convenient.
@param active_vertices A sequence of ordered vertices of subgraphs to be
included in the problem.
@param options include all settings for solving the shortest path problem.
@pre There must be at least two vertices in active_vertices.
@throws std::exception if the vertices are not connected.
@throws std::exception if two vertices are connected by multiple edges. This
may happen if one connects two graphs through multiple subspaces, which is
currently not supported with this method.
@throws std::exception if the program cannot be written as a convex
optimization consumable by one of the standard solvers.*/
std::pair<trajectories::CompositeTrajectory<double>,
solvers::MathematicalProgramResult>
SolveConvexRestriction(
const std::vector<
const geometry::optimization::GraphOfConvexSets::Vertex*>&
active_vertices,
const geometry::optimization::GraphOfConvexSetsOptions& options = {});
/** Provide a heuristic estimate of the complexity of the underlying
GCS mathematical program, for regression testing purposes.
Here we sum the total number of variable appearances in our costs and
constraints as a rough approximation of the complexity of the subproblems. */
double EstimateComplexity() const;
/** Returns a vector of all subgraphs. */
std::vector<Subgraph*> GetSubgraphs() const;
/** Returns a vector of all edges between subgraphs. */
std::vector<EdgesBetweenSubgraphs*> GetEdgesBetweenSubgraphs() const;
/** Getter for the underlying GraphOfConvexSets. This is intended primarily
for inspecting the resulting programs. */
const geometry::optimization::GraphOfConvexSets& graph_of_convex_sets()
const {
return gcs_;
}
/** Normalizes each trajectory segment to one second in duration.
Reconstructs the path r(s) from the solution trajectory q(t) of
`SolvePath()` s.t. each segment of the resulting trajectory
will be one second long. The start time will match the original start time.
@param trajectory The solution trajectory returned by `SolvePath()`.
@throws std::exception if not all trajectory segments of the
CompositeTrajectory are of type BezierCurve<double>
*/
static trajectories::CompositeTrajectory<double> NormalizeSegmentTimes(
const trajectories::CompositeTrajectory<double>& trajectory);
/** Unwraps a trajectory with continuous revolute joints into a continuous
trajectory in the Euclidean space. Trajectories produced by
GcsTrajectoryOptimization for robotic systems with continuous revolute joints
may include apparent discontinuities, where a multiple of 2π is
instantaneously added to a joint value at the boundary between two adjacent
segments of the trajectory. This function removes such discontinuities by
adding or subtracting the appropriate multiple of 2π, "unwrapping" the
trajectory into a continuous representation suitable for downstream tasks
that do not take the joint wraparound into account.
@param gcs_trajectory The trajectory to unwrap.
@param continuous_revolute_joints The indices of the continuous revolute
joints.
@param tol The numerical tolerance used to determine if two subsequent
segments start and end at the same value modulo 2π for continuous revolute
joints.
@param starting_rounds A vector of integers that sets the starting rounds for
each continuous revolute joint. Given integer k for the starting_round of a
joint, its initial position will be wrapped into [2πk , 2π(k+1)). If the
starting rounds are not provided, the initial position of @p trajectory will
be unchanged.
@returns an unwrapped (continous in Euclidean space) CompositeTrajectory.
@throws std::exception if
starting_rounds.size()!=continuous_revolute_joints.size().
@throws std::exception if continuous_revolute_joints contain repeated indices
and/or indices outside the range [0, gcs_trajectory.rows()).
@throws std::exception if the gcs_trajectory is not continuous on the
manifold defined by the continuous_revolute_joints, i.e., the shift between
two consecutive segments is not an integer multiple of 2π (within a tolerance
of `tol` radians).
@throws std::exception if all the segments are not of type BezierCurve.
Other types are not supported yet. Note that currently the output of
GcsTrajectoryOptimization::SolvePath() is a CompositeTrajectory of
BezierCurves.
*/
static trajectories::CompositeTrajectory<double> UnwrapToContinousTrajectory(
const trajectories::CompositeTrajectory<double>& gcs_trajectory,
std::vector<int> continuous_revolute_joints,
std::optional<std::vector<int>> starting_rounds = std::nullopt,
double tol = 1e-8);
private:
const int num_positions_;
const std::vector<int> continuous_revolute_joints_;
trajectories::CompositeTrajectory<double>
ReconstructTrajectoryFromSolutionPath(
std::vector<const geometry::optimization::GraphOfConvexSets::Edge*> edges,
const solvers::MathematicalProgramResult& result);
// Adds a Edge to gcs_ with the name "{u.name} -> {v.name}".
geometry::optimization::GraphOfConvexSets::Edge* AddEdge(
geometry::optimization::GraphOfConvexSets::Vertex* u,
geometry::optimization::GraphOfConvexSets::Vertex* v);
geometry::optimization::GraphOfConvexSets gcs_;
// Store the subgraphs by reference.
std::vector<std::unique_ptr<Subgraph>> subgraphs_;
std::vector<std::unique_ptr<EdgesBetweenSubgraphs>> subgraph_edges_;
std::map<const geometry::optimization::GraphOfConvexSets::Vertex*, Subgraph*>
vertex_to_subgraph_;
std::vector<double> global_time_costs_;
std::vector<Eigen::MatrixXd> global_path_length_costs_;
std::vector<std::pair<Eigen::VectorXd, Eigen::VectorXd>>
global_velocity_bounds_{};
std::vector<std::tuple<Eigen::VectorXd, Eigen::VectorXd, int>>
global_nonlinear_derivative_bounds_{};
std::vector<int> global_path_continuity_constraints_{};
std::vector<int> global_continuity_constraints_{};
};
/** Returns a list of indices in the plant's generalized positions which
correspond to a continuous revolute joint (a revolute joint with no joint
limits). This includes the revolute component of a planar joint */
std::vector<int> GetContinuousRevoluteJointIndices(
const multibody::MultibodyPlant<double>& plant);
} // namespace trajectory_optimization
} // namespace planning
} // namespace drake