RSDK-4051 tp space should be able to continuously sample ptgs (#2792)

Author: biotinker

components/base/kinematicbase/differentialDrive.go

@@ -13,7 +13,7 @@ import (
 	utils "go.viam.com/utils"
 
 	"go.viam.com/rdk/components/base"
-	"go.viam.com/rdk/motionplan"
+	"go.viam.com/rdk/motionplan/ik"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/services/motion"
 	"go.viam.com/rdk/spatialmath"
@@ -186,7 +186,7 @@ func (ddk *differentialDriveKinematics) GoToInputs(ctx context.Context, desired
 		if prevInputs == nil {
 			prevInputs = currentInputs
 		}
-		positionChange := motionplan.L2InputMetric(&motionplan.Segment{
+		positionChange := ik.L2InputMetric(&ik.Segment{
 			StartConfiguration: prevInputs,
 			EndConfiguration:   currentInputs,
 		})

components/base/kinematicbase/ptgKinematics.go

@@ -74,11 +74,12 @@ func wrapWithPTGKinematics(
 		return nil, err
 	}
 
-	frame, err := referenceframe.NewPTGFrameFromTurningRadius(
+	frame, err := tpspace.NewPTGFrameFromTurningRadius(
 		b.Name().ShortName(),
+		logger,
 		baseMillimetersPerSecond,
 		baseTurningRadius,
-		0, // pass 0 to use the default
+		0, // pass 0 to use the default refDist
 		geometries,
 	)
 	if err != nil {
@@ -122,15 +123,15 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputs []referenc
 	ptgk.logger.Debugf("GoToInputs going to %v", inputs)
 
 	selectedPTG := ptgk.ptgs[int(math.Round(inputs[ptgIndex].Value))]
-	selectedTraj := selectedPTG.Trajectory(uint(math.Round(inputs[trajectoryIndexWithinPTG].Value)))
+	selectedTraj, err := selectedPTG.Trajectory(inputs[trajectoryIndexWithinPTG].Value, inputs[distanceAlongTrajectoryIndex].Value)
+	if err != nil {
+		return multierr.Combine(err, ptgk.Base.Stop(ctx, nil))
+	}
 
 	lastTime := 0.
-	for i, trajNode := range selectedTraj {
-		if trajNode.Dist > inputs[distanceAlongTrajectoryIndex].Value {
-			ptgk.logger.Debugf("finished executing trajectory after %d steps", i)
-			// We have reached the desired distance along the given trajectory
-			break
-		}
+	for _, trajNode := range selectedTraj {
+		// TODO: Most trajectories update their velocities infrequently, or sometimes never.
+		// This function could be improved by looking ahead through the trajectory and minimizing the amount of SetVelocity calls.
 		timestep := time.Duration((trajNode.Time-lastTime)*1000*1000) * time.Microsecond
 		lastTime = trajNode.Time
 		linVel := r3.Vector{0, trajNode.LinVelMMPS, 0}

components/base/kinematicbase/ptgKinematics_test.go

@@ -51,3 +51,53 @@ func TestPTGKinematics(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, plan, test.ShouldNotBeNil)
 }
+
+func TestPTGKinematicsWithGeom(t *testing.T) {
+	logger := golog.NewTestLogger(t)
+
+	name, err := resource.NewFromString("is:a:fakebase")
+	test.That(t, err, test.ShouldBeNil)
+
+	baseGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{1, 1, 1}, "")
+	test.That(t, err, test.ShouldBeNil)
+
+	b := &fake.Base{
+		Named:         name.AsNamed(),
+		Geometry:      []spatialmath.Geometry{baseGeom},
+		WidthMeters:   0.2,
+		TurningRadius: 0.3,
+	}
+
+	ctx := context.Background()
+
+	kbOpt := NewKinematicBaseOptions()
+	kbOpt.AngularVelocityDegsPerSec = 0
+	kb, err := WrapWithKinematics(ctx, b, logger, nil, nil, kbOpt)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, kb, test.ShouldNotBeNil)
+	ptgBase, ok := kb.(*ptgBaseKinematics)
+	test.That(t, ok, test.ShouldBeTrue)
+	test.That(t, ptgBase, test.ShouldNotBeNil)
+
+	dstPIF := referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPoseFromPoint(r3.Vector{X: 2000, Y: 0, Z: 0}))
+
+	fs := referenceframe.NewEmptyFrameSystem("test")
+	f := kb.Kinematics()
+	test.That(t, err, test.ShouldBeNil)
+	fs.AddFrame(f, fs.World())
+	inputMap := referenceframe.StartPositions(fs)
+
+	obstacle, err := spatialmath.NewBox(spatialmath.NewPoseFromPoint(r3.Vector{1000, 0, 0}), r3.Vector{1, 1, 1}, "")
+	test.That(t, err, test.ShouldBeNil)
+
+	geoms := []spatialmath.Geometry{obstacle}
+	worldState, err := referenceframe.NewWorldState(
+		[]*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)},
+		nil,
+	)
+	test.That(t, err, test.ShouldBeNil)
+
+	plan, err := motionplan.PlanMotion(ctx, logger, dstPIF, f, inputMap, fs, worldState, nil, nil)
+	test.That(t, err, test.ShouldBeNil)
+	test.That(t, plan, test.ShouldNotBeNil)
+}

motionplan/cBiRRT.go

@@ -13,6 +13,7 @@ import (
 	"github.com/edaniels/golog"
 	"go.viam.com/utils"
 
+	"go.viam.com/rdk/motionplan/ik"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/spatialmath"
 )
@@ -55,7 +56,7 @@ func newCbirrtOptions(planOpts *plannerOptions) (*cbirrtOptions, error) {
 // https://ieeexplore.ieee.org/document/5152399/
 type cBiRRTMotionPlanner struct {
 	*planner
-	fastGradDescent *NloptIK
+	fastGradDescent *ik.NloptIK
 	algOpts         *cbirrtOptions
 }
 
@@ -74,7 +75,7 @@ func newCBiRRTMotionPlanner(
 		return nil, err
 	}
 	// nlopt should try only once
-	nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold)
+	nlopt, err := ik.CreateNloptIKSolver(frame, logger, 1, true)
 	if err != nil {
 		return nil, err
 	}
@@ -132,7 +133,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 		rrt.maps = planSeed.maps
 	}
 	mp.logger.Infof("goal node: %v\n", rrt.maps.optNode.Q())
-	target := referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5)
+	target := newConfigurationNode(referenceframe.InterpolateInputs(seed, rrt.maps.optNode.Q(), 0.5))
 
 	map1, map2 := rrt.maps.startMap, rrt.maps.goalMap
 
@@ -163,13 +164,13 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 		default:
 		}
 
-		tryExtend := func(target []referenceframe.Input) (node, node, error) {
+		tryExtend := func(target node) (node, node, error) {
 			// attempt to extend maps 1 and 2 towards the target
 			utils.PanicCapturingGo(func() {
-				m1chan <- nm1.nearestNeighbor(nmContext, mp.planOpts, newConfigurationNode(target), map1)
+				m1chan <- nm1.nearestNeighbor(nmContext, mp.planOpts, target, map1)
 			})
 			utils.PanicCapturingGo(func() {
-				m2chan <- nm2.nearestNeighbor(nmContext, mp.planOpts, newConfigurationNode(target), map2)
+				m2chan <- nm2.nearestNeighbor(nmContext, mp.planOpts, target, map2)
 			})
 			nearest1 := <-m1chan
 			nearest2 := <-m2chan
@@ -186,10 +187,10 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 			rseed2 := rand.New(rand.NewSource(int64(mp.randseed.Int())))
 
 			utils.PanicCapturingGo(func() {
-				mp.constrainedExtend(ctx, rseed1, map1, nearest1, newConfigurationNode(target), m1chan)
+				mp.constrainedExtend(ctx, rseed1, map1, nearest1, target, m1chan)
 			})
 			utils.PanicCapturingGo(func() {
-				mp.constrainedExtend(ctx, rseed2, map2, nearest2, newConfigurationNode(target), m2chan)
+				mp.constrainedExtend(ctx, rseed2, map2, nearest2, target, m2chan)
 			})
 			map1reached := <-m1chan
 			map2reached := <-m2chan
@@ -206,24 +207,24 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 			return
 		}
 
-		reachedDelta := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})
+		reachedDelta := mp.planOpts.DistanceFunc(&ik.Segment{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 {
-			target = referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5)
+			target = newConfigurationNode(referenceframe.InterpolateInputs(map1reached.Q(), map2reached.Q(), 0.5))
 			map1reached, map2reached, err = tryExtend(target)
 			if err != nil {
 				rrt.solutionChan <- &rrtPlanReturn{planerr: err, maps: rrt.maps}
 				return
 			}
-			reachedDelta = mp.planOpts.DistanceFunc(&Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})
+			reachedDelta = mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: map1reached.Q(), EndConfiguration: map2reached.Q()})
 		}
 
 		// Solved!
 		if reachedDelta <= mp.planOpts.JointSolveDist {
 			mp.logger.Debugf("CBiRRT found solution after %d iterations", i)
 			cancel()
-			path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached})
+			path := extractPath(rrt.maps.startMap, rrt.maps.goalMap, &nodePair{map1reached, map2reached}, true)
 			rrt.solutionChan <- &rrtPlanReturn{steps: path, maps: rrt.maps}
 			return
 		}
@@ -268,8 +269,8 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
 		default:
 		}
 
-		dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()})
-		oldDist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()})
+		dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: near.Q(), EndConfiguration: target.Q()})
+		oldDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: target.Q()})
 		switch {
 		case dist < mp.planOpts.JointSolveDist:
 			mchan <- near
@@ -286,7 +287,7 @@ func (mp *cBiRRTMotionPlanner) constrainedExtend(
 		newNear = mp.constrainNear(ctx, randseed, oldNear.Q(), newNear)
 
 		if newNear != nil {
-			nearDist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear})
+			nearDist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: oldNear.Q(), EndConfiguration: newNear})
 			if nearDist < math.Pow(mp.planOpts.JointSolveDist, 3) {
 				if !doubled {
 					doubled = true
@@ -343,7 +344,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 			return nil
 		}
 
-		newArc := &Segment{
+		newArc := &ik.Segment{
 			StartPosition:      seedPos,
 			EndPosition:        goalPos,
 			StartConfiguration: seedInputs,
@@ -356,29 +357,29 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		if ok {
 			return target
 		}
-		solutionGen := make(chan []referenceframe.Input, 1)
+		solutionGen := make(chan *ik.Solution, 1)
 		// Spawn the IK solver to generate solutions until done
 		err = mp.fastGradDescent.Solve(ctx, solutionGen, target, mp.planOpts.pathMetric, randseed.Int())
 		// We should have zero or one solutions
-		var solved []referenceframe.Input
+		var solved *ik.Solution
 		select {
 		case solved = <-solutionGen:
 		default:
 		}
 		close(solutionGen)
-		if err != nil {
+		if err != nil || solved == nil {
 			return nil
 		}
 
 		ok, failpos := mp.planOpts.CheckSegmentAndStateValidity(
-			&Segment{StartConfiguration: seedInputs, EndConfiguration: solved, Frame: mp.frame},
+			&ik.Segment{StartConfiguration: seedInputs, EndConfiguration: solved.Configuration, Frame: mp.frame},
 			mp.planOpts.Resolution,
 		)
 		if ok {
-			return solved
+			return solved.Configuration
 		}
 		if failpos != nil {
-			dist := mp.planOpts.DistanceFunc(&Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration})
+			dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: target, EndConfiguration: failpos.EndConfiguration})
 			if dist > mp.planOpts.JointSolveDist {
 				// If we have a first failing position, and that target is updating (no infinite loop), then recurse
 				seedInputs = failpos.StartConfiguration
@@ -439,7 +440,7 @@ func (mp *cBiRRTMotionPlanner) smoothPath(
 			// 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(&Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()})
+			dist := mp.planOpts.DistanceFunc(&ik.Segment{StartConfiguration: inputSteps[i].Q(), EndConfiguration: reached.Q()})
 			if dist < mp.planOpts.JointSolveDist {
 				for _, hitCorner := range hitCorners {
 					hitCorner.SetCorner(false)

motionplan/cBiRRT_test.go

@@ -10,6 +10,7 @@ import (
 	"go.viam.com/test"
 	"go.viam.com/utils"
 
+	"go.viam.com/rdk/motionplan/ik"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/spatialmath"
 	rutils "go.viam.com/rdk/utils"
@@ -38,7 +39,7 @@ 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.SetGoalMetric(NewSquaredNormMetric(goalPos))
+	opt.SetGoalMetric(ik.NewSquaredNormMetric(goalPos))
 	mp, err := newCBiRRTMotionPlanner(m, rand.New(rand.NewSource(42)), logger, opt)
 	test.That(t, err, test.ShouldBeNil)
 	cbirrt, _ := mp.(*cBiRRTMotionPlanner)
@@ -80,7 +81,7 @@ func TestSimpleLinearMotion(t *testing.T) {
 		cbirrt.constrainedExtend(ctx, cbirrt.randseed, goalMap, near2, seedReached, m1chan)
 	})
 	goalReached := <-m1chan
-	dist := opt.DistanceFunc(&Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()})
+	dist := opt.DistanceFunc(&ik.Segment{StartConfiguration: seedReached.Q(), EndConfiguration: goalReached.Q()})
 	test.That(t, dist < cbirrt.planOpts.JointSolveDist, test.ShouldBeTrue)
 
 	seedReached.SetCorner(true)