diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 6a2b1e29f3c..4ede542e03d 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -15,7 +15,6 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" ) const ( @@ -30,11 +29,14 @@ const ( type cbirrtOptions struct { // Number of IK solutions with which to seed the goal side of the bidirectional tree. SolutionsToSeed int `json:"solutions_to_seed"` + + // This is how far cbirrt will try to extend the map towards a goal per-step. Determined from FrameStep + qstep map[string][]float64 } // newCbirrtOptions creates a struct controlling the running of a single invocation of cbirrt. All values are pre-set to reasonable // defaults, but can be tweaked if needed. -func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) { +func newCbirrtOptions(planOpts *plannerOptions, lfs *linearizedFrameSystem) (*cbirrtOptions, error) { algOpts := &cbirrtOptions{ SolutionsToSeed: defaultSolutionsToSeed, } @@ -47,6 +49,7 @@ func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) { if err != nil { return nil, err } + algOpts.qstep = getFrameSteps(lfs, defaultFrameStep) return algOpts, nil } @@ -62,7 +65,7 @@ type cBiRRTMotionPlanner struct { // newCBiRRTMotionPlannerWithSeed creates a cBiRRTMotionPlanner object with a user specified random seed. func newCBiRRTMotionPlanner( - frame referenceframe.Frame, + fss referenceframe.FrameSystem, seed *rand.Rand, logger logging.Logger, opt *plannerOptions, @@ -70,16 +73,16 @@ func newCBiRRTMotionPlanner( if opt == nil { return nil, errNoPlannerOptions } - mp, err := newPlanner(frame, seed, logger, opt) + mp, err := newPlanner(fss, seed, logger, opt) if err != nil { return nil, err } // nlopt should try only once - nlopt, err := ik.CreateNloptSolver(frame.DoF(), logger, 1, true, true) + nlopt, err := ik.CreateNloptSolver(mp.lfs.dof, logger, 1, true, true) if err != nil { return nil, err } - algOpts, err := newCbirrtOptions(opt) + algOpts, err := newCbirrtOptions(opt, mp.lfs) if err != nil { return nil, err } @@ -90,8 +93,8 @@ func newCBiRRTMotionPlanner( }, nil } -func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input) ([]node, error) { - mp.planOpts.SetGoal(goal) +func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) { + mp.planOpts.setGoal(goal) solutionChan := make(chan *rrtSolution, 1) utils.PanicCapturingGo(func() { mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan}) @@ -107,7 +110,7 @@ func (mp *cBiRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, // Separating this allows other things to call rrtBackgroundRunner in parallel allowing the thread-agnostic Plan to be accessible. func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( ctx context.Context, - seed []referenceframe.Input, + seed map[string][]referenceframe.Input, rrt *rrtParallelPlannerShared, ) { defer close(rrt.solutionChan) @@ -134,7 +137,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( rrt.maps = planSeed.maps } mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q()) - interpConfig, err := mp.frame.Interpolate(seed, rrt.maps.optNode.Q(), 0.5) + interpConfig, err := referenceframe.InterpolateFS(mp.fss, seed, rrt.maps.optNode.Q(), 0.5) if err != nil { rrt.solutionChan <- &rrtSolution{err: err} return @@ -148,19 +151,6 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( defer close(m1chan) defer close(m2chan) - seedPos, err := mp.frame.Transform(seed) - if err != nil { - rrt.solutionChan <- &rrtSolution{err: err} - return - } - - mp.logger.CDebugf(ctx, - "running CBiRRT from start pose %v with start map of size %d and goal map of size %d", - spatialmath.PoseToProtobuf(seedPos), - len(rrt.maps.startMap), - len(rrt.maps.goalMap), - ) - for i := 0; i < mp.planOpts.PlanIter; i++ { select { case <-ctx.Done(): @@ -216,11 +206,11 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( return } - reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached - if reachedDelta > mp.planOpts.JointSolveDist { - targetConf, err := mp.frame.Interpolate(map1reached.Q(), map2reached.Q(), 0.5) + if reachedDelta > mp.planOpts.InputIdentDist { + targetConf, err := referenceframe.InterpolateFS(mp.fss, map1reached.Q(), map2reached.Q(), 0.5) if err != nil { rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps} return @@ -231,11 +221,11 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps} return } - reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta = mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) } // Solved! - if reachedDelta <= mp.planOpts.JointSolveDist { + if reachedDelta <= mp.planOpts.InputIdentDist { mp.logger.CDebugf(ctx, "CBiRRT found solution after %d iterations", i) cancel() path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}, true) @@ -264,8 +254,16 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( mchan chan node, ) { // Allow qstep to be doubled as a means to escape from configurations which gradient descend to their seed - qstep := make([]float64, len(mp.planOpts.qstep)) - copy(qstep, mp.planOpts.qstep) + deepCopyQstep := func() map[string][]float64 { + qstep := map[string][]float64{} + for fName, fStep := range mp.algOpts.qstep { + newStep := make([]float64, len(fStep)) + copy(newStep, fStep) + qstep[fName] = newStep + } + return qstep + } + qstep := deepCopyQstep() doubled := false oldNear := near @@ -283,10 +281,10 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( default: } - dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) - oldDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()}) + dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) + oldDist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()}) switch { - case dist < mp.planOpts.JointSolveDist: + case dist < mp.planOpts.InputIdentDist: mchan <- near return case dist > oldDist: @@ -296,20 +294,22 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( oldNear = near - newNear := fixedStepInterpolation(near, target, mp.planOpts.qstep) + newNear := fixedStepInterpolation(near, target, mp.algOpts.qstep) // Check whether newNear meets constraints, and if not, update it to a configuration that does meet constraints (or nil) newNear = mp.constrainNear(ctx, randseed, oldNear.Q(), newNear) if newNear != nil { - nearDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear}) - if nearDist < math.Pow(mp.planOpts.JointSolveDist, 3) { + nearDist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: oldNear.Q(), EndConfiguration: newNear}) + if nearDist < math.Pow(mp.planOpts.InputIdentDist, 3) { if !doubled { doubled = true // Check if doubling qstep will allow escape from the identical configuration // If not, we terminate and return. // If so, qstep will be reset to its original value after the rescue. - for i, q := range qstep { - qstep[i] = q * 2.0 + for f, frameQ := range qstep { + for i, q := range frameQ { + qstep[f][i] = q * 2.0 + } } continue } @@ -319,7 +319,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend( return } if doubled { - copy(qstep, mp.planOpts.qstep) + qstep = deepCopyQstep() doubled = false } // constrainNear will ensure path between oldNear and newNear satisfies constraints along the way @@ -339,8 +339,8 @@ func (mp *cBiRRTMotionPlanner) constrainNear( ctx context.Context, randseed *rand.Rand, seedInputs, - target []referenceframe.Input, -) []referenceframe.Input { + target map[string][]referenceframe.Input, +) map[string][]referenceframe.Input { for i := 0; i < maxNearIter; i++ { select { case <-ctx.Done(): @@ -348,40 +348,25 @@ func (mp *cBiRRTMotionPlanner) constrainNear( default: } - seedPos, err := mp.frame.Transform(seedInputs) - if err != nil { - return nil - } - goalPos, err := mp.frame.Transform(target) - if err != nil { - return nil - } - - newArc := &ik.Segment{ - StartPosition: seedPos, - EndPosition: goalPos, + newArc := &ik.SegmentFS{ StartConfiguration: seedInputs, EndConfiguration: target, - Frame: mp.frame, + FS: mp.fss, } // Check if the arc of "seedInputs" to "target" is valid - ok, _ := mp.planOpts.CheckSegmentAndStateValidity(newArc, mp.planOpts.Resolution) + ok, _ := mp.planOpts.CheckSegmentAndStateValidityFS(newArc, mp.planOpts.Resolution) if ok { return target } solutionGen := make(chan *ik.Solution, 1) + linearSeed, err := mp.lfs.mapToSlice(target) + if err != nil { + return nil + } + // Spawn the IK solver to generate solutions until done - err = ik.SolveMetric( - ctx, - mp.fastGradDescent, - mp.frame, - solutionGen, - target, - mp.planOpts.pathMetric, - randseed.Int(), - mp.logger, - ) + err = mp.fastGradDescent.Solve(ctx, solutionGen, linearSeed, mp.linearizeFSmetric(mp.planOpts.pathMetric), randseed.Int()) // We should have zero or one solutions var solved *ik.Solution select { @@ -392,21 +377,25 @@ func (mp *cBiRRTMotionPlanner) constrainNear( if err != nil || solved == nil { return nil } + solutionMap, err := mp.lfs.sliceToMap(solved.Configuration) + if err != nil { + return nil + } - ok, failpos := mp.planOpts.CheckSegmentAndStateValidity( - &ik.Segment{ + ok, failpos := mp.planOpts.CheckSegmentAndStateValidityFS( + &ik.SegmentFS{ StartConfiguration: seedInputs, - EndConfiguration: referenceframe.FloatsToInputs(solved.Configuration), - Frame: mp.frame, + EndConfiguration: solutionMap, + FS: mp.fss, }, mp.planOpts.Resolution, ) if ok { - return referenceframe.FloatsToInputs(solved.Configuration) + return solutionMap } if failpos != nil { - dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) - if dist > mp.planOpts.JointSolveDist { + dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration}) + if dist > mp.planOpts.InputIdentDist { // If we have a first failing position, and that target is updating (no infinite loop), then recurse seedInputs = failpos.StartConfiguration target = failpos.EndConfiguration @@ -463,8 +452,8 @@ func (mp *cBiRRTMotionPlanner) smoothPath(ctx context.Context, inputSteps []node // Note this could technically replace paths with "longer" paths i.e. with more waypoints. // However, smoothed paths are invariably more intuitive and smooth, and lend themselves to future shortening, // so we allow elongation here. - dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()}) - if dist < mp.planOpts.JointSolveDist { + dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()}) + if dist < mp.planOpts.InputIdentDist { for _, hitCorner := range hitCorners { hitCorner.SetCorner(false) } @@ -486,22 +475,26 @@ func (mp *cBiRRTMotionPlanner) smoothPath(ctx context.Context, inputSteps []node // getFrameSteps will return a slice of positive values representing the largest amount a particular DOF of a frame should // move in any given step. The second argument is a float describing the percentage of the total movement. -func getFrameSteps(f referenceframe.Frame, percentTotalMovement float64) []float64 { - dof := f.DoF() - pos := make([]float64, len(dof)) - for i, lim := range dof { - l, u := lim.Min, lim.Max - - // Default to [-999,999] as range if limits are infinite - if l == math.Inf(-1) { - l = -999 - } - if u == math.Inf(1) { - u = 999 - } +func getFrameSteps(lfs *linearizedFrameSystem, percentTotalMovement float64) map[string][]float64 { + frameQstep := map[string][]float64{} + for _, f := range lfs.frames { + dof := f.DoF() + pos := make([]float64, len(dof)) + for i, lim := range dof { + l, u := lim.Min, lim.Max + + // Default to [-999,999] as range if limits are infinite + if l == math.Inf(-1) { + l = -999 + } + if u == math.Inf(1) { + u = 999 + } - jRange := math.Abs(u - l) - pos[i] = jRange * percentTotalMovement + jRange := math.Abs(u - l) + pos[i] = jRange * percentTotalMovement + } + frameQstep[f.Name()] = pos } - return pos + return frameQstep } diff --git a/motionplan/cBiRRT_test.go b/motionplan/cBiRRT_test.go index 5378471cf47..7ad5da6a4c3 100644 --- a/motionplan/cBiRRT_test.go +++ b/motionplan/cBiRRT_test.go @@ -38,19 +38,21 @@ func TestSimpleLinearMotion(t *testing.T) { goalPos := spatialmath.NewPose(r3.Vector{X: 206, Y: 100, Z: 120.5}, &spatialmath.OrientationVectorDegrees{OY: -1}) - opt := newBasicPlannerOptions(m) - opt.SetGoal(goalPos) - mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(42)), logger, opt) + opt := newBasicPlannerOptions() + opt.setGoal(PathStep{m.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos)}) + fs := referenceframe.NewEmptyFrameSystem("") + fs.AddFrame(m, fs.World()) + mp, err := newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) cbirrt, _ := mp.(*cBiRRTMotionPlanner) - solutions, err := mp.getSolutions(ctx, home7) + solutions, err := mp.getSolutions(ctx, map[string][]referenceframe.Input{m.Name(): home7}) test.That(t, err, test.ShouldBeNil) - near1 := &basicNode{q: home7} + near1 := &basicNode{q: map[string][]referenceframe.Input{m.Name(): home7}} seedMap := make(map[node]node) seedMap[near1] = nil - target := interp + target := map[string][]referenceframe.Input{m.Name(): interp} goalMap := make(map[node]node) @@ -63,7 +65,7 @@ func TestSimpleLinearMotion(t *testing.T) { } nn := &neighborManager{nCPU: nCPU} - _, err = newCbirrtOptions(opt) + _, err = newCbirrtOptions(opt, cbirrt.lfs) test.That(t, err, test.ShouldBeNil) m1chan := make(chan node, 1) @@ -81,8 +83,8 @@ func TestSimpleLinearMotion(t *testing.T) { cbirrt.constrainedExtend(ctx, cbirrt.randseed, goalMap, near2, seedReached, m1chan) }) goalReached := <-m1chan - dist := opt.DistanceFunc(&ik.Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()}) - test.That(t, dist < cbirrt.planOpts.JointSolveDist, test.ShouldBeTrue) + dist := opt.confDistanceFunc(&ik.SegmentFS{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()}) + test.That(t, dist < cbirrt.planOpts.InputIdentDist, test.ShouldBeTrue) seedReached.SetCorner(true) goalReached.SetCorner(true) diff --git a/motionplan/check.go b/motionplan/check.go index c18b721a8ce..ec9ce88651f 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -40,21 +40,13 @@ func CheckPlan( return errors.New("wayPointIdx outside of plan bounds") } - // construct solverFrame - // Note that this requires all frames which move as part of the plan, to have an - // entry in the very first plan waypoint - sf, err := newSolverFrame(fs, checkFrame.Name(), referenceframe.World, plan.Trajectory()[0]) - if err != nil { - return err - } // construct planager - sfPlanner, err := newPlanManager(sf, logger, defaultRandomSeed) + sfPlanner, err := newPlanManager(fs, logger, defaultRandomSeed) if err != nil { return err } // This should be done for any plan whose configurations are specified in relative terms rather than absolute ones. // Currently this is only TP-space, so we check if the PTG length is >0. - // The solver frame will have had its PTGs filled in the newPlanManager() call, if applicable. if sfPlanner.useTPspace { return checkPlanRelative(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } @@ -69,6 +61,7 @@ func checkPlanRelative( lookAheadDistanceMM float64, sfPlanner *planManager, ) error { + var err error toWorld := func(pif *referenceframe.PoseInFrame, inputs map[string][]referenceframe.Input) (*referenceframe.PoseInFrame, error) { transformable, err := fs.Transform(inputs, pif, referenceframe.World) if err != nil { @@ -85,22 +78,10 @@ func checkPlanRelative( plan := executionState.Plan() zeroPosePIF := referenceframe.NewPoseInFrame(checkFrame.Name(), spatialmath.NewZeroPose()) - // determine plan's starting pose - planStartPoseWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[0]) - if err != nil { - return err - } - - // determine plan's ending pose - planEndPoseWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[len(plan.Path())-1]) - if err != nil { - return err - } - // setup the planOpts. Poses should be in world frame. This allows us to know e.g. which obstacles may ephemerally collide. if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( - planStartPoseWorld.Pose(), - planEndPoseWorld.Pose(), + plan.Path()[0], + plan.Path()[len(plan.Path())-1], plan.Trajectory()[0], worldState, nil, @@ -114,21 +95,6 @@ func checkPlanRelative( currentInputs := executionState.CurrentInputs() wayPointIdx := executionState.Index() - sf := sfPlanner.frame - - // construct first segment's start configuration - // get checkFrame's currentInputs - // *currently* it is guaranteed that a relative frame will constitute 100% of a solver frame's dof - checkFrameCurrentInputs, err := sf.mapToSlice(currentInputs) - if err != nil { - return err - } - - // construct first segment's end configuration - arcEndInputs, err := sf.mapToSlice(plan.Trajectory()[wayPointIdx]) - if err != nil { - return err - } // construct first segment's startPosition currentPoses := executionState.CurrentPoses() @@ -175,15 +141,20 @@ func checkPlanRelative( expectedCurrentPose := spatialmath.PoseBetweenInverse(remainingArcPose, expectedArcEndInWorld.Pose()) errorState := spatialmath.PoseBetween(expectedCurrentPose, currentPoseInWorld.Pose()) currentArcEndPose := spatialmath.Compose(expectedArcEndInWorld.Pose(), errorState) + frameTrajectory, err := plan.Trajectory().GetFrameInputs(checkFrame.Name()) + if err != nil { + return err + } segments := make([]*ik.Segment, 0, len(plan.Path())-wayPointIdx) // pre-pend to segments so we can connect to the input we have not finished actuating yet + segments = append(segments, &ik.Segment{ StartPosition: currentPoseInWorld.Pose(), EndPosition: currentArcEndPose, - StartConfiguration: checkFrameCurrentInputs, - EndConfiguration: arcEndInputs, - Frame: sf, + StartConfiguration: frameCurrentInputs, + EndConfiguration: frameTrajectory[wayPointIdx], + Frame: checkFrame, }) lastArcEndPose := currentArcEndPose @@ -195,17 +166,18 @@ func checkPlanRelative( return err } thisArcEndPose := spatialmath.Compose(thisArcEndPoseInWorld.Pose(), errorState) - startInputs := plan.Trajectory()[i-1] - nextInputs := plan.Trajectory()[i] - segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) - if err != nil { - return err + segment := &ik.Segment{ + StartPosition: lastArcEndPose, + EndPosition: thisArcEndPose, + StartConfiguration: frameTrajectory[i-1], + EndConfiguration: frameTrajectory[i], + Frame: checkFrame, } lastArcEndPose = thisArcEndPose segments = append(segments, segment) } - return checkSegments(sfPlanner, segments, lookAheadDistanceMM) + return checkSegments(sfPlanner, segments, lookAheadDistanceMM, checkFrame) } func checkPlanAbsolute( @@ -216,7 +188,6 @@ func checkPlanAbsolute( lookAheadDistanceMM float64, sfPlanner *planManager, ) error { - sf := sfPlanner.frame plan := executionState.Plan() startingInputs := plan.Trajectory()[0] currentInputs := executionState.CurrentInputs() @@ -242,15 +213,11 @@ func checkPlanAbsolute( offsetPlan := OffsetPlan(plan, errorState) // get plan poses for checkFrame - poses, err := offsetPlan.Path().GetFramePoses(checkFrame.Name()) - if err != nil { - return err - } - startPose := currentPoseIF.Pose() + poses := offsetPlan.Path() // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( - startPose, + executionState.CurrentPoses(), poses[len(poses)-1], startingInputs, worldState, @@ -262,65 +229,101 @@ func checkPlanAbsolute( } // create a list of segments to iterate through - segments := make([]*ik.Segment, 0, len(poses)-wayPointIdx) + segments := make([]*ik.SegmentFS, 0, len(poses)-wayPointIdx) // iterate through remaining plan and append remaining segments to check for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { - segment, err := createSegment(sf, poses[i], poses[i+1], offsetPlan.Trajectory()[i], offsetPlan.Trajectory()[i+1]) - if err != nil { - return err + segment := &ik.SegmentFS{ + StartConfiguration: offsetPlan.Trajectory()[i], + EndConfiguration: offsetPlan.Trajectory()[i+1], + FS: sfPlanner.fss, } segments = append(segments, segment) } - return checkSegments(sfPlanner, segments, lookAheadDistanceMM) + return checkSegmentsFS(sfPlanner, segments, lookAheadDistanceMM) } -// createSegment is a function to ease segment creation for solver frames. -func createSegment( - sf *solverFrame, - currPose, nextPose spatialmath.Pose, - currInput, nextInput map[string][]referenceframe.Input, -) (*ik.Segment, error) { - var currInputSlice, nextInputSlice []referenceframe.Input - var err error - if currInput != nil { - currInputSlice, err = sf.mapToSlice(currInput) - if err != nil { - return nil, err +func checkSegmentsFS(sfPlanner *planManager, segments []*ik.SegmentFS, lookAheadDistanceMM float64) error { + // go through segments and check that we satisfy constraints + moving, _ := sfPlanner.frameLists() + dists := map[string]float64{} + for _, segment := range segments { + ok, lastValid := sfPlanner.planOpts.CheckSegmentAndStateValidityFS(segment, sfPlanner.planOpts.Resolution) + if !ok { + checkConf := segment.StartConfiguration + if lastValid != nil { + checkConf = lastValid.EndConfiguration + } + ok, reason := sfPlanner.planOpts.CheckStateFSConstraints(&ik.StateFS{Configuration: checkConf, FS: sfPlanner.fss}) + if !ok { + reason = " reason: " + reason + } else { + reason = "" + } + return fmt.Errorf("found constraint violation or collision in segment between %v and %v at %v %s", + segment.StartConfiguration, + segment.EndConfiguration, + checkConf, + reason, + ) } - } - nextInputSlice, err = sf.mapToSlice(nextInput) - if err != nil { - return nil, err - } - segment := &ik.Segment{ - StartPosition: currPose, - EndPosition: nextPose, - StartConfiguration: currInputSlice, - EndConfiguration: nextInputSlice, - Frame: sf, - } + for _, checkFrame := range moving { + poseInPathStart, err := sfPlanner.fss.Transform( + segment.EndConfiguration, + referenceframe.NewZeroPoseInFrame(checkFrame), + referenceframe.World, + ) + if err != nil { + return err + } + poseInPathEnd, err := sfPlanner.fss.Transform( + segment.StartConfiguration, + referenceframe.NewZeroPoseInFrame(checkFrame), + referenceframe.World, + ) + if err != nil { + return err + } - return segment, nil + currDist := poseInPathEnd.(*referenceframe.PoseInFrame).Pose().Point().Distance( + poseInPathStart.(*referenceframe.PoseInFrame).Pose().Point(), + ) + // Check if look ahead distance has been reached + currentTravelDistanceMM := dists[checkFrame] + currDist + if currentTravelDistanceMM > lookAheadDistanceMM { + return nil + } + dists[checkFrame] = currentTravelDistanceMM + } + } + return nil } -func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDistanceMM float64) error { +// TODO: Remove this function. +func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDistanceMM float64, checkFrame referenceframe.Frame) error { // go through segments and check that we satisfy constraints - // TODO(RSDK-5007): If we can make interpolate a method on Frame the need to write this out will be lessened and we should be - // able to call CheckStateConstraintsAcrossSegment directly. var totalTravelDistanceMM float64 for _, segment := range segments { interpolatedConfigurations, err := interpolateSegment(segment, sfPlanner.planOpts.Resolution) if err != nil { return err } + parent, err := sfPlanner.fss.Parent(checkFrame) + if err != nil { + return err + } for _, interpConfig := range interpolatedConfigurations { - poseInPath, err := sfPlanner.frame.Transform(interpConfig) + poseInPathTf, err := sfPlanner.fss.Transform( + map[string][]referenceframe.Input{checkFrame.Name(): interpConfig}, + referenceframe.NewZeroPoseInFrame(checkFrame.Name()), + parent.Name(), + ) if err != nil { return err } + poseInPath := poseInPathTf.(*referenceframe.PoseInFrame).Pose() // Check if look ahead distance has been reached currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) @@ -331,7 +334,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist // define State which only houses inputs, pose information not needed since we cannot get arcs from // an interpolating poses, this would only yield a straight line. interpolatedState := &ik.State{ - Frame: sfPlanner.frame, + Frame: checkFrame, Configuration: interpConfig, } diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 0f9cc4474ea..384d7a214f1 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -16,8 +16,6 @@ import ( spatial "go.viam.com/rdk/spatialmath" ) -var defaultMinStepCount = 2 - // Given a constraint input with only frames and input positions, calculates the corresponding poses as needed. func resolveSegmentsToPositions(segment *ik.Segment) error { if segment.StartPosition == nil { @@ -76,174 +74,28 @@ func resolveStatesToPositions(state *ik.State) error { return nil } +// SegmentFSConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid. +// If the returned bool is true, the constraint is satisfied and the segment is valid. +type SegmentFSConstraint func(*ik.SegmentFS) bool + // SegmentConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid. // If the returned bool is true, the constraint is satisfied and the segment is valid. type SegmentConstraint func(*ik.Segment) bool +// StateFSConstraint tests whether a given robot configuration is valid +// If the returned bool is true, the constraint is satisfied and the state is valid. +type StateFSConstraint func(*ik.StateFS) bool + // StateConstraint tests whether a given robot configuration is valid // If the returned bool is true, the constraint is satisfied and the state is valid. type StateConstraint func(*ik.State) bool -// ConstraintHandler is a convenient wrapper for constraint handling which is likely to be common among most motion -// planners. Including a constraint handler as an anonymous struct member allows reuse. -type ConstraintHandler struct { - segmentConstraints map[string]SegmentConstraint - stateConstraints map[string]StateConstraint -} - -// CheckStateConstraints will check a given input against all state constraints. -// Return values are: -// -- a bool representing whether all constraints passed -// -- if failing, a string naming the failed constraint. -func (c *ConstraintHandler) CheckStateConstraints(state *ik.State) (bool, string) { - for name, cFunc := range c.stateConstraints { - pass := cFunc(state) - if !pass { - return false, name - } - } - return true, "" -} - -// CheckSegmentConstraints will check a given input against all segment constraints. -// Return values are: -// -- a bool representing whether all constraints passed -// -- if failing, a string naming the failed constraint. -func (c *ConstraintHandler) CheckSegmentConstraints(segment *ik.Segment) (bool, string) { - for name, cFunc := range c.segmentConstraints { - pass := cFunc(segment) - if !pass { - return false, name - } - } - return true, "" -} - -// CheckStateConstraintsAcrossSegment will interpolate the given input from the StartInput to the EndInput, and ensure that all intermediate -// states as well as both endpoints satisfy all state constraints. If all constraints are satisfied, then this will return `true, nil`. -// If any constraints fail, this will return false, and an Segment representing the valid portion of the segment, if any. If no -// part of the segment is valid, then `false, nil` is returned. -func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *ik.Segment, resolution float64) (bool, *ik.Segment) { - interpolatedConfigurations, err := interpolateSegment(ci, resolution) - if err != nil { - return false, nil - } - var lastGood []referenceframe.Input - for i, interpConfig := range interpolatedConfigurations { - interpC := &ik.State{Frame: ci.Frame, Configuration: interpConfig} - if resolveStatesToPositions(interpC) != nil { - return false, nil - } - pass, _ := c.CheckStateConstraints(interpC) - if !pass { - if i == 0 { - // fail on start pos - return false, nil - } - return false, &ik.Segment{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood} - } - lastGood = interpC.Configuration - } - - return true, nil -} - -// interpolateSegment is a helper function which produces a list of intermediate inputs, between the start and end -// configuration of a segment at a given resolution value. -func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.Input, error) { - // ensure we have cartesian positions - if err := resolveSegmentsToPositions(ci); err != nil { - return nil, err - } - - steps := PathStepCount(ci.StartPosition, ci.EndPosition, resolution) - if steps < defaultMinStepCount { - // Minimum step count ensures we are not missing anything - steps = defaultMinStepCount - } - - var interpolatedConfigurations [][]referenceframe.Input - for i := 0; i <= steps; i++ { - interp := float64(i) / float64(steps) - interpConfig, err := ci.Frame.Interpolate(ci.StartConfiguration, ci.EndConfiguration, interp) - if err != nil { - return nil, err - } - interpolatedConfigurations = append(interpolatedConfigurations, interpConfig) - } - return interpolatedConfigurations, nil -} - -// CheckSegmentAndStateValidity will check an segment input and confirm that it 1) meets all segment constraints, and 2) meets all -// state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, -// provided that segment also meets segment constraints. -func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *ik.Segment, resolution float64) (bool, *ik.Segment) { - valid, subSegment := c.CheckStateConstraintsAcrossSegment(segment, resolution) - if !valid { - if subSegment != nil { - subSegmentValid, _ := c.CheckSegmentConstraints(subSegment) - if subSegmentValid { - return false, subSegment - } - } - return false, nil - } - // all states are valid - valid, _ = c.CheckSegmentConstraints(segment) - return valid, nil -} - -// AddStateConstraint will add or overwrite a constraint function with a given name. A constraint function should return true -// if the given position satisfies the constraint. -func (c *ConstraintHandler) AddStateConstraint(name string, cons StateConstraint) { - if c.stateConstraints == nil { - c.stateConstraints = map[string]StateConstraint{} - } - c.stateConstraints[name] = cons -} - -// RemoveStateConstraint will remove the given constraint. -func (c *ConstraintHandler) RemoveStateConstraint(name string) { - delete(c.stateConstraints, name) -} - -// StateConstraints will list all state constraints by name. -func (c *ConstraintHandler) StateConstraints() []string { - names := make([]string, 0, len(c.stateConstraints)) - for name := range c.stateConstraints { - names = append(names, name) - } - return names -} - -// AddSegmentConstraint will add or overwrite a constraint function with a given name. A constraint function should return true -// if the given position satisfies the constraint. -func (c *ConstraintHandler) AddSegmentConstraint(name string, cons SegmentConstraint) { - if c.segmentConstraints == nil { - c.segmentConstraints = map[string]SegmentConstraint{} - } - c.segmentConstraints[name] = cons -} - -// RemoveSegmentConstraint will remove the given constraint. -func (c *ConstraintHandler) RemoveSegmentConstraint(name string) { - delete(c.segmentConstraints, name) -} - -// SegmentConstraints will list all segment constraints by name. -func (c *ConstraintHandler) SegmentConstraints() []string { - names := make([]string, 0, len(c.segmentConstraints)) - for name := range c.segmentConstraints { - names = append(names, name) - } - return names -} - func createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, worldGeometries, boundingRegions []spatial.Geometry, allowedCollisions []*Collision, collisionBufferMM float64, -) (map[string]StateConstraint, error) { +) (map[string]StateFSConstraint, map[string]StateConstraint, error) { + constraintFSMap := map[string]StateFSConstraint{} constraintMap := map[string]StateConstraint{} var err error @@ -256,14 +108,14 @@ func createAllCollisionConstraints( if zeroCG == nil { zeroCG, err = setupZeroCG(movingRobotGeometries, worldGeometries, allowedCollisions, collisionBufferMM) if err != nil { - return nil, err + return nil, nil, err } } for _, collision := range zeroCG.collisions(collisionBufferMM) { if collision.name1 == octree.Label() { - return nil, fmt.Errorf("starting collision between SLAM map and %s, cannot move", collision.name2) + return nil, nil, fmt.Errorf("starting collision between SLAM map and %s, cannot move", collision.name2) } else if collision.name2 == octree.Label() { - return nil, fmt.Errorf("starting collision between SLAM map and %s, cannot move", collision.name1) + return nil, nil, fmt.Errorf("starting collision between SLAM map and %s, cannot move", collision.name1) } } } @@ -272,9 +124,15 @@ func createAllCollisionConstraints( // create constraint to keep moving geometries from hitting world state obstacles obstacleConstraint, err := NewCollisionConstraint(movingRobotGeometries, worldGeometries, allowedCollisions, false, collisionBufferMM) if err != nil { - return nil, err + return nil, nil, err + } + // create constraint to keep moving geometries from hitting world state obstacles + obstacleConstraintFS, err := NewCollisionConstraintFS(movingRobotGeometries, worldGeometries, allowedCollisions, false, collisionBufferMM) + if err != nil { + return nil, nil, err } constraintMap[defaultObstacleConstraintDesc] = obstacleConstraint + constraintFSMap[defaultObstacleConstraintDesc] = obstacleConstraintFS } if len(boundingRegions) > 0 { @@ -292,20 +150,35 @@ func createAllCollisionConstraints( false, collisionBufferMM) if err != nil { - return nil, err + return nil, nil, err + } + robotConstraintFS, err := NewCollisionConstraintFS( + movingRobotGeometries, + staticRobotGeometries, + allowedCollisions, + false, + collisionBufferMM) + if err != nil { + return nil, nil, err } constraintMap[defaultRobotCollisionConstraintDesc] = robotConstraint + constraintFSMap[defaultRobotCollisionConstraintDesc] = robotConstraintFS } // create constraint to keep moving geometries from hitting themselves if len(movingRobotGeometries) > 1 { selfCollisionConstraint, err := NewCollisionConstraint(movingRobotGeometries, nil, allowedCollisions, false, collisionBufferMM) if err != nil { - return nil, err + return nil, nil, err } constraintMap[defaultSelfCollisionConstraintDesc] = selfCollisionConstraint + selfCollisionConstraintFS, err := NewCollisionConstraintFS(movingRobotGeometries, nil, allowedCollisions, false, collisionBufferMM) + if err != nil { + return nil, nil, err + } + constraintFSMap[defaultSelfCollisionConstraintDesc] = selfCollisionConstraintFS } - return constraintMap, nil + return constraintFSMap, constraintMap, nil } func setupZeroCG(moving, static []spatial.Geometry, @@ -373,10 +246,55 @@ func NewCollisionConstraint( return constraint, nil } +// NewCollisionConstraintFS is the most general method to create a collision constraint for a frame system, +// which will be violated if geometries constituting the given frame ever come into collision with obstacle geometries +// outside of the collisions present for the observationInput. Collisions specified as collisionSpecifications will also be ignored. +// If reportDistances is false, this check will be done as fast as possible, if true maximum information will be available for debugging. +func NewCollisionConstraintFS( + moving, static []spatial.Geometry, + collisionSpecifications []*Collision, + reportDistances bool, + collisionBufferMM float64, +) (StateFSConstraint, error) { + zeroCG, err := setupZeroCG(moving, static, collisionSpecifications, collisionBufferMM) + if err != nil { + return nil, err + } + + // create constraint from reference collision graph + constraint := func(state *ik.StateFS) bool { + // Use FrameSystemGeometries to get all geometries in the frame system + internalGeometries, err := referenceframe.FrameSystemGeometries(state.FS, state.Configuration) + if err != nil { + return false + } + + var internalGeoms []spatial.Geometry + for _, geosInFrame := range internalGeometries { + internalGeoms = append(internalGeoms, geosInFrame.Geometries()...) + } + + cg, err := newCollisionGraph(internalGeoms, static, zeroCG, reportDistances, collisionBufferMM) + if err != nil { + return false + } + return len(cg.collisions(collisionBufferMM)) == 0 + } + return constraint, nil +} + // NewAbsoluteLinearInterpolatingConstraint provides a Constraint whose valid manifold allows a specified amount of deviation from the // shortest straight-line path between the start and the goal. linTol is the allowed linear deviation in mm, orientTol is the allowed // orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations. func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, ik.StateMetric) { + // Account for float error + if linTol < defaultEpsilon { + linTol = defaultEpsilon + } + if orientTol < defaultEpsilon { + orientTol = defaultEpsilon + } + orientConstraint, orientMetric := NewSlerpOrientationConstraint(from, to, orientTol) lineConstraint, lineMetric := NewLineConstraint(from.Point(), to.Point(), linTol) interpMetric := ik.CombineMetrics(orientMetric, lineMetric) @@ -387,15 +305,131 @@ func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, ori return f, interpMetric } +// CreateAbsoluteLinearInterpolatingConstraintFS provides a Constraint whose valid manifold allows a specified amount of deviation from the +// shortest straight-line path between the start and the goal. linTol is the allowed linear deviation in mm, orientTol is the allowed +// orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations. +func CreateAbsoluteLinearInterpolatingConstraintFS( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + linTol, orientTol float64, +) (StateFSConstraint, ik.StateFSMetric, error) { + // Create a linear constraint for each goal + + metricMap := map[string]ik.StateMetric{} + constraintMap := map[string]StateConstraint{} + + for frame, goal := range to { + startPiF, ok := from[frame] + if !ok { + startPiFTf, err := fs.Transform(startCfg, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return nil, nil, err + } + startPiF = startPiFTf.(*referenceframe.PoseInFrame) + } + constraint, metric := NewAbsoluteLinearInterpolatingConstraint(startPiF.Pose(), goal.Pose(), linTol, orientTol) + + metricMap[frame] = metric + constraintMap[frame] = constraint + } + + allMetric := func(state *ik.StateFS) float64 { + score := 0. + for frame, goal := range to { + if metric, ok := metricMap[frame]; ok { + pif, err := fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + score += math.Inf(1) + } + score += metric(&ik.State{Position: pif.(*referenceframe.PoseInFrame).Pose()}) + } + } + return score + } + + allConstraint := func(state *ik.StateFS) bool { + for frame, goal := range to { + if constraint, ok := constraintMap[frame]; ok { + pif, err := fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return false + } + pass := constraint(&ik.State{Position: pif.(*referenceframe.PoseInFrame).Pose()}) + if !pass { + return false + } + } + } + return true + } + return allConstraint, allMetric, nil +} + // NewProportionalLinearInterpolatingConstraint will provide the same metric and constraint as NewAbsoluteLinearInterpolatingConstraint, // except that allowable linear and orientation deviation is scaled based on the distance from start to goal. -func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, ik.StateMetric) { - orientTol := epsilon * ik.OrientDist(from.Orientation(), to.Orientation()) - linTol := epsilon * from.Point().Distance(to.Point()) +func NewProportionalLinearInterpolatingConstraint( + from, to spatial.Pose, + linEpsilon, orientEpsilon float64, +) (StateConstraint, ik.StateMetric) { + orientTol := orientEpsilon * ik.OrientDist(from.Orientation(), to.Orientation()) + linTol := linEpsilon * from.Point().Distance(to.Point()) return NewAbsoluteLinearInterpolatingConstraint(from, to, linTol, orientTol) } +// CreateProportionalLinearInterpolatingConstraintFS will provide the same metric and constraint as +// CreateAbsoluteLinearInterpolatingConstraintFS, except that allowable linear and orientation deviation is scaled based on the distance +// from start to goal. +func CreateProportionalLinearInterpolatingConstraintFS( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + linTol, orientTol float64, +) (StateFSConstraint, ik.StateFSMetric, error) { + // Create a pseudolinear constraint for each goal + metricMap := map[string]ik.StateMetric{} + constraintMap := map[string]StateConstraint{} + + for frame, goal := range to { + startPiF, ok := from[frame] + if !ok { + startPiFTf, err := fs.Transform(startCfg, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return nil, nil, err + } + startPiF = startPiFTf.(*referenceframe.PoseInFrame) + } + constraint, metric := NewProportionalLinearInterpolatingConstraint(startPiF.Pose(), goal.Pose(), linTol, orientTol) + + metricMap[frame] = metric + constraintMap[frame] = constraint + } + + allMetric := func(state *ik.StateFS) float64 { + score := 0. + for frame, cfg := range state.Configuration { + if metric, ok := metricMap[frame]; ok { + score += metric(&ik.State{Configuration: cfg, Frame: fs.Frame(frame)}) + } + } + return score + } + + allConstraint := func(state *ik.StateFS) bool { + for frame, cfg := range state.Configuration { + if constraint, ok := constraintMap[frame]; ok { + pass := constraint(&ik.State{Configuration: cfg, Frame: fs.Frame(frame)}) + if !pass { + return false + } + } + } + return true + } + return allConstraint, allMetric, nil +} + // NewSlerpOrientationConstraint will measure the orientation difference between the orientation of two poses, and return a constraint that // returns whether a given orientation is within a given tolerance distance of the shortest segment between the two orientations, as // well as a metric which returns the distance to that valid region. @@ -468,10 +502,6 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta // which will bring a pose into the valid constraint space. // tolerance refers to the closeness to the line necessary to be a valid pose in mm. func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, ik.StateMetric) { - if pt1.Distance(pt2) < defaultEpsilon { - tolerance = defaultEpsilon - } - gradFunc := func(state *ik.State) float64 { return math.Max(spatial.DistToLineSegment(pt1, pt2, state.Position.Point())-tolerance, 0) } @@ -543,15 +573,22 @@ func NewBoundingRegionConstraint(robotGeoms, boundingRegions []spatial.Geometry, } } -// LinearConstraint specifies that the component being moved should move linearly relative to its goal. -// It does not constrain the motion of components other than the `component_name` specified in motion.Move. +// LinearConstraint specifies that the components being moved should move linearly relative to their goals. type LinearConstraint struct { LineToleranceMm float64 // Max linear deviation from straight-line between start and goal, in mm. OrientationToleranceDegs float64 } -// OrientationConstraint specifies that the component being moved will not deviate its orientation beyond some threshold relative -// to the goal. It does not constrain the motion of components other than the `component_name` specified in motion.Move. +// PseudolinearConstraint specifies that the component being moved should not deviate from the straight-line path to their goal by +// more than a factor proportional to the distance from start to goal. +// For example, if a component is moving 100mm, then a LineToleranceFactor of 1.0 means that the component will remain within a 100mm +// radius of the straight-line start-goal path. +type PseudolinearConstraint struct { + LineToleranceFactor float64 + OrientationToleranceFactor float64 +} + +// OrientationConstraint specifies that the components being moved will not deviate orientation beyond some threshold. type OrientationConstraint struct { OrientationToleranceDegs float64 } @@ -571,6 +608,7 @@ type CollisionSpecification struct { // It serves as a convenenient RDK wrapper for the protobuf object. type Constraints struct { LinearConstraint []LinearConstraint + PseudolinearConstraint []PseudolinearConstraint OrientationConstraint []OrientationConstraint CollisionSpecification []CollisionSpecification } @@ -579,6 +617,7 @@ type Constraints struct { func NewEmptyConstraints() *Constraints { return &Constraints{ LinearConstraint: make([]LinearConstraint, 0), + PseudolinearConstraint: make([]PseudolinearConstraint, 0), OrientationConstraint: make([]OrientationConstraint, 0), CollisionSpecification: make([]CollisionSpecification, 0), } @@ -587,11 +626,13 @@ func NewEmptyConstraints() *Constraints { // NewConstraints initializes a Constraints object with user-defined LinearConstraint, OrientationConstraint, and CollisionSpecification. func NewConstraints( linConstraints []LinearConstraint, + pseudoConstraints []PseudolinearConstraint, orientConstraints []OrientationConstraint, collSpecifications []CollisionSpecification, ) *Constraints { return &Constraints{ LinearConstraint: linConstraints, + PseudolinearConstraint: pseudoConstraints, OrientationConstraint: orientConstraints, CollisionSpecification: collSpecifications, } @@ -654,6 +695,7 @@ func ConstraintsFromProtobuf(pbConstraint *motionpb.Constraints) *Constraints { return NewConstraints( linConstraintFromProto(pbConstraint.LinearConstraint), + []PseudolinearConstraint{}, orientConstraintFromProto(pbConstraint.OrientationConstraint), collSpecFromProto(pbConstraint.CollisionSpecification), ) @@ -728,6 +770,19 @@ func (c *Constraints) GetLinearConstraint() []LinearConstraint { return nil } +// AddPseudolinearConstraint appends a PseudolinearConstraint to a Constraints object. +func (c *Constraints) AddPseudolinearConstraint(plinConstraint PseudolinearConstraint) { + c.PseudolinearConstraint = append(c.PseudolinearConstraint, plinConstraint) +} + +// GetPseudolinearConstraint checks if the Constraints object is nil and if not then returns its PseudolinearConstraint field. +func (c *Constraints) GetPseudolinearConstraint() []PseudolinearConstraint { + if c != nil { + return c.PseudolinearConstraint + } + return nil +} + // AddOrientationConstraint appends a OrientationConstraint to a Constraints object. func (c *Constraints) AddOrientationConstraint(orientConstraint OrientationConstraint) { c.OrientationConstraint = append(c.OrientationConstraint, orientConstraint) @@ -753,3 +808,139 @@ func (c *Constraints) GetCollisionSpecification() []CollisionSpecification { } return nil } + +// CreateSlerpOrientationConstraintFS will measure the orientation difference between the orientation of two poses across a frame system, +// and return a constraint that returns whether given orientations are within a given tolerance distance of the shortest segment between +// their respective orientations, as well as a metric which returns the distance to that valid region. +func CreateSlerpOrientationConstraintFS( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + tolerance float64, +) (StateFSConstraint, ik.StateFSMetric, error) { + metricMap := map[string]ik.StateMetric{} + constraintMap := map[string]StateConstraint{} + + for frame, goal := range to { + startPiF, ok := from[frame] + if !ok { + startPiFTf, err := fs.Transform(startCfg, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return nil, nil, err + } + startPiF = startPiFTf.(*referenceframe.PoseInFrame) + } + orientConstraint, orientMetric := NewSlerpOrientationConstraint(startPiF.Pose(), goal.Pose(), tolerance) + + metricMap[frame] = orientMetric + constraintMap[frame] = orientConstraint + } + + allMetric := func(state *ik.StateFS) float64 { + score := 0. + for frame, goal := range to { + if metric, ok := metricMap[frame]; ok { + currPose, err := fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + score = math.Inf(1) + break + } + score += metric(&ik.State{ + Configuration: state.Configuration[frame], + Position: currPose.(*referenceframe.PoseInFrame).Pose(), + Frame: fs.Frame(frame), + }) + } + } + return score + } + + allConstraint := func(state *ik.StateFS) bool { + for frame, goal := range to { + if constraint, ok := constraintMap[frame]; ok { + currPose, err := fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return false + } + pass := constraint(&ik.State{ + Configuration: state.Configuration[frame], + Position: currPose.(*referenceframe.PoseInFrame).Pose(), + Frame: fs.Frame(frame), + }) + if !pass { + return false + } + } + } + return true + } + return allConstraint, allMetric, nil +} + +// CreateLineConstraintFS will measure the linear distance between the positions of two poses across a frame system, +// and return a constraint that checks whether given positions are within a specified tolerance distance of the shortest +// line segment between their respective positions, as well as a metric which returns the distance to that valid region. +func CreateLineConstraintFS( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + tolerance float64, +) (StateFSConstraint, ik.StateFSMetric, error) { + metricMap := map[string]ik.StateMetric{} + constraintMap := map[string]StateConstraint{} + + for frame, goal := range to { + startPiF, ok := from[frame] + if !ok { + startPiFTf, err := fs.Transform(startCfg, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return nil, nil, err + } + startPiF = startPiFTf.(*referenceframe.PoseInFrame) + } + lineConstraint, lineMetric := NewLineConstraint(startPiF.Pose().Point(), goal.Pose().Point(), tolerance) + + metricMap[frame] = lineMetric + constraintMap[frame] = lineConstraint + } + + allMetric := func(state *ik.StateFS) float64 { + score := 0. + for frame, goal := range to { + if metric, ok := metricMap[frame]; ok { + currPose, err := fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + score = math.Inf(1) + break + } + score += metric(&ik.State{ + Configuration: state.Configuration[frame], + Position: currPose.(*referenceframe.PoseInFrame).Pose(), + Frame: fs.Frame(frame), + }) + } + } + return score + } + + allConstraint := func(state *ik.StateFS) bool { + for frame, goal := range to { + if constraint, ok := constraintMap[frame]; ok { + currPose, err := fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent()) + if err != nil { + return false + } + pass := constraint(&ik.State{ + Configuration: state.Configuration[frame], + Position: currPose.(*referenceframe.PoseInFrame).Pose(), + Frame: fs.Frame(frame), + }) + if !pass { + return false + } + } + } + return true + } + return allConstraint, allMetric, nil +} diff --git a/motionplan/constraint_handler.go b/motionplan/constraint_handler.go new file mode 100644 index 00000000000..814645599e7 --- /dev/null +++ b/motionplan/constraint_handler.go @@ -0,0 +1,355 @@ +//go:build !no_cgo + +package motionplan + +import ( + "fmt" + + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" +) + +var defaultMinStepCount = 2 + +// ConstraintHandler is a convenient wrapper for constraint handling which is likely to be common among most motion +// planners. Including a constraint handler as an anonymous struct member allows reuse. +type ConstraintHandler struct { + segmentConstraints map[string]SegmentConstraint + segmentFSConstraints map[string]SegmentFSConstraint + stateConstraints map[string]StateConstraint + stateFSConstraints map[string]StateFSConstraint +} + +// CheckStateConstraints will check a given input against all state constraints. +// Return values are: +// -- a bool representing whether all constraints passed +// -- if failing, a string naming the failed constraint. +func (c *ConstraintHandler) CheckStateConstraints(state *ik.State) (bool, string) { + for name, cFunc := range c.stateConstraints { + pass := cFunc(state) + if !pass { + return false, name + } + } + return true, "" +} + +// CheckStateFSConstraints will check a given input against all FS state constraints. +// Return values are: +// -- a bool representing whether all constraints passed +// -- if failing, a string naming the failed constraint. +func (c *ConstraintHandler) CheckStateFSConstraints(state *ik.StateFS) (bool, string) { + for name, cFunc := range c.stateFSConstraints { + pass := cFunc(state) + if !pass { + return false, name + } + } + return true, "" +} + +// CheckSegmentConstraints will check a given input against all segment constraints. +// Return values are: +// -- a bool representing whether all constraints passed +// -- if failing, a string naming the failed constraint. +func (c *ConstraintHandler) CheckSegmentConstraints(segment *ik.Segment) (bool, string) { + for name, cFunc := range c.segmentConstraints { + pass := cFunc(segment) + if !pass { + return false, name + } + } + return true, "" +} + +// CheckSegmentFSConstraints will check a given input against all FS segment constraints. +// Return values are: +// -- a bool representing whether all constraints passed +// -- if failing, a string naming the failed constraint. +func (c *ConstraintHandler) CheckSegmentFSConstraints(segment *ik.SegmentFS) (bool, string) { + for name, cFunc := range c.segmentFSConstraints { + pass := cFunc(segment) + if !pass { + return false, name + } + } + return true, "" +} + +// CheckStateConstraintsAcrossSegment will interpolate the given input from the StartInput to the EndInput, and ensure that all intermediate +// states as well as both endpoints satisfy all state constraints. If all constraints are satisfied, then this will return `true, nil`. +// If any constraints fail, this will return false, and an Segment representing the valid portion of the segment, if any. If no +// part of the segment is valid, then `false, nil` is returned. +func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *ik.Segment, resolution float64) (bool, *ik.Segment) { + interpolatedConfigurations, err := interpolateSegment(ci, resolution) + if err != nil { + return false, nil + } + var lastGood []referenceframe.Input + for i, interpConfig := range interpolatedConfigurations { + interpC := &ik.State{Frame: ci.Frame, Configuration: interpConfig} + if resolveStatesToPositions(interpC) != nil { + return false, nil + } + pass, _ := c.CheckStateConstraints(interpC) + if !pass { + if i == 0 { + // fail on start pos + return false, nil + } + return false, &ik.Segment{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood, Frame: ci.Frame} + } + lastGood = interpC.Configuration + } + + return true, nil +} + +// interpolateSegment is a helper function which produces a list of intermediate inputs, between the start and end +// configuration of a segment at a given resolution value. +func interpolateSegment(ci *ik.Segment, resolution float64) ([][]referenceframe.Input, error) { + // ensure we have cartesian positions + if err := resolveSegmentsToPositions(ci); err != nil { + return nil, err + } + + steps := PathStepCount(ci.StartPosition, ci.EndPosition, resolution) + if steps < defaultMinStepCount { + // Minimum step count ensures we are not missing anything + steps = defaultMinStepCount + } + + var interpolatedConfigurations [][]referenceframe.Input + for i := 0; i <= steps; i++ { + interp := float64(i) / float64(steps) + interpConfig, err := ci.Frame.Interpolate(ci.StartConfiguration, ci.EndConfiguration, interp) + if err != nil { + return nil, err + } + interpolatedConfigurations = append(interpolatedConfigurations, interpConfig) + } + return interpolatedConfigurations, nil +} + +// interpolateSegmentFS is a helper function which produces a list of intermediate inputs, between the start and end +// configuration of a segment at a given resolution value. +func interpolateSegmentFS(ci *ik.SegmentFS, resolution float64) ([]map[string][]referenceframe.Input, error) { + // Find the frame with the most steps by calculating steps for each frame + maxSteps := defaultMinStepCount + for frameName, startConfig := range ci.StartConfiguration { + if len(startConfig) == 0 { + // No need to interpolate 0dof frames + continue + } + endConfig, exists := ci.EndConfiguration[frameName] + if !exists { + return nil, fmt.Errorf("frame %s exists in start config but not in end config", frameName) + } + + // Get frame from FrameSystem + frame := ci.FS.Frame(frameName) + if frame == nil { + return nil, fmt.Errorf("frame %s exists in start config but not in framesystem", frameName) + } + + // Calculate positions for this frame's start and end configs + startPos, err := frame.Transform(startConfig) + if err != nil { + return nil, err + } + endPos, err := frame.Transform(endConfig) + if err != nil { + return nil, err + } + + // Calculate steps needed for this frame + steps := PathStepCount(startPos, endPos, resolution) + if steps > maxSteps { + maxSteps = steps + } + } + + // Create interpolated configurations for all frames + var interpolatedConfigurations []map[string][]referenceframe.Input + for i := 0; i <= maxSteps; i++ { + interp := float64(i) / float64(maxSteps) + frameConfigs := make(map[string][]referenceframe.Input) + + // Interpolate each frame's configuration + for frameName, startConfig := range ci.StartConfiguration { + endConfig := ci.EndConfiguration[frameName] + frame := ci.FS.Frame(frameName) + + interpConfig, err := frame.Interpolate(startConfig, endConfig, interp) + if err != nil { + return nil, err + } + frameConfigs[frameName] = interpConfig + } + + interpolatedConfigurations = append(interpolatedConfigurations, frameConfigs) + } + + return interpolatedConfigurations, nil +} + +// CheckSegmentAndStateValidity will check an segment input and confirm that it 1) meets all segment constraints, and 2) meets all +// state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, +// provided that segment also meets segment constraints. +func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *ik.Segment, resolution float64) (bool, *ik.Segment) { + valid, subSegment := c.CheckStateConstraintsAcrossSegment(segment, resolution) + if !valid { + if subSegment != nil { + subSegmentValid, _ := c.CheckSegmentConstraints(subSegment) + if subSegmentValid { + return false, subSegment + } + } + return false, nil + } + // all states are valid + valid, _ = c.CheckSegmentConstraints(segment) + return valid, nil +} + +// AddStateConstraint will add or overwrite a constraint function with a given name. A constraint function should return true +// if the given position satisfies the constraint. +func (c *ConstraintHandler) AddStateConstraint(name string, cons StateConstraint) { + if c.stateConstraints == nil { + c.stateConstraints = map[string]StateConstraint{} + } + name = name + "_" + fmt.Sprintf("%p", cons) + c.stateConstraints[name] = cons +} + +// RemoveStateConstraint will remove the given constraint. +func (c *ConstraintHandler) RemoveStateConstraint(name string) { + delete(c.stateConstraints, name) +} + +// StateConstraints will list all state constraints by name. +func (c *ConstraintHandler) StateConstraints() []string { + names := make([]string, 0, len(c.stateConstraints)) + for name := range c.stateConstraints { + names = append(names, name) + } + return names +} + +// AddSegmentConstraint will add or overwrite a constraint function with a given name. A constraint function should return true +// if the given position satisfies the constraint. +func (c *ConstraintHandler) AddSegmentConstraint(name string, cons SegmentConstraint) { + if c.segmentConstraints == nil { + c.segmentConstraints = map[string]SegmentConstraint{} + } + // Add function address to name to prevent collisions + name = name + "_" + fmt.Sprintf("%p", cons) + c.segmentConstraints[name] = cons +} + +// RemoveSegmentConstraint will remove the given constraint. +func (c *ConstraintHandler) RemoveSegmentConstraint(name string) { + delete(c.segmentConstraints, name) +} + +// SegmentConstraints will list all segment constraints by name. +func (c *ConstraintHandler) SegmentConstraints() []string { + names := make([]string, 0, len(c.segmentConstraints)) + for name := range c.segmentConstraints { + names = append(names, name) + } + return names +} + +// AddStateFSConstraint will add or overwrite a constraint function with a given name. A constraint function should return true +// if the given position satisfies the constraint. +func (c *ConstraintHandler) AddStateFSConstraint(name string, cons StateFSConstraint) { + if c.stateFSConstraints == nil { + c.stateFSConstraints = map[string]StateFSConstraint{} + } + name = name + "_" + fmt.Sprintf("%p", cons) + c.stateFSConstraints[name] = cons +} + +// RemoveStateFSConstraint will remove the given constraint. +func (c *ConstraintHandler) RemoveStateFSConstraint(name string) { + delete(c.stateFSConstraints, name) +} + +// StateFSConstraints will list all FS state constraints by name. +func (c *ConstraintHandler) StateFSConstraints() []string { + names := make([]string, 0, len(c.stateFSConstraints)) + for name := range c.stateFSConstraints { + names = append(names, name) + } + return names +} + +// AddSegmentFSConstraint will add or overwrite a constraint function with a given name. A constraint function should return true +// if the given position satisfies the constraint. +func (c *ConstraintHandler) AddSegmentFSConstraint(name string, cons SegmentFSConstraint) { + if c.segmentFSConstraints == nil { + c.segmentFSConstraints = map[string]SegmentFSConstraint{} + } + name = name + "_" + fmt.Sprintf("%p", cons) + c.segmentFSConstraints[name] = cons +} + +// RemoveSegmentFSConstraint will remove the given constraint. +func (c *ConstraintHandler) RemoveSegmentFSConstraint(name string) { + delete(c.segmentFSConstraints, name) +} + +// SegmentFSConstraints will list all FS segment constraints by name. +func (c *ConstraintHandler) SegmentFSConstraints() []string { + names := make([]string, 0, len(c.segmentFSConstraints)) + for name := range c.segmentFSConstraints { + names = append(names, name) + } + return names +} + +// CheckStateConstraintsAcrossSegmentFS will interpolate the given input from the StartConfiguration to the EndConfiguration, and ensure +// that all intermediate states as well as both endpoints satisfy all state constraints. If all constraints are satisfied, then this will +// return `true, nil`. If any constraints fail, this will return false, and an SegmentFS representing the valid portion of the segment, +// if any. If no part of the segment is valid, then `false, nil` is returned. +func (c *ConstraintHandler) CheckStateConstraintsAcrossSegmentFS(ci *ik.SegmentFS, resolution float64) (bool, *ik.SegmentFS) { + interpolatedConfigurations, err := interpolateSegmentFS(ci, resolution) + if err != nil { + return false, nil + } + var lastGood map[string][]referenceframe.Input + for i, interpConfig := range interpolatedConfigurations { + interpC := &ik.StateFS{FS: ci.FS, Configuration: interpConfig} + pass, _ := c.CheckStateFSConstraints(interpC) + if !pass { + if i == 0 { + // fail on start pos + return false, nil + } + return false, &ik.SegmentFS{StartConfiguration: ci.StartConfiguration, EndConfiguration: lastGood, FS: ci.FS} + } + lastGood = interpC.Configuration + } + + return true, nil +} + +// CheckSegmentAndStateValidityFS will check a segment input and confirm that it 1) meets all segment constraints, and 2) meets all +// state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, +// provided that segment also meets segment constraints. +func (c *ConstraintHandler) CheckSegmentAndStateValidityFS(segment *ik.SegmentFS, resolution float64) (bool, *ik.SegmentFS) { + valid, subSegment := c.CheckStateConstraintsAcrossSegmentFS(segment, resolution) + if !valid { + if subSegment != nil { + subSegmentValid, _ := c.CheckSegmentFSConstraints(subSegment) + if subSegmentValid { + return false, subSegment + } + } + return false, nil + } + // all states are valid + valid, _ = c.CheckSegmentFSConstraints(segment) + return valid, nil +} diff --git a/motionplan/constraint_test.go b/motionplan/constraint_test.go index aba13c99398..71bd24ba5c9 100644 --- a/motionplan/constraint_test.go +++ b/motionplan/constraint_test.go @@ -23,28 +23,30 @@ func TestIKTolerances(t *testing.T) { m, err := frame.ParseModelJSONFile(utils.ResolveFile("referenceframe/testjson/ur5eDH.json"), "") test.That(t, err, test.ShouldBeNil) - mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(1)), logger, newBasicPlannerOptions(m)) + fs := frame.NewEmptyFrameSystem("") + fs.AddFrame(m, fs.World()) + mp, err := newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(1)), logger, newBasicPlannerOptions()) test.That(t, err, test.ShouldBeNil) // Test inability to arrive at another position due to orientation - pos := spatial.NewPoseFromProtobuf(&commonpb.Pose{ + pos := PathStep{m.Name(): frame.NewPoseInFrame(frame.World, spatial.NewPoseFromProtobuf(&commonpb.Pose{ X: -46, Y: 0, Z: 372, OX: -1.78, OY: -3.3, OZ: -1.11, - }) - _, err = mp.plan(context.Background(), pos, frame.FloatsToInputs([]float64{0, 0})) + }))} + _, err = mp.plan(context.Background(), pos, map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}) test.That(t, err, test.ShouldNotBeNil) // Now verify that setting tolerances to zero allows the same arm to reach that position - opt := newBasicPlannerOptions(m) + opt := newBasicPlannerOptions() opt.goalMetricConstructor = ik.NewPositionOnlyMetric opt.SetMaxSolutions(50) - mp, err = newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(1)), logger, opt) + mp, err = newCBiRRTMotionPlanner(fs, rand.New(rand.NewSource(1)), logger, opt) test.That(t, err, test.ShouldBeNil) - _, err = mp.plan(context.Background(), pos, frame.FloatsToInputs(make([]float64, 6))) + _, err = mp.plan(context.Background(), pos, map[string][]frame.Input{m.Name(): frame.FloatsToInputs(make([]float64, 6))}) test.That(t, err, test.ShouldBeNil) } @@ -66,8 +68,8 @@ func TestConstraintPath(t *testing.T) { test.That(t, failCI, test.ShouldBeNil) test.That(t, ok, test.ShouldBeTrue) - // Test interpolating - constraint, _ := NewProportionalLinearInterpolatingConstraint(ci.StartPosition, ci.EndPosition, 0.01) + // Test interpolating with a proportional constraint, should pass + constraint, _ := NewProportionalLinearInterpolatingConstraint(ci.StartPosition, ci.EndPosition, 0.01, 0.01) handler.AddStateConstraint("interp", constraint) ok, failCI = handler.CheckSegmentAndStateValidity(ci, 0.5) test.That(t, failCI, test.ShouldBeNil) @@ -132,11 +134,6 @@ func TestLineFollow(t *testing.T) { OY: -1, }) - validFunc, gradFunc := NewLineConstraint(p1.Point(), p2.Point(), 0.001) - - pointGrad := gradFunc(&ik.State{Position: query}) - test.That(t, pointGrad, test.ShouldBeLessThan, 0.001*0.001) - fs := frame.NewEmptyFrameSystem("test") m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") @@ -151,40 +148,47 @@ func TestLineFollow(t *testing.T) { test.That(t, err, test.ShouldBeNil) goalFrame := fs.World() - // Create a frame to solve for, and an IK solver with that frame. - sf, err := newSolverFrame(fs, markerFrame.Name(), goalFrame.Name(), frame.StartPositions(fs)) + opt := newBasicPlannerOptions() + startCfg := map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp1)} + from := PathStep{markerFrame.Name(): frame.NewPoseInFrame(markerFrame.Name(), p1)} + to := PathStep{markerFrame.Name(): frame.NewPoseInFrame(goalFrame.Name(), p2)} + + validFunc, gradFunc, err := CreateLineConstraintFS(fs, startCfg, from, to, 0.001) test.That(t, err, test.ShouldBeNil) - opt := newBasicPlannerOptions(sf) - opt.SetPathMetric(gradFunc) - opt.AddStateConstraint("whiteboard", validFunc) + _, innerGradFunc := NewLineConstraint(p1.Point(), p2.Point(), 0.001) + pointGrad := innerGradFunc(&ik.State{Position: query}) + test.That(t, pointGrad, test.ShouldBeLessThan, 0.001*0.001) - ok, lastGood := opt.CheckSegmentAndStateValidity( - &ik.Segment{ - StartConfiguration: sf.InputFromProtobuf(mp1), - EndConfiguration: sf.InputFromProtobuf(mp2), - Frame: sf, + opt.SetPathMetric(gradFunc) + opt.AddStateFSConstraint("whiteboard", validFunc) + + // This tests that we are able to advance partway, but not entirely, to the goal while keeping constraints, and return the last good + // partway position + ok, lastGood := opt.CheckSegmentAndStateValidityFS( + &ik.SegmentFS{ + StartConfiguration: map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp1)}, + EndConfiguration: map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mp2)}, + FS: fs, }, - 1, + 0.001, ) test.That(t, ok, test.ShouldBeFalse) + test.That(t, lastGood, test.ShouldNotBeNil) // lastGood.StartConfiguration and EndConfiguration should pass constraints - lastGood.Frame = sf - stateCheck := &ik.State{Configuration: lastGood.StartConfiguration, Frame: lastGood.Frame} - pass, _ := opt.CheckStateConstraints(stateCheck) + stateCheck := &ik.StateFS{Configuration: lastGood.StartConfiguration, FS: fs} + pass, _ := opt.CheckStateFSConstraints(stateCheck) test.That(t, pass, test.ShouldBeTrue) stateCheck.Configuration = lastGood.EndConfiguration - stateCheck.Position = nil - pass, _ = opt.CheckStateConstraints(stateCheck) + pass, _ = opt.CheckStateFSConstraints(stateCheck) test.That(t, pass, test.ShouldBeTrue) // Check that a deviating configuration will fail - stateCheck.Configuration = sf.InputFromProtobuf(mpFail) - stateCheck.Position = nil - pass, failName := opt.CheckStateConstraints(stateCheck) + stateCheck.Configuration = map[string][]frame.Input{m.Name(): m.InputFromProtobuf(mpFail)} + pass, failName := opt.CheckStateFSConstraints(stateCheck) test.That(t, pass, test.ShouldBeFalse) - test.That(t, failName, test.ShouldEqual, "whiteboard") + test.That(t, failName, test.ShouldStartWith, "whiteboard") } func TestCollisionConstraints(t *testing.T) { @@ -215,15 +219,11 @@ func TestCollisionConstraints(t *testing.T) { fs := frame.NewEmptyFrameSystem("test") err = fs.AddFrame(model, fs.Frame(frame.World)) test.That(t, err, test.ShouldBeNil) - sf, err := newSolverFrame(fs, model.Name(), frame.World, frame.StartPositions(fs)) - test.That(t, err, test.ShouldBeNil) seedMap := frame.StartPositions(fs) - frameInputs, err := sf.mapToSlice(seedMap) - test.That(t, err, test.ShouldBeNil) handler := &ConstraintHandler{} // create robot collision entities - movingGeometriesInFrame, err := sf.Geometries(frameInputs) + movingGeometriesInFrame, err := model.Geometries(seedMap[model.Name()]) movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World test.That(t, err, test.ShouldBeNil) @@ -232,7 +232,7 @@ func TestCollisionConstraints(t *testing.T) { frameSystemGeometries, err := frame.FrameSystemGeometries(fs, seedMap) test.That(t, err, test.ShouldBeNil) for name, geometries := range frameSystemGeometries { - if !sf.movingFrame(name) { + if name != model.Name() { staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) } } @@ -243,7 +243,7 @@ func TestCollisionConstraints(t *testing.T) { worldGeometries, err := worldState.ObstaclesInWorldFrame(fs, seedMap) test.That(t, err, test.ShouldBeNil) - collisionConstraints, err := createAllCollisionConstraints( + _, collisionConstraints, err := createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, worldGeometries.Geometries(), @@ -260,7 +260,7 @@ func TestCollisionConstraints(t *testing.T) { t.Run(fmt.Sprintf("Test %d", i), func(t *testing.T) { response, failName := handler.CheckStateConstraints(&ik.State{Configuration: c.input, Frame: model}) test.That(t, response, test.ShouldEqual, c.expected) - test.That(t, failName, test.ShouldEqual, c.failName) + test.That(t, failName, test.ShouldStartWith, c.failName) }) } } @@ -283,15 +283,11 @@ func BenchmarkCollisionConstraints(b *testing.B) { fs := frame.NewEmptyFrameSystem("test") err = fs.AddFrame(model, fs.Frame(frame.World)) test.That(b, err, test.ShouldBeNil) - sf, err := newSolverFrame(fs, model.Name(), frame.World, frame.StartPositions(fs)) - test.That(b, err, test.ShouldBeNil) seedMap := frame.StartPositions(fs) - frameInputs, err := sf.mapToSlice(seedMap) - test.That(b, err, test.ShouldBeNil) handler := &ConstraintHandler{} // create robot collision entities - movingGeometriesInFrame, err := sf.Geometries(frameInputs) + movingGeometriesInFrame, err := model.Geometries(seedMap[model.Name()]) movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World test.That(b, err, test.ShouldBeNil) @@ -300,7 +296,7 @@ func BenchmarkCollisionConstraints(b *testing.B) { frameSystemGeometries, err := frame.FrameSystemGeometries(fs, seedMap) test.That(b, err, test.ShouldBeNil) for name, geometries := range frameSystemGeometries { - if !sf.movingFrame(name) { + if name != model.Name() { staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) } } @@ -311,7 +307,7 @@ func BenchmarkCollisionConstraints(b *testing.B) { worldGeometries, err := worldState.ObstaclesInWorldFrame(fs, seedMap) test.That(b, err, test.ShouldBeNil) - collisionConstraints, err := createAllCollisionConstraints( + _, collisionConstraints, err := createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, worldGeometries.Geometries(), diff --git a/motionplan/errors.go b/motionplan/errors.go index fc3b5eece17..8f68b2cdc36 100644 --- a/motionplan/errors.go +++ b/motionplan/errors.go @@ -24,7 +24,8 @@ var ( errHighReplanCost = errors.New("unable to create a new plan within replanCostFactor from the original") - errBadPlanImpl = errors.New("rrtPlan is the only supported implementation of Plan by this function") + // TODO: This should eventually be possible. + errMixedFrameTypes = errors.New("unable to plan for PTG and non-PTG frames simultaneously") ) func genIKConstraintErr(failures map[string]int, constraintFailCnt int) error { diff --git a/motionplan/ik/combinedInverseKinematics.go b/motionplan/ik/combinedInverseKinematics.go index 61450f653b7..5f2791b2c3b 100644 --- a/motionplan/ik/combinedInverseKinematics.go +++ b/motionplan/ik/combinedInverseKinematics.go @@ -21,17 +21,17 @@ type combinedIK struct { limits []referenceframe.Limit } -// CreateCombinedIKFrameSolver creates a combined parallel IK solver that operates on a frame with a number of nlopt solvers equal to the +// CreateCombinedIKSolver creates a combined parallel IK solver that operates on a frame with a number of nlopt solvers equal to the // nCPU passed in. Each will be given a different random seed. When asked to solve, all solvers will be run in parallel // and the first valid found solution will be returned. -func CreateCombinedIKFrameSolver( - model referenceframe.Frame, +func CreateCombinedIKSolver( + limits []referenceframe.Limit, logger logging.Logger, nCPU int, goalThreshold float64, ) (Solver, error) { ik := &combinedIK{} - ik.limits = model.DoF() + ik.limits = limits if nCPU == 0 { nCPU = 1 } diff --git a/motionplan/ik/metrics.go b/motionplan/ik/metrics.go index a430d9bb6e1..5fe3ecf25d9 100644 --- a/motionplan/ik/metrics.go +++ b/motionplan/ik/metrics.go @@ -11,7 +11,7 @@ import ( const orientationDistanceScaling = 10. -// Segment contains all the information a constraint needs to determine validity for a movement. +// Segment is a referenceframe.Frame-specific contains all the information a constraint needs to determine validity for a movement. // It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. // Pose fields may be empty, and may be filled in by a constraint that needs them. type Segment struct { @@ -22,43 +22,84 @@ type Segment struct { Frame referenceframe.Frame } +// SegmentFS is a referenceframe.FrameSystem-specific contains all the information a constraint needs to determine validity for a movement. +// It contains the starting inputs, the ending inputs, and the framesystem it refers to. +type SegmentFS struct { + StartConfiguration map[string][]referenceframe.Input + EndConfiguration map[string][]referenceframe.Input + FS referenceframe.FrameSystem +} + func (s *Segment) String() string { + startPosString := "nil" + endPosString := "nil" + if s.StartPosition != nil { + startPosString = fmt.Sprintf("%v", spatial.PoseToProtobuf(s.StartPosition)) + } + if s.EndPosition != nil { + endPosString = fmt.Sprintf("%v", spatial.PoseToProtobuf(s.EndPosition)) + } return fmt.Sprintf( - "Segment: \n\t StartPosition: %v,\n\t EndPosition: %v,\n\t StartConfiguration:%v,\n\t EndConfiguration:%v,\n\t Frame: %v", - spatial.PoseToProtobuf(s.StartPosition), - spatial.PoseToProtobuf(s.EndPosition), + "Segment: \n\t StartPosition: %s,\n\t EndPosition: %s,\n\t StartConfiguration:%v,\n\t EndConfiguration:%v,\n\t Frame: %v", + startPosString, + endPosString, s.StartConfiguration, s.EndConfiguration, s.Frame, ) } -// State contains all the information a constraint needs to determine validity for a movement. -// It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. -// Pose fields may be empty, and may be filled in by a constraint that needs them. +// State contains all the information a constraint needs to determine validity for a particular state or configuration. +// It contains inputs, the corresponding poses, and the frame it refers to. +// Pose field may be empty, and may be filled in by a constraint that needs it. type State struct { Position spatial.Pose Configuration []referenceframe.Input Frame referenceframe.Frame } +// StateFS contains all the information a constraint needs to determine validity for a particular state or configuration of an entire +// framesystem. It contains inputs, the corresponding poses, and the frame it refers to. +// Pose field may be empty, and may be filled in by a constraint that needs it. +type StateFS struct { + Configuration map[string][]referenceframe.Input + FS referenceframe.FrameSystem +} + // StateMetric are functions which, given a State, produces some score. Lower is better. // This is used for gradient descent to converge upon a goal pose, for example. type StateMetric func(*State) float64 +// StateFSMetric are functions which, given a StateFS, produces some score. Lower is better. +// This is used for gradient descent to converge upon a goal pose, for example. +type StateFSMetric func(*StateFS) float64 + // SegmentMetric are functions which produce some score given an Segment. Lower is better. // This is used to sort produced IK solutions by goodness, for example. type SegmentMetric func(*Segment) float64 -// NewZeroMetric always returns zero as the distance between two points. +// SegmentFSMetric are functions which produce some score given an SegmentFS. Lower is better. +// This is used to sort produced IK solutions by goodness, for example. +type SegmentFSMetric func(*SegmentFS) float64 + +// NewZeroMetric always returns zero as the distance. func NewZeroMetric() StateMetric { return func(from *State) float64 { return 0 } } +// NewZeroFSMetric always returns zero as the distance. +func NewZeroFSMetric() StateFSMetric { + return func(from *StateFS) float64 { return 0 } +} + type combinableStateMetric struct { metrics []StateMetric } +type combinableStateFSMetric struct { + metrics []StateFSMetric +} + func (m *combinableStateMetric) combinedDist(input *State) float64 { dist := 0. for _, metric := range m.metrics { @@ -67,6 +108,14 @@ func (m *combinableStateMetric) combinedDist(input *State) float64 { return dist } +func (m *combinableStateFSMetric) combinedDist(input *StateFS) float64 { + dist := 0. + for _, metric := range m.metrics { + dist += metric(input) + } + return dist +} + // CombineMetrics will take a variable number of Metrics and return a new Metric which will combine all given metrics into one, summing // their distances. func CombineMetrics(metrics ...StateMetric) StateMetric { @@ -74,6 +123,13 @@ func CombineMetrics(metrics ...StateMetric) StateMetric { return cm.combinedDist } +// CombineFSMetrics will take a variable number of StateFSMetrics and return a new StateFSMetric which will combine all given metrics into +// one, summing their distances. +func CombineFSMetrics(metrics ...StateFSMetric) StateFSMetric { + cm := &combinableStateFSMetric{metrics: metrics} + return cm.combinedDist +} + // OrientDist returns the arclength between two orientations in degrees. func OrientDist(o1, o2 spatial.Orientation) float64 { return math.Abs(utils.RadToDeg(spatial.QuatToR4AA(spatial.OrientationBetween(o1, o2).Quaternion()).Theta)) @@ -125,14 +181,7 @@ func NewScaledSquaredNormMetric(goal spatial.Pose, orientationDistanceScale floa // TODO: RSDK-6053 this should probably be done more flexibly. func NewPosWeightSquaredNormMetric(goal spatial.Pose) StateMetric { weightedSqNormDist := func(query *State) float64 { - // Increase weight for orientation since it's a small number - orientDelta := spatial.QuatToR3AA(spatial.OrientationBetween( - goal.Orientation(), - query.Position.Orientation(), - ).Quaternion()).Mul(orientationDistanceScaling).Norm2() - // Also, we multiply delta.Point() by 0.1, effectively measuring in cm rather than mm. - ptDelta := goal.Point().Mul(0.1).Sub(query.Position.Point().Mul(0.1)).Norm2() - return ptDelta + orientDelta + return WeightedSquaredNormSegmentMetric(&Segment{StartPosition: query.Position, EndPosition: goal}) } return weightedSqNormDist } @@ -187,9 +236,46 @@ func NewSquaredNormSegmentMetric(orientationScaleFactor float64) SegmentMetric { // SquaredNormNoOrientSegmentMetric is a metric which will return the cartesian distance between the two positions. func SquaredNormNoOrientSegmentMetric(segment *Segment) float64 { delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition) - // Increase weight for orientation since it's a small number return delta.Point().Norm2() } +// WeightedSquaredNormSegmentMetric is a distance function between two poses to be used for gradient descent. +// This changes the magnitude of the position delta used to be smaller and avoid numeric instability issues that happens with large floats. +// It also scales the orientation distance to give more weight to it. +func WeightedSquaredNormSegmentMetric(segment *Segment) float64 { + // Increase weight for orientation since it's a small number + orientDelta := spatial.QuatToR3AA(spatial.OrientationBetween( + segment.EndPosition.Orientation(), + segment.StartPosition.Orientation(), + ).Quaternion()).Mul(orientationDistanceScaling).Norm2() + // Also, we multiply delta.Point() by 0.1, effectively measuring in cm rather than mm. + ptDelta := segment.EndPosition.Point().Mul(0.1).Sub(segment.StartPosition.Point().Mul(0.1)).Norm2() + return ptDelta + orientDelta +} + // TODO(RSDK-2557): Writing a PenetrationDepthMetric will allow cbirrt to path along the sides of obstacles rather than terminating // the RRT tree when an obstacle is hit + +// FSConfigurationDistance is a fs metric which will sum the abs differences in each input from start to end. +func FSConfigurationDistance(segment *SegmentFS) float64 { + score := 0. + for frame, cfg := range segment.StartConfiguration { + if endCfg, ok := segment.EndConfiguration[frame]; ok && len(cfg) == len(endCfg) { + for i, val := range cfg { + score += math.Abs(val.Value - endCfg[i].Value) + } + } + } + return score +} + +// FSConfigurationL2Distance is a fs metric which will sum the L2 norm differences in each input from start to end. +func FSConfigurationL2Distance(segment *SegmentFS) float64 { + score := 0. + for frame, cfg := range segment.StartConfiguration { + if endCfg, ok := segment.EndConfiguration[frame]; ok && len(cfg) == len(endCfg) { + score += referenceframe.InputsL2Distance(cfg, endCfg) + } + } + return score +} diff --git a/motionplan/ik/nloptInverseKinematics.go b/motionplan/ik/nloptInverseKinematics.go index d3a81b61546..e1a4e4b4702 100644 --- a/motionplan/ik/nloptInverseKinematics.go +++ b/motionplan/ik/nloptInverseKinematics.go @@ -4,6 +4,7 @@ package ik import ( "context" + "fmt" "math/rand" "sync" @@ -70,7 +71,6 @@ func CreateNloptSolver( iter = defaultMaxIter } ik.maxIterations = iter - ik.exact = exact ik.useRelTol = useRelTol @@ -89,6 +89,9 @@ func (ik *nloptIK) Solve(ctx context.Context, minFunc func([]float64) float64, rseed int, ) error { + if len(seed) != len(ik.limits) { + return fmt.Errorf("nlopt initialized with %d dof but seed was length %d", len(ik.limits), len(seed)) + } //nolint: gosec randSeed := rand.New(rand.NewSource(int64(rseed))) var err error diff --git a/motionplan/ik/solver_test.go b/motionplan/ik/solver_test.go index 536852604b9..9f1aa1b4ae6 100644 --- a/motionplan/ik/solver_test.go +++ b/motionplan/ik/solver_test.go @@ -25,7 +25,7 @@ func TestCombinedIKinematics(t *testing.T) { logger := logging.NewTestLogger(t) m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKFrameSolver(m, logger, nCPU, defaultGoalThreshold) + ik, err := CreateCombinedIKSolver(m.DoF(), logger, nCPU, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) // Test ability to arrive at another position @@ -52,7 +52,7 @@ func TestUR5NloptIKinematics(t *testing.T) { m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/universalrobots/ur5e.json"), "") test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKFrameSolver(m, logger, nCPU, defaultGoalThreshold) + ik, err := CreateCombinedIKSolver(m.DoF(), logger, nCPU, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) goalJP := frame.JointPositionsFromRadians([]float64{-4.128, 2.71, 2.798, 2.3, 1.291, 0.62}) @@ -67,7 +67,7 @@ func TestCombinedCPUs(t *testing.T) { logger := logging.NewTestLogger(t) m, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm7_kinematics.json"), "") test.That(t, err, test.ShouldBeNil) - ik, err := CreateCombinedIKFrameSolver(m, logger, runtime.NumCPU()/400000, defaultGoalThreshold) + ik, err := CreateCombinedIKSolver(m.DoF(), logger, runtime.NumCPU()/400000, defaultGoalThreshold) test.That(t, err, test.ShouldBeNil) test.That(t, len(ik.(*combinedIK).solvers), test.ShouldEqual, 1) } diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index e466c97e5cd..c18c0b49eca 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -5,20 +5,19 @@ package motionplan import ( "context" - "encoding/json" "fmt" + "math" "math/rand" "sort" "sync" "time" "github.com/pkg/errors" - commonpb "go.viam.com/api/common/v1" "go.viam.com/utils" "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" - frame "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -27,31 +26,32 @@ import ( type motionPlanner interface { // Plan will take a context, a goal position, and an input start state and return a series of state waypoints which // should be visited in order to arrive at the goal while satisfying all constraints - plan(context.Context, spatialmath.Pose, []frame.Input) ([]node, error) + plan(context.Context, PathStep, map[string][]referenceframe.Input) ([]node, error) // Everything below this point should be covered by anything that wraps the generic `planner` smoothPath(context.Context, []node) []node - checkPath([]frame.Input, []frame.Input) bool - checkInputs([]frame.Input) bool - getSolutions(context.Context, []frame.Input) ([]node, error) + checkPath(map[string][]referenceframe.Input, map[string][]referenceframe.Input) bool + checkInputs(map[string][]referenceframe.Input) bool + getSolutions(context.Context, map[string][]referenceframe.Input) ([]node, error) opt() *plannerOptions sample(node, int) (node, error) } -type plannerConstructor func(frame.Frame, *rand.Rand, logging.Logger, *plannerOptions) (motionPlanner, error) +type plannerConstructor func(referenceframe.FrameSystem, *rand.Rand, logging.Logger, *plannerOptions) (motionPlanner, error) // PlanRequest is a struct to store all the data necessary to make a call to PlanMotion. type PlanRequest struct { Logger logging.Logger - Goal *frame.PoseInFrame - Frame frame.Frame - FrameSystem frame.FrameSystem + Goal *referenceframe.PoseInFrame + Frame referenceframe.Frame + FrameSystem referenceframe.FrameSystem StartPose spatialmath.Pose - StartConfiguration map[string][]frame.Input - WorldState *frame.WorldState + StartConfiguration map[string][]referenceframe.Input + WorldState *referenceframe.WorldState BoundingRegions []spatialmath.Geometry Constraints *Constraints Options map[string]interface{} + allGoals PathStep // TODO: this should replace Goal and Frame } // validatePlanRequest ensures PlanRequests are not malformed. @@ -69,7 +69,7 @@ func (req *PlanRequest) validatePlanRequest() error { if req.FrameSystem == nil { return errors.New("PlanRequest cannot have nil framesystem") } else if req.FrameSystem.Frame(req.Frame.Name()) == nil { - return frame.NewFrameMissingError(req.Frame.Name()) + return referenceframe.NewFrameMissingError(req.Frame.Name()) } if req.Goal == nil { @@ -78,7 +78,7 @@ func (req *PlanRequest) validatePlanRequest() error { goalParentFrame := req.Goal.Parent() if req.FrameSystem.Frame(goalParentFrame) == nil { - return frame.NewParentFrameMissingError(req.Goal.Name(), goalParentFrame) + return referenceframe.NewParentFrameMissingError(req.Goal.Name(), goalParentFrame) } if len(req.BoundingRegions) > 0 { @@ -87,7 +87,7 @@ func (req *PlanRequest) validatePlanRequest() error { buffer = defaultCollisionBufferMM } // check that the request frame's geometries are within or in collision with the bounding regions - robotGifs, err := req.Frame.Geometries(make([]frame.Input, len(req.Frame.DoF()))) + robotGifs, err := req.Frame.Geometries(make([]referenceframe.Input, len(req.Frame.DoF()))) if err != nil { return err } @@ -115,12 +115,15 @@ func (req *PlanRequest) validatePlanRequest() error { return errors.Errorf("%s does not have a start configuration", req.Frame.Name()) } if frameDOF != len(seedMap) { - return frame.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) + return referenceframe.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) } } else if ok && frameDOF != len(seedMap) { - return frame.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) + return referenceframe.NewIncorrectDoFError(len(seedMap), len(req.Frame.DoF())) } + // TODO: This should replace Frame and Goal + req.allGoals = map[string]*referenceframe.PoseInFrame{req.Frame.Name(): req.Goal} + return nil } @@ -135,21 +138,21 @@ func PlanMotion(ctx context.Context, request *PlanRequest) (Plan, error) { func PlanFrameMotion(ctx context.Context, logger logging.Logger, dst spatialmath.Pose, - f frame.Frame, - seed []frame.Input, + f referenceframe.Frame, + seed []referenceframe.Input, constraints *Constraints, planningOpts map[string]interface{}, -) ([][]frame.Input, error) { +) ([][]referenceframe.Input, error) { // ephemerally create a framesystem containing just the frame for the solve - fs := frame.NewEmptyFrameSystem("") + fs := referenceframe.NewEmptyFrameSystem("") if err := fs.AddFrame(f, fs.World()); err != nil { return nil, err } plan, err := PlanMotion(ctx, &PlanRequest{ Logger: logger, - Goal: frame.NewPoseInFrame(frame.World, dst), + Goal: referenceframe.NewPoseInFrame(referenceframe.World, dst), Frame: f, - StartConfiguration: map[string][]frame.Input{f.Name(): seed}, + StartConfiguration: map[string][]referenceframe.Input{f.Name(): seed}, FrameSystem: fs, Constraints: constraints, Options: planningOpts, @@ -167,16 +170,6 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC if err := request.validatePlanRequest(); err != nil { return nil, err } - - // Create a frame to solve for, and an IK solver with that frame. - sf, err := newSolverFrame(request.FrameSystem, request.Frame.Name(), request.Goal.Parent(), request.StartConfiguration) - if err != nil { - return nil, err - } - if len(sf.DoF()) == 0 { - return nil, errors.New("solver frame has no degrees of freedom, cannot perform inverse kinematics") - } - request.Logger.CDebugf(ctx, "constraint specs for this step: %v", request.Constraints) request.Logger.CDebugf(ctx, "motion config for this step: %v", request.Options) @@ -184,38 +177,10 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC if seed, ok := request.Options["rseed"].(int); ok { rseed = seed } - sfPlanner, err := newPlanManager(sf, request.Logger, rseed) + sfPlanner, err := newPlanManager(request.FrameSystem, request.Logger, rseed) if err != nil { return nil, err } - // Check if the PlanRequest's options specify "complex" - // This is a list of poses in the same frame as the goal through which the robot must pass - if complexOption, ok := request.Options["complex"]; ok { - if complexPosesList, ok := complexOption.([]interface{}); ok { - // If "complex" is specified and is a list of PoseInFrame, use it for planning. - requestCopy := *request - delete(requestCopy.Options, "complex") - complexPoses := make([]spatialmath.Pose, 0, len(complexPosesList)) - for _, iface := range complexPosesList { - complexPoseJSON, err := json.Marshal(iface) - if err != nil { - return nil, err - } - complexPosePb := &commonpb.Pose{} - err = json.Unmarshal(complexPoseJSON, complexPosePb) - if err != nil { - return nil, err - } - complexPoses = append(complexPoses, spatialmath.NewPoseFromProtobuf(complexPosePb)) - } - multiGoalPlan, err := sfPlanner.PlanMultiWaypoint(ctx, &requestCopy, complexPoses) - if err != nil { - return nil, err - } - return multiGoalPlan, nil - } - return nil, errors.New("Invalid 'complex' option type. Expected a list of protobuf poses") - } newPlan, err := sfPlanner.PlanSingleWaypoint(ctx, request, currentPlan) if err != nil { @@ -223,8 +188,8 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC } if replanCostFactor > 0 && currentPlan != nil { - initialPlanCost := currentPlan.Trajectory().EvaluateCost(sfPlanner.opt().ScoreFunc) - finalPlanCost := newPlan.Trajectory().EvaluateCost(sfPlanner.opt().ScoreFunc) + initialPlanCost := currentPlan.Trajectory().EvaluateCost(sfPlanner.opt().scoreFunc) + finalPlanCost := newPlan.Trajectory().EvaluateCost(sfPlanner.opt().scoreFunc) request.Logger.CDebugf(ctx, "initialPlanCost %f adjusted with cost factor to %f, replan cost %f", initialPlanCost, initialPlanCost*replanCostFactor, finalPlanCost, @@ -239,22 +204,32 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC } type planner struct { + fss referenceframe.FrameSystem + lfs *linearizedFrameSystem solver ik.Solver - frame frame.Frame logger logging.Logger randseed *rand.Rand start time.Time planOpts *plannerOptions } -func newPlanner(frame frame.Frame, seed *rand.Rand, logger logging.Logger, opt *plannerOptions) (*planner, error) { - solver, err := ik.CreateCombinedIKFrameSolver(frame, logger, opt.NumThreads, opt.GoalThreshold) +func newPlanner(fss referenceframe.FrameSystem, seed *rand.Rand, logger logging.Logger, opt *plannerOptions) (*planner, error) { + lfs, err := newLinearizedFrameSystem(fss) + if err != nil { + return nil, err + } + if opt == nil { + opt = newBasicPlannerOptions() + } + + solver, err := ik.CreateCombinedIKSolver(lfs.dof, logger, opt.NumThreads, opt.GoalThreshold) if err != nil { return nil, err } mp := &planner{ solver: solver, - frame: frame, + fss: fss, + lfs: lfs, logger: logger, randseed: seed, planOpts: opt, @@ -262,20 +237,20 @@ func newPlanner(frame frame.Frame, seed *rand.Rand, logger logging.Logger, opt * return mp, nil } -func (mp *planner) checkInputs(inputs []frame.Input) bool { - ok, _ := mp.planOpts.CheckStateConstraints(&ik.State{ +func (mp *planner) checkInputs(inputs map[string][]referenceframe.Input) bool { + ok, _ := mp.planOpts.CheckStateFSConstraints(&ik.StateFS{ Configuration: inputs, - Frame: mp.frame, + FS: mp.fss, }) return ok } -func (mp *planner) checkPath(seedInputs, target []frame.Input) bool { - ok, _ := mp.planOpts.CheckSegmentAndStateValidity( - &ik.Segment{ +func (mp *planner) checkPath(seedInputs, target map[string][]referenceframe.Input) bool { + ok, _ := mp.planOpts.CheckSegmentAndStateValidityFS( + &ik.SegmentFS{ StartConfiguration: seedInputs, EndConfiguration: target, - Frame: mp.frame, + FS: mp.fss, }, mp.planOpts.Resolution, ) @@ -286,14 +261,29 @@ func (mp *planner) sample(rSeed node, sampleNum int) (node, error) { // If we have done more than 50 iterations, start seeding off completely random positions 2 at a time // The 2 at a time is to ensure random seeds are added onto both the seed and goal maps. if sampleNum >= mp.planOpts.IterBeforeRand && sampleNum%4 >= 2 { - return newConfigurationNode(frame.RandomFrameInputs(mp.frame, mp.randseed)), nil + randomInputs := make(map[string][]referenceframe.Input) + for _, name := range mp.fss.FrameNames() { + f := mp.fss.Frame(name) + if f != nil && len(f.DoF()) > 0 { + randomInputs[name] = referenceframe.RandomFrameInputs(f, mp.randseed) + } + } + return newConfigurationNode(randomInputs), nil } + // Seeding nearby to valid points results in much faster convergence in less constrained space - q, err := frame.RestrictedRandomFrameInputs(mp.frame, mp.randseed, 0.1, rSeed.Q()) - if err != nil { - return nil, err + newInputs := make(map[string][]referenceframe.Input) + for name, inputs := range rSeed.Q() { + f := mp.fss.Frame(name) + if f != nil && len(f.DoF()) > 0 { + q, err := referenceframe.RestrictedRandomFrameInputs(f, mp.randseed, 0.1, inputs) + if err != nil { + return nil, err + } + newInputs[name] = q + } } - return newConfigurationNode(q), nil + return newConfigurationNode(newInputs), nil } func (mp *planner) opt() *plannerOptions { @@ -328,11 +318,12 @@ func (mp *planner) smoothPath(ctx context.Context, path []node) []node { firstEdge := mp.randseed.Intn(len(path) - 2) secondEdge := firstEdge + 1 + mp.randseed.Intn((len(path)-2)-firstEdge) - wayPoint1, err := mp.frame.Interpolate(path[firstEdge].Q(), path[firstEdge+1].Q(), waypoints[mp.randseed.Intn(3)]) + // Use the frame system to interpolate between configurations + wayPoint1, err := referenceframe.InterpolateFS(mp.fss, path[firstEdge].Q(), path[firstEdge+1].Q(), waypoints[mp.randseed.Intn(3)]) if err != nil { return path } - wayPoint2, err := mp.frame.Interpolate(path[secondEdge].Q(), path[secondEdge+1].Q(), waypoints[mp.randseed.Intn(3)]) + wayPoint2, err := referenceframe.InterpolateFS(mp.fss, path[secondEdge].Q(), path[secondEdge+1].Q(), waypoints[mp.randseed.Intn(3)]) if err != nil { return path } @@ -352,17 +343,13 @@ func (mp *planner) smoothPath(ctx context.Context, path []node) []node { // getSolutions will initiate an IK solver for the given position and seed, collect solutions, and score them by constraints. // If maxSolutions is positive, once that many solutions have been collected, the solver will terminate and return that many solutions. // If minScore is positive, if a solution scoring below that amount is found, the solver will terminate and return that one solution. -func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node, error) { +func (mp *planner) getSolutions(ctx context.Context, seed map[string][]referenceframe.Input) ([]node, error) { // Linter doesn't properly handle loop labels nSolutions := mp.planOpts.MaxSolutions if nSolutions == 0 { nSolutions = defaultSolutionsToSeed } - seedPos, err := mp.frame.Transform(seed) - if err != nil { - return nil, err - } if mp.planOpts.goalMetric == nil { return nil, errors.New("metric is nil") } @@ -375,14 +362,21 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node var activeSolvers sync.WaitGroup defer activeSolvers.Wait() activeSolvers.Add(1) + + linearSeed, err := mp.lfs.mapToSlice(seed) + if err != nil { + return nil, err + } + + minFunc := mp.linearizeFSmetric(mp.planOpts.goalMetric) // Spawn the IK solver to generate solutions until done utils.PanicCapturingGo(func() { defer close(ikErr) defer activeSolvers.Done() - ikErr <- ik.SolveMetric(ctxWithCancel, mp.solver, mp.frame, solutionGen, seed, mp.planOpts.goalMetric, mp.randseed.Int(), mp.logger) + ikErr <- mp.solver.Solve(ctxWithCancel, solutionGen, linearSeed, minFunc, mp.randseed.Int()) }) - solutions := map[float64][]frame.Input{} + solutions := map[float64]map[string][]referenceframe.Input{} // A map keeping track of which constraints fail failures := map[string]int{} @@ -399,25 +393,33 @@ IK: select { case stepSolution := <-solutionGen: - step := frame.FloatsToInputs(stepSolution.Configuration) + + step, err := mp.lfs.sliceToMap(stepSolution.Configuration) + if err != nil { + return nil, err + } + alteredStep := mp.nonchainMinimize(seed, step) + if alteredStep != nil { + // if nil, step is guaranteed to fail the below check, but we want to do it anyway to capture the failure reason + step = alteredStep + } // Ensure the end state is a valid one - statePass, failName := mp.planOpts.CheckStateConstraints(&ik.State{ + statePass, failName := mp.planOpts.CheckStateFSConstraints(&ik.StateFS{ Configuration: step, - Frame: mp.frame, + FS: mp.fss, }) if statePass { - stepArc := &ik.Segment{ + stepArc := &ik.SegmentFS{ StartConfiguration: seed, - StartPosition: seedPos, EndConfiguration: step, - Frame: mp.frame, + FS: mp.fss, } - arcPass, failName := mp.planOpts.CheckSegmentConstraints(stepArc) + arcPass, failName := mp.planOpts.CheckSegmentFSConstraints(stepArc) if arcPass { - score := mp.planOpts.goalArcScore(stepArc) + score := mp.planOpts.confDistanceFunc(stepArc) if score < mp.planOpts.MinScore && mp.planOpts.MinScore > 0 { - solutions = map[float64][]frame.Input{} + solutions = map[float64]map[string][]referenceframe.Input{} solutions[score] = step // good solution, stopping early break IK @@ -483,3 +485,67 @@ IK: } return orderedSolutions, nil } + +// linearize the goal metric for use with solvers. +func (mp *planner) linearizeFSmetric(metric ik.StateFSMetric) func([]float64) float64 { + return func(query []float64) float64 { + inputs, err := mp.lfs.sliceToMap(query) + if err != nil { + return math.Inf(1) + } + val := metric(&ik.StateFS{Configuration: inputs, FS: mp.fss}) + return val + } +} + +func (mp *planner) frameLists() (moving, nonmoving []string) { + movingMap := map[string]referenceframe.Frame{} + for _, chain := range mp.planOpts.motionChains { + for _, frame := range chain.frames { + movingMap[frame.Name()] = frame + } + } + + for _, frameName := range mp.fss.FrameNames() { + if _, ok := movingMap[frameName]; ok { + moving = append(moving, frameName) + } else { + nonmoving = append(nonmoving, frameName) + } + } + return moving, nonmoving +} + +// The purpose of this function is to allow solves that require the movement of components not in a motion chain, while preventing wild or +// random motion of these components unnecessarily. A classic example would be a scene with two arms. One arm is given a goal in World +// which it could reach, but the other arm is in the way. Randomly seeded IK will produce a valid configuration for the moving arm, and a +// random configuration for the other. This function attempts to replace that random configuration with the seed configuration, if valid, +// and if invalid will interpolate the solved random configuration towards the seed and set its configuration to the closest valid +// configuration to the seed. +func (mp *planner) nonchainMinimize(seed, step map[string][]referenceframe.Input) map[string][]referenceframe.Input { + moving, nonmoving := mp.frameLists() + // Create a map with nonmoving configurations replaced with their seed values + alteredStep := map[string][]referenceframe.Input{} + for _, frame := range moving { + alteredStep[frame] = step[frame] + } + for _, frame := range nonmoving { + alteredStep[frame] = seed[frame] + } + if mp.checkInputs(alteredStep) { + return alteredStep + } + // Failing constraints with nonmoving frames at seed. Find the closest passing configuration to seed. + + _, lastGood := mp.planOpts.CheckStateConstraintsAcrossSegmentFS( + &ik.SegmentFS{ + StartConfiguration: step, + EndConfiguration: alteredStep, + FS: mp.fss, + }, mp.planOpts.Resolution, + ) + if lastGood != nil { + return lastGood.EndConfiguration + } + return nil +} diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 0b1c8fda434..eecd69eff1d 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -32,10 +32,10 @@ var logger = logging.FromZapCompatible(zap.Must(zap.Config{ }.Build()).Sugar()) type planConfig struct { - Start []frame.Input - Goal spatialmath.Pose - RobotFrame frame.Frame - Options *plannerOptions + Start map[string][]frame.Input + Goal PathStep + FS frame.FrameSystem + Options *plannerOptions } type planConfigConstructor func() (*planConfig, error) @@ -97,17 +97,34 @@ func constrainedXArmMotion() (*planConfig, error) { // Test ability to arrive at another position pos := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -206, Y: 100, Z: 120, OZ: -1}) - opt := newBasicPlannerOptions(model) + opt := newBasicPlannerOptions() opt.SmoothIter = 2 orientMetric := ik.NewPoseFlexOVMetricConstructor(0.09) + // Create a temporary frame system for the transformation + fs := frame.NewEmptyFrameSystem("") + err = fs.AddFrame(model, fs.World()) + if err != nil { + return nil, err + } + oFunc := ik.OrientDistToRegion(pos.Orientation(), 0.1) - oFuncMet := func(from *ik.State) float64 { - err := resolveStatesToPositions(from) + oFuncMet := func(from *ik.StateFS) float64 { if err != nil { return math.Inf(1) } - return oFunc(from.Position.Orientation()) + + // Transform the current state to get the pose + currPose, err := fs.Transform( + from.Configuration, + frame.NewZeroPoseInFrame(model.Name()), + frame.World, + ) + if err != nil { + return math.Inf(1) + } + + return oFunc(currPose.(*frame.PoseInFrame).Pose().Orientation()) } orientConstraint := func(cInput *ik.State) bool { err := resolveStatesToPositions(cInput) @@ -122,11 +139,15 @@ func constrainedXArmMotion() (*planConfig, error) { opt.SetPathMetric(oFuncMet) opt.AddStateConstraint("orientation", orientConstraint) + start := map[string][]frame.Input{model.Name(): home7} + goal := PathStep{model.Name(): frame.NewPoseInFrame(frame.World, pos)} + opt.fillMotionChains(fs, goal) + return &planConfig{ - Start: home7, - Goal: pos, - RobotFrame: model, - Options: opt, + Start: start, + Goal: goal, + FS: fs, + Options: opt, }, nil } @@ -204,23 +225,16 @@ func simple2DMap() (*planConfig, error) { } // setup planner options - opt := newBasicPlannerOptions(model) + opt := newBasicPlannerOptions() startInput := frame.StartPositions(fs) startInput[modelName] = frame.FloatsToInputs([]float64{-90., 90., 0}) - goal := spatialmath.NewPoseFromPoint(r3.Vector{X: 90, Y: 90, Z: 0}) - opt.SetGoal(goal) - sf, err := newSolverFrame(fs, modelName, frame.World, startInput) - if err != nil { - return nil, err - } - seedMap := frame.StartPositions(fs) - frameInputs, err := sf.mapToSlice(seedMap) - if err != nil { - return nil, err - } + goalPose := spatialmath.NewPoseFromPoint(r3.Vector{X: 90, Y: 90, Z: 0}) + goal := PathStep{modelName: frame.NewPoseInFrame(frame.World, goalPose)} + opt.setGoal(goal) + seedMap := frame.StartPositions(fs) // create robot collision entities - movingGeometriesInFrame, err := sf.Geometries(frameInputs) + movingGeometriesInFrame, err := model.Geometries(seedMap[model.Name()]) movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World if err != nil { return nil, err @@ -233,7 +247,7 @@ func simple2DMap() (*planConfig, error) { return nil, err } for name, geometries := range frameSystemGeometries { - if !sf.movingFrame(name) { + if name != model.Name() { staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) } } @@ -246,7 +260,7 @@ func simple2DMap() (*planConfig, error) { return nil, err } - collisionConstraints, err := createAllCollisionConstraints( + _, collisionConstraints, err := createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, worldGeometries.Geometries(), @@ -259,12 +273,13 @@ func simple2DMap() (*planConfig, error) { for name, constraint := range collisionConstraints { opt.AddStateConstraint(name, constraint) } + opt.fillMotionChains(fs, goal) return &planConfig{ - Start: startInput[modelName], - Goal: goal, - RobotFrame: model, - Options: opt, + Start: startInput, + Goal: goal, + FS: fs, + Options: opt, }, nil } @@ -281,42 +296,35 @@ func simpleXArmMotion() (*planConfig, error) { return nil, err } - goal := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: 206, Y: 100, Z: 120, OZ: -1}) + goal := PathStep{ + xarm.Name(): frame.NewPoseInFrame(frame.World, spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: 206, Y: 100, Z: 120, OZ: -1})), + } // setup planner options - opt := newBasicPlannerOptions(xarm) + opt := newBasicPlannerOptions() opt.SmoothIter = 20 - opt.SetGoal(goal) - sf, err := newSolverFrame(fs, xarm.Name(), frame.World, frame.StartPositions(fs)) - if err != nil { - return nil, err - } - seedMap := frame.StartPositions(fs) - frameInputs, err := sf.mapToSlice(seedMap) - if err != nil { - return nil, err - } + opt.setGoal(goal) // create robot collision entities - movingGeometriesInFrame, err := sf.Geometries(frameInputs) - movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World + movingGeometriesInFrame, err := xarm.Geometries(home7) if err != nil { return nil, err } + movingRobotGeometries := movingGeometriesInFrame.Geometries() // find all geometries that are not moving but are in the frame system staticRobotGeometries := make([]spatialmath.Geometry, 0) - frameSystemGeometries, err := frame.FrameSystemGeometries(fs, seedMap) + frameSystemGeometries, err := frame.FrameSystemGeometries(fs, frame.StartPositions(fs)) if err != nil { return nil, err } for name, geometries := range frameSystemGeometries { - if !sf.movingFrame(name) { + if name != xarm.Name() { staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) } } - collisionConstraints, err := createAllCollisionConstraints( + fsCollisionConstraints, collisionConstraints, err := createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, nil, @@ -330,12 +338,17 @@ func simpleXArmMotion() (*planConfig, error) { for name, constraint := range collisionConstraints { opt.AddStateConstraint(name, constraint) } + for name, constraint := range fsCollisionConstraints { + opt.AddStateFSConstraint(name, constraint) + } + start := map[string][]frame.Input{xarm.Name(): home7} + opt.fillMotionChains(fs, goal) return &planConfig{ - Start: home7, - Goal: goal, - RobotFrame: xarm, - Options: opt, + Start: start, + Goal: goal, + FS: fs, + Options: opt, }, nil } @@ -345,46 +358,41 @@ func simpleUR5eMotion() (*planConfig, error) { if err != nil { return nil, err } + fs := frame.NewEmptyFrameSystem("test") if err = fs.AddFrame(ur5e, fs.Frame(frame.World)); err != nil { return nil, err } - goal := spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -750, Y: -250, Z: 200, OX: -1}) + + goal := PathStep{ + ur5e.Name(): frame.NewPoseInFrame(frame.World, spatialmath.NewPoseFromProtobuf(&commonpb.Pose{X: -750, Y: -250, Z: 200, OX: -1})), + } // setup planner options - opt := newBasicPlannerOptions(ur5e) + opt := newBasicPlannerOptions() opt.SmoothIter = 20 - opt.SetGoal(goal) - sf, err := newSolverFrame(fs, ur5e.Name(), frame.World, frame.StartPositions(fs)) - if err != nil { - return nil, err - } - seedMap := frame.StartPositions(fs) - frameInputs, err := sf.mapToSlice(seedMap) - if err != nil { - return nil, err - } + opt.setGoal(goal) // create robot collision entities - movingGeometriesInFrame, err := sf.Geometries(frameInputs) - movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World + movingGeometriesInFrame, err := ur5e.Geometries(home6) if err != nil { return nil, err } + movingRobotGeometries := movingGeometriesInFrame.Geometries() // find all geometries that are not moving but are in the frame system staticRobotGeometries := make([]spatialmath.Geometry, 0) - frameSystemGeometries, err := frame.FrameSystemGeometries(fs, seedMap) + frameSystemGeometries, err := frame.FrameSystemGeometries(fs, frame.StartPositions(fs)) if err != nil { return nil, err } for name, geometries := range frameSystemGeometries { - if !sf.movingFrame(name) { + if name != ur5e.Name() { staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) } } - collisionConstraints, err := createAllCollisionConstraints( + fsCollisionConstraints, collisionConstraints, err := createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, nil, @@ -398,12 +406,17 @@ func simpleUR5eMotion() (*planConfig, error) { for name, constraint := range collisionConstraints { opt.AddStateConstraint(name, constraint) } + for name, constraint := range fsCollisionConstraints { + opt.AddStateFSConstraint(name, constraint) + } + start := map[string][]frame.Input{ur5e.Name(): home6} + opt.fillMotionChains(fs, goal) return &planConfig{ - Start: home6, - Goal: goal, - RobotFrame: ur5e, - Options: opt, + Start: start, + Goal: goal, + FS: fs, + Options: opt, }, nil } @@ -415,7 +428,7 @@ func testPlanner(t *testing.T, plannerFunc plannerConstructor, config planConfig // plan cfg, err := config() test.That(t, err, test.ShouldBeNil) - mp, err := plannerFunc(cfg.RobotFrame, rand.New(rand.NewSource(int64(seed))), logger, cfg.Options) + mp, err := plannerFunc(cfg.FS, rand.New(rand.NewSource(int64(seed))), logger, cfg.Options) test.That(t, err, test.ShouldBeNil) nodes, err := mp.plan(context.Background(), cfg.Goal, cfg.Start) test.That(t, err, test.ShouldBeNil) @@ -423,10 +436,10 @@ func testPlanner(t *testing.T, plannerFunc plannerConstructor, config planConfig // test that path doesn't violate constraints test.That(t, len(nodes), test.ShouldBeGreaterThanOrEqualTo, 2) for j := 0; j < len(nodes)-1; j++ { - ok, _ := cfg.Options.ConstraintHandler.CheckSegmentAndStateValidity(&ik.Segment{ + ok, _ := cfg.Options.ConstraintHandler.CheckSegmentAndStateValidityFS(&ik.SegmentFS{ StartConfiguration: nodes[j].Q(), EndConfiguration: nodes[j+1].Q(), - Frame: cfg.RobotFrame, + FS: cfg.FS, }, cfg.Options.Resolution) test.That(t, ok, test.ShouldBeTrue) } @@ -696,48 +709,6 @@ func TestSliceUniq(t *testing.T) { test.That(t, len(uniqd), test.ShouldEqual, 3) } -func TestSolverFrameGeometries(t *testing.T) { - t.Parallel() - fs := makeTestFS(t) - sf, err := newSolverFrame(fs, "xArmVgripper", frame.World, frame.StartPositions(fs)) - test.That(t, err, test.ShouldBeNil) - - sfPlanner, err := newPlanManager(sf, logger, 1) - test.That(t, err, test.ShouldBeNil) - - request := &PlanRequest{ - Logger: logger, - Frame: fs.Frame("xArmVgripper"), - FrameSystem: fs, - StartConfiguration: sf.sliceToMap(make([]frame.Input, len(sf.DoF()))), - Goal: frame.NewPoseInFrame(frame.World, spatialmath.NewPoseFromPoint(r3.Vector{300, 300, 100})), - Options: map[string]interface{}{"smooth_iter": 5}, - } - - err = request.validatePlanRequest() - test.That(t, err, test.ShouldBeNil) - - plan, err := sfPlanner.PlanSingleWaypoint( - context.Background(), - request, - nil, - ) - test.That(t, err, test.ShouldBeNil) - inputs, err := sf.mapToSlice(plan.Trajectory()[len(plan.Trajectory())-1]) - test.That(t, err, test.ShouldBeNil) - gf, err := sf.Geometries(inputs) - test.That(t, err, test.ShouldBeNil) - test.That(t, gf, test.ShouldNotBeNil) - - geoms := gf.Geometries() - for _, geom := range geoms { - if geom.Label() == "xArmVgripper" { - gripperCenter := geom.Pose().Point() - test.That(t, spatialmath.R3VectorAlmostEqual(gripperCenter, r3.Vector{300, 300, 0}, 1e-2), test.ShouldBeTrue) - } - } -} - func TestArmConstraintSpecificationSolve(t *testing.T) { fs := frame.NewEmptyFrameSystem("") x, err := frame.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "") @@ -824,26 +795,25 @@ func TestMovementWithGripper(t *testing.T) { // TODO(rb): move these tests to a separate repo eventually, as they take up too much time for general CI pipeline t.Skip() - // setup solverFrame and planning query + // setup frame system and planning query fs := makeTestFS(t) fs.RemoveFrame(fs.Frame("urOffset")) - sf, err := newSolverFrame(fs, "xArmVgripper", frame.World, frame.StartPositions(fs)) - test.That(t, err, test.ShouldBeNil) goal := spatialmath.NewPose(r3.Vector{500, 0, -300}, &spatialmath.OrientationVector{OZ: -1}) - zeroPosition := sf.sliceToMap(make([]frame.Input, len(sf.DoF()))) + startConfig := frame.StartPositions(fs) + + // linearly plan with the gripper + motionConfig := map[string]interface{}{ + "motion_profile": LinearMotionProfile, + } request := &PlanRequest{ Logger: logger, - StartConfiguration: zeroPosition, + StartConfiguration: startConfig, Goal: frame.NewPoseInFrame(frame.World, goal), + Frame: fs.Frame("xArmVgripper"), + FrameSystem: fs, + Options: motionConfig, } - - // linearly plan with the gripper - motionConfig := make(map[string]interface{}) - motionConfig["motion_profile"] = LinearMotionProfile - sfPlanner, err := newPlanManager(sf, logger, 1) - test.That(t, err, test.ShouldBeNil) - request.Options = motionConfig - solution, err := sfPlanner.PlanSingleWaypoint(context.Background(), request, nil) + solution, err := PlanMotion(context.Background(), request) test.That(t, err, test.ShouldBeNil) test.That(t, solution, test.ShouldNotBeNil) @@ -855,39 +825,29 @@ func TestMovementWithGripper(t *testing.T) { nil, ) test.That(t, err, test.ShouldBeNil) - sfPlanner, err = newPlanManager(sf, logger, 1) - test.That(t, err, test.ShouldBeNil) request.WorldState = worldState - solution, err = sfPlanner.PlanSingleWaypoint(context.Background(), request, nil) + solution, err = PlanMotion(context.Background(), request) test.That(t, err, test.ShouldBeNil) test.That(t, solution, test.ShouldNotBeNil) // plan with end of arm with gripper attached - this will fail - sf, err = newSolverFrame(fs, "xArm6", frame.World, frame.StartPositions(fs)) - test.That(t, err, test.ShouldBeNil) + request.Frame = fs.Frame("xArm6") goal = spatialmath.NewPose(r3.Vector{500, 0, -100}, &spatialmath.OrientationVector{OZ: -1}) request.Goal = frame.NewPoseInFrame(frame.World, goal) - sfPlanner, err = newPlanManager(sf, logger, 1) - test.That(t, err, test.ShouldBeNil) - _, err = sfPlanner.PlanSingleWaypoint(context.Background(), request, nil) + _, err = PlanMotion(context.Background(), request) test.That(t, err, test.ShouldNotBeNil) // remove linear constraint and try again - sfPlanner, err = newPlanManager(sf, logger, 1) - test.That(t, err, test.ShouldBeNil) request.Options = nil - solution, err = sfPlanner.PlanSingleWaypoint(context.Background(), request, nil) + solution, err = PlanMotion(context.Background(), request) test.That(t, err, test.ShouldBeNil) test.That(t, solution, test.ShouldNotBeNil) // remove gripper and try with linear constraint fs.RemoveFrame(fs.Frame("xArmVgripper")) - sf, err = newSolverFrame(fs, "xArm6", frame.World, frame.StartPositions(fs)) - test.That(t, err, test.ShouldBeNil) - sfPlanner, err = newPlanManager(sf, logger, 1) - test.That(t, err, test.ShouldBeNil) + request.Frame = fs.Frame("xArm6") request.Options = motionConfig - solution, err = sfPlanner.PlanSingleWaypoint(context.Background(), request, nil) + solution, err = PlanMotion(context.Background(), request) test.That(t, err, test.ShouldBeNil) test.That(t, solution, test.ShouldNotBeNil) } @@ -1064,7 +1024,11 @@ func TestPtgPosOnlyBidirectional(t *testing.T) { // If bidirectional planning worked properly, this plan should wind up at the goal with an orientation of Theta = 180 degrees bidirectionalPlan, err := planToTpspaceRec(bidirectionalPlanRaw, kinematicFrame) test.That(t, err, test.ShouldBeNil) - test.That(t, spatialmath.PoseAlmostCoincidentEps(goal, bidirectionalPlan[len(bidirectionalPlan)-1].Pose(), 5), test.ShouldBeTrue) + test.That(t, spatialmath.PoseAlmostCoincidentEps( + goal, + bidirectionalPlan[len(bidirectionalPlan)-1].Poses()[kinematicFrame.Name()].Pose(), + 5, + ), test.ShouldBeTrue) } func TestValidatePlanRequest(t *testing.T) { diff --git a/motionplan/nearestNeighbor.go b/motionplan/nearestNeighbor.go index 5025f545cf4..d89aefb9214 100644 --- a/motionplan/nearestNeighbor.go +++ b/motionplan/nearestNeighbor.go @@ -8,8 +8,6 @@ import ( "sort" "go.viam.com/utils" - - "go.viam.com/rdk/motionplan/ik" ) const defaultNeighborsBeforeParallelization = 1000 @@ -35,10 +33,7 @@ func kNearestNeighbors(planOpts *plannerOptions, tree rrtMap, target node, neigh allCosts := make([]*neighbor, 0) for rrtnode := range tree { - dist := planOpts.DistanceFunc(&ik.Segment{ - StartConfiguration: target.Q(), - EndConfiguration: rrtnode.Q(), - }) + dist := planOpts.nodeDistanceFunc(target, rrtnode) allCosts = append(allCosts, &neighbor{dist: dist, node: rrtnode}) } // sort neighbors by their distance to target first so that first nearest neighbor isn't always the start node of tree @@ -82,17 +77,7 @@ func (nm *neighborManager) nearestNeighbor( bestDist := math.Inf(1) var best node for k := range tree { - seg := &ik.Segment{ - StartConfiguration: seed.Q(), - EndConfiguration: k.Q(), - } - if pose := seed.Pose(); pose != nil { - seg.StartPosition = pose - } - if pose := k.Pose(); pose != nil { - seg.EndPosition = pose - } - dist := planOpts.DistanceFunc(seg) + dist := planOpts.nodeDistanceFunc(seed, k) if dist < bestDist { bestDist = dist best = k @@ -161,17 +146,7 @@ func (nm *neighborManager) nnWorker(ctx context.Context, planOpts *plannerOption default: } - seg := &ik.Segment{ - StartConfiguration: nm.seedPos.Q(), - EndConfiguration: candidate.Q(), - } - if pose := nm.seedPos.Pose(); pose != nil { - seg.StartPosition = pose - } - if pose := candidate.Pose(); pose != nil { - seg.EndPosition = pose - } - dist := planOpts.DistanceFunc(seg) + dist := planOpts.nodeDistanceFunc(nm.seedPos, candidate) if dist < bestDist { bestDist = dist best = candidate diff --git a/motionplan/nearestNeighbor_test.go b/motionplan/nearestNeighbor_test.go index c3a5996ff6a..6b580439e59 100644 --- a/motionplan/nearestNeighbor_test.go +++ b/motionplan/nearestNeighbor_test.go @@ -17,29 +17,29 @@ func TestNearestNeighbor(t *testing.T) { nm := &neighborManager{nCPU: 2, parallelNeighbors: 1000} rrtMap := map[node]node{} - j := &basicNode{q: []referenceframe.Input{{0.0}}} + j := &basicNode{q: map[string][]referenceframe.Input{"": {{0.0}}}} // We add ~110 nodes to the set of candidates. This is smaller than the configured // `parallelNeighbors` or 1000 meaning the `nearestNeighbor` call will be evaluated in series. for i := 1.0; i < 110.0; i++ { - iSol := &basicNode{q: []referenceframe.Input{{i}}} + iSol := &basicNode{q: map[string][]referenceframe.Input{"": {{i}}}} rrtMap[iSol] = j j = iSol } ctx := context.Background() - seed := []referenceframe.Input{{23.1}} - opt := newBasicPlannerOptions(referenceframe.NewZeroStaticFrame("test-frame")) + seed := map[string][]referenceframe.Input{"": {{23.1}}} + opt := newBasicPlannerOptions() nn := nm.nearestNeighbor(ctx, opt, &basicNode{q: seed}, rrtMap) - test.That(t, nn.Q()[0].Value, test.ShouldAlmostEqual, 23.0) + test.That(t, nn.Q()[""][0].Value, test.ShouldAlmostEqual, 23.0) // We add more nodes to trip the 1000 threshold. The `nearestNeighbor` call will use `nCPU` (2) // goroutines for evaluation. for i := 120.0; i < 1100.0; i++ { - iSol := &basicNode{q: []referenceframe.Input{{i}}} + iSol := &basicNode{q: map[string][]referenceframe.Input{"": {{i}}}} rrtMap[iSol] = j j = iSol } - seed = []referenceframe.Input{{723.6}} + seed = map[string][]referenceframe.Input{"": {{723.6}}} nn = nm.nearestNeighbor(ctx, opt, &basicNode{q: seed}, rrtMap) - test.That(t, nn.Q()[0].Value, test.ShouldAlmostEqual, 724.0) + test.That(t, nn.Q()[""][0].Value, test.ShouldAlmostEqual, 724.0) } diff --git a/motionplan/plan.go b/motionplan/plan.go index 8efbbb87d8f..974d69c1d2f 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -96,18 +96,19 @@ func (traj Trajectory) String() string { } // EvaluateCost calculates a cost to a trajectory as measured by the given distFunc Metric. -func (traj Trajectory) EvaluateCost(distFunc ik.SegmentMetric) float64 { +func (traj Trajectory) EvaluateCost(distFunc ik.SegmentFSMetric) float64 { var totalCost float64 last := map[string][]referenceframe.Input{} - for _, step := range traj { - for frame, inputs := range step { - if len(inputs) > 0 { - if lastInputs, ok := last[frame]; ok { - cost := distFunc(&ik.Segment{StartConfiguration: lastInputs, EndConfiguration: inputs}) - totalCost += cost - } - last[frame] = inputs - } + for i, step := range traj { + if i != 0 { + cost := distFunc(&ik.SegmentFS{ + StartConfiguration: last, + EndConfiguration: step, + }) + totalCost += cost + } + for k, v := range step { + last[k] = v } } return totalCost @@ -117,13 +118,12 @@ func (traj Trajectory) EvaluateCost(distFunc ik.SegmentMetric) float64 { // The pose of the PathStep is the pose at the end of the corresponding set of inputs in the Trajectory. type Path []PathStep -func newPath(solution []node, sf *solverFrame) (Path, error) { +func newPath(solution []node, fs referenceframe.FrameSystem) (Path, error) { path := make(Path, 0, len(solution)) - for _, step := range solution { - inputMap := sf.sliceToMap(step.Q()) + for _, inputNode := range solution { poseMap := make(map[string]*referenceframe.PoseInFrame) - for frame := range inputMap { - tf, err := sf.fss.Transform(inputMap, referenceframe.NewPoseInFrame(frame, spatialmath.NewZeroPose()), referenceframe.World) + for frame := range inputNode.Q() { + tf, err := fs.Transform(inputNode.Q(), referenceframe.NewPoseInFrame(frame, spatialmath.NewZeroPose()), referenceframe.World) if err != nil { return nil, err } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 900500f530a..3ad14b97788 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -35,146 +35,77 @@ const ( // motionplan.PlanMotion() -> SolvableFrameSystem.SolveWaypointsWithOptions() -> planManager.planSingleWaypoint(). type planManager struct { *planner - frame *solverFrame activeBackgroundWorkers sync.WaitGroup - useTPspace bool + useTPspace bool + ptgFrameName string } func newPlanManager( - frame *solverFrame, + fss referenceframe.FrameSystem, logger logging.Logger, seed int, ) (*planManager, error) { //nolint: gosec - p, err := newPlanner(frame, rand.New(rand.NewSource(int64(seed))), logger, newBasicPlannerOptions(frame)) + p, err := newPlanner(fss, rand.New(rand.NewSource(int64(seed))), logger, nil) if err != nil { return nil, err } - return &planManager{planner: p, frame: frame, useTPspace: len(frame.PTGSolvers()) > 0}, nil -} -// PlanMultiWaypoint plans a motion through multiple waypoints, using identical constraints for each -// Unlike PlanSingleWaypoint, this does not break up individual. -func (pm *planManager) PlanMultiWaypoint(ctx context.Context, request *PlanRequest, goals []spatialmath.Pose) (Plan, error) { - if pm.useTPspace { - return nil, errors.New("TPspace does not support multi-waypoint planning") - } + return &planManager{ + planner: p, + }, nil +} +// PlanSingleWaypoint will solve the solver frame to one individual pose. If you have multiple waypoints to hit, call this multiple times. +// Any constraints, etc, will be held for the entire motion. +func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { if request.StartPose != nil { - request.Logger.Warn("plan request passed a start pose, but non-relative plans will use the pose from transforming StartConfiguration") - } - seed, err := pm.frame.mapToSlice(request.StartConfiguration) - if err != nil { - return nil, err - } - startPose, err := pm.frame.Transform(seed) - if err != nil { - return nil, err - } - - var cancel func() - - request.Logger.CInfof(ctx, - "planning motion for frame %s\nGoal: %v\nStarting seed map %v\n, startPose %v\n, worldstate: %v\n", - request.Frame.Name(), - referenceframe.PoseInFrameToProtobuf(request.Goal), - request.StartConfiguration, - spatialmath.PoseToProtobuf(startPose), - request.WorldState.String(), - ) - - // set timeout for entire planning process if specified - if timeout, ok := request.Options["timeout"].(float64); ok { - ctx, cancel = context.WithTimeout(ctx, time.Duration(timeout*float64(time.Second))) - } - if cancel != nil { - defer cancel() - } - - goalPos := request.Goal.Pose() - // If we are world rooted, translate the goal pose into the world frame - if pm.frame.worldRooted { - tf, err := pm.frame.fss.Transform(request.StartConfiguration, request.Goal, referenceframe.World) - if err != nil { - return nil, err - } - goalPos = tf.(*referenceframe.PoseInFrame).Pose() + request.Logger.Warn( + "plan request passed a start pose, but non-relative plans will use the pose from transforming StartConfiguration", + ) } - goals = append(goals, goalPos) + startPoses := PathStep{} - planners := make([]motionPlanner, len(goals)) - for i, goal := range goals { - opt, err := pm.plannerSetupFromMoveRequest( - startPose, - goal, + // Use frame system to get configuration and transform + for frame, goal := range request.allGoals { + startPose, err := pm.fss.Transform( request.StartConfiguration, - request.WorldState, - request.BoundingRegions, - request.Constraints, - request.Options, + referenceframe.NewPoseInFrame(frame, spatialmath.NewZeroPose()), + goal.Parent(), ) if err != nil { return nil, err } - opt.SetGoal(goal) + startPoses[frame] = startPose.(*referenceframe.PoseInFrame) - //nolint: gosec - pathPlanner, err := opt.PlannerConstructor( - pm.frame, - rand.New(rand.NewSource(int64(pm.randseed.Int()))), - pm.logger, - opt, + request.Logger.CInfof(ctx, + "planning motion for frame %s\nGoal: %v\nStarting seed map %v\n, startPose %v\n, worldstate: %v\n", + request.Frame.Name(), + referenceframe.PoseInFrameToProtobuf(goal), + request.StartConfiguration, + referenceframe.PoseInFrameToProtobuf(startPose.(*referenceframe.PoseInFrame)), + request.WorldState.String(), ) - if err != nil { - return nil, err - } - planners[i] = pathPlanner - - startPose = goal // Update startPose for the next iteration } - - plan, err := pm.planAtomicWaypoints(ctx, goals, seed, planners, nil) - pm.activeBackgroundWorkers.Wait() + opt, err := pm.plannerSetupFromMoveRequest( + startPoses, + request.allGoals, + request.StartConfiguration, + request.WorldState, + request.BoundingRegions, + request.Constraints, + request.Options, + ) if err != nil { - return nil, fmt.Errorf("failed to plan path for valid goals: %w", err) + return nil, err } - - return plan, nil -} - -// PlanSingleWaypoint will solve the solver frame to one individual pose. If you have multiple waypoints to hit, call this multiple times. -// Any constraints, etc, will be held for the entire motion. -func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { if pm.useTPspace { return pm.planRelativeWaypoint(ctx, request, seedPlan) } - if request.StartPose != nil { - request.Logger.Warn("plan request passed a start pose, but non-relative plans will use the pose from transforming StartConfiguration") - } - seed, err := pm.frame.mapToSlice(request.StartConfiguration) - if err != nil { - return nil, err - } - startPose, err := pm.frame.Transform(seed) - if err != nil { - return nil, err - } - goalPos := request.Goal.Pose() - - var cancel func() - - request.Logger.CInfof(ctx, - "planning motion for frame %s\nGoal: %v\nStarting seed map %v\n, startPose %v\n, worldstate: %v\n", - request.Frame.Name(), - referenceframe.PoseInFrameToProtobuf(request.Goal), - request.StartConfiguration, - spatialmath.PoseToProtobuf(startPose), - request.WorldState.String(), - ) - // set timeout for entire planning process if specified + var cancel func() if timeout, ok := request.Options["timeout"].(float64); ok { ctx, cancel = context.WithTimeout(ctx, time.Duration(timeout*float64(time.Second))) } @@ -182,18 +113,24 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ defer cancel() } - // If we are world rooted, translate the goal pose into the world frame - if pm.frame.worldRooted { - tf, err := pm.frame.fss.Transform(request.StartConfiguration, request.Goal, referenceframe.World) - if err != nil { - return nil, err + // Transform goal poses into world frame if needed + alteredGoals := PathStep{} + for _, chain := range opt.motionChains { + if chain.worldRooted { + tf, err := pm.fss.Transform(request.StartConfiguration, request.Goal, referenceframe.World) + if err != nil { + return nil, err + } + alteredGoals[chain.solveFrameName] = tf.(*referenceframe.PoseInFrame) + } else { + alteredGoals[chain.solveFrameName] = request.allGoals[chain.solveFrameName] } - goalPos = tf.(*referenceframe.PoseInFrame).Pose() } - var goals []spatialmath.Pose + var goals []PathStep var opts []*plannerOptions + // TODO: This is a necessary optimization for linear motion to be performant subWaypoints := false // linear motion profile has known intermediate points, so solving can be broken up and sped up @@ -215,14 +152,26 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ if !ok { pathStepSize = defaultPathStepSize } - numSteps := PathStepCount(startPose, goalPos, pathStepSize) - from := startPose + numSteps := 0 + for frame, pif := range alteredGoals { + // Calculate steps needed for this frame + steps := PathStepCount(startPoses[frame].Pose(), pif.Pose(), pathStepSize) + if steps > numSteps { + numSteps = steps + } + } + + from := startPoses for i := 1; i < numSteps; i++ { by := float64(i) / float64(numSteps) - to := spatialmath.Interpolate(startPose, goalPos, by) + to := PathStep{} + for frame, pif := range alteredGoals { + toPose := spatialmath.Interpolate(startPoses[frame].Pose(), pif.Pose(), by) + to[frame] = referenceframe.NewPoseInFrame(pif.Parent(), toPose) + } goals = append(goals, to) - opt, err := pm.plannerSetupFromMoveRequest( + wpOpt, err := pm.plannerSetupFromMoveRequest( from, to, request.StartConfiguration, @@ -234,29 +183,29 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ if err != nil { return nil, err } - opt.SetGoal(to) - opts = append(opts, opt) + wpOpt.setGoal(to) + opts = append(opts, wpOpt) from = to } - startPose = from - } - goals = append(goals, goalPos) - opt, err := pm.plannerSetupFromMoveRequest( - startPose, - goalPos, - request.StartConfiguration, - request.WorldState, - request.BoundingRegions, - request.Constraints, - request.Options, - ) - if err != nil { - return nil, err + // Update opt to be just the last step + opt, err = pm.plannerSetupFromMoveRequest( + from, + request.allGoals, + request.StartConfiguration, + request.WorldState, + request.BoundingRegions, + request.Constraints, + request.Options, + ) + if err != nil { + return nil, err + } } pm.planOpts = opt - opt.SetGoal(goalPos) + opt.setGoal(alteredGoals) opts = append(opts, opt) + goals = append(goals, alteredGoals) planners := make([]motionPlanner, 0, len(opts)) // Set up planners for later execution @@ -264,7 +213,7 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ // Build planner //nolint: gosec pathPlanner, err := opt.PlannerConstructor( - pm.frame, + pm.fss, rand.New(rand.NewSource(int64(pm.randseed.Int()))), pm.logger, opt, @@ -278,13 +227,13 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ // If we have multiple sub-waypoints, make sure the final goal is not unreachable. if len(goals) > 1 { // Viability check; ensure that the waypoint is not impossible to reach - _, err = planners[0].getSolutions(ctx, seed) + _, err = planners[0].getSolutions(ctx, request.StartConfiguration) if err != nil { return nil, err } } - plan, err := pm.planAtomicWaypoints(ctx, goals, seed, planners, seedPlan) + plan, err := pm.planAtomicWaypoints(ctx, goals, request.StartConfiguration, planners, seedPlan) pm.activeBackgroundWorkers.Wait() if err != nil { if len(goals) > 1 { @@ -300,8 +249,8 @@ func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequ // intermediate waypoint. Waypoints here refer to points that the software has generated to. func (pm *planManager) planAtomicWaypoints( ctx context.Context, - goals []spatialmath.Pose, - seed []referenceframe.Input, + goals []PathStep, + seed map[string][]referenceframe.Input, planners []motionPlanner, seedPlan Plan, ) (Plan, error) { @@ -312,7 +261,7 @@ func (pm *planManager) planAtomicWaypoints( // try to solve each goal, one at a time for i, goal := range goals { - pm.logger.Debug("start planning for ", spatialmath.PoseToProtobuf(goal)) + pm.logger.Debug("start planning for ", goal.ToProto()) // Check if ctx is done between each waypoint select { case <-ctx.Done(): @@ -345,22 +294,22 @@ func (pm *planManager) planAtomicWaypoints( if err != nil { return nil, err } - pm.logger.Debug("completed planning for ", spatialmath.PoseToProtobuf(goals[i])) + pm.logger.Debug("completed planning for ", goals[i].ToProto()) resultSlices = append(resultSlices, steps...) } - return newRRTPlan(resultSlices, pm.frame, pm.useTPspace) + return newRRTPlan(resultSlices, pm.fss, pm.useTPspace, nil) } // planSingleAtomicWaypoint attempts to plan a single waypoint. It may optionally be pre-seeded with rrt maps; these will be passed to the // planner if supported, or ignored if not. func (pm *planManager) planSingleAtomicWaypoint( ctx context.Context, - goal spatialmath.Pose, - seed []referenceframe.Input, + goal PathStep, + seed map[string][]referenceframe.Input, pathPlanner motionPlanner, maps *rrtMaps, -) ([]referenceframe.Input, *resultPromise, error) { +) (map[string][]referenceframe.Input, *resultPromise, error) { if parPlan, ok := pathPlanner.(rrtParallelPlanner); ok { // rrtParallelPlanner supports solution look-ahead for parallel waypoint solving // This will set that up, and if we get a result on `endpointPreview`, then the next iteration will be started, and the steps @@ -407,8 +356,8 @@ func (pm *planManager) planSingleAtomicWaypoint( // as necessary. func (pm *planManager) planParallelRRTMotion( ctx context.Context, - goal spatialmath.Pose, - seed []referenceframe.Input, + goal PathStep, + seed map[string][]referenceframe.Input, pathPlanner rrtParallelPlanner, endpointPreview chan node, solutionChan chan *rrtSolution, @@ -473,7 +422,7 @@ func (pm *planManager) planParallelRRTMotion( if pathPlanner.opt().Fallback != nil { //nolint: gosec fallbackPlanner, err = pathPlanner.opt().Fallback.PlannerConstructor( - pm.frame, + pm.fss, rand.New(rand.NewSource(int64(pm.randseed.Int()))), pm.logger, pathPlanner.opt().Fallback, @@ -526,8 +475,9 @@ func (pm *planManager) planParallelRRTMotion( // Receive the newly smoothed path from our original solve, and score it finalSteps.steps = <-smoothChan score := math.Inf(1) + if finalSteps.steps != nil { - score = pm.frame.nodesToTrajectory(finalSteps.steps).EvaluateCost(pm.opt().ScoreFunc) + score = nodesToTrajectory(finalSteps.steps).EvaluateCost(pm.opt().scoreFunc) } // If we ran a fallback, retrieve the result and compare to the smoothed path @@ -536,7 +486,7 @@ func (pm *planManager) planParallelRRTMotion( if err == nil { // If the fallback successfully found a path, check if it is better than our smoothed previous path. // The fallback should emerge pre-smoothed, so that should be a non-issue - altCost := pm.frame.nodesToTrajectory(alternate).EvaluateCost(pm.opt().ScoreFunc) + altCost := nodesToTrajectory(alternate).EvaluateCost(pm.opt().scoreFunc) if altCost < score { pm.logger.CDebugf(ctx, "replacing path with score %f with better score %f", score, altCost) finalSteps = &rrtSolution{steps: alternate} @@ -558,22 +508,23 @@ func (pm *planManager) planParallelRRTMotion( // This is where the map[string]interface{} passed in via `extra` is used to decide how planning happens. func (pm *planManager) plannerSetupFromMoveRequest( - from, to spatialmath.Pose, + from, to PathStep, seedMap map[string][]referenceframe.Input, worldState *referenceframe.WorldState, boundingRegions []spatialmath.Geometry, constraints *Constraints, planningOpts map[string]interface{}, ) (*plannerOptions, error) { + if constraints == nil { + // Constraints may be nil, but if a motion profile is set in planningOpts we need it to be a valid pointer to an empty struct. + constraints = &Constraints{} + } planAlg := "" - // This will adjust the goal position to make movements more intuitive when using incrementation near poles - to = fixOvIncrement(to, from) - // Start with normal options - opt := newBasicPlannerOptions(pm.frame) + opt := newBasicPlannerOptions() opt.extra = planningOpts - opt.StartPose = from + opt.startPoses = from collisionBufferMM := defaultCollisionBufferMM collisionBufferMMRaw, ok := planningOpts["collision_buffer_mm"] @@ -587,35 +538,70 @@ func (pm *planManager) plannerSetupFromMoveRequest( } } - // extract inputs corresponding to the frame - frameInputs, err := pm.frame.mapToSlice(seedMap) + err := opt.fillMotionChains(pm.fss, to) if err != nil { return nil, err } - - // create robot collision entities - movingGeometriesInFrame, err := pm.frame.Geometries(frameInputs) - if err != nil { - return nil, err // no geometries defined for frame + if len(opt.motionChains) < 1 { + return nil, errors.New("must have at least one motion chain") + } + // create motion chains for each goal, and error check for PTG frames + // TODO: currently, if any motion chain has a PTG frame, that must be the only motion chain and that frame must be the only + // frame in the chain with nonzero DoF. Eventually this need not be the case. + pm.useTPspace = false + for _, chain := range opt.motionChains { + for _, movingFrame := range chain.frames { + if _, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { + if pm.useTPspace { + return nil, errors.New("only one PTG frame can be planned for at a time") + } + if len(opt.motionChains) > 1 { + return nil, errMixedFrameTypes + } + pm.useTPspace = true + pm.ptgFrameName = movingFrame.Name() + } else if len(movingFrame.DoF()) > 0 { + if pm.useTPspace { + return nil, errMixedFrameTypes + } + } + } } - movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World + if pm.useTPspace { + opt.Resolution = defaultPTGCollisionResolution + } + + movingRobotGeometries := []spatialmath.Geometry{} // find all geometries that are not moving but are in the frame system - staticRobotGeometries := make([]spatialmath.Geometry, 0) - frameSystemGeometries, err := referenceframe.FrameSystemGeometries(pm.frame.fss, seedMap) + staticRobotGeometries := []spatialmath.Geometry{} + frameSystemGeometries, err := referenceframe.FrameSystemGeometries(pm.fss, seedMap) if err != nil { return nil, err } for name, geometries := range frameSystemGeometries { - if !pm.frame.movingFrame(name) { - staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) + moving := false + for _, chain := range opt.motionChains { + if chain.movingFS.Frame(name) != nil { + moving = true + movingRobotGeometries = append(movingRobotGeometries, geometries.Geometries()...) + break + } + } + if !moving { + // Non-motion-chain frames with nonzero DoF can still move out of the way + if len(pm.fss.Frame(name).DoF()) > 0 { + movingRobotGeometries = append(movingRobotGeometries, geometries.Geometries()...) + } else { + staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) + } } } // Note that all obstacles in worldState are assumed to be static so it is ok to transform them into the world frame // TODO(rb) it is bad practice to assume that the current inputs of the robot correspond to the passed in world state // the state that observed the worldState should ultimately be included as part of the worldState message - worldGeometries, err := worldState.ObstaclesInWorldFrame(pm.frame.fss, seedMap) + worldGeometries, err := worldState.ObstaclesInWorldFrame(pm.fss, seedMap) if err != nil { return nil, err } @@ -626,7 +612,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( } // add collision constraints - collisionConstraints, err := createAllCollisionConstraints( + fsCollisionConstraints, stateCollisionConstraints, err := createAllCollisionConstraints( movingRobotGeometries, staticRobotGeometries, worldGeometries.Geometries(), @@ -637,11 +623,13 @@ func (pm *planManager) plannerSetupFromMoveRequest( if err != nil { return nil, err } - for name, constraint := range collisionConstraints { + // For TPspace + for name, constraint := range stateCollisionConstraints { opt.AddStateConstraint(name, constraint) } - - hasTopoConstraint := opt.addPbTopoConstraints(from, to, constraints) + for name, constraint := range fsCollisionConstraints { + opt.AddStateFSConstraint(name, constraint) + } // error handling around extracting motion_profile information from map[string]interface{} var motionProfile string @@ -653,6 +641,50 @@ func (pm *planManager) plannerSetupFromMoveRequest( } } + opt.profile = FreeMotionProfile + + switch motionProfile { + case LinearMotionProfile: + opt.profile = LinearMotionProfile + // Linear constraints + linTol, ok := planningOpts["line_tolerance"].(float64) + if !ok { + // Default + linTol = defaultLinearDeviation + } + orientTol, ok := planningOpts["orient_tolerance"].(float64) + if !ok { + // Default + orientTol = defaultOrientationDeviation + } + constraints.AddLinearConstraint(LinearConstraint{linTol, orientTol}) + case PseudolinearMotionProfile: + opt.profile = PseudolinearMotionProfile + tolerance, ok := planningOpts["tolerance"].(float64) + if !ok { + // Default + tolerance = defaultPseudolinearTolerance + } + constraints.AddPseudolinearConstraint(PseudolinearConstraint{tolerance, tolerance}) + case OrientationMotionProfile: + opt.profile = OrientationMotionProfile + tolerance, ok := planningOpts["tolerance"].(float64) + if !ok { + // Default + tolerance = defaultOrientationDeviation + } + constraints.AddOrientationConstraint(OrientationConstraint{tolerance}) + case PositionOnlyMotionProfile: + opt.profile = PositionOnlyMotionProfile + if !pm.useTPspace || opt.PositionSeeds <= 0 { + opt.goalMetricConstructor = ik.NewPositionOnlyMetric + } + } + + hasTopoConstraint, err := opt.addPbTopoConstraints(pm.fss, seedMap, from, to, constraints) + if err != nil { + return nil, err + } // convert map to json, then to a struct, overwriting present defaults jsonString, err := json.Marshal(planningOpts) if err != nil { @@ -693,63 +725,18 @@ func (pm *planManager) plannerSetupFromMoveRequest( if pm.useTPspace { // overwrite default with TP space opt.PlannerConstructor = newTPSpaceMotionPlanner - // Distances are computed in cartesian space rather than configuration space - opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(defaultTPspaceOrientationScale) + + // Distances are computed in cartesian space rather than configuration space. + opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(defaultTPspaceOrientationScale) // If we have PTGs, then we calculate distances using the PTG-specific distance function. // Otherwise we just use squared norm on inputs. - opt.ScoreFunc = tpspace.PTGSegmentMetric + opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{pm.ptgFrameName}) planAlg = "tpspace" opt.relativeInputs = true } - switch motionProfile { - case LinearMotionProfile: - opt.profile = LinearMotionProfile - // Linear constraints - linTol, ok := planningOpts["line_tolerance"].(float64) - if !ok { - // Default - linTol = defaultLinearDeviation - } - orientTol, ok := planningOpts["orient_tolerance"].(float64) - if !ok { - // Default - orientTol = defaultOrientationDeviation - } - constraint, pathMetric := NewAbsoluteLinearInterpolatingConstraint(from, to, linTol, orientTol) - opt.AddStateConstraint(defaultLinearConstraintDesc, constraint) - opt.pathMetric = pathMetric - case PseudolinearMotionProfile: - opt.profile = PseudolinearMotionProfile - tolerance, ok := planningOpts["tolerance"].(float64) - if !ok { - // Default - tolerance = defaultPseudolinearTolerance - } - constraint, pathMetric := NewProportionalLinearInterpolatingConstraint(from, to, tolerance) - opt.AddStateConstraint(defaultPseudolinearConstraintDesc, constraint) - opt.pathMetric = pathMetric - case OrientationMotionProfile: - opt.profile = OrientationMotionProfile - tolerance, ok := planningOpts["tolerance"].(float64) - if !ok { - // Default - tolerance = defaultOrientationDeviation - } - constraint, pathMetric := NewSlerpOrientationConstraint(from, to, tolerance) - opt.AddStateConstraint(defaultOrientationConstraintDesc, constraint) - opt.pathMetric = pathMetric - case PositionOnlyMotionProfile: - opt.profile = PositionOnlyMotionProfile - if !pm.useTPspace || opt.PositionSeeds <= 0 { - opt.goalMetricConstructor = ik.NewPositionOnlyMetric - } - case FreeMotionProfile: - // No restrictions on motion - fallthrough - default: - opt.profile = FreeMotionProfile + if opt.profile == FreeMotionProfile || opt.profile == PositionOnlyMotionProfile { if planAlg == "" { // set up deep copy for fallback try1 := deepAtomicCopyMap(planningOpts) @@ -767,6 +754,7 @@ func (pm *planManager) plannerSetupFromMoveRequest( opt = try1Opt } } + return opt, nil } @@ -777,7 +765,7 @@ func (pm *planManager) goodPlan(pr *rrtSolution, opt *plannerOptions) (bool, flo if pr.maps.optNode.Cost() <= 0 { return true, solutionCost } - solutionCost = pm.frame.nodesToTrajectory(pr.steps).EvaluateCost(opt.ScoreFunc) + solutionCost = nodesToTrajectory(pr.steps).EvaluateCost(opt.scoreFunc) if solutionCost < pr.maps.optNode.Cost()*defaultOptimalityMultiple { return true, solutionCost } @@ -786,26 +774,28 @@ func (pm *planManager) goodPlan(pr *rrtSolution, opt *plannerOptions) (bool, flo return false, solutionCost } -func (pm *planManager) planToRRTGoalMap(plan Plan, goal spatialmath.Pose) (*rrtMaps, error) { - // TODO: make this work with any implementation of Plan - var rrt *rrtPlan - var ok bool - if rrt, ok = plan.(*rrtPlan); !ok { - return nil, errBadPlanImpl +func (pm *planManager) planToRRTGoalMap(plan Plan, goal PathStep) (*rrtMaps, error) { + traj := plan.Trajectory() + path := plan.Path() + if len(traj) != len(path) { + return nil, errors.New("plan trajectory and path should be the same length") + } + planNodes := make([]node, 0, len(traj)) + for i, tConf := range traj { + planNodes = append(planNodes, &basicNode{q: tConf, poses: path[i]}) } - planNodes := rrt.nodes if pm.useTPspace { // Fill in positions from the old origin to where the goal was during the last run - planNodesOld, err := rectifyTPspacePath(planNodes, pm.frame, spatialmath.NewZeroPose()) + planNodesOld, err := rectifyTPspacePath(planNodes, pm.fss.Frame(pm.ptgFrameName), spatialmath.NewZeroPose()) if err != nil { return nil, err } // Figure out where our new starting point is relative to our last one, and re-rectify using the new adjusted location - oldGoal := planNodesOld[len(planNodesOld)-1].Pose() - pathDiff := spatialmath.PoseBetween(oldGoal, goal) - planNodes, err = rectifyTPspacePath(planNodes, pm.frame, pathDiff) + oldGoal := planNodesOld[len(planNodesOld)-1].Poses()[pm.ptgFrameName].Pose() + pathDiff := spatialmath.PoseBetween(oldGoal, goal[pm.ptgFrameName].Pose()) + planNodes, err = rectifyTPspacePath(planNodes, pm.fss.Frame(pm.ptgFrameName), pathDiff) if err != nil { return nil, err } @@ -816,12 +806,10 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal spatialmath.Pose) (*rrtM for i := len(planNodes) - 1; i >= 0; i-- { if i != 0 { // Fill in costs - cost := pm.opt().DistanceFunc(&ik.Segment{ + cost := pm.opt().confDistanceFunc(&ik.SegmentFS{ StartConfiguration: planNodes[i-1].Q(), - StartPosition: planNodes[i-1].Pose(), EndConfiguration: planNodes[i].Q(), - EndPosition: planNodes[i].Pose(), - Frame: pm.frame, + FS: pm.fss, }) planNodes[i].SetCost(cost) } @@ -829,9 +817,8 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal spatialmath.Pose) (*rrtM lastNode = planNodes[i] } - startNode := &basicNode{q: make([]referenceframe.Input, len(pm.frame.DoF())), pose: spatialmath.NewZeroPose()} maps := &rrtMaps{ - startMap: map[node]node{startNode: nil}, + startMap: map[node]node{}, goalMap: goalMap, } @@ -841,22 +828,9 @@ func (pm *planManager) planToRRTGoalMap(plan Plan, goal spatialmath.Pose) (*rrtM // planRelativeWaypoint will solve the solver frame to one individual pose. This is used for solverframes whose inputs are relative, that // is, the pose returned by `Transform` is a transformation rather than an absolute position. func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { - anyNonzero := false // Whether non-PTG frames exist - for _, movingFrame := range pm.frame.frames { - if _, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { - continue - } else if len(movingFrame.DoF()) > 0 { - anyNonzero = true - } - if anyNonzero { - return nil, errors.New("cannot combine ptg with other nonzero DOF frames in a single planning call") - } - } - if request.StartPose == nil { return nil, errors.New("must provide a startPose if solving for PTGs") } - startPose := request.StartPose request.Logger.CInfof(ctx, @@ -871,7 +845,7 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe pm.logger.Debug("$type,X,Y") pm.logger.Debugf("$SG,%f,%f", startPose.Point().X, startPose.Point().Y) pm.logger.Debugf("$SG,%f,%f", request.Goal.Pose().Point().X, request.Goal.Pose().Point().Y) - gifs, err := request.WorldState.ObstaclesInWorldFrame(pm.frame.fss, request.StartConfiguration) + gifs, err := request.WorldState.ObstaclesInWorldFrame(pm.fss, request.StartConfiguration) if err == nil { for _, geom := range gifs.Geometries() { pts := geom.ToPoints(1.) @@ -893,32 +867,42 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe defer cancel() } - tf, err := pm.frame.fss.Transform(request.StartConfiguration, request.Goal, referenceframe.World) + startMap := map[string]*referenceframe.PoseInFrame{pm.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, startPose)} + + // Use frame name instead of Frame object + tf, err := pm.fss.Transform(request.StartConfiguration, request.Goal, referenceframe.World) if err != nil { return nil, err } - goalPos := tf.(*referenceframe.PoseInFrame).Pose() + goalPose := tf.(*referenceframe.PoseInFrame).Pose() + goalMap := map[string]*referenceframe.PoseInFrame{pm.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, goalPose)} opt, err := pm.plannerSetupFromMoveRequest( - startPose, goalPos, request.StartConfiguration, request.WorldState, request.BoundingRegions, request.Constraints, request.Options, + startMap, + goalMap, + request.StartConfiguration, + request.WorldState, + request.BoundingRegions, + request.Constraints, + request.Options, ) if err != nil { return nil, err } - // re-root the frame system on the relative frame - relativeOnlyFS, err := pm.frame.fss.FrameSystemSubset(request.Frame) + // Create frame system subset using frame name + relativeOnlyFS, err := pm.fss.FrameSystemSubset(pm.fss.Frame(request.Frame.Name())) if err != nil { return nil, err } - pm.frame.fss = relativeOnlyFS + pm.fss = relativeOnlyFS pm.planOpts = opt - opt.SetGoal(goalPos) + opt.setGoal(goalMap) // Build planner //nolint: gosec pathPlanner, err := opt.PlannerConstructor( - pm.frame, + pm.fss, rand.New(rand.NewSource(int64(pm.randseed.Int()))), pm.logger, opt, @@ -926,28 +910,33 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe if err != nil { return nil, err } - zeroInputs := make([]referenceframe.Input, len(pm.frame.DoF())) + zeroInputs := map[string][]referenceframe.Input{} + zeroInputs[pm.ptgFrameName] = make([]referenceframe.Input, len(pm.fss.Frame(pm.ptgFrameName).DoF())) maps := &rrtMaps{} if seedPlan != nil { - maps, err = pm.planToRRTGoalMap(seedPlan, goalPos) + // TODO: This probably needs to be flipped? Check if these paths are ever used. + maps, err = pm.planToRRTGoalMap(seedPlan, goalMap) if err != nil { return nil, err } } if pm.opt().PositionSeeds > 0 && pm.opt().profile == PositionOnlyMotionProfile { - err = maps.fillPosOnlyGoal(goalPos, pm.opt().PositionSeeds, len(pm.frame.DoF())) + err = maps.fillPosOnlyGoal(goalMap, pm.opt().PositionSeeds) if err != nil { return nil, err } } else { - goalNode := &basicNode{q: zeroInputs, pose: spatialmath.Compose(goalPos, flipPose)} + goalMapFlip := map[string]*referenceframe.PoseInFrame{ + pm.ptgFrameName: referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.Compose(goalPose, flipPose)), + } + goalNode := &basicNode{q: zeroInputs, poses: goalMapFlip} maps.goalMap = map[node]node{goalNode: nil} } - startNode := &basicNode{q: zeroInputs, pose: startPose} + startNode := &basicNode{q: zeroInputs, poses: startMap} maps.startMap = map[node]node{startNode: nil} // Plan the single waypoint, and accumulate objects which will be used to constrauct the plan after all planning has finished - _, future, err := pm.planSingleAtomicWaypoint(ctx, goalPos, zeroInputs, pathPlanner, maps) + _, future, err := pm.planSingleAtomicWaypoint(ctx, goalMap, zeroInputs, pathPlanner, maps) if err != nil { return nil, err } @@ -956,7 +945,7 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe return nil, err } - return newRRTPlan(steps, pm.frame, pm.useTPspace) + return newRRTPlan(steps, pm.fss, pm.useTPspace, startPose) } // Copy any atomic values. @@ -967,3 +956,11 @@ func deepAtomicCopyMap(opt map[string]interface{}) map[string]interface{} { } return optCopy } + +func nodesToTrajectory(nodes []node) Trajectory { + traj := make(Trajectory, 0, len(nodes)) + for _, n := range nodes { + traj = append(traj, n.Q()) + } + return traj +} diff --git a/motionplan/plan_test.go b/motionplan/plan_test.go index a46280244b5..45bf0b693cc 100644 --- a/motionplan/plan_test.go +++ b/motionplan/plan_test.go @@ -25,28 +25,27 @@ func TestEvaluateTrajectory(t *testing.T) { map[string][]referenceframe.Input{"": {{1.}, {2.}, {3.}}}, } // Test no change - score := plan.EvaluateCost(ik.L2InputMetric) + score := plan.EvaluateCost(ik.FSConfigurationL2Distance) test.That(t, score, test.ShouldAlmostEqual, 0) // Test L2 for "", and nothing for plan with only one entry plan = append(plan, map[string][]referenceframe.Input{"": {{4.}, {5.}, {6.}}, "test": {{2.}, {3.}, {4.}}}) - score = plan.EvaluateCost(ik.L2InputMetric) + score = plan.EvaluateCost(ik.FSConfigurationL2Distance) test.That(t, score, test.ShouldAlmostEqual, math.Sqrt(27)) // Test cumulative L2 after returning to original inputs plan = append(plan, map[string][]referenceframe.Input{"": {{1.}, {2.}, {3.}}}) - score = plan.EvaluateCost(ik.L2InputMetric) + score = plan.EvaluateCost(ik.FSConfigurationL2Distance) test.That(t, score, test.ShouldAlmostEqual, math.Sqrt(27)*2) // Test that the "test" inputs are properly evaluated after skipping a step plan = append(plan, map[string][]referenceframe.Input{"test": {{3.}, {5.}, {6.}}}) - score = plan.EvaluateCost(ik.L2InputMetric) + score = plan.EvaluateCost(ik.FSConfigurationL2Distance) test.That(t, score, test.ShouldAlmostEqual, math.Sqrt(27)*2+3) - // Evaluated with the tp-space metric, should be the sum of the distance values (third input) ignoring the first input set for each - // named input set - score = plan.EvaluateCost(tpspace.PTGSegmentMetric) - test.That(t, score, test.ShouldAlmostEqual, 18) + // Evaluated with the tp-space metric, should be the sum of the distance values (third input) ignoring the first input step + score = plan.EvaluateCost(tpspace.NewPTGDistanceMetric([]string{"", "test"})) + test.That(t, score, test.ShouldAlmostEqual, 22) } func TestPlanStep(t *testing.T) { diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index 0c699e04d80..9a6ca4405a4 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -3,10 +3,10 @@ package motionplan import ( + "math" "runtime" "go.viam.com/rdk/motionplan/ik" - "go.viam.com/rdk/motionplan/tpspace" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -83,32 +83,29 @@ const ( ) // NewBasicPlannerOptions specifies a set of basic options for the planner. -func newBasicPlannerOptions(frame referenceframe.Frame) *plannerOptions { +func newBasicPlannerOptions() *plannerOptions { opt := &plannerOptions{} opt.goalMetricConstructor = ik.NewSquaredNormMetric - opt.goalArcScore = ik.JointMetric - opt.DistanceFunc = ik.L2InputMetric - opt.ScoreFunc = ik.L2InputMetric - opt.pathMetric = ik.NewZeroMetric() // By default, the distance to the valid manifold is zero, unless constraints say otherwise + opt.confDistanceFunc = ik.FSConfigurationL2Distance + opt.poseDistanceFunc = ik.WeightedSquaredNormSegmentMetric + opt.nodeDistanceFunc = nodeConfigurationDistanceFunc + opt.scoreFunc = ik.FSConfigurationL2Distance + opt.pathMetric = ik.NewZeroFSMetric() // By default, the distance to the valid manifold is zero, unless constraints say otherwise // opt.goalMetric is intentionally unset as it is likely dependent on the goal itself. - // TODO: RSDK-6079 this should be properly used, and deduplicated with defaultEpsilon, JointSolveDist, etc. + // TODO: RSDK-6079 this should be properly used, and deduplicated with defaultEpsilon, InputIdentDist, etc. opt.GoalThreshold = 0.1 // Set defaults opt.MaxSolutions = defaultSolutionsToSeed opt.MinScore = defaultMinIkScore opt.Resolution = defaultResolution - if ptgFrame, isPTGframe := frame.(tpspace.PTGProvider); isPTGframe && len(ptgFrame.PTGSolvers()) > 0 { - opt.Resolution = defaultPTGCollisionResolution - } opt.Timeout = defaultTimeout opt.PositionSeeds = defaultTPspacePositionOnlySeeds opt.PlanIter = defaultPlanIter opt.FrameStep = defaultFrameStep - opt.JointSolveDist = defaultJointSolveDist + opt.InputIdentDist = defaultInputIdentDist opt.IterBeforeRand = defaultIterBeforeRand - opt.qstep = getFrameSteps(frame, defaultFrameStep) // Note the direct reference to a default here. // This is due to a Go compiler issue where it will incorrectly refuse to compile with a circular reference error if this @@ -125,10 +122,11 @@ func newBasicPlannerOptions(frame referenceframe.Frame) *plannerOptions { // plannerOptions are a set of options to be passed to a planner which will specify how to solve a motion planning problem. type plannerOptions struct { ConstraintHandler + motionChains []*motionChain goalMetricConstructor func(spatialmath.Pose) ik.StateMetric - goalMetric ik.StateMetric // Distance function which converges to the final goal position - goalArcScore ik.SegmentMetric - pathMetric ik.StateMetric // Distance function which converges on the valid manifold of intermediate path states + goalMetric ik.StateFSMetric // Distance function which converges to the final goal position + pathMetric ik.StateFSMetric // Distance function which converges on the valid manifold of intermediate path states + nodeDistanceFunc func(node, node) float64 // Node distance function used for nearest neighbor extra map[string]interface{} @@ -163,8 +161,8 @@ type plannerOptions struct { // The maximum percent of a joints range of motion to allow per step. FrameStep float64 `json:"frame_step"` - // If the dot product between two sets of joint angles is less than this, consider them identical. - JointSolveDist float64 `json:"joint_solve_dist"` + // If the dot product between two sets of inputs is less than this, consider them identical. + InputIdentDist float64 `json:"input_ident_dist"` // Number of iterations to mrun before beginning to accept randomly seeded locations. IterBeforeRand int `json:"iter_before_rand"` @@ -172,16 +170,16 @@ type plannerOptions struct { // Number of seeds to pre-generate for bidirectional position-only solving. PositionSeeds int `json:"position_seeds"` - // This is how far cbirrt will try to extend the map towards a goal per-step. Determined from FrameStep - qstep []float64 + startPoses PathStep // The starting poses of the plan. Useful when planning for frames with relative inputs. - StartPose spatialmath.Pose // The starting pose of the plan. Useful when planning for frames with relative inputs. + // poseDistanceFunc is the function that the planner will use to measure the degree of "closeness" between two poses + poseDistanceFunc ik.SegmentMetric - // DistanceFunc is the function that the planner will use to measure the degree of "closeness" between two states of the robot - DistanceFunc ik.SegmentMetric + // confDistanceFunc is the function that the planner will use to measure the degree of "closeness" between two states of the robot + confDistanceFunc ik.SegmentFSMetric - // ScoreFunc is the function that the planner will use to evaluate a plan for final cost comparisons. - ScoreFunc ik.SegmentMetric + // scoreFunc is the function that the planner will use to evaluate a plan for final cost comparisons. + scoreFunc ik.SegmentFSMetric // profile is the string representing the motion profile profile string @@ -195,13 +193,33 @@ type plannerOptions struct { relativeInputs bool } -// SetMetric sets the distance metric for the solver. -func (p *plannerOptions) SetGoal(goal spatialmath.Pose) { - p.goalMetric = p.goalMetricConstructor(goal) +// setGoal sets the distance metric for the solver. +func (p *plannerOptions) setGoal(goal PathStep) { + metrics := map[string]ik.StateMetric{} + for frame, goalInFrame := range goal { + metrics[frame] = p.goalMetricConstructor(goalInFrame.Pose()) + } + goalMetricFS := func(state *ik.StateFS) float64 { + score := 0. + for frame, goalMetric := range metrics { + poseParent := goal[frame].Parent() + currPose, err := state.FS.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), poseParent) + if err != nil { + score += math.Inf(1) + } + score += goalMetric(&ik.State{ + Position: currPose.(*referenceframe.PoseInFrame).Pose(), + Configuration: state.Configuration[frame], + Frame: state.FS.Frame(frame), + }) + } + return score + } + p.goalMetric = goalMetricFS } // SetPathDist sets the distance metric for the solver to move a constraint-violating point into a valid manifold. -func (p *plannerOptions) SetPathMetric(m ik.StateMetric) { +func (p *plannerOptions) SetPathMetric(m ik.StateFSMetric) { p.pathMetric = m } @@ -217,20 +235,47 @@ func (p *plannerOptions) SetMinScore(minScore float64) { // addPbConstraints will add all constraints from the protobuf constraint specification. This will deal with only the topological // constraints. It will return a bool indicating whether there are any to add. -func (p *plannerOptions) addPbTopoConstraints(from, to spatialmath.Pose, constraints *Constraints) bool { +func (p *plannerOptions) addPbTopoConstraints( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + constraints *Constraints, +) (bool, error) { topoConstraints := false for _, linearConstraint := range constraints.GetLinearConstraint() { topoConstraints = true - p.addLinearConstraints(from, to, linearConstraint) + // TODO RSDK-9224: Our proto for constraints does not allow the specification of which frames should be constrainted relative to + // which other frames. If there is only one goal specified, then we assume that the constraint is between the moving and goal frame. + err := p.addLinearConstraints(fs, startCfg, from, to, linearConstraint) + if err != nil { + return false, err + } + } + for _, pseudolinearConstraint := range constraints.GetPseudolinearConstraint() { + // pseudolinear constraints + err := p.addPseudolinearConstraints(fs, startCfg, from, to, pseudolinearConstraint) + if err != nil { + return false, err + } } for _, orientationConstraint := range constraints.GetOrientationConstraint() { topoConstraints = true - p.addOrientationConstraints(from, to, orientationConstraint) + // TODO RSDK-9224: Our proto for constraints does not allow the specification of which frames should be constrainted relative to + // which other frames. If there is only one goal specified, then we assume that the constraint is between the moving and goal frame. + err := p.addOrientationConstraints(fs, startCfg, from, to, orientationConstraint) + if err != nil { + return false, err + } } - return topoConstraints + return topoConstraints, nil } -func (p *plannerOptions) addLinearConstraints(from, to spatialmath.Pose, linConstraint LinearConstraint) { +func (p *plannerOptions) addLinearConstraints( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + linConstraint LinearConstraint, +) error { // Linear constraints linTol := linConstraint.LineToleranceMm if linTol == 0 { @@ -241,18 +286,72 @@ func (p *plannerOptions) addLinearConstraints(from, to spatialmath.Pose, linCons if orientTol == 0 { orientTol = defaultOrientationDeviation } - constraint, pathDist := NewAbsoluteLinearInterpolatingConstraint(from, to, linTol, orientTol) - p.AddStateConstraint(defaultLinearConstraintDesc, constraint) + constraint, pathDist, err := CreateAbsoluteLinearInterpolatingConstraintFS(fs, startCfg, from, to, linTol, orientTol) + if err != nil { + return err + } + p.AddStateFSConstraint(defaultLinearConstraintDesc, constraint) + + p.pathMetric = ik.CombineFSMetrics(p.pathMetric, pathDist) + return nil +} + +func (p *plannerOptions) addPseudolinearConstraints( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + plinConstraint PseudolinearConstraint, +) error { + // Linear constraints + linTol := plinConstraint.LineToleranceFactor + if linTol == 0 { + // Default + linTol = defaultPseudolinearTolerance + } + orientTol := plinConstraint.OrientationToleranceFactor + if orientTol == 0 { + orientTol = defaultPseudolinearTolerance + } + // ~ constraint, pathDist, err := CreateAbsoluteLinearInterpolatingConstraintFS(fs, startCfg, from, to, linTol, orientTol) + constraint, pathDist, err := CreateProportionalLinearInterpolatingConstraintFS(fs, startCfg, from, to, linTol, orientTol) + if err != nil { + return err + } + p.AddStateFSConstraint(defaultPseudolinearConstraintDesc, constraint) - p.pathMetric = ik.CombineMetrics(p.pathMetric, pathDist) + p.pathMetric = ik.CombineFSMetrics(p.pathMetric, pathDist) + return nil } -func (p *plannerOptions) addOrientationConstraints(from, to spatialmath.Pose, orientConstraint OrientationConstraint) { +func (p *plannerOptions) addOrientationConstraints( + fs referenceframe.FrameSystem, + startCfg map[string][]referenceframe.Input, + from, to PathStep, + orientConstraint OrientationConstraint, +) error { orientTol := orientConstraint.OrientationToleranceDegs if orientTol == 0 { orientTol = defaultOrientationDeviation } - constraint, pathDist := NewSlerpOrientationConstraint(from, to, orientTol) - p.AddStateConstraint(defaultOrientationConstraintDesc, constraint) - p.pathMetric = ik.CombineMetrics(p.pathMetric, pathDist) + constraint, pathDist, err := CreateSlerpOrientationConstraintFS(fs, startCfg, from, to, orientTol) + if err != nil { + return err + } + p.AddStateFSConstraint(defaultOrientationConstraintDesc, constraint) + p.pathMetric = ik.CombineFSMetrics(p.pathMetric, pathDist) + return nil +} + +func (p *plannerOptions) fillMotionChains(fs referenceframe.FrameSystem, to PathStep) error { + motionChains := make([]*motionChain, 0, len(to)) + + for frame, goal := range to { + chain, err := motionChainFromGoal(fs, frame, goal) + if err != nil { + return err + } + motionChains = append(motionChains, chain) + } + p.motionChains = motionChains + return nil } diff --git a/motionplan/rrt.go b/motionplan/rrt.go index 1252d19f9ba..e6243d09568 100644 --- a/motionplan/rrt.go +++ b/motionplan/rrt.go @@ -17,10 +17,10 @@ const ( defaultPlanIter = 3000 // The maximum percent of a joints range of motion to allow per step. - defaultFrameStep = 0.015 + defaultFrameStep = 0.01 - // If the dot product between two sets of joint angles is less than this, consider them identical. - defaultJointSolveDist = 0.0001 + // If the dot product between two sets of configurations is less than this, consider them identical. + defaultInputIdentDist = 0.0001 // Number of iterations to run before beginning to accept randomly seeded locations. defaultIterBeforeRand = 50 @@ -28,7 +28,7 @@ const ( type rrtParallelPlanner interface { motionPlanner - rrtBackgroundRunner(context.Context, []referenceframe.Input, *rrtParallelPlannerShared) + rrtBackgroundRunner(context.Context, map[string][]referenceframe.Input, *rrtParallelPlannerShared) } type rrtParallelPlannerShared struct { @@ -51,7 +51,7 @@ type rrtMaps struct { optNode node // The highest quality IK solution } -func (maps *rrtMaps) fillPosOnlyGoal(goal spatialmath.Pose, posSeeds, dof int) error { +func (maps *rrtMaps) fillPosOnlyGoal(goal PathStep, posSeeds int) error { thetaStep := 360. / float64(posSeeds) if maps == nil { return errors.New("cannot call method fillPosOnlyGoal on nil maps") @@ -60,9 +60,17 @@ func (maps *rrtMaps) fillPosOnlyGoal(goal spatialmath.Pose, posSeeds, dof int) e maps.goalMap = map[node]node{} } for i := 0; i < posSeeds; i++ { + newMap := PathStep{} + for frame, goal := range goal { + newMap[frame] = referenceframe.NewPoseInFrame( + frame, + spatialmath.NewPose(goal.Pose().Point(), &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: float64(i) * thetaStep}), + ) + } + goalNode := &basicNode{ - q: make([]referenceframe.Input, dof), - pose: spatialmath.NewPose(goal.Point(), &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: float64(i) * thetaStep}), + q: make(map[string][]referenceframe.Input), + poses: newMap, } maps.goalMap[goalNode] = nil } @@ -71,7 +79,7 @@ func (maps *rrtMaps) fillPosOnlyGoal(goal spatialmath.Pose, posSeeds, dof int) e // initRRTsolutions will create the maps to be used by a RRT-based algorithm. It will generate IK solutions to pre-populate the goal // map, and will check if any of those goals are able to be directly interpolated to. -func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referenceframe.Input) *rrtSolution { +func initRRTSolutions(ctx context.Context, mp motionPlanner, seed map[string][]referenceframe.Input) *rrtSolution { rrt := &rrtSolution{ maps: &rrtMaps{ startMap: map[node]node{}, @@ -89,7 +97,7 @@ func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referencefra } // the smallest interpolated distance between the start and end input represents a lower bound on cost - optimalCost := mp.opt().DistanceFunc(&ik.Segment{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) + optimalCost := mp.opt().confDistanceFunc(&ik.SegmentFS{StartConfiguration: seed, EndConfiguration: solutions[0].Q()}) rrt.maps.optNode = &basicNode{q: solutions[0].Q(), cost: optimalCost} // Check for direct interpolation for the subset of IK solutions within some multiple of optimal @@ -98,7 +106,7 @@ func initRRTSolutions(ctx context.Context, mp motionPlanner, seed []referencefra // initialize maps and check whether direct interpolation is an option for _, solution := range solutions { if canInterp { - cost := mp.opt().DistanceFunc(&ik.Segment{StartConfiguration: seed, EndConfiguration: solution.Q()}) + cost := mp.opt().confDistanceFunc(&ik.SegmentFS{StartConfiguration: seed, EndConfiguration: solution.Q()}) if cost < optimalCost*defaultOptimalityMultiple { if mp.checkPath(seed, solution.Q()) { rrt.steps = []node{seedNode, solution} @@ -130,18 +138,26 @@ func shortestPath(maps *rrtMaps, nodePairs []*nodePair) *rrtSolution { // fixedStepInterpolation returns inputs at qstep distance along the path from start to target // if start and target have the same Input value, then no step increment is made. -func fixedStepInterpolation(start, target node, qstep []float64) []referenceframe.Input { - newNear := make([]referenceframe.Input, 0, len(start.Q())) - for j, nearInput := range start.Q() { - if nearInput.Value == target.Q()[j].Value { - newNear = append(newNear, nearInput) - } else { - v1, v2 := nearInput.Value, target.Q()[j].Value - newVal := math.Min(qstep[j], math.Abs(v2-v1)) - // get correct sign - newVal *= (v2 - v1) / math.Abs(v2-v1) - newNear = append(newNear, referenceframe.Input{nearInput.Value + newVal}) +func fixedStepInterpolation(start, target node, qstep map[string][]float64) map[string][]referenceframe.Input { + newNear := make(map[string][]referenceframe.Input) + + // Iterate through each frame's inputs + for frameName, startInputs := range start.Q() { + targetInputs := target.Q()[frameName] + frameSteps := make([]referenceframe.Input, len(startInputs)) + + for j, nearInput := range startInputs { + if nearInput.Value == targetInputs[j].Value { + frameSteps[j] = nearInput + } else { + v1, v2 := nearInput.Value, targetInputs[j].Value + newVal := math.Min(qstep[frameName][j], math.Abs(v2-v1)) + // get correct sign + newVal *= (v2 - v1) / math.Abs(v2-v1) + frameSteps[j] = referenceframe.Input{Value: nearInput.Value + newVal} + } } + newNear[frameName] = frameSteps } return newNear } @@ -150,23 +166,23 @@ func fixedStepInterpolation(start, target node, qstep []float64) []referencefram // TODO: This is somewhat redundant with a State. type node interface { // return the configuration associated with the node - Q() []referenceframe.Input + Q() map[string][]referenceframe.Input Cost() float64 SetCost(float64) - Pose() spatialmath.Pose + Poses() PathStep Corner() bool SetCorner(bool) } type basicNode struct { - q []referenceframe.Input + q map[string][]referenceframe.Input cost float64 - pose spatialmath.Pose + poses PathStep corner bool } // Special case constructors for nodes without costs to return NaN. -func newConfigurationNode(q []referenceframe.Input) node { +func newConfigurationNode(q map[string][]referenceframe.Input) node { return &basicNode{ q: q, cost: math.NaN(), @@ -174,7 +190,7 @@ func newConfigurationNode(q []referenceframe.Input) node { } } -func (n *basicNode) Q() []referenceframe.Input { +func (n *basicNode) Q() map[string][]referenceframe.Input { return n.q } @@ -186,8 +202,8 @@ func (n *basicNode) SetCost(cost float64) { n.cost = cost } -func (n *basicNode) Pose() spatialmath.Pose { - return n.pose +func (n *basicNode) Poses() PathStep { + return n.poses } func (n *basicNode) Corner() bool { @@ -266,7 +282,7 @@ type rrtPlan struct { nodes []node } -func newRRTPlan(solution []node, sf *solverFrame, relative bool) (Plan, error) { +func newRRTPlan(solution []node, fss referenceframe.FrameSystem, relative bool, offsetPose spatialmath.Pose) (Plan, error) { if len(solution) < 2 { if len(solution) == 1 { // Started at the goal, nothing to do @@ -275,8 +291,8 @@ func newRRTPlan(solution []node, sf *solverFrame, relative bool) (Plan, error) { return nil, errors.New("cannot construct a Plan using fewer than two nodes") } } - traj := sf.nodesToTrajectory(solution) - path, err := newPath(solution, sf) + traj := nodesToTrajectory(solution) + path, err := newPath(solution, fss) if err != nil { return nil, err } @@ -289,7 +305,7 @@ func newRRTPlan(solution []node, sf *solverFrame, relative bool) (Plan, error) { var plan Plan plan = &rrtPlan{SimplePlan: *NewSimplePlan(path, traj), nodes: solution} if relative { - plan = OffsetPlan(plan, solution[0].Pose()) + plan = OffsetPlan(plan, offsetPose) } return plan, nil } diff --git a/motionplan/rrtStarConnect.go b/motionplan/rrtStarConnect.go index 424d60b821f..8d842c58b65 100644 --- a/motionplan/rrtStarConnect.go +++ b/motionplan/rrtStarConnect.go @@ -13,7 +13,6 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" ) const ( @@ -28,11 +27,14 @@ const ( type rrtStarConnectOptions struct { // The number of nearest neighbors to consider when adding a new sample to the tree NeighborhoodSize int `json:"neighborhood_size"` + + // This is how far rrtStarConnect will try to extend the map towards a goal per-step + qstep map[string][]float64 } // newRRTStarConnectOptions creates a struct controlling the running of a single invocation of the algorithm. // All values are pre-set to reasonable defaults, but can be tweaked if needed. -func newRRTStarConnectOptions(planOpts *plannerOptions) (*rrtStarConnectOptions, error) { +func newRRTStarConnectOptions(planOpts *plannerOptions, lfs *linearizedFrameSystem) (*rrtStarConnectOptions, error) { algOpts := &rrtStarConnectOptions{ NeighborhoodSize: defaultNeighborhoodSize, } @@ -45,6 +47,10 @@ func newRRTStarConnectOptions(planOpts *plannerOptions) (*rrtStarConnectOptions, if err != nil { return nil, err } + + // Initialize qstep using the same helper function as CBIRRT + algOpts.qstep = getFrameSteps(lfs, defaultFrameStep) + return algOpts, nil } @@ -58,7 +64,7 @@ type rrtStarConnectMotionPlanner struct { // NewRRTStarConnectMotionPlannerWithSeed creates a rrtStarConnectMotionPlanner object with a user specified random seed. func newRRTStarConnectMotionPlanner( - frame referenceframe.Frame, + fss referenceframe.FrameSystem, seed *rand.Rand, logger logging.Logger, opt *plannerOptions, @@ -66,19 +72,19 @@ func newRRTStarConnectMotionPlanner( if opt == nil { return nil, errNoPlannerOptions } - mp, err := newPlanner(frame, seed, logger, opt) + mp, err := newPlanner(fss, seed, logger, opt) if err != nil { return nil, err } - algOpts, err := newRRTStarConnectOptions(opt) + algOpts, err := newRRTStarConnectOptions(opt, mp.lfs) if err != nil { return nil, err } return &rrtStarConnectMotionPlanner{mp, algOpts}, nil } -func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input) ([]node, error) { - mp.planOpts.SetGoal(goal) +func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) { + mp.planOpts.setGoal(goal) solutionChan := make(chan *rrtSolution, 1) utils.PanicCapturingGo(func() { mp.rrtBackgroundRunner(ctx, seed, &rrtParallelPlannerShared{nil, nil, solutionChan}) @@ -97,7 +103,7 @@ func (mp *rrtStarConnectMotionPlanner) plan(ctx context.Context, goal spatialmat // rrtBackgroundRunner will execute the plan. Plan() will call rrtBackgroundRunner in a separate thread and wait for results. // Separating this allows other things to call rrtBackgroundRunner in parallel allowing the thread-agnostic Plan to be accessible. func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, - seed []referenceframe.Input, + seed map[string][]referenceframe.Input, rrt *rrtParallelPlannerShared, ) { mp.logger.CDebug(ctx, "Starting RRT*") @@ -119,7 +125,7 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, } rrt.maps = planSeed.maps } - targetConf, err := mp.frame.Interpolate(seed, rrt.maps.optNode.Q(), 0.5) + targetConf, err := referenceframe.InterpolateFS(mp.fss, seed, rrt.maps.optNode.Q(), 0.5) if err != nil { rrt.solutionChan <- &rrtSolution{err: err} return @@ -179,11 +185,11 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, return } - reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) // Second iteration; extend maps 1 and 2 towards the halfway point between where they reached - if reachedDelta > mp.planOpts.JointSolveDist { - targetConf, err = mp.frame.Interpolate(map1reached.Q(), map2reached.Q(), 0.5) + if reachedDelta > mp.planOpts.InputIdentDist { + targetConf, err = referenceframe.InterpolateFS(mp.fss, map1reached.Q(), map2reached.Q(), 0.5) if err != nil { rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps} return @@ -194,25 +200,20 @@ func (mp *rrtStarConnectMotionPlanner) rrtBackgroundRunner(ctx context.Context, rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps} return } - reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) + reachedDelta = mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()}) } // Solved - if reachedDelta <= mp.planOpts.JointSolveDist { + if reachedDelta <= mp.planOpts.InputIdentDist { // target was added to both map shared = append(shared, &nodePair{map1reached, map2reached}) // Check if we can return if nSolved%defaultOptimalityCheckIter == 0 { solution := shortestPath(rrt.maps, shared) - // can't use a Trajectory constructor here because can't guarantee its a solverframe being used, so build one manually - traj := Trajectory{} - for _, step := range solution.steps { - traj = append(traj, map[string][]referenceframe.Input{mp.frame.Name(): step.Q()}) - } - + traj := nodesToTrajectory(solution.steps) // if cost of trajectory is sufficiently small, exit early - solutionCost := traj.EvaluateCost(mp.planOpts.ScoreFunc) + solutionCost := traj.EvaluateCost(mp.planOpts.scoreFunc) if solutionCost-rrt.maps.optNode.Cost() < defaultOptimalityThreshold*rrt.maps.optNode.Cost() { mp.logger.CDebug(ctx, "RRT* progress: sufficiently optimal path found, exiting") rrt.solutionChan <- solution @@ -257,20 +258,20 @@ func (mp *rrtStarConnectMotionPlanner) extend( default: } - dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) - if dist < mp.planOpts.JointSolveDist { + dist := mp.planOpts.confDistanceFunc(&ik.SegmentFS{StartConfiguration: near.Q(), EndConfiguration: target.Q()}) + if dist < mp.planOpts.InputIdentDist { mchan <- near return } oldNear = near - newNear := fixedStepInterpolation(near, target, mp.planOpts.qstep) + newNear := fixedStepInterpolation(near, target, mp.algOpts.qstep) // Check whether oldNear -> newNear path is a valid segment, and if not then set to nil if !mp.checkPath(oldNear.Q(), newNear) { break } - extendCost := mp.planOpts.DistanceFunc(&ik.Segment{ + extendCost := mp.planOpts.confDistanceFunc(&ik.SegmentFS{ StartConfiguration: oldNear.Q(), EndConfiguration: near.Q(), }) @@ -286,7 +287,7 @@ func (mp *rrtStarConnectMotionPlanner) extend( } // check to see if a shortcut is possible, and rewire the node if it is - connectionCost := mp.planOpts.DistanceFunc(&ik.Segment{ + connectionCost := mp.planOpts.confDistanceFunc(&ik.SegmentFS{ StartConfiguration: thisNeighbor.node.Q(), EndConfiguration: near.Q(), }) diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go deleted file mode 100644 index a4f260c7495..00000000000 --- a/motionplan/solverFrame.go +++ /dev/null @@ -1,394 +0,0 @@ -package motionplan - -import ( - "errors" - - "go.uber.org/multierr" - pb "go.viam.com/api/component/arm/v1" - - "go.viam.com/rdk/motionplan/tpspace" - frame "go.viam.com/rdk/referenceframe" - spatial "go.viam.com/rdk/spatialmath" -) - -// solverFrames are meant to be ephemerally created each time a frame system solution is created, and fulfills the -// Frame interface so that it can be passed to inverse kinematics. -type solverFrame struct { - name string - fss frame.FrameSystem - // List of names of all frames that could move, used for collision detection - // As an example a gripper attached to an arm which is moving relative to World, would not be in frames below but in this object - movingFS frame.FrameSystem - frames []frame.Frame // all frames directly between and including solveFrame and goalFrame. Order not important. - solveFrameName string - goalFrameName string - // If this is true, then goals are translated to their position in `World` before solving. - // This is useful when e.g. moving a gripper relative to a point seen by a camera built into that gripper - // TODO(pl): explore allowing this to be frames other than world - worldRooted bool - origSeed map[string][]frame.Input // stores starting locations of all frames in fss that are NOT in `frames` - - ptgs []tpspace.PTGSolver -} - -func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, seedMap map[string][]frame.Input) (*solverFrame, error) { - // get goal frame - goalFrame := fs.Frame(goalFrameName) - if goalFrame == nil { - return nil, frame.NewFrameMissingError(goalFrameName) - } - goalFrameList, err := fs.TracebackFrame(goalFrame) - if err != nil { - return nil, err - } - - // get solve frame - solveFrame := fs.Frame(solveFrameName) - if solveFrame == nil { - return nil, frame.NewFrameMissingError(solveFrameName) - } - solveFrameList, err := fs.TracebackFrame(solveFrame) - if err != nil { - return nil, err - } - if len(solveFrameList) == 0 { - return nil, errors.New("solveFrameList was empty") - } - - movingFS := func(frameList []frame.Frame) (frame.FrameSystem, error) { - // Find first moving frame - var moveF frame.Frame - for i := len(frameList) - 1; i >= 0; i-- { - if len(frameList[i].DoF()) != 0 { - moveF = frameList[i] - break - } - } - if moveF == nil { - return frame.NewEmptyFrameSystem(""), nil - } - return fs.FrameSystemSubset(moveF) - } - - // find pivot frame between goal and solve frames - var moving frame.FrameSystem - var frames []frame.Frame - worldRooted := false - pivotFrame, err := findPivotFrame(solveFrameList, goalFrameList) - if err != nil { - return nil, err - } - if pivotFrame.Name() == frame.World { - frames = uniqInPlaceSlice(append(solveFrameList, goalFrameList...)) - moving, err = movingFS(solveFrameList) - if err != nil { - return nil, err - } - movingSubset2, err := movingFS(goalFrameList) - if err != nil { - return nil, err - } - if err = moving.MergeFrameSystem(movingSubset2, moving.World()); err != nil { - return nil, err - } - } else { - dof := 0 - var solveMovingList []frame.Frame - var goalMovingList []frame.Frame - - // Get minimal set of frames from solve frame to goal frame - for _, frame := range solveFrameList { - if frame == pivotFrame { - break - } - dof += len(frame.DoF()) - frames = append(frames, frame) - solveMovingList = append(solveMovingList, frame) - } - for _, frame := range goalFrameList { - if frame == pivotFrame { - break - } - dof += len(frame.DoF()) - frames = append(frames, frame) - goalMovingList = append(goalMovingList, frame) - } - - // If shortest path has 0 dof (e.g. a camera attached to a gripper), translate goal to world frame - if dof == 0 { - worldRooted = true - frames = solveFrameList - moving, err = movingFS(solveFrameList) - if err != nil { - return nil, err - } - } else { - // Get all child nodes of pivot node - moving, err = movingFS(solveMovingList) - if err != nil { - return nil, err - } - movingSubset2, err := movingFS(goalMovingList) - if err != nil { - return nil, err - } - if err = moving.MergeFrameSystem(movingSubset2, moving.World()); err != nil { - return nil, err - } - } - } - - origSeed := map[string][]frame.Input{} - // deep copy of seed map - for k, v := range seedMap { - origSeed[k] = v - } - for _, frame := range frames { - delete(origSeed, frame.Name()) - } - - var ptgs []tpspace.PTGSolver - anyPTG := false // Whether PTG frames have been observed - for _, movingFrame := range frames { - if ptgFrame, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { - if anyPTG { - return nil, errors.New("only one PTG frame can be planned for at a time") - } - anyPTG = true - ptgs = ptgFrame.PTGSolvers() - } - } - - return &solverFrame{ - name: solveFrame.Name() + "_" + goalFrame.Name(), - fss: fs, - movingFS: moving, - frames: frames, - solveFrameName: solveFrame.Name(), - goalFrameName: goalFrame.Name(), - worldRooted: worldRooted, - origSeed: origSeed, - ptgs: ptgs, - }, nil -} - -// Name returns the name of the solver referenceframe. -func (sf *solverFrame) Name() string { - return sf.name -} - -// Transform returns the pose between the two frames of this solver for a given set of inputs. -func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { - if len(inputs) != len(sf.DoF()) { - return nil, frame.NewIncorrectDoFError(len(inputs), len(sf.DoF())) - } - pf := frame.NewPoseInFrame(sf.solveFrameName, spatial.NewZeroPose()) - solveName := sf.goalFrameName - if sf.worldRooted { - solveName = frame.World - } - tf, err := sf.fss.Transform(sf.sliceToMap(inputs), pf, solveName) - if err != nil { - return nil, err - } - return tf.(*frame.PoseInFrame).Pose(), nil -} - -// Interpolate interpolates the given amount between the two sets of inputs. -func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame.Input, error) { - if len(from) != len(sf.DoF()) { - return nil, frame.NewIncorrectDoFError(len(from), len(sf.DoF())) - } - if len(to) != len(sf.DoF()) { - return nil, frame.NewIncorrectDoFError(len(to), len(sf.DoF())) - } - interp := make([]frame.Input, 0, len(to)) - posIdx := 0 - for _, currFrame := range sf.frames { - dof := len(currFrame.DoF()) + posIdx - fromSubset := from[posIdx:dof] - toSubset := to[posIdx:dof] - posIdx = dof - - interpSub, err := currFrame.Interpolate(fromSubset, toSubset, by) - if err != nil { - return nil, err - } - - interp = append(interp, interpSub...) - } - return interp, nil -} - -// InputFromProtobuf converts pb.JointPosition to inputs. -func (sf *solverFrame) InputFromProtobuf(jp *pb.JointPositions) []frame.Input { - inputs := make([]frame.Input, 0, len(jp.Values)) - posIdx := 0 - for _, transform := range sf.frames { - dof := len(transform.DoF()) + posIdx - jPos := jp.Values[posIdx:dof] - posIdx = dof - - inputs = append(inputs, transform.InputFromProtobuf(&pb.JointPositions{Values: jPos})...) - } - return inputs -} - -// ProtobufFromInput converts inputs to pb.JointPosition. -func (sf *solverFrame) ProtobufFromInput(input []frame.Input) *pb.JointPositions { - jPos := &pb.JointPositions{} - posIdx := 0 - for _, transform := range sf.frames { - dof := len(transform.DoF()) + posIdx - jPos.Values = append(jPos.Values, transform.ProtobufFromInput(input[posIdx:dof]).Values...) - posIdx = dof - } - return jPos -} - -// Geometry takes a solverFrame and a list of joint angles in radians and computes the 3D space occupied by each of the -// geometries in the solverFrame in the reference frame of the World frame. -func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFrame, error) { - if len(inputs) != len(sf.DoF()) { - return nil, frame.NewIncorrectDoFError(len(inputs), len(sf.DoF())) - } - var errAll error - inputMap := sf.sliceToMap(inputs) - sfGeometries := []spatial.Geometry{} - for _, fName := range sf.movingFS.FrameNames() { - f := sf.fss.Frame(fName) - if f == nil { - return nil, frame.NewFrameMissingError(fName) - } - inputs, err := frame.GetFrameInputs(f, inputMap) - if err != nil { - return nil, err - } - gf, err := f.Geometries(inputs) - if gf == nil { - // only propagate errors that result in nil geometry - multierr.AppendInto(&errAll, err) - continue - } - var tf frame.Transformable - tf, err = sf.fss.Transform(inputMap, gf, frame.World) - if err != nil { - return nil, err - } - sfGeometries = append(sfGeometries, tf.(*frame.GeometriesInFrame).Geometries()...) - } - return frame.NewGeometriesInFrame(frame.World, sfGeometries), errAll -} - -// DoF returns the summed DoF of all frames between the two solver frames. -func (sf *solverFrame) DoF() []frame.Limit { - var limits []frame.Limit - for _, frame := range sf.frames { - limits = append(limits, frame.DoF()...) - } - return limits -} - -// PTGSolvers passes through the PTGs of the solving tp-space frame if it exists, otherwise nil. -func (sf *solverFrame) PTGSolvers() []tpspace.PTGSolver { - return sf.ptgs -} - -func (sf *solverFrame) movingFrame(name string) bool { - return sf.movingFS.Frame(name) != nil -} - -// mapToSlice will flatten a map of inputs into a slice suitable for input to inverse kinematics, by concatenating -// the inputs together in the order of the frames in sf.frames. -func (sf *solverFrame) mapToSlice(inputMap map[string][]frame.Input) ([]frame.Input, error) { - var inputs []frame.Input - for _, f := range sf.frames { - if len(f.DoF()) == 0 { - continue - } - input, err := frame.GetFrameInputs(f, inputMap) - if err != nil { - return nil, err - } - inputs = append(inputs, input...) - } - return inputs, nil -} - -func (sf *solverFrame) sliceToMap(inputSlice []frame.Input) map[string][]frame.Input { - inputs := map[string][]frame.Input{} - for k, v := range sf.origSeed { - inputs[k] = v - } - i := 0 - for _, frame := range sf.frames { - if len(frame.DoF()) == 0 { - continue - } - fLen := i + len(frame.DoF()) - inputs[frame.Name()] = inputSlice[i:fLen] - i = fLen - } - return inputs -} - -func (sf solverFrame) MarshalJSON() ([]byte, error) { - return nil, errors.New("cannot serialize solverFrame") -} - -func (sf *solverFrame) AlmostEquals(otherFrame frame.Frame) bool { - return false -} - -// TODO: move this from being a method on sf to a normal helper in plan.go -// nodesToTrajectory takes a slice of nodes and converts it to a trajectory. -func (sf solverFrame) nodesToTrajectory(nodes []node) Trajectory { - traj := make(Trajectory, 0, len(nodes)) - for _, n := range nodes { - stepMap := sf.sliceToMap(n.Q()) - traj = append(traj, stepMap) - } - return traj -} - -// uniqInPlaceSlice will deduplicate the values in a slice using in-place replacement on the slice. This is faster than -// a solution using append(). -// This function does not remove anything from the input slice, but it does rearrange the elements. -func uniqInPlaceSlice(s []frame.Frame) []frame.Frame { - seen := make(map[frame.Frame]struct{}, len(s)) - j := 0 - for _, v := range s { - if _, ok := seen[v]; ok { - continue - } - seen[v] = struct{}{} - s[j] = v - j++ - } - return s[:j] -} - -// findPivotFrame finds the first common frame in two ordered lists of frames. -func findPivotFrame(frameList1, frameList2 []frame.Frame) (frame.Frame, error) { - // find shorter list - shortList := frameList1 - longList := frameList2 - if len(frameList1) > len(frameList2) { - shortList = frameList2 - longList = frameList1 - } - - // cache names seen in shorter list - nameSet := make(map[string]struct{}, len(shortList)) - for _, frame := range shortList { - nameSet[frame.Name()] = struct{}{} - } - - // look for already seen names in longer list - for _, frame := range longList { - if _, ok := nameSet[frame.Name()]; ok { - return frame, nil - } - } - return nil, errors.New("no path from solve frame to goal frame") -} diff --git a/motionplan/tpSpaceRRT.go b/motionplan/tpSpaceRRT.go index 709c734904e..e620398ea79 100644 --- a/motionplan/tpSpaceRRT.go +++ b/motionplan/tpSpaceRRT.go @@ -114,7 +114,8 @@ type nodeAndError struct { type tpSpaceRRTMotionPlanner struct { *planner algOpts *tpspaceOptions - tpFrame tpspace.PTGProvider + tpFrame referenceframe.Frame + solvers []tpspace.PTGSolver // This tracks the nodes added to the goal tree in an ordered fashion. Nodes will always be added to this slice in the // same order, yielding deterministic results when the goal tree is iterated over. @@ -123,7 +124,7 @@ type tpSpaceRRTMotionPlanner struct { // newTPSpaceMotionPlanner creates a newTPSpaceMotionPlanner object with a user specified random seed. func newTPSpaceMotionPlanner( - frame referenceframe.Frame, + fss referenceframe.FrameSystem, seed *rand.Rand, logger logging.Logger, opt *plannerOptions, @@ -132,44 +133,53 @@ func newTPSpaceMotionPlanner( return nil, errNoPlannerOptions } - mp, err := newPlanner(frame, seed, logger, opt) + mp, err := newPlanner(fss, seed, logger, opt) if err != nil { return nil, err } - - tpFrame, ok := mp.frame.(tpspace.PTGProvider) - if !ok { - return nil, fmt.Errorf("frame %v must be a PTGProvider", mp.frame) - } - tpPlanner := &tpSpaceRRTMotionPlanner{ planner: mp, - tpFrame: tpFrame, } + // TODO: Only one motion chain allowed if tpspace + if len(opt.motionChains) != 1 { + return nil, fmt.Errorf("exactly one motion chain permitted for tpspace, but planner option had %d", len(opt.motionChains)) + } + for _, frame := range opt.motionChains[0].frames { + if tpFrame, ok := frame.(tpspace.PTGProvider); ok { + tpPlanner.tpFrame = frame + tpPlanner.solvers = tpFrame.PTGSolvers() + } + } + tpPlanner.setupTPSpaceOptions() return tpPlanner, nil } // TODO: seed is not immediately useful for TP-space. -func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Pose, seed []referenceframe.Input) ([]node, error) { - mp.planOpts.SetGoal(goal) +func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, goal PathStep, seed map[string][]referenceframe.Input) ([]node, error) { + mp.planOpts.setGoal(goal) solutionChan := make(chan *rrtSolution, 1) - seedPos := mp.opt().StartPose + seedPos := mp.opt().startPoses - startNode := &basicNode{q: make([]referenceframe.Input, len(mp.frame.DoF())), cost: 0, pose: seedPos, corner: false} + startNode := &basicNode{q: make(map[string][]referenceframe.Input, len(mp.lfs.dof)), cost: 0, poses: seedPos, corner: false} maps := &rrtMaps{startMap: map[node]node{startNode: nil}} if mp.opt().PositionSeeds > 0 && mp.opt().profile == PositionOnlyMotionProfile { - err := maps.fillPosOnlyGoal(goal, mp.opt().PositionSeeds, len(mp.frame.DoF())) + err := maps.fillPosOnlyGoal(goal, mp.opt().PositionSeeds) if err != nil { return nil, err } } else { goalNode := &basicNode{ - q: make([]referenceframe.Input, len(mp.frame.DoF())), - cost: 0, - pose: spatialmath.Compose(goal, flipPose), + q: make(map[string][]referenceframe.Input), + cost: 0, + poses: PathStep{ + mp.tpFrame.Name(): referenceframe.NewPoseInFrame( + referenceframe.World, + spatialmath.Compose(goal[mp.tpFrame.Name()].Pose(), flipPose), + ), + }, corner: false, } maps.goalMap = map[node]node{goalNode: nil} @@ -194,41 +204,59 @@ func (mp *tpSpaceRRTMotionPlanner) plan(ctx context.Context, goal spatialmath.Po } } +func (mp *tpSpaceRRTMotionPlanner) poseToPathStep(pose spatialmath.Pose) PathStep { + return PathStep{mp.tpFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, pose)} +} + +func (mp *tpSpaceRRTMotionPlanner) pathStepToPose(step PathStep) spatialmath.Pose { + return step[mp.tpFrame.Name()].Pose() +} + // planRunner will execute the plan. Plan() will call planRunner in a separate thread and wait for results. // Separating this allows other things to call planRunner in parallel allowing the thread-agnostic Plan to be accessible. func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( ctx context.Context, - _ []referenceframe.Input, // TODO: this may be needed for smoothing + _ map[string][]referenceframe.Input, // TODO: this may be needed for smoothing rrt *rrtParallelPlannerShared, ) { defer close(rrt.solutionChan) // get start and goal poses - var startPose spatialmath.Pose + var startPoses PathStep var goalPose spatialmath.Pose var goalNode node goalScore := math.Inf(1) for k, v := range rrt.maps.startMap { if v == nil { - if k.Pose() != nil { - startPose = k.Pose() + if k.Poses() != nil { + startPoses = k.Poses() } else { - rrt.solutionChan <- &rrtSolution{err: fmt.Errorf("node %v must provide a Pose", k)} + rrt.solutionChan <- &rrtSolution{err: fmt.Errorf("start node %v must provide poses", k)} return } break } } + startPoseIF, ok := startPoses[mp.tpFrame.Name()] + if !ok { + rrt.solutionChan <- &rrtSolution{err: fmt.Errorf("start node did not provide pose for tpspace frame %s", mp.tpFrame.Name())} + return + } + startPose := startPoseIF.Pose() for k, v := range rrt.maps.goalMap { if v == nil { // There may be more than one node in the tree which satisfies the goal, i.e. its parent is nil. // However for the purposes of this we can just take the first one we see. - if k.Pose() != nil { - dist := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: startPose, EndPosition: k.Pose()}) + if k.Poses() != nil { + dist := mp.planOpts.poseDistanceFunc( + &ik.Segment{ + StartPosition: startPose, + EndPosition: k.Poses()[mp.tpFrame.Name()].Pose(), + }) if dist < goalScore { // Update to use the closest goal to the start. // This is necessary in order to solve deterministically. - goalPose = k.Pose() + goalPose = k.Poses()[mp.tpFrame.Name()].Pose() goalScore = dist goalNode = k } @@ -243,7 +271,7 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( publishFinishedPath := func(path []node) { // If we've reached the goal, extract the path from the RRT trees and return - correctedPath, err := rectifyTPspacePath(path, mp.frame, startPose) + correctedPath, err := rectifyTPspacePath(path, mp.tpFrame, startPose) if err != nil { rrt.solutionChan <- &rrtSolution{err: err, maps: rrt.maps} return @@ -251,13 +279,13 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( // Print debug info if requested if pathdebug { - allPtgs := mp.tpFrame.PTGSolvers() + allPtgs := mp.solvers lastPose := startPose for _, mynode := range correctedPath { - trajPts, err := allPtgs[int(mynode.Q()[0].Value)].Trajectory( - mynode.Q()[1].Value, - mynode.Q()[2].Value, - mynode.Q()[3].Value, + trajPts, err := allPtgs[int(mynode.Q()[mp.tpFrame.Name()][0].Value)].Trajectory( + mynode.Q()[mp.tpFrame.Name()][1].Value, + mynode.Q()[mp.tpFrame.Name()][2].Value, + mynode.Q()[mp.tpFrame.Name()][3].Value, mp.planOpts.Resolution, ) if err != nil { @@ -291,12 +319,13 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( midPtNormalized := midPt.Sub(startPose.Point()) midOrient := &spatialmath.OrientationVector{OZ: 1, Theta: math.Atan2(-midPtNormalized.X, midPtNormalized.Y)} - midptNode := &basicNode{pose: spatialmath.NewPose(midPt, midOrient), cost: midPt.Sub(startPose.Point()).Norm()} + midptNode := &basicNode{poses: mp.poseToPathStep(spatialmath.NewPose(midPt, midOrient)), cost: midPt.Sub(startPose.Point()).Norm()} var randPosNode node = midptNode for iter := 0; iter < mp.planOpts.PlanIter; iter++ { if pathdebug { - mp.logger.Debugf("$RRTGOAL,%f,%f", randPosNode.Pose().Point().X, randPosNode.Pose().Point().Y) + randPose := mp.pathStepToPose(randPosNode.Poses()) + mp.logger.Debugf("$RRTGOAL,%f,%f", randPose.Point().X, randPose.Point().Y) } mp.logger.CDebugf(ctx, "TP Space RRT iteration %d", iter) if ctx.Err() != nil { @@ -308,7 +337,7 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( m1chan <- mp.attemptExtension(ctx, randPosNode, rrt.maps.startMap, false) }) utils.PanicCapturingGo(func() { - m2chan <- mp.attemptExtension(ctx, flipNode(randPosNode), rrt.maps.goalMap, true) + m2chan <- mp.attemptExtension(ctx, flipNodePoses(randPosNode), rrt.maps.goalMap, true) }) seedReached := <-m1chan goalReached := <-m2chan @@ -322,40 +351,45 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( var reachedDelta float64 if seedReached.node != nil && goalReached.node != nil { // Flip the orientation of the goal node for distance calculation and seed extension - reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{ - StartPosition: seedReached.node.Pose(), - EndPosition: flipNode(goalReached.node).Pose(), + reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ + StartPosition: mp.pathStepToPose(seedReached.node.Poses()), + EndPosition: mp.pathStepToPose(flipNodePoses(goalReached.node).Poses()), }) if reachedDelta > mp.planOpts.GoalThreshold { // If both maps extended, but did not reach the same point, then attempt to extend them towards each other - seedReached = mp.attemptExtension(ctx, flipNode(goalReached.node), rrt.maps.startMap, false) + seedReached = mp.attemptExtension(ctx, flipNodePoses(goalReached.node), rrt.maps.startMap, false) if seedReached.error != nil { rrt.solutionChan <- &rrtSolution{err: seedReached.error, maps: rrt.maps} return } if seedReached.node != nil { - reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{ - StartPosition: seedReached.node.Pose(), - EndPosition: flipNode(goalReached.node).Pose(), + reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ + StartPosition: mp.pathStepToPose(seedReached.node.Poses()), + EndPosition: mp.pathStepToPose(flipNodePoses(goalReached.node).Poses()), }) if reachedDelta > mp.planOpts.GoalThreshold { - goalReached = mp.attemptExtension(ctx, flipNode(seedReached.node), rrt.maps.goalMap, true) + goalReached = mp.attemptExtension(ctx, flipNodePoses(seedReached.node), rrt.maps.goalMap, true) if goalReached.error != nil { rrt.solutionChan <- &rrtSolution{err: goalReached.error, maps: rrt.maps} return } } if goalReached.node != nil { - reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{ - StartPosition: seedReached.node.Pose(), - EndPosition: flipNode(goalReached.node).Pose(), + reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ + StartPosition: mp.pathStepToPose(seedReached.node.Poses()), + EndPosition: mp.pathStepToPose(flipNodePoses(goalReached.node).Poses()), }) } } } if reachedDelta <= mp.planOpts.GoalThreshold { // If we've reached the goal, extract the path from the RRT trees and return - path := extractTPspacePath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalReached.node}) + path := extractTPspacePath( + mp.tpFrame.Name(), + rrt.maps.startMap, + rrt.maps.goalMap, + &nodePair{a: seedReached.node, b: goalReached.node}, + ) publishFinishedPath(path) return } @@ -387,7 +421,7 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( } attempts++ - seedReached := mp.attemptExtension(ctx, flipNode(goalMapNode), rrt.maps.startMap, false) + seedReached := mp.attemptExtension(ctx, flipNodePoses(goalMapNode), rrt.maps.startMap, false) if seedReached.error != nil { rrt.solutionChan <- &rrtSolution{err: seedReached.error, maps: rrt.maps} return @@ -395,13 +429,18 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner( if seedReached.node == nil { continue } - reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{ - StartPosition: seedReached.node.Pose(), - EndPosition: flipNode(goalMapNode).Pose(), + reachedDelta = mp.planOpts.poseDistanceFunc(&ik.Segment{ + StartPosition: mp.pathStepToPose(seedReached.node.Poses()), + EndPosition: mp.pathStepToPose(flipNodePoses(goalMapNode).Poses()), }) if reachedDelta <= mp.planOpts.GoalThreshold { // If we've reached the goal, extract the path from the RRT trees and return - path := extractTPspacePath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{a: seedReached.node, b: goalMapNode}) + path := extractTPspacePath( + mp.tpFrame.Name(), + rrt.maps.startMap, + rrt.maps.goalMap, + &nodePair{a: seedReached.node, b: goalMapNode}, + ) paths = append(paths, path) } } @@ -443,13 +482,13 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( // Get the distance function that will find the nearest RRT map node in TP-space of *this* PTG ptgDistOpt, distMap := mp.make2DTPSpaceDistanceOptions(curPtg) - nm := &neighborManager{nCPU: mp.planOpts.NumThreads / len(mp.tpFrame.PTGSolvers())} + nm := &neighborManager{nCPU: mp.planOpts.NumThreads / len(mp.solvers)} nm.parallelNeighbors = 10 var successNode node - var solution *ik.Solution var err error + if nearest == nil { // Get nearest neighbor to rand config in tree using this PTG nearest = nm.nearestNeighbor(ctx, ptgDistOpt, randPosNode, rrt) @@ -457,7 +496,7 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errNoNeighbors } - rawVal, ok := distMap.Load(nearest.Pose()) + rawVal, ok := distMap.Load(mp.pathStepToPose(nearest.Poses())) if !ok { mp.logger.Error("nearest neighbor failed to find nearest pose in distMap") return nil, errNoNeighbors @@ -469,47 +508,58 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errNoNeighbors } } else { - solution, err = mp.ptgSolution(curPtg, nearest.Pose(), randPosNode.Pose()) + solution, err = mp.ptgSolution(curPtg, mp.pathStepToPose(nearest.Poses()), mp.pathStepToPose(randPosNode.Poses())) if err != nil || solution == nil { return nil, err } } - targetFunc := defaultGoalMetricConstructor(spatialmath.PoseBetween(nearest.Pose(), randPosNode.Pose())) - // TODO: We could potentially improve solving by first getting the rough distance to the randPosNode to any point in the rrt tree, - // then dynamically expanding or contracting the limits of IK to be some fraction of that distance. // Get cartesian distance from NN to rand - arcStartPose := nearest.Pose() + arcStartPose := mp.pathStepToPose(nearest.Poses()) successNodes := []node{} arcPose := spatialmath.NewZeroPose() // This will be the relative pose that is the delta from one end of the combined traj to the other. + // We may produce more than one consecutive arc. Reduce the one configuration to several 2dof arcs for i := 0; i < len(solution.Configuration); i += 2 { - subNode := newConfigurationNode(referenceframe.FloatsToInputs(solution.Configuration[i : i+2])) + subConfig := map[string][]referenceframe.Input{ + mp.tpFrame.Name(): referenceframe.FloatsToInputs(solution.Configuration[i : i+2]), + } + subNode := newConfigurationNode(subConfig) // Check collisions along this traj and get the longest distance viable - trajK, err := curPtg.Trajectory(subNode.Q()[0].Value, 0, subNode.Q()[1].Value, mp.planOpts.Resolution) + trajK, err := curPtg.Trajectory( + subNode.Q()[mp.tpFrame.Name()][0].Value, + 0, + subNode.Q()[mp.tpFrame.Name()][1].Value, + mp.planOpts.Resolution, + ) if err != nil { return nil, err } + goodNode := mp.checkTraj(trajK, arcStartPose) if goodNode == nil { break } - partialExtend := false - for i, val := range subNode.Q() { - if goodNode.Q()[i] != val { + partialExtend := false + for i, val := range subNode.Q()[mp.tpFrame.Name()] { + if goodNode.Q()[mp.tpFrame.Name()][i] != val { partialExtend = true } } - arcPose = spatialmath.Compose(arcPose, goodNode.Pose()) + + arcPose = spatialmath.Compose(arcPose, mp.pathStepToPose(goodNode.Poses())) // add the last node in trajectory - arcStartPose = spatialmath.Compose(arcStartPose, goodNode.Pose()) + arcStartPose = spatialmath.Compose(arcStartPose, mp.pathStepToPose(goodNode.Poses())) + successNode = &basicNode{ - q: []referenceframe.Input{{float64(ptgNum)}, goodNode.Q()[0], {0}, goodNode.Q()[1]}, + q: map[string][]referenceframe.Input{ + mp.tpFrame.Name(): {{float64(ptgNum)}, goodNode.Q()[mp.tpFrame.Name()][0], {0}, goodNode.Q()[mp.tpFrame.Name()][1]}, + }, cost: goodNode.Cost(), - pose: arcStartPose, + poses: mp.poseToPathStep(arcStartPose), corner: false, } successNodes = append(successNodes, successNode) @@ -522,16 +572,19 @@ func (mp *tpSpaceRRTMotionPlanner) getExtensionCandidate( return nil, errInvalidCandidate } + targetFunc := defaultGoalMetricConstructor(spatialmath.PoseBetween(arcStartPose, mp.pathStepToPose(randPosNode.Poses()))) bestDist := targetFunc(&ik.State{Position: arcPose}) cand := &candidate{dist: bestDist, treeNode: nearest, newNodes: successNodes} // check if this successNode is too close to nodes already in the tree, and if so, do not add. - // Get nearest neighbor to new node that's already in the tree. Note that this uses cartesian distance (planOpts.DistanceFunc) rather - // than the TP-space distance functions in algOpts. + // Get nearest neighbor to new node that's already in the tree. Note that this uses cartesian distance (planOpts.poseDistanceFunc) + // rather than the TP-space distance functions in algOpts. nearest = nm.nearestNeighbor(ctx, mp.planOpts, successNode, rrt) if nearest != nil { - dist := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: successNode.Pose(), EndPosition: nearest.Pose()}) - // Ensure successNode is sufficiently far from the nearest node already existing in the tree + dist := mp.planOpts.poseDistanceFunc(&ik.Segment{ + StartPosition: mp.pathStepToPose(successNode.Poses()), + EndPosition: mp.pathStepToPose(nearest.Poses()), + }) // If too close, don't add a new node if dist < mp.algOpts.identicalNodeDistance { cand = nil @@ -547,7 +600,8 @@ func (mp *tpSpaceRRTMotionPlanner) checkTraj(trajK []*tpspace.TrajNode, arcStart for i := 0; i < len(trajK); i++ { trajPt := trajK[i] - trajState := &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.frame} + trajState := &ik.State{Position: spatialmath.Compose(arcStartPose, trajPt.Pose), Frame: mp.tpFrame} + // In addition to checking every `Resolution`, we also check both endpoints. ok, _ := mp.planOpts.CheckStateConstraints(trajState) if !ok { @@ -568,16 +622,24 @@ func (mp *tpSpaceRRTMotionPlanner) checkTraj(trajK []*tpspace.TrajNode, arcStart } okNode := &basicNode{ - q: []referenceframe.Input{{trajPt.Alpha}, {trajPt.Dist}}, - cost: trajPt.Dist, - pose: trajPt.Pose, + q: map[string][]referenceframe.Input{ + mp.tpFrame.Name(): {{trajPt.Alpha}, {trajPt.Dist}}, + }, + cost: trajPt.Dist, + poses: mp.poseToPathStep(trajPt.Pose), + corner: false, } passed = append(passed, okNode) } + + lastTrajPt := trajK[len(trajK)-1] return &basicNode{ - q: []referenceframe.Input{{trajK[(len(trajK) - 1)].Alpha}, {trajK[(len(trajK) - 1)].Dist}}, - cost: trajK[(len(trajK) - 1)].Dist, - pose: passed[len(passed)-1].Pose(), + q: map[string][]referenceframe.Input{ + mp.tpFrame.Name(): {{lastTrajPt.Alpha}, {lastTrajPt.Dist}}, + }, + cost: lastTrajPt.Dist, + poses: passed[len(passed)-1].Poses(), + corner: false, } } @@ -593,7 +655,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( var seedNode node maxReseeds := 1 // Will be updated as necessary lastIteration := false - candChan := make(chan *candidate, len(mp.tpFrame.PTGSolvers())) + candChan := make(chan *candidate, len(mp.solvers)) defer close(candChan) var activeSolvers sync.WaitGroup defer activeSolvers.Wait() @@ -606,7 +668,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( } candidates := []*candidate{} - for ptgNum, curPtg := range mp.tpFrame.PTGSolvers() { + for ptgNum, curPtg := range mp.solvers { // Find the best traj point for each traj family, and store for later comparison ptgNumPar, curPtgPar := ptgNum, curPtg activeSolvers.Add(1) @@ -627,7 +689,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( }) } - for i := 0; i < len(mp.tpFrame.PTGSolvers()); i++ { + for i := 0; i < len(mp.solvers); i++ { select { case <-ctx.Done(): return &nodeAndError{nil, ctx.Err()} @@ -653,9 +715,9 @@ func (mp *tpSpaceRRTMotionPlanner) attemptExtension( endNode := reseedCandidate.newNodes[len(reseedCandidate.newNodes)-1] distTravelledByCandidate := 0. for _, newNode := range reseedCandidate.newNodes { - distTravelledByCandidate += math.Abs(newNode.Q()[3].Value - newNode.Q()[2].Value) + distTravelledByCandidate += math.Abs(newNode.Q()[mp.tpFrame.Name()][3].Value - newNode.Q()[mp.tpFrame.Name()][2].Value) } - distToGoal := endNode.Pose().Point().Distance(goalNode.Pose().Point()) + distToGoal := mp.pathStepToPose(endNode.Poses()).Point().Distance(mp.pathStepToPose(goalNode.Poses()).Point()) if distToGoal < mp.planOpts.GoalThreshold || lastIteration { // Reached the goal position, or otherwise failed to fully extend to the end of a trajectory return &nodeAndError{endNode, nil} @@ -712,15 +774,15 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( treeNode := bestCand.treeNode // The node already in the tree to which we are parenting newNodes := bestCand.newNodes // The node we are adding because it was the best extending PTG for _, newNode := range newNodes { - ptgNum := int(newNode.Q()[0].Value) - randAlpha := newNode.Q()[1].Value - randDist := newNode.Q()[3].Value - newNode.Q()[2].Value + ptgNum := int(newNode.Q()[mp.tpFrame.Name()][0].Value) + randAlpha := newNode.Q()[mp.tpFrame.Name()][1].Value + randDist := newNode.Q()[mp.tpFrame.Name()][3].Value - newNode.Q()[mp.tpFrame.Name()][2].Value - trajK, err := mp.tpFrame.PTGSolvers()[ptgNum].Trajectory(randAlpha, 0, randDist, mp.planOpts.Resolution) + trajK, err := mp.solvers[ptgNum].Trajectory(randAlpha, 0, randDist, mp.planOpts.Resolution) if err != nil { return nil, err } - arcStartPose := treeNode.Pose() + arcStartPose := mp.pathStepToPose(treeNode.Poses()) lastDist := 0. sinceLastNode := 0. @@ -747,9 +809,11 @@ func (mp *tpSpaceRRTMotionPlanner) extendMap( if sinceLastNode > mp.algOpts.addNodeEvery { // add the last node in trajectory addedNode = &basicNode{ - q: referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, 0, trajPt.Dist}), + q: map[string][]referenceframe.Input{ + mp.tpFrame.Name(): referenceframe.FloatsToInputs([]float64{float64(ptgNum), randAlpha, 0, trajPt.Dist}), + }, cost: trajPt.Dist, - pose: trajState.Position, + poses: mp.poseToPathStep(trajState.Position), corner: false, } rrt[addedNode] = treeNode @@ -809,7 +873,7 @@ func (mp *tpSpaceRRTMotionPlanner) ptgSolution(ptg tpspace.PTGSolver, // Also returns a pointer to a sync.Map of nearest poses -> ik.Solution so the (expensive to compute) solution can be reused. func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTGSolver) (*plannerOptions, *sync.Map) { m := sync.Map{} - opts := newBasicPlannerOptions(mp.frame) + opts := newBasicPlannerOptions() segMetric := func(seg *ik.Segment) float64 { // When running NearestNeighbor: // StartPosition is the seed/query @@ -827,7 +891,14 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTGS return solution.Score } - opts.DistanceFunc = segMetric + opts.poseDistanceFunc = segMetric + opts.nodeDistanceFunc = func(node1, node2 node) float64 { + return segMetric(&ik.Segment{ + StartPosition: mp.pathStepToPose(node1.Poses()), + EndPosition: mp.pathStepToPose(node2.Poses()), + }) + } + opts.Resolution = defaultPTGCollisionResolution return opts, &m } @@ -836,7 +907,7 @@ func (mp *tpSpaceRRTMotionPlanner) make2DTPSpaceDistanceOptions(ptg tpspace.PTGS func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) []node { toIter := int(math.Min(float64(len(path)*len(path))/2, float64(mp.planOpts.SmoothIter))) currCost := sumCosts(path) - smoothPlannerMP, err := newTPSpaceMotionPlanner(mp.frame, mp.randseed, mp.logger, mp.planOpts) + smoothPlannerMP, err := newTPSpaceMotionPlanner(mp.fss, mp.randseed, mp.logger, mp.planOpts) if err != nil { return path } @@ -878,13 +949,13 @@ func (mp *tpSpaceRRTMotionPlanner) smoothPath(ctx context.Context, path []node) } if pathdebug { - allPtgs := mp.tpFrame.PTGSolvers() - lastPose := path[0].Pose() + allPtgs := mp.solvers + lastPose := mp.pathStepToPose(path[0].Poses()) for _, mynode := range path { - trajPts, err := allPtgs[int(mynode.Q()[0].Value)].Trajectory( - mynode.Q()[1].Value, - mynode.Q()[2].Value, - mynode.Q()[3].Value, + trajPts, err := allPtgs[int(mynode.Q()[mp.tpFrame.Name()][0].Value)].Trajectory( + mynode.Q()[mp.tpFrame.Name()][1].Value, + mynode.Q()[mp.tpFrame.Name()][2].Value, + mynode.Q()[mp.tpFrame.Name()][3].Value, mp.planOpts.Resolution, ) if err != nil { @@ -916,19 +987,21 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( ) ([]node, error) { startMap := map[node]node{} var parent node - parentPose := path[0].Pose() + parentPose := mp.pathStepToPose(path[0].Poses()) for j := 0; j <= firstEdge; j++ { pathNode := path[j] startMap[pathNode] = parent for adjNum := defaultSmoothChunkCount - 1; adjNum > 0; adjNum-- { - fullQ := pathNode.Q() + fullQ := pathNode.Q()[mp.tpFrame.Name()] adj := (fullQ[3].Value - fullQ[2].Value) * (float64(adjNum) / float64(defaultSmoothChunkCount)) - newQ := []referenceframe.Input{fullQ[0], fullQ[1], fullQ[2], {fullQ[3].Value - adj}} - trajK, err := smoother.tpFrame.PTGSolvers()[int(math.Round(newQ[0].Value))].Trajectory( - newQ[1].Value, - newQ[2].Value, - newQ[3].Value, + newQ := map[string][]referenceframe.Input{ + mp.tpFrame.Name(): {fullQ[0], fullQ[1], fullQ[2], {fullQ[3].Value - adj}}, + } + trajK, err := smoother.solvers[int(math.Round(newQ[mp.tpFrame.Name()][0].Value))].Trajectory( + newQ[mp.tpFrame.Name()][1].Value, + newQ[mp.tpFrame.Name()][2].Value, + newQ[mp.tpFrame.Name()][3].Value, mp.planOpts.Resolution, ) if err != nil { @@ -938,13 +1011,13 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( intNode := &basicNode{ q: newQ, cost: pathNode.Cost() - math.Abs(adj), - pose: spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose), + poses: mp.poseToPathStep(spatialmath.Compose(parentPose, trajK[len(trajK)-1].Pose)), corner: false, } startMap[intNode] = parent } parent = pathNode - parentPose = parent.Pose() + parentPose = mp.pathStepToPose(parent.Poses()) } // TODO: everything below this point can become an invocation of `smoother.planRunner` reached := smoother.attemptExtension(ctx, path[secondEdge], startMap, false) @@ -952,7 +1025,11 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( return nil, errors.New("could not extend to smoothing destination") } - reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: reached.Pose(), EndPosition: path[secondEdge].Pose()}) + reachedDelta := mp.planOpts.poseDistanceFunc(&ik.Segment{ + StartPosition: mp.pathStepToPose(reached.node.Poses()), + EndPosition: mp.pathStepToPose(path[secondEdge].Poses()), + }) + // If we tried the goal and have a close-enough XY location, check if the node is good enough to be a final goal if reachedDelta > mp.planOpts.GoalThreshold { return nil, errors.New("could not precisely reach smoothing destination") @@ -970,7 +1047,7 @@ func (mp *tpSpaceRRTMotionPlanner) attemptSmooth( // so this step will replace it. newInputSteps = append(newInputSteps, path[len(path)-1]) } - return rectifyTPspacePath(newInputSteps, mp.frame, path[0].Pose()) + return rectifyTPspacePath(newInputSteps, mp.tpFrame, mp.pathStepToPose(path[0].Poses())) } func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { @@ -983,10 +1060,14 @@ func (mp *tpSpaceRRTMotionPlanner) sample(rSeed node, iter int) (node, error) { randPosY := float64(mp.randseed.Intn(int(rDist))) randPosTheta := math.Pi * (mp.randseed.Float64() - 0.5) randPos := spatialmath.NewPose( - r3.Vector{rSeed.Pose().Point().X + (randPosX - rDist/2.), rSeed.Pose().Point().Y + (randPosY - rDist/2.), 0}, + r3.Vector{ + mp.pathStepToPose(rSeed.Poses()).Point().X + (randPosX - rDist/2.), + mp.pathStepToPose(rSeed.Poses()).Point().Y + (randPosY - rDist/2.), + 0, + }, &spatialmath.OrientationVector{OZ: 1, Theta: randPosTheta}, ) - return &basicNode{pose: randPos}, nil + return &basicNode{poses: mp.poseToPathStep(randPos)}, nil } // rectifyTPspacePath is needed because of how trees are currently stored. As trees grow from the start or goal, the Pose stored in the node @@ -998,7 +1079,7 @@ func rectifyTPspacePath(path []node, frame referenceframe.Frame, startPose spati correctedPath := []node{} runningPose := startPose for _, wp := range path { - wpPose, err := frame.Transform(wp.Q()) + wpPose, err := frame.Transform(wp.Q()[frame.Name()]) if err != nil { return nil, err } @@ -1007,7 +1088,7 @@ func rectifyTPspacePath(path []node, frame referenceframe.Frame, startPose spati thisNode := &basicNode{ q: wp.Q(), cost: wp.Cost(), - pose: runningPose, + poses: PathStep{frame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, runningPose)}, corner: wp.Corner(), } correctedPath = append(correctedPath, thisNode) @@ -1015,7 +1096,7 @@ func rectifyTPspacePath(path []node, frame referenceframe.Frame, startPose spati return correctedPath, nil } -func extractTPspacePath(startMap, goalMap map[node]node, pair *nodePair) []node { +func extractTPspacePath(fName string, startMap, goalMap map[node]node, pair *nodePair) []node { // need to figure out which of the two nodes is in the start map var startReached, goalReached node if _, ok := startMap[pair.a]; ok { @@ -1027,7 +1108,19 @@ func extractTPspacePath(startMap, goalMap map[node]node, pair *nodePair) []node // extract the path to the seed path := make([]node, 0) for startReached != nil { - path = append(path, startReached) + if startMap[startReached] == nil { + path = append(path, + &basicNode{ + q: map[string][]referenceframe.Input{ + fName: {{0}, {0}, {0}, {0}}, + }, + cost: startReached.Cost(), + poses: startReached.Poses(), + corner: startReached.Corner(), + }) + } else { + path = append(path, startReached) + } startReached = startMap[startReached] } @@ -1038,16 +1131,32 @@ func extractTPspacePath(startMap, goalMap map[node]node, pair *nodePair) []node // extract the path to the goal for goalReached != nil { - goalReachedReversed := &basicNode{ - q: []referenceframe.Input{ - goalReached.Q()[0], - goalReached.Q()[1], - goalReached.Q()[3], - goalReached.Q()[2], - }, - cost: goalReached.Cost(), - pose: spatialmath.Compose(goalReached.Pose(), flipPose), - corner: goalReached.Corner(), + goalPiF := goalReached.Poses()[fName] + var goalReachedReversed node + if goalMap[goalReached] == nil { + // Add the final node + goalReachedReversed = &basicNode{ + q: map[string][]referenceframe.Input{ + fName: {{0}, {0}, {0}, {0}}, + }, + cost: goalReached.Cost(), + poses: PathStep{fName: referenceframe.NewPoseInFrame(goalPiF.Parent(), spatialmath.Compose(goalPiF.Pose(), flipPose))}, + corner: goalReached.Corner(), + } + } else { + goalReachedReversed = &basicNode{ + q: map[string][]referenceframe.Input{ + fName: { + goalReached.Q()[fName][0], + goalReached.Q()[fName][1], + goalReached.Q()[fName][3], + goalReached.Q()[fName][2], + }, + }, + cost: goalReached.Cost(), + poses: PathStep{fName: referenceframe.NewPoseInFrame(goalPiF.Parent(), spatialmath.Compose(goalPiF.Pose(), flipPose))}, + corner: goalReached.Corner(), + } } path = append(path, goalReachedReversed) goalReached = goalMap[goalReached] @@ -1056,11 +1165,17 @@ func extractTPspacePath(startMap, goalMap map[node]node, pair *nodePair) []node } // Returns a new node whose orientation is flipped 180 degrees from the provided node. -func flipNode(n node) node { +// It does NOT flip the configurations/inputs. +func flipNodePoses(n node) node { + flippedPoses := PathStep{} + for f, pif := range n.Poses() { + flippedPoses[f] = referenceframe.NewPoseInFrame(pif.Parent(), spatialmath.Compose(pif.Pose(), flipPose)) + } + return &basicNode{ q: n.Q(), cost: n.Cost(), - pose: spatialmath.Compose(n.Pose(), flipPose), + poses: flippedPoses, corner: n.Corner(), } } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 0218fcce0fc..2c82d71e6c9 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -40,21 +40,37 @@ func TestPtgRrtBidirectional(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) + fs := referenceframe.NewEmptyFrameSystem("test") + fs.AddFrame(ackermanFrame, fs.World()) + goalPos := spatialmath.NewPose(r3.Vector{X: 200, Y: 7000, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}) + goalMap := PathStep{ + ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos), + } + + opt := newBasicPlannerOptions() + opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(30.) + opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{ackermanFrame.Name()}) + opt.PlannerConstructor = newTPSpaceMotionPlanner + opt.relativeInputs = true + opt.startPoses = PathStep{ + ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), + } + opt.setGoal(goalMap) + opt.fillMotionChains(fs, goalMap) - opt := newBasicPlannerOptions(ackermanFrame) - opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) - opt.StartPose = spatialmath.NewZeroPose() - mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + mp, err := newTPSpaceMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, ok := mp.(*tpSpaceRRTMotionPlanner) + test.That(t, ok, test.ShouldBeTrue) + if pathdebug { tp.logger.Debug("$type,X,Y") tp.logger.Debugf("$SG,%f,%f", 0., 0.) tp.logger.Debugf("$SG,%f,%f", goalPos.Point().X, goalPos.Point().Y) } - test.That(t, ok, test.ShouldBeTrue) - plan, err := tp.plan(ctx, goalPos, nil) + + plan, err := tp.plan(ctx, goalMap, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThanOrEqualTo, 2) } @@ -78,16 +94,22 @@ func TestPtgWithObstacle(t *testing.T) { ctx := context.Background() - goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) - + // Create frame system and add ackerman frame fs := referenceframe.NewEmptyFrameSystem("test") fs.AddFrame(ackermanFrame, fs.World()) - opt := newBasicPlannerOptions(ackermanFrame) - opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) - opt.StartPose = spatialmath.NewPoseFromPoint(r3.Vector{0, -1000, 0}) - opt.GoalThreshold = 5 - // obstacles + // Set up start and goal poses + startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, -1000, 0}) + goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) + + startMap := map[string]*referenceframe.PoseInFrame{ + ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + } + goalMap := map[string]*referenceframe.PoseInFrame{ + ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos), + } + + // Create obstacles obstacle1, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, -500, 0}), r3.Vector{180, 1800, 1}, "") test.That(t, err, test.ShouldBeNil) obstacle2, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{3300, 1800, 0}), r3.Vector{180, 1800, 1}, "") @@ -103,56 +125,47 @@ func TestPtgWithObstacle(t *testing.T) { geoms := []spatialmath.Geometry{obstacle1, obstacle2, obstacle3, obstacle4, obstacle5, obstacle6} + // Set up world state worldState, err := referenceframe.NewWorldState( []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)}, nil, ) test.That(t, err, test.ShouldBeNil) - sf, err := newSolverFrame(fs, ackermanFrame.Name(), referenceframe.World, nil) - test.That(t, err, test.ShouldBeNil) - - seedMap := referenceframe.StartPositions(fs) - frameInputs, err := sf.mapToSlice(seedMap) - test.That(t, err, test.ShouldBeNil) - // create robot collision entities - movingGeometriesInFrame, err := sf.Geometries(frameInputs) - movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World - test.That(t, err, test.ShouldBeNil) - - // find all geometries that are not moving but are in the frame system - staticRobotGeometries := make([]spatialmath.Geometry, 0) - frameSystemGeometries, err := referenceframe.FrameSystemGeometries(fs, seedMap) - test.That(t, err, test.ShouldBeNil) - for name, geometries := range frameSystemGeometries { - if !sf.movingFrame(name) { - staticRobotGeometries = append(staticRobotGeometries, geometries.Geometries()...) - } - } - - // Note that all obstacles in worldState are assumed to be static so it is ok to transform them into the world frame - // TODO(rb) it is bad practice to assume that the current inputs of the robot correspond to the passed in world state - // the state that observed the worldState should ultimately be included as part of the worldState message - worldGeometries, err := worldState.ObstaclesInWorldFrame(fs, seedMap) + // Initialize planner options + opt := newBasicPlannerOptions() + opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(30.) + opt.GoalThreshold = 5 + opt.PlannerConstructor = newTPSpaceMotionPlanner + opt.relativeInputs = true + opt.startPoses = startMap + opt.setGoal(goalMap) + opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{ackermanFrame.Name()}) + opt.fillMotionChains(fs, goalMap) + + // Create collision constraints + worldGeometries, err := worldState.ObstaclesInWorldFrame(fs, nil) test.That(t, err, test.ShouldBeNil) - collisionConstraints, err := createAllCollisionConstraints( - movingRobotGeometries, - staticRobotGeometries, + _, collisionConstraints, err := createAllCollisionConstraints( + geometries, // moving geometries + nil, // static robot geometries worldGeometries.Geometries(), nil, nil, defaultCollisionBufferMM, ) - test.That(t, err, test.ShouldBeNil) for name, constraint := range collisionConstraints { opt.AddStateConstraint(name, constraint) } - mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + // Create and initialize planner + mp, err := newTPSpaceMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, _ := mp.(*tpSpaceRRTMotionPlanner) + + // Debug logging if enabled if pathdebug { tp.logger.Debug("$type,X,Y") for _, geom := range geoms { @@ -163,18 +176,21 @@ func TestPtgWithObstacle(t *testing.T) { } } } - tp.logger.Debugf("$SG,%f,%f", opt.StartPose.Point().X, opt.StartPose.Point().Y) + tp.logger.Debugf("$SG,%f,%f", startPose.Point().X, startPose.Point().Y) tp.logger.Debugf("$SG,%f,%f", goalPos.Point().X, goalPos.Point().Y) } - plan, err := tp.plan(ctx, goalPos, nil) + // Plan and verify results + plan, err := tp.plan(ctx, goalMap, nil) test.That(t, err, test.ShouldBeNil) test.That(t, len(plan), test.ShouldBeGreaterThan, 2) + // Test smoothing tp.planOpts.SmoothIter = 80 - newplan := tp.smoothPath(ctx, plan) test.That(t, newplan, test.ShouldNotBeNil) + + // Compare costs oldcost := 0. smoothcost := 0. for _, planNode := range plan { @@ -206,9 +222,27 @@ func TestTPsmoothing(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - opt := newBasicPlannerOptions(ackermanFrame) - opt.DistanceFunc = ik.NewSquaredNormSegmentMetric(30.) - mp, err := newTPSpaceMotionPlanner(ackermanFrame, rand.New(rand.NewSource(42)), logger, opt) + // Create frame system and add ackerman frame + fs := referenceframe.NewEmptyFrameSystem("test") + fs.AddFrame(ackermanFrame, fs.World()) + + // Initialize planner options + opt := newBasicPlannerOptions() + opt.poseDistanceFunc = ik.NewSquaredNormSegmentMetric(30.) + opt.scoreFunc = tpspace.NewPTGDistanceMetric([]string{ackermanFrame.Name()}) + opt.PlannerConstructor = newTPSpaceMotionPlanner + opt.relativeInputs = true + + // Needed to determine motion chains + goalPos := spatialmath.NewPoseFromPoint(r3.Vector{X: 6500, Y: 0, Z: 0}) + goalMap := map[string]*referenceframe.PoseInFrame{ + ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, goalPos), + } + + opt.fillMotionChains(fs, goalMap) + + // Create and initialize planner + mp, err := newTPSpaceMotionPlanner(fs, rand.New(rand.NewSource(42)), logger, opt) test.That(t, err, test.ShouldBeNil) tp, _ := mp.(*tpSpaceRRTMotionPlanner) @@ -234,12 +268,12 @@ func TestTPsmoothing(t *testing.T) { plan := []node{} for _, inp := range planInputs { thisNode := &basicNode{ - q: inp, + q: map[string][]referenceframe.Input{ackermanFrame.Name(): inp}, cost: inp[3].Value - inp[2].Value, } plan = append(plan, thisNode) } - plan, err = rectifyTPspacePath(plan, tp.frame, spatialmath.NewZeroPose()) + plan, err = rectifyTPspacePath(plan, tp.tpFrame, spatialmath.NewZeroPose()) test.That(t, err, test.ShouldBeNil) tp.planOpts.SmoothIter = 20 @@ -261,7 +295,7 @@ func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { nodes := []node{} for _, inp := range plan.Trajectory() { thisNode := &basicNode{ - q: inp[f.Name()], + q: inp, cost: math.Abs(inp[f.Name()][3].Value - inp[f.Name()][2].Value), } nodes = append(nodes, thisNode) diff --git a/motionplan/tpspace/ptg.go b/motionplan/tpspace/ptg.go index 4d744df6035..a29c3e627ae 100644 --- a/motionplan/tpspace/ptg.go +++ b/motionplan/tpspace/ptg.go @@ -174,6 +174,21 @@ func PTGSegmentMetric(segment *ik.Segment) float64 { return segment.EndConfiguration[len(segment.EndConfiguration)-1].Value } +// NewPTGDistanceMetric creates a metric which returns the TP-space distance traversed in a segment for a frame. Since PTG inputs are +// relative, the distance travelled is the distance field of the ending configuration. +func NewPTGDistanceMetric(ptgFrames []string) ik.SegmentFSMetric { + return func(segment *ik.SegmentFS) float64 { + score := 0. + for _, ptgFrame := range ptgFrames { + if frameCfg, ok := segment.EndConfiguration[ptgFrame]; ok { + score += frameCfg[len(frameCfg)-1].Value + } + } + // If there's no matching configuration in the end, then the frame does not move + return score + } +} + // PTGIKSeed will generate a consistent set of valid, in-bounds inputs to be used with a PTGSolver as a seed for gradient descent. func PTGIKSeed(ptg PTGSolver) []referenceframe.Input { inputs := []referenceframe.Input{} diff --git a/motionplan/utils.go b/motionplan/utils.go index 75cf1301104..c9e92cc490b 100644 --- a/motionplan/utils.go +++ b/motionplan/utils.go @@ -3,8 +3,12 @@ package motionplan import ( + "errors" + "fmt" "math" + "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" ) @@ -25,63 +29,6 @@ func PathStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int { return int(nSteps) + 1 } -// fixOvIncrement will detect whether the given goal position is a precise orientation increment of the current -// position, in which case it will detect whether we are leaving a pole. If we are an OV increment and leaving a pole, -// then Theta will be adjusted to give an expected smooth movement. The adjusted goal will be returned. Otherwise the -// original goal is returned. -// Rationale: if clicking the increment buttons in the interface, the user likely wants the most intuitive motion -// posible. If setting values manually, the user likely wants exactly what they requested. -func fixOvIncrement(goal, seed spatialmath.Pose) spatialmath.Pose { - epsilon := 0.01 - goalPt := goal.Point() - goalOrientation := goal.Orientation().OrientationVectorDegrees() - seedPt := seed.Point() - seedOrientation := seed.Orientation().OrientationVectorDegrees() - - // Nothing to do for spatial translations or theta increments - r := utils.Float64AlmostEqual(goalOrientation.OZ, seedOrientation.OZ, epsilon) - _ = r - if !spatialmath.R3VectorAlmostEqual(goalPt, seedPt, epsilon) || - !utils.Float64AlmostEqual(goalOrientation.Theta, seedOrientation.Theta, epsilon) { - return goal - } - // Check if seed is pointing directly at pole - if 1-math.Abs(seedOrientation.OZ) > epsilon || !utils.Float64AlmostEqual(goalOrientation.OZ, seedOrientation.OZ, epsilon) { - return goal - } - - // we only care about negative xInc - xInc := goalOrientation.OX - seedOrientation.OX - yInc := math.Abs(goalOrientation.OY - seedOrientation.OY) - var adj float64 - if utils.Float64AlmostEqual(goalOrientation.OX, seedOrientation.OX, epsilon) { - // no OX movement - if !utils.Float64AlmostEqual(yInc, 0.1, epsilon) && !utils.Float64AlmostEqual(yInc, 0.01, epsilon) { - // nonstandard increment - return goal - } - // If wanting to point towards +Y and OZ<0, add 90 to theta, otherwise subtract 90 - if goalOrientation.OY-seedOrientation.OY > 0 { - adj = 90 - } else { - adj = -90 - } - } else { - if (!utils.Float64AlmostEqual(xInc, -0.1, epsilon) && !utils.Float64AlmostEqual(xInc, -0.01, epsilon)) || - !utils.Float64AlmostEqual(goalOrientation.OY, seedOrientation.OY, epsilon) { - return goal - } - // If wanting to point towards -X, increment by 180. Values over 180 or under -180 will be automatically wrapped - adj = 180 - } - if goalOrientation.OZ > 0 { - adj *= -1 - } - goalOrientation.Theta += adj - - return spatialmath.NewPose(goalPt, goalOrientation) -} - type resultPromise struct { steps []node future chan *rrtSolution @@ -98,3 +45,245 @@ func (r *resultPromise) result() ([]node, error) { } return planReturn.steps, nil } + +// linearizedFrameSystem wraps a framesystem, allowing conversion in a known order between a map[string][]inputs and a flat array of floats, +// useful for being able to call IK solvers against framesystems. +type linearizedFrameSystem struct { + fss referenceframe.FrameSystem + frames []referenceframe.Frame // cached ordering of frames. Order is unimportant but cannot change once set. + dof []referenceframe.Limit +} + +func newLinearizedFrameSystem(fss referenceframe.FrameSystem) (*linearizedFrameSystem, error) { + frames := []referenceframe.Frame{} + dof := []referenceframe.Limit{} + for _, fName := range fss.FrameNames() { + frame := fss.Frame(fName) + if frame == nil { + return nil, fmt.Errorf("frame %s was returned in list of frame names, but was not found in frame system", fName) + } + frames = append(frames, frame) + dof = append(dof, frame.DoF()...) + } + return &linearizedFrameSystem{ + fss: fss, + frames: frames, + dof: dof, + }, nil +} + +// mapToSlice will flatten a map of inputs into a slice suitable for input to inverse kinematics, by concatenating +// the inputs together in the order of the frames in sf.frames. +func (lfs *linearizedFrameSystem) mapToSlice(inputs map[string][]referenceframe.Input) ([]float64, error) { + var floatSlice []float64 + for _, frame := range lfs.frames { + if len(frame.DoF()) == 0 { + continue + } + input, ok := inputs[frame.Name()] + if !ok { + return nil, fmt.Errorf("frame %s missing from input map", frame.Name()) + } + for _, i := range input { + floatSlice = append(floatSlice, i.Value) + } + } + return floatSlice, nil +} + +func (lfs *linearizedFrameSystem) sliceToMap(floatSlice []float64) (map[string][]referenceframe.Input, error) { + inputs := map[string][]referenceframe.Input{} + i := 0 + for _, frame := range lfs.frames { + if len(frame.DoF()) == 0 { + continue + } + frameInputs := make([]referenceframe.Input, len(frame.DoF())) + for j := range frame.DoF() { + if i >= len(floatSlice) { + return nil, fmt.Errorf("not enough values in float slice for frame %s", frame.Name()) + } + frameInputs[j] = referenceframe.Input{Value: floatSlice[i]} + i++ + } + inputs[frame.Name()] = frameInputs + } + return inputs, nil +} + +// motionChain structs are meant to be ephemerally created for each individual goal in a motion request, and calculates the shortest +// path between components in the framesystem allowing knowledge of which frames may move. +type motionChain struct { + // List of names of all frames that could move, used for collision detection + // As an example a gripper attached to an arm which is moving relative to World, would not be in frames below but in this object + movingFS referenceframe.FrameSystem + frames []referenceframe.Frame // all frames directly between and including solveFrame and goalFrame. Order not important. + solveFrameName string + goalFrameName string + // If this is true, then goals are translated to their position in `World` before solving. + // This is useful when e.g. moving a gripper relative to a point seen by a camera built into that gripper + // TODO(pl): explore allowing this to be frames other than world + worldRooted bool +} + +func motionChainFromGoal(fs referenceframe.FrameSystem, moveFrame string, goal *referenceframe.PoseInFrame) (*motionChain, error) { + // get goal frame + goalFrame := fs.Frame(goal.Parent()) + if goalFrame == nil { + return nil, referenceframe.NewFrameMissingError(goal.Parent()) + } + goalFrameList, err := fs.TracebackFrame(goalFrame) + if err != nil { + return nil, err + } + + // get solve frame + solveFrame := fs.Frame(moveFrame) + if solveFrame == nil { + return nil, referenceframe.NewFrameMissingError(moveFrame) + } + solveFrameList, err := fs.TracebackFrame(solveFrame) + if err != nil { + return nil, err + } + if len(solveFrameList) == 0 { + return nil, errors.New("solveFrameList was empty") + } + + movingFS := func(frameList []referenceframe.Frame) (referenceframe.FrameSystem, error) { + // Find first moving frame + var moveF referenceframe.Frame + for i := len(frameList) - 1; i >= 0; i-- { + if len(frameList[i].DoF()) != 0 { + moveF = frameList[i] + break + } + } + if moveF == nil { + return referenceframe.NewEmptyFrameSystem(""), nil + } + return fs.FrameSystemSubset(moveF) + } + + // find pivot frame between goal and solve frames + var moving referenceframe.FrameSystem + var frames []referenceframe.Frame + worldRooted := false + pivotFrame, err := findPivotFrame(solveFrameList, goalFrameList) + if err != nil { + return nil, err + } + if pivotFrame.Name() == referenceframe.World { + frames = uniqInPlaceSlice(append(solveFrameList, goalFrameList...)) + moving, err = movingFS(solveFrameList) + if err != nil { + return nil, err + } + movingSubset2, err := movingFS(goalFrameList) + if err != nil { + return nil, err + } + if err = moving.MergeFrameSystem(movingSubset2, moving.World()); err != nil { + return nil, err + } + } else { + dof := 0 + var solveMovingList []referenceframe.Frame + var goalMovingList []referenceframe.Frame + + // Get minimal set of frames from solve frame to goal frame + for _, frame := range solveFrameList { + if frame == pivotFrame { + break + } + dof += len(frame.DoF()) + frames = append(frames, frame) + solveMovingList = append(solveMovingList, frame) + } + for _, frame := range goalFrameList { + if frame == pivotFrame { + break + } + dof += len(frame.DoF()) + frames = append(frames, frame) + goalMovingList = append(goalMovingList, frame) + } + + // If shortest path has 0 dof (e.g. a camera attached to a gripper), translate goal to world frame + if dof == 0 { + worldRooted = true + frames = solveFrameList + moving, err = movingFS(solveFrameList) + if err != nil { + return nil, err + } + } else { + // Get all child nodes of pivot node + moving, err = movingFS(solveMovingList) + if err != nil { + return nil, err + } + movingSubset2, err := movingFS(goalMovingList) + if err != nil { + return nil, err + } + if err = moving.MergeFrameSystem(movingSubset2, moving.World()); err != nil { + return nil, err + } + } + } + + return &motionChain{ + movingFS: moving, + frames: frames, + solveFrameName: solveFrame.Name(), + goalFrameName: goalFrame.Name(), + worldRooted: worldRooted, + }, nil +} + +// findPivotFrame finds the first common frame in two ordered lists of frames. +func findPivotFrame(frameList1, frameList2 []referenceframe.Frame) (referenceframe.Frame, error) { + // find shorter list + shortList := frameList1 + longList := frameList2 + if len(frameList1) > len(frameList2) { + shortList = frameList2 + longList = frameList1 + } + + // cache names seen in shorter list + nameSet := make(map[string]struct{}, len(shortList)) + for _, frame := range shortList { + nameSet[frame.Name()] = struct{}{} + } + + // look for already seen names in longer list + for _, frame := range longList { + if _, ok := nameSet[frame.Name()]; ok { + return frame, nil + } + } + return nil, errors.New("no path from solve frame to goal frame") +} + +// uniqInPlaceSlice will deduplicate the values in a slice using in-place replacement on the slice. This is faster than +// a solution using append(). +// This function does not remove anything from the input slice, but it does rearrange the elements. +func uniqInPlaceSlice(s []referenceframe.Frame) []referenceframe.Frame { + seen := make(map[referenceframe.Frame]struct{}, len(s)) + j := 0 + for _, v := range s { + if _, ok := seen[v]; ok { + continue + } + seen[v] = struct{}{} + s[j] = v + j++ + } + return s[:j] +} + +func nodeConfigurationDistanceFunc(node1, node2 node) float64 { + return ik.FSConfigurationL2Distance(&ik.SegmentFS{StartConfiguration: node1.Q(), EndConfiguration: node2.Q()}) +} diff --git a/motionplan/utils_test.go b/motionplan/utils_test.go deleted file mode 100644 index 54bf9211002..00000000000 --- a/motionplan/utils_test.go +++ /dev/null @@ -1,63 +0,0 @@ -package motionplan - -import ( - "testing" - - commonpb "go.viam.com/api/common/v1" - "go.viam.com/test" - - "go.viam.com/rdk/spatialmath" -) - -func TestFixOvIncrement(t *testing.T) { - pos1 := commonpb.Pose{ - X: -66, - Y: -133, - Z: 372, - Theta: 15, - OX: 0, - OY: 1, - OZ: 0, - } - pos2 := commonpb.Pose{ - X: pos1.X, - Y: pos1.Y, - Z: pos1.Z, - Theta: pos1.Theta, - OX: pos1.OX, - OY: pos1.OY, - OZ: pos1.OZ, - } - - // Increment, but we're not pointing at Z axis, so should do nothing - pos2.OX = -0.1 - outpos := fixOvIncrement(spatialmath.NewPoseFromProtobuf(&pos2), spatialmath.NewPoseFromProtobuf(&pos1)) - test.That(t, outpos, test.ShouldResemble, spatialmath.NewPoseFromProtobuf(&pos2)) - - // point at positive Z axis, decrement OX, should subtract 180 - pos1.OZ = 1 - pos2.OZ = 1 - pos1.OY = 0 - pos2.OY = 0 - outpos = fixOvIncrement(spatialmath.NewPoseFromProtobuf(&pos2), spatialmath.NewPoseFromProtobuf(&pos1)) - test.That(t, outpos.Orientation().OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, -165) - - // Spatial translation is incremented, should do nothing - pos2.X -= 0.1 - outpos = fixOvIncrement(spatialmath.NewPoseFromProtobuf(&pos2), spatialmath.NewPoseFromProtobuf(&pos1)) - test.That(t, outpos, test.ShouldResemble, spatialmath.NewPoseFromProtobuf(&pos2)) - - // Point at -Z, increment OY - pos2.X += 0.1 - pos2.OX += 0.1 - pos1.OZ = -1 - pos2.OZ = -1 - pos2.OY = 0.1 - outpos = fixOvIncrement(spatialmath.NewPoseFromProtobuf(&pos2), spatialmath.NewPoseFromProtobuf(&pos1)) - test.That(t, outpos.Orientation().OrientationVectorDegrees().Theta, test.ShouldAlmostEqual, 105) - - // OX and OY are both incremented, should do nothing - pos2.OX += 0.1 - outpos = fixOvIncrement(spatialmath.NewPoseFromProtobuf(&pos2), spatialmath.NewPoseFromProtobuf(&pos1)) - test.That(t, outpos, test.ShouldResemble, spatialmath.NewPoseFromProtobuf(&pos2)) -} diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 5a06a5f0fe4..3578c10b73f 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -481,6 +481,30 @@ func StartPositions(fs FrameSystem) map[string][]Input { return positions } +// InterpolateFS interpolates. +func InterpolateFS(fs FrameSystem, from, to map[string][]Input, by float64) (map[string][]Input, error) { + interp := make(map[string][]Input) + for fn, fromInputs := range from { + if len(fromInputs) == 0 { + continue + } + frame := fs.Frame(fn) + if frame == nil { + return nil, NewFrameMissingError(fn) + } + toInputs, ok := to[fn] + if !ok { + return nil, fmt.Errorf("frame with name %s not found in `to` interpolation inputs", fn) + } + interpInputs, err := frame.Interpolate(fromInputs, toInputs, by) + if err != nil { + return nil, err + } + interp[fn] = interpInputs + } + return interp, nil +} + // FrameSystemToPCD takes in a framesystem and returns a map where all elements are // the point representation of their geometry type with respect to the world. func FrameSystemToPCD(system FrameSystem, inputs map[string][]Input, logger logging.Logger) (map[string][]r3.Vector, error) { diff --git a/referenceframe/input.go b/referenceframe/input.go index 9ce24f4c880..19b3ab28446 100644 --- a/referenceframe/input.go +++ b/referenceframe/input.go @@ -112,7 +112,7 @@ func GetFrameInputs(frame Frame, inputMap map[string][]Input) ([]Input, error) { return input, nil } -// InputsL2Distance returns the square of the two-norm between the from and to vectors. +// InputsL2Distance returns the square of the two-norm (the sqrt of the sum of the squares) between two Input sets. func InputsL2Distance(from, to []Input) float64 { if len(from) != len(to) { return math.Inf(1) diff --git a/referenceframe/transformable.go b/referenceframe/transformable.go index 0c548c21d14..10922413b57 100644 --- a/referenceframe/transformable.go +++ b/referenceframe/transformable.go @@ -31,6 +31,14 @@ func NewPoseInFrame(frame string, pose spatialmath.Pose) *PoseInFrame { } } +// NewZeroPoseInFrame is a convenience method that creates a PoseInFrame with the specified Frame and a zero pose. +func NewZeroPoseInFrame(frame string) *PoseInFrame { + return &PoseInFrame{ + parent: frame, + pose: spatialmath.NewZeroPose(), + } +} + // Parent returns the name of the frame in which the pose was observed. Needed for Transformable interface. func (pF *PoseInFrame) Parent() string { return pF.parent