RSDK-4051 tp space should be able to continuously sample ptgs

components/base/kinematicbase/ptgKinematics.go

@@ -14,6 +14,7 @@ import (
 	utils "go.viam.com/utils"
 
 	"go.viam.com/rdk/components/base"
+	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/motionplan/tpspace"
 	"go.viam.com/rdk/referenceframe"
 	rdkutils "go.viam.com/rdk/utils"
@@ -74,11 +75,12 @@ func wrapWithPTGKinematics(
 		return nil, err
 	}
 
-	frame, err := referenceframe.NewPTGFrameFromTurningRadius(
+	frame, err := motionplan.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 +124,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

@@ -74,7 +74,7 @@ func newCBiRRTMotionPlanner(
 		return nil, err
 	}
 	// nlopt should try only once
-	nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold)
+	nlopt, err := CreateNloptIKSolver(frame, logger, 1, opt.GoalThreshold, false)
 	if err != nil {
 		return nil, err
 	}
@@ -223,7 +223,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 		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
 		}
@@ -356,11 +356,11 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		if ok {
 			return target
 		}
-		solutionGen := make(chan []referenceframe.Input, 1)
+		solutionGen := make(chan *IKSolution, 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 *IKSolution
 		select {
 		case solved = <-solutionGen:
 		default:
@@ -371,11 +371,11 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		}
 
 		ok, failpos := mp.planOpts.CheckSegmentAndStateValidity(
-			&Segment{StartConfiguration: seedInputs, EndConfiguration: solved, Frame: mp.frame},
+			&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})

motionplan/combinedInverseKinematics.go

@@ -30,7 +30,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP
 		nCPU = 1
 	}
 	for i := 1; i <= nCPU; i++ {
-		nlopt, err := CreateNloptIKSolver(model, logger, -1, goalThreshold)
+		nlopt, err := CreateNloptIKSolver(model, logger, -1, goalThreshold, false)
 		nlopt.id = i
 		if err != nil {
 			return nil, err
@@ -43,7 +43,7 @@ func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCP
 
 func runSolver(ctx context.Context,
 	solver InverseKinematics,
-	c chan<- []referenceframe.Input,
+	c chan<- *IKSolution,
 	seed []referenceframe.Input,
 	m StateMetric,
 	rseed int,
@@ -54,7 +54,7 @@ func runSolver(ctx context.Context,
 // Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint
 // positions. If unable to solve, the returned error will be non-nil.
 func (ik *CombinedIK) Solve(ctx context.Context,
-	c chan<- []referenceframe.Input,
+	c chan<- *IKSolution,
 	seed []referenceframe.Input,
 	m StateMetric,
 	rseed int,
@@ -63,7 +63,7 @@ func (ik *CombinedIK) Solve(ctx context.Context,
 	ctxWithCancel, cancel := context.WithCancel(ctx)
 	defer cancel()
 
-	errChan := make(chan error, len(ik.solvers))
+	errChan := make(chan error, len(ik.solvers)+1)
 	var activeSolvers sync.WaitGroup
 	defer activeSolvers.Wait()
 	activeSolvers.Add(len(ik.solvers))
@@ -89,6 +89,7 @@ func (ik *CombinedIK) Solve(ctx context.Context,
 	for !done {
 		select {
 		case <-ctx.Done():
+			activeSolvers.Wait()
 			return ctx.Err()
 		default:
 		}
@@ -110,6 +111,7 @@ func (ik *CombinedIK) Solve(ctx context.Context,
 		// Collect return errors from all solvers
 		select {
 		case <-ctx.Done():
+			activeSolvers.Wait()
 			return ctx.Err()
 		default:
 		}
@@ -120,6 +122,7 @@ func (ik *CombinedIK) Solve(ctx context.Context,
 			collectedErrs = multierr.Combine(collectedErrs, err)
 		}
 	}
+	activeSolvers.Wait()
 	return collectedErrs
 }
 

motionplan/errors.go

@@ -13,6 +13,12 @@ var (
 	errNoPlannerOptions = errors.New("PlannerOptions are required but have not been specified")
 
 	errIKConstraint = "all IK solutions failed constraints. Failures: "
+
+	errNoNeighbors = errors.New("no neighbors found")
+
+	errInvalidCandidate = errors.New("candidate did not meet constraints")
+
+	errNoCandidates = errors.New("no candidates passed in, skipping")
 )
 
 func genIKConstraintErr(failures map[string]int, constraintFailCnt int) error {

motionplan/inverseKinematics.go

@@ -10,7 +10,15 @@ import (
 // solutions to the provided channel until cancelled or otherwise completes.
 type InverseKinematics interface {
 	// Solve receives a context, the goal arm position, and current joint angles.
-	Solve(context.Context, chan<- []referenceframe.Input, []referenceframe.Input, StateMetric, int) error
+	Solve(context.Context, chan<- *IKSolution, []referenceframe.Input, StateMetric, int) error
+}
+
+// IKSolution is the struct returned from an IK solver. It contains the solution configuration, the score of the solution, and a flag
+// indicating whether that configuration and score met the solution criteria requested by the caller.
+type IKSolution struct {
+	Configuration []referenceframe.Input
+	Score         float64
+	Partial       bool
 }
 
 func limitsToArrays(limits []referenceframe.Limit) ([]float64, []float64) {

motionplan/kinematic_test.go

@@ -354,7 +354,7 @@ func TestCombinedCPUs(t *testing.T) {
 }
 
 func solveTest(ctx context.Context, solver InverseKinematics, goal spatial.Pose, seed []frame.Input) ([][]frame.Input, error) {
-	solutionGen := make(chan []frame.Input)
+	solutionGen := make(chan *IKSolution)
 	ikErr := make(chan error)
 	ctxWithCancel, cancel := context.WithCancel(ctx)
 	defer cancel()
@@ -378,7 +378,7 @@ IK:
 
 		select {
 		case step := <-solutionGen:
-			solutions = append(solutions, step)
+			solutions = append(solutions, step.Configuration)
 			// Skip the return check below until we have nothing left to read from solutionGen
 			continue IK
 		default:

motionplan/metrics.go

@@ -113,11 +113,19 @@ func L2InputMetric(segment *Segment) float64 {
 	return referenceframe.InputsL2Distance(segment.StartConfiguration, segment.EndConfiguration)
 }
 
-// SquaredNormSegmentMetric is a metric which will return the cartesian distance between the two positions.
-func SquaredNormSegmentMetric(segment *Segment) float64 {
-	delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition)
-	// Increase weight for orientation since it's a small number
-	return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(orientationDistanceScaling).Norm2()
+// NewSquaredNormSegmentMetric returns a metric which will return the cartesian distance between the two positions.
+func NewSquaredNormSegmentMetric() SegmentMetric {
+	return NewSquaredNormSegmentMetricWithScaling(orientationDistanceScaling)
+}
+
+// NewSquaredNormSegmentMetricWithScaling returns a metric which will return the cartesian distance between the two positions.
+// It allows the caller to choose the scaling level of orientation.
+func NewSquaredNormSegmentMetricWithScaling(scaleFactor float64) SegmentMetric {
+	return func(segment *Segment) float64 {
+		delta := spatial.PoseDelta(segment.StartPosition, segment.EndPosition)
+		// Increase weight for orientation since it's a small number
+		return delta.Point().Norm2() + spatial.QuatToR3AA(delta.Orientation().Quaternion()).Mul(scaleFactor).Norm2()
+	}
 }
 
 // SquaredNormNoOrientSegmentMetric is a metric which will return the cartesian distance between the two positions.

motionplan/motionPlanner.go

@@ -5,6 +5,7 @@ import (
 	"context"
 	"math/rand"
 	"sort"
+	"sync"
 	"time"
 
 	"github.com/edaniels/golog"
@@ -265,11 +266,15 @@ func (mp *planner) getSolutions(ctx context.Context, seed []frame.Input) ([]node
 	ctxWithCancel, cancel := context.WithCancel(ctx)
 	defer cancel()
 
-	solutionGen := make(chan []frame.Input)
+	solutionGen := make(chan *IKSolution, mp.planOpts.NumThreads)
 	ikErr := make(chan error, 1)
+	var activeSolvers sync.WaitGroup
+	defer activeSolvers.Wait()
+	activeSolvers.Add(1)
 	// Spawn the IK solver to generate solutions until done
 	utils.PanicCapturingGo(func() {
 		defer close(ikErr)
+		defer activeSolvers.Done()
 		ikErr <- mp.solver.Solve(ctxWithCancel, solutionGen, seed, mp.planOpts.goalMetric, mp.randseed.Int())
 	})
 
@@ -289,7 +294,8 @@ IK:
 		}
 
 		select {
-		case step := <-solutionGen:
+		case stepSolution := <-solutionGen:
+			step := stepSolution.Configuration
 			// Ensure the end state is a valid one
 			statePass, failName := mp.planOpts.CheckStateConstraints(&State{
 				Configuration: step,

motionplan/nloptInverseKinematics.go

@@ -38,12 +38,16 @@ type NloptIK struct {
 	solveEpsilon  float64
 	logger        golog.Logger
 	jump          float64
+
+	// Nlopt will try to minimize a configuration for whatever is passed in. If partial is true, then the solver will emit partial
+	// solutions where it was not able to meet the goal criteria but still was able to improve upon the seed.
+	partial bool
 }
 
 // CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on
 // which Transform() will be called, a logger, and the number of iterations to run. If the iteration count is less than 1, it will be set
 // to the default of 5000.
-func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64) (*NloptIK, error) {
+func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64, partial bool) (*NloptIK, error) {
 	ik := &NloptIK{logger: logger}
 
 	ik.model = mdl
@@ -60,15 +64,16 @@ func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int
 	ik.lowerBound, ik.upperBound = limitsToArrays(mdl.DoF())
 	// How much to adjust joints to determine slope
 	ik.jump = 0.00000001
+	ik.partial = partial
 
 	return ik, nil
 }
 
 // Solve runs the actual solver and sends any solutions found to the given channel.
 func (ik *NloptIK) Solve(ctx context.Context,
-	c chan<- []referenceframe.Input,
+	solutionChan chan<- *IKSolution,
 	seed []referenceframe.Input,
-	m StateMetric,
+	solveMetric StateMetric,
 	rseed int,
 ) error {
 	//nolint: gosec
@@ -109,7 +114,7 @@ func (ik *NloptIK) Solve(ctx context.Context,
 		}
 		mInput.Configuration = inputs
 		mInput.Position = eePos
-		dist := m(mInput)
+		dist := solveMetric(mInput)
 
 		if len(gradient) > 0 {
 			for i := range gradient {
@@ -125,7 +130,7 @@ func (ik *NloptIK) Solve(ctx context.Context,
 				}
 				mInput.Configuration = inputs
 				mInput.Position = eePos
-				dist2 := m(mInput)
+				dist2 := solveMetric(mInput)
 
 				gradient[i] = (dist2 - dist) / ik.jump
 			}
@@ -186,11 +191,16 @@ func (ik *NloptIK) Solve(ctx context.Context,
 			err = multierr.Combine(err, nloptErr)
 		}
 
-		if result < ik.epsilon*ik.epsilon {
+		if result < ik.epsilon*ik.epsilon || (solutionRaw != nil && ik.partial) {
 			select {
 			case <-ctx.Done():
 				return err
-			case c <- referenceframe.FloatsToInputs(solutionRaw):
+			default:
+			}
+			solutionChan <- &IKSolution{
+				Configuration: referenceframe.FloatsToInputs(solutionRaw),
+				Score:         result,
+				Partial:       result >= ik.epsilon*ik.epsilon,
 			}
 			solutionsFound++
 		}

motionplan/nloptInverseKinematics_test.go

@@ -18,7 +18,7 @@ func TestCreateNloptIKSolver(t *testing.T) {
 	logger := golog.NewTestLogger(t)
 	m, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/xarm/xarm6_kinematics.json"), "")
 	test.That(t, err, test.ShouldBeNil)
-	ik, err := CreateNloptIKSolver(m, logger, -1, defaultGoalThreshold)
+	ik, err := CreateNloptIKSolver(m, logger, -1, defaultGoalThreshold, false)
 	test.That(t, err, test.ShouldBeNil)
 	ik.id = 1