dual arm teleop & smoothing

services/motion/builtin/builtin.go

@@ -97,6 +97,17 @@ type Config struct {
 
 	// example { "arm" : { "3" : { "min" : 0, "max" : 2 } } }
 	InputRangeOverride map[string]map[string]referenceframe.Limit `json:"input_range_override"`
+
+	// TeleopSmallMoveRad is the max joint displacement (radians) below which
+	// the arm's built-in interpolation is enabled for smooth motion. Default 0.005.
+	TeleopSmallMoveRad float64 `json:"teleop_small_move_rad"`
+	// TeleopInterpolateOverride when true forces interpolation ON for all teleop
+	// movements regardless of size. Useful for testing.
+	TeleopInterpolateOverride bool `json:"teleop_interpolate_override"`
+	// TeleopSmoothAlpha is the EMA smoothing factor for joint positions (0-1).
+	// Lower = smoother but more latency, higher = more responsive but less smooth.
+	// 0 = frozen, 1 = no smoothing. Default 0.5.
+	TeleopSmoothAlpha float64 `json:"teleop_smooth_alpha"`
 }
 
 func (c *Config) shouldWritePlan(start time.Time, err error) bool {
@@ -532,67 +543,43 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.
 	return plan, err
 }
 
-// planTeleop is a low-latency variant of plan() for the teleop pipeline.
-// It uses a pre-built frame system and caller-provided fsInputs (merged from
-// live CurrentInputs + planning head) to avoid per-call overhead.
-func (ms *builtIn) planTeleop(
+// planTeleopMulti plans a trajectory for multiple components simultaneously.
+// It builds a multi-frame goal from the given poses map, allowing the planner
+// to find collision-free paths for all arms jointly.
+func (ms *builtIn) planTeleopMulti(
 	ctx context.Context,
-	req motion.MoveReq,
+	goals referenceframe.FrameSystemPoses,
+	extra map[string]interface{},
 	frameSys *referenceframe.FrameSystem,
 	fsInputs referenceframe.FrameSystemInputs,
 	logger logging.Logger,
 ) (motionplan.Plan, error) {
-	ctx, span := trace.StartSpan(ctx, "motion::builtin::planTeleop")
+	ctx, span := trace.StartSpan(ctx, "motion::builtin::planTeleopMulti")
 	defer span.End()
 
-	movingFrame := frameSys.Frame(req.ComponentName)
-	if movingFrame == nil {
-		return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName)
-	}
-
-	startState, waypoints, err := waypointsFromRequest(req, fsInputs)
-	if err != nil {
-		return nil, err
-	}
-	if len(waypoints) == 0 {
-		return nil, errors.New("could not find any waypoints to plan for in MoveRequest. Fill in Destination or goal_state")
-	}
-
-	if req.Extra != nil {
-		req.Extra["waypoints"] = nil
-	}
-
-	// Re-evaluate goal poses to be in the frame of World.
-	worldWaypoints := []*armplanning.PlanState{}
-	solvingFrame := referenceframe.World
-	for _, wp := range waypoints {
-		if wp.Poses() != nil {
-			step := referenceframe.FrameSystemPoses{}
-			for fName, destination := range wp.Poses() {
-				tf, err := frameSys.Transform(fsInputs.ToLinearInputs(), destination, solvingFrame)
-				if err != nil {
-					return nil, err
-				}
-				goalPose, _ := tf.(*referenceframe.PoseInFrame)
-				step[fName] = goalPose
-			}
-			worldWaypoints = append(worldWaypoints, armplanning.NewPlanState(step, wp.Configuration()))
-		} else {
-			worldWaypoints = append(worldWaypoints, wp)
+	// Transform all goal poses to world frame.
+	worldGoals := make(referenceframe.FrameSystemPoses, len(goals))
+	for fName, destination := range goals {
+		tf, err := frameSys.Transform(fsInputs.ToLinearInputs(), destination, referenceframe.World)
+		if err != nil {
+			return nil, err
 		}
+		goalPose, _ := tf.(*referenceframe.PoseInFrame)
+		worldGoals[fName] = goalPose
 	}
 
-	planOpts, err := armplanning.NewPlannerOptionsFromExtra(req.Extra)
+	startState := armplanning.NewPlanState(nil, fsInputs)
+	goalState := armplanning.NewPlanState(worldGoals, nil)
+
+	planOpts, err := armplanning.NewPlannerOptionsFromExtra(extra)
 	if err != nil {
 		return nil, err
 	}
 
 	planRequest := &armplanning.PlanRequest{
 		FrameSystem:    frameSys,
-		Goals:          worldWaypoints,
+		Goals:          []*armplanning.PlanState{goalState},
 		StartState:     startState,
-		WorldState:     req.WorldState,
-		Constraints:    req.Constraints,
 		PlannerOptions: planOpts,
 	}
 
@@ -688,6 +675,7 @@ func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory
 			if err != nil {
 				return err
 			}
+
 			if err := ie.GoToInputs(ctx, inputs...); err != nil {
 				// If there is an error on GoToInputs, stop the component if possible before returning the error
 				if actuator, ok := r.(inputEnabledActuator); ok {