for small movements, focus on smaller joint movements in one thread

Co-authored-by: Peter LoVerso <[email protected]>

Author: erh

motionplan/armplanning/cBiRRT.go

@@ -367,7 +367,7 @@ func (mp *cBiRRTMotionPlanner) constrainNear(
 		}
 
 		// Spawn the IK solver to generate solutions until done
-		err = mp.fastGradDescent.Solve(ctx, solutionGen, linearSeed, mp.linearizeFSmetric(mp.ConstraintHandler.pathMetric), randseed.Int())
+		err = mp.fastGradDescent.Solve(ctx, solutionGen, linearSeed, 0, 0, mp.linearizeFSmetric(mp.ConstraintHandler.pathMetric), randseed.Int())
 		// We should have zero or one solutions
 		var solved *ik.Solution
 		select {

motionplan/armplanning/cmd-plan/cmd-plan.go

@@ -11,6 +11,7 @@ import (
 	"time"
 
 	"go.viam.com/rdk/logging"
+	"go.viam.com/rdk/motionplan"
 	"go.viam.com/rdk/motionplan/armplanning"
 	"go.viam.com/rdk/referenceframe"
 )
@@ -26,11 +27,17 @@ func realMain() error {
 	ctx := context.Background()
 	logger := logging.NewLogger("cmd-plan")
 
+	pseudolinearLine := flag.Float64("pseudolinear-line", 0, "")
+	pseudolinearOrientation := flag.Float64("pseudolinear-orientation", 0, "")
+	seed := flag.Int("seed", 0, "")
+
 	flag.Parse()
 	if len(flag.Args()) == 0 {
 		return fmt.Errorf("need a json file")
 	}
 
+	logger.Infof("reading plan from %s", flag.Arg(0))
+
 	content, err := os.ReadFile(flag.Arg(0))
 	if err != nil {
 		return err
@@ -43,6 +50,12 @@ func realMain() error {
 		return err
 	}
 
+	if *pseudolinearLine > 0 || *pseudolinearOrientation > 0 {
+		req.Constraints.AddPseudolinearConstraint(motionplan.PseudolinearConstraint{*pseudolinearLine, *pseudolinearOrientation})
+	}
+
+	req.PlannerOptions.RandomSeed = *seed
+
 	start := time.Now()
 
 	plan, err := armplanning.PlanMotion(ctx, logger, &req)
@@ -76,9 +89,10 @@ func realMain() error {
 			mylog.Printf("\t\t\t %v", t[c])
 			if idx > 0 {
 				p := plan.Trajectory()[idx-1][c]
-				mylog.Printf("\t\t\t\t distances l2: %0.4f Linf %0.4f",
+				mylog.Printf("\t\t\t\t distances l2: %0.4f Linf %0.4f cartesion: %0.2f",
 					referenceframe.InputsL2Distance(p, t[c]),
 					referenceframe.InputsLinfDistance(p, t[c]),
+					pp.Pose().Point().Distance(plan.Path()[idx-1][c].Pose().Point()),
 				)
 			}
 		}