77 "github.com/pkg/errors"
88
99 "go.viam.com/rdk/motionplan/ik"
10+ "go.viam.com/rdk/pointcloud"
1011 "go.viam.com/rdk/referenceframe"
12+ "go.viam.com/rdk/spatialmath"
1113)
1214
1315var defaultMinStepCount = 2
@@ -19,6 +21,224 @@ type ConstraintHandler struct {
1921 segmentFSConstraints map [string ]SegmentFSConstraint
2022 stateConstraints map [string ]StateConstraint
2123 stateFSConstraints map [string ]StateFSConstraint
24+ pathMetric ik.StateFSMetric // Distance function which converges on the valid manifold of intermediate path states
25+ }
26+
27+ func newEmptyConstraintHandler () * ConstraintHandler {
28+ handler := ConstraintHandler {}
29+ handler .pathMetric = ik .NewZeroFSMetric ()
30+ return & handler
31+ }
32+
33+ func newConstraintHandler (
34+ opt * plannerOptions ,
35+ constraints * Constraints ,
36+ from , to * PlanState ,
37+ fs referenceframe.FrameSystem ,
38+ seedMap referenceframe.FrameSystemInputs ,
39+ worldState * referenceframe.WorldState ,
40+ boundingRegions []spatialmath.Geometry ,
41+ ) (* ConstraintHandler , error ) {
42+ handler := newEmptyConstraintHandler ()
43+
44+ startPoses , err := from .ComputePoses (fs )
45+ if err != nil {
46+ return nil , err
47+ }
48+ goalPoses , err := to .ComputePoses (fs )
49+ if err != nil {
50+ return nil , err
51+ }
52+
53+ frameSystemGeometries , err := referenceframe .FrameSystemGeometries (fs , seedMap )
54+ if err != nil {
55+ return nil , err
56+ }
57+
58+ movingRobotGeometries , staticRobotGeometries := opt .motionChains .geometries (fs , frameSystemGeometries )
59+
60+ obstaclesInFrame , err := worldState .ObstaclesInWorldFrame (fs , seedMap )
61+ if err != nil {
62+ return nil , err
63+ }
64+ worldGeometries := obstaclesInFrame .Geometries ()
65+
66+ frameNames := map [string ]bool {}
67+ for _ , fName := range fs .FrameNames () {
68+ frameNames [fName ] = true
69+ }
70+
71+ allowedCollisions , err := collisionSpecifications (
72+ constraints .GetCollisionSpecification (),
73+ frameSystemGeometries ,
74+ frameNames ,
75+ worldState .ObstacleNames (),
76+ )
77+ if err != nil {
78+ return nil , err
79+ }
80+
81+ // If we are planning on a SLAM map we want to not allow a collision with the pointcloud to start our move call
82+ // Typically starting collisions are whitelisted,
83+ // TODO: This is not the most robust way to deal with this but is better than driving through walls.
84+ if opt .useTPspace () {
85+ var zeroCG * collisionGraph
86+ for _ , geom := range worldGeometries {
87+ if octree , ok := geom .(* pointcloud.BasicOctree ); ok {
88+ if zeroCG == nil {
89+ zeroCG , err = setupZeroCG (movingRobotGeometries , worldGeometries , allowedCollisions , false , opt .CollisionBufferMM )
90+ if err != nil {
91+ return nil , err
92+ }
93+ }
94+ // Check if a moving geometry is in collision with a pointcloud. If so, error.
95+ for _ , collision := range zeroCG .collisions (opt .CollisionBufferMM ) {
96+ if collision .name1 == octree .Label () {
97+ return nil , fmt .Errorf ("starting collision between SLAM map and %s, cannot move" , collision .name2 )
98+ } else if collision .name2 == octree .Label () {
99+ return nil , fmt .Errorf ("starting collision between SLAM map and %s, cannot move" , collision .name1 )
100+ }
101+ }
102+ }
103+ }
104+ }
105+
106+ // add collision constraints
107+ fsCollisionConstraints , stateCollisionConstraints , err := createAllCollisionConstraints (
108+ movingRobotGeometries ,
109+ staticRobotGeometries ,
110+ worldGeometries ,
111+ boundingRegions ,
112+ allowedCollisions ,
113+ opt .CollisionBufferMM ,
114+ )
115+ if err != nil {
116+ return nil , err
117+ }
118+ // For TPspace
119+ for name , constraint := range stateCollisionConstraints {
120+ handler .AddStateConstraint (name , constraint )
121+ }
122+ for name , constraint := range fsCollisionConstraints {
123+ handler .AddStateFSConstraint (name , constraint )
124+ }
125+
126+ constraints .updateFromOptions (opt )
127+
128+ _ , err = handler .addTopoConstraints (fs , seedMap , startPoses , goalPoses , constraints )
129+ if err != nil {
130+ return nil , err
131+ }
132+
133+ return handler , nil
134+ }
135+
136+ // addPbConstraints will add all constraints from the passed Constraint struct. This will deal with only the topological
137+ // constraints. It will return a bool indicating whether there are any to add.
138+ func (c * ConstraintHandler ) addTopoConstraints (
139+ fs referenceframe.FrameSystem ,
140+ startCfg referenceframe.FrameSystemInputs ,
141+ from , to referenceframe.FrameSystemPoses ,
142+ constraints * Constraints ,
143+ ) (bool , error ) {
144+ topoConstraints := false
145+ for _ , linearConstraint := range constraints .GetLinearConstraint () {
146+ topoConstraints = true
147+ // TODO RSDK-9224: Our proto for constraints does not allow the specification of which frames should be constrainted relative to
148+ // which other frames. If there is only one goal specified, then we assume that the constraint is between the moving and goal frame.
149+ err := c .addLinearConstraints (fs , startCfg , from , to , linearConstraint )
150+ if err != nil {
151+ return false , err
152+ }
153+ }
154+ for _ , pseudolinearConstraint := range constraints .GetPseudolinearConstraint () {
155+ // pseudolinear constraints
156+ err := c .addPseudolinearConstraints (fs , startCfg , from , to , pseudolinearConstraint )
157+ if err != nil {
158+ return false , err
159+ }
160+ }
161+ for _ , orientationConstraint := range constraints .GetOrientationConstraint () {
162+ topoConstraints = true
163+ // TODO RSDK-9224: Our proto for constraints does not allow the specification of which frames should be constrainted relative to
164+ // which other frames. If there is only one goal specified, then we assume that the constraint is between the moving and goal frame.
165+ err := c .addOrientationConstraints (fs , startCfg , from , to , orientationConstraint )
166+ if err != nil {
167+ return false , err
168+ }
169+ }
170+ return topoConstraints , nil
171+ }
172+
173+ func (c * ConstraintHandler ) addLinearConstraints (
174+ fs referenceframe.FrameSystem ,
175+ startCfg referenceframe.FrameSystemInputs ,
176+ from , to referenceframe.FrameSystemPoses ,
177+ linConstraint LinearConstraint ,
178+ ) error {
179+ // Linear constraints
180+ linTol := linConstraint .LineToleranceMm
181+ if linTol == 0 {
182+ // Default
183+ linTol = defaultLinearDeviation
184+ }
185+ orientTol := linConstraint .OrientationToleranceDegs
186+ if orientTol == 0 {
187+ orientTol = defaultOrientationDeviation
188+ }
189+ constraint , pathDist , err := CreateAbsoluteLinearInterpolatingConstraintFS (fs , startCfg , from , to , linTol , orientTol )
190+ if err != nil {
191+ return err
192+ }
193+ c .AddStateFSConstraint (defaultConstraintName , constraint )
194+
195+ c .pathMetric = ik .CombineFSMetrics (c .pathMetric , pathDist )
196+ return nil
197+ }
198+
199+ func (c * ConstraintHandler ) addPseudolinearConstraints (
200+ fs referenceframe.FrameSystem ,
201+ startCfg referenceframe.FrameSystemInputs ,
202+ from , to referenceframe.FrameSystemPoses ,
203+ plinConstraint PseudolinearConstraint ,
204+ ) error {
205+ // Linear constraints
206+ linTol := plinConstraint .LineToleranceFactor
207+ if linTol == 0 {
208+ // Default
209+ linTol = defaultPseudolinearTolerance
210+ }
211+ orientTol := plinConstraint .OrientationToleranceFactor
212+ if orientTol == 0 {
213+ orientTol = defaultPseudolinearTolerance
214+ }
215+ constraint , pathDist , err := CreateProportionalLinearInterpolatingConstraintFS (fs , startCfg , from , to , linTol , orientTol )
216+ if err != nil {
217+ return err
218+ }
219+ c .AddStateFSConstraint (defaultConstraintName , constraint )
220+
221+ c .pathMetric = ik .CombineFSMetrics (c .pathMetric , pathDist )
222+ return nil
223+ }
224+
225+ func (c * ConstraintHandler ) addOrientationConstraints (
226+ fs referenceframe.FrameSystem ,
227+ startCfg referenceframe.FrameSystemInputs ,
228+ from , to referenceframe.FrameSystemPoses ,
229+ orientConstraint OrientationConstraint ,
230+ ) error {
231+ orientTol := orientConstraint .OrientationToleranceDegs
232+ if orientTol == 0 {
233+ orientTol = defaultOrientationDeviation
234+ }
235+ constraint , pathDist , err := CreateSlerpOrientationConstraintFS (fs , startCfg , from , to , orientTol )
236+ if err != nil {
237+ return err
238+ }
239+ c .AddStateFSConstraint (defaultConstraintName , constraint )
240+ c .pathMetric = ik .CombineFSMetrics (c .pathMetric , pathDist )
241+ return nil
22242}
23243
24244// CheckStateConstraints will check a given input against all state constraints.
0 commit comments