POC traj gen in motion service

components/arm/fake/fake.go

@@ -13,6 +13,7 @@ import (
 	models3d "go.viam.com/rdk/components/arm/fake/3d_models"
 	"go.viam.com/rdk/logging"
 	"go.viam.com/rdk/motionplan"
+	"go.viam.com/rdk/motionplan/armplanning"
 	"go.viam.com/rdk/referenceframe"
 	"go.viam.com/rdk/resource"
 	"go.viam.com/rdk/spatialmath"
@@ -255,6 +256,39 @@ func (a *Arm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Inp
 	return a.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
 }
 
+// DoCommand handles traj-gen do_command keys. It reports capability support and executes
+// a precomputed trajectory by moving through each configuration in order.
+func (a *Arm) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {
+	resp := map[string]interface{}{}
+
+	if _, ok := cmd[armplanning.DoCommandKeySupportsExecuteTrajGenPlan]; ok {
+		resp[armplanning.DoCommandKeySupportsExecuteTrajGenPlan] = true
+	}
+
+	if payload, ok := cmd[armplanning.DoCommandKeyExecuteTrajGenPlan]; ok {
+		payloadMap, ok := payload.(map[string]any)
+		if !ok {
+			return nil, errors.Errorf("execute_traj_gen_plan payload must be a map, got %T", payload)
+		}
+		configsRaw, ok := payloadMap["configurations_rads"]
+		if !ok {
+			return nil, errors.New("execute_traj_gen_plan payload missing configurations_rads")
+		}
+		configs, ok := configsRaw.([][]float64)
+		if !ok {
+			return nil, errors.Errorf("configurations_rads has unexpected type %T", configsRaw)
+		}
+		positions := make([][]referenceframe.Input, len(configs))
+		copy(positions, configs)
+		if err := a.MoveThroughJointPositions(ctx, positions, nil, nil); err != nil {
+			return nil, err
+		}
+		resp[armplanning.DoCommandKeyExecuteTrajGenPlan] = true
+	}
+
+	return resp, nil
+}
+
 // Close does nothing.
 func (a *Arm) Close(ctx context.Context) error {
 	a.mu.Lock()

motionplan/armplanning/api.go

@@ -183,19 +183,17 @@ type PlanMeta struct {
 }
 
 // PlanMotion plans a motion from a provided plan request.
-func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanRequest) (motionplan.Plan, *PlanMeta, error) {
-	logger := parentLogger.Sublogger("mp")
-
-	start := time.Now()
-	meta := &PlanMeta{}
-	ctx, span := trace.StartSpan(ctx, "PlanMotion")
-	defer func() {
-		meta.Duration = time.Since(start)
-		span.End()
-	}()
-
+// planWaypoints runs validation, sets up the planner, and executes multi-waypoint planning.
+// It populates meta.GoalsProcessed (and meta.Partial on a partial-plan error).
+// The span/timing setup is left to the caller so that it covers the caller's full lifetime.
+func planWaypoints(
+	ctx context.Context,
+	logger logging.Logger,
+	request *PlanRequest,
+	meta *PlanMeta,
+) ([]*referenceframe.LinearInputs, error) {
 	if err := request.validatePlanRequest(); err != nil {
-		return nil, meta, err
+		return nil, err
 	}
 	logger.CDebugf(ctx, "constraint specs for this step: %v", request.Constraints)
 	logger.CDebugf(ctx, "motion config for this step: %v", request.PlannerOptions)
@@ -209,12 +207,12 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR
 	// goal configurations. However, the blocker here is the lack of a "known good" configuration used to determine which obstacles
 	// are allowed to collide with one another.
 	if request.StartState.structuredConfiguration == nil {
-		return nil, meta, errors.New("must populate start state configuration")
+		return nil, errors.New("must populate start state configuration")
 	}
 
 	sfPlanner, err := newPlanManager(ctx, logger, request, meta)
 	if err != nil {
-		return nil, meta, err
+		return nil, err
 	}
 
 	trajAsInps, goalsProcessed, err := sfPlanner.planMultiWaypoint(ctx)
@@ -224,11 +222,30 @@ func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanR
 			meta.PartialError = err
 			logger.Infof("returning partial plan, error: %v", err)
 		} else {
-			return nil, meta, err
+			return nil, err
 		}
 	}
 
 	meta.GoalsProcessed = goalsProcessed
+	return trajAsInps, nil
+}
+
+// PlanMotion plans a motion from a provided plan request.
+func PlanMotion(ctx context.Context, parentLogger logging.Logger, request *PlanRequest) (motionplan.Plan, *PlanMeta, error) {
+	logger := parentLogger.Sublogger("mp")
+
+	start := time.Now()
+	meta := &PlanMeta{}
+	ctx, span := trace.StartSpan(ctx, "PlanMotion")
+	defer func() {
+		meta.Duration = time.Since(start)
+		span.End()
+	}()
+
+	trajAsInps, err := planWaypoints(ctx, logger, request, meta)
+	if err != nil {
+		return nil, meta, err
+	}
 
 	t, err := motionplan.NewSimplePlanFromTrajectory(trajAsInps, request.FrameSystem)
 	if err != nil {

motionplan/armplanning/check.go

@@ -0,0 +1,202 @@
+package armplanning
+
+import (
+	"context"
+	"fmt"
+	"strings"
+
+	"go.viam.com/rdk/logging"
+	"go.viam.com/rdk/motionplan"
+	"go.viam.com/rdk/referenceframe"
+	"go.viam.com/rdk/spatialmath"
+)
+
+// CheckPlanFromRequest checks a plan for collisions by interpolating between each pair of
+// consecutive trajectory steps. It is a convenience wrapper around CheckPlan that extracts the
+// necessary information from a PlanRequest.
+//
+// Moving frames are auto-detected by analysing which frames have changing inputs across the
+// trajectory. Collisions that exist at the start of the trajectory are automatically allowed
+// throughout the plan so that the arm is not penalised for pre-existing contact.
+//
+// Returns nil if no collision is found, or an error describing the first detected collision.
+func CheckPlanFromRequest(
+	ctx context.Context,
+	logger logging.Logger,
+	req *PlanRequest,
+	plan motionplan.Plan,
+) error {
+	ws := req.WorldState
+	if ws == nil {
+		ws = referenceframe.NewEmptyWorldState()
+	}
+	return CheckPlan(ctx, logger, ws, req.FrameSystem, plan)
+}
+
+// CheckPlan checks a plan for collisions by interpolating between each pair of consecutive
+// trajectory steps.
+//
+// Moving frames are auto-detected by analysing which frames have changing inputs across the
+// trajectory. Collisions that exist at the start of the trajectory are automatically allowed
+// throughout the plan.
+//
+// Returns nil if no collision is found, or an error describing the first detected collision.
+func CheckPlan(
+	ctx context.Context,
+	logger logging.Logger,
+	worldState *referenceframe.WorldState,
+	fs *referenceframe.FrameSystem,
+	plan motionplan.Plan,
+) error {
+	traj := plan.Trajectory()
+	if len(traj) < 2 {
+		return nil
+	}
+
+	// Convert to LinearInputs once for reuse.
+	linearTraj := make([]*referenceframe.LinearInputs, len(traj))
+	for i, step := range traj {
+		linearTraj[i] = step.ToLinearInputs()
+	}
+	startInputs := linearTraj[0]
+
+	// Get frame geometries at the start configuration.
+	frameSystemGeometries, err := referenceframe.FrameSystemGeometriesLinearInputs(fs, startInputs)
+	if err != nil {
+		return err
+	}
+
+	// Auto-detect which frames are actually moving in this trajectory.
+	movingFrames := detectMovingFrames(traj)
+
+	// Split geometries into moving and static based on the detected moving frames.
+	var movingGeos, staticGeos []spatialmath.Geometry
+	for _, geoms := range frameSystemGeometries {
+		for _, geom := range geoms.Geometries() {
+			if belongsToMovingFrame(geom.Label(), movingFrames) {
+				movingGeos = append(movingGeos, geom)
+			} else {
+				staticGeos = append(staticGeos, geom)
+			}
+		}
+	}
+
+	// Get world obstacle geometries at the start configuration.
+	obstaclesInFrame, err := worldState.ObstaclesInWorldFrame(fs, startInputs.ToFrameSystemInputs())
+	if err != nil {
+		return err
+	}
+	worldGeos := obstaclesInFrame.Geometries()
+
+	collisionBufferMM := NewBasicPlannerOptions().CollisionBufferMM
+
+	// Determine which collisions already exist at the start so we can ignore them.
+	var allowedCollisions []motionplan.Collision
+	if len(movingGeos) > 0 {
+		if len(worldGeos) > 0 {
+			cols, _, err := motionplan.CheckCollisions(movingGeos, worldGeos, nil, collisionBufferMM, true)
+			if err != nil {
+				return err
+			}
+			allowedCollisions = append(allowedCollisions, cols...)
+		}
+		if len(staticGeos) > 0 {
+			cols, _, err := motionplan.CheckCollisions(movingGeos, staticGeos, nil, collisionBufferMM, true)
+			if err != nil {
+				return err
+			}
+			allowedCollisions = append(allowedCollisions, cols...)
+		}
+		if len(movingGeos) > 1 {
+			cols, _, err := motionplan.CheckCollisions(movingGeos, movingGeos, nil, collisionBufferMM, true)
+			if err != nil {
+				return err
+			}
+			allowedCollisions = append(allowedCollisions, cols...)
+		}
+	}
+
+	collisionConstraints, err := motionplan.CreateAllCollisionConstraints(
+		fs, movingGeos, staticGeos, worldGeos, allowedCollisions, collisionBufferMM,
+	)
+	if err != nil {
+		return err
+	}
+
+	checker := motionplan.NewEmptyConstraintChecker(logger)
+	checker.SetCollisionConstraints(collisionConstraints)
+
+	resolution := defaultResolution
+	for i := 0; i < len(linearTraj)-1; i++ {
+		seg := &motionplan.SegmentFS{
+			StartConfiguration: linearTraj[i],
+			EndConfiguration:   linearTraj[i+1],
+			FS:                 fs,
+		}
+		if _, err := checker.CheckStateConstraintsAcrossSegmentFS(ctx, seg, resolution, true); err != nil {
+			return fmt.Errorf("collision in segment %d (waypoints %d→%d): %w", i, i, i+1, err)
+		}
+	}
+
+	return nil
+}
+
+// detectMovingFrames returns the set of frame names whose inputs change at any point in the
+// trajectory. If no frames change (e.g. all waypoints are identical) every frame in the
+// trajectory is treated as moving.
+//
+// Runs in a single pass, comparing each step to the previous one.
+func detectMovingFrames(trajectory motionplan.Trajectory) map[string]bool {
+	movingFrames := make(map[string]bool)
+	framesInTraj := make(map[string]bool)
+
+	var prev referenceframe.FrameSystemInputs
+	first := true
+	for _, step := range trajectory {
+		for name := range step {
+			framesInTraj[name] = true
+		}
+		if first {
+			prev = step
+			first = false
+			continue
+		}
+		for name, inputs := range step {
+			if movingFrames[name] {
+				continue
+			}
+			prevInputs := prev[name]
+			if len(inputs) != len(prevInputs) {
+				movingFrames[name] = true
+				continue
+			}
+			for j := range inputs {
+				if inputs[j] != prevInputs[j] {
+					movingFrames[name] = true
+					break
+				}
+			}
+		}
+		prev = step
+	}
+
+	// Fallback: if nothing moved, treat all frames in the trajectory as moving.
+	if len(movingFrames) == 0 {
+		for name := range framesInTraj {
+			movingFrames[name] = true
+		}
+	}
+
+	return movingFrames
+}
+
+// belongsToMovingFrame returns true when the geometry label starts with one of the moving frame
+// names followed by a colon (e.g. "myArm:link1" belongs to frame "myArm").
+func belongsToMovingFrame(label string, movingFrames map[string]bool) bool {
+	for name := range movingFrames {
+		if strings.HasPrefix(label, name+":") {
+			return true
+		}
+	}
+	return false
+}