Make grab() intelligent and add speed to arm + gripper

README.md

@@ -57,6 +57,44 @@ The following attributes are available for the arm component:
 | `baudrate`          | int      | Optional  | The baud rate for serial communication. Default is `1000000`.                                |
 | `servo_ids`         | []int    | Optional  | List of servo IDs for the arm joints. Default is `[1, 2, 3, 4, 5]`.                         |
 | `timeout`           | duration | Optional  | Communication timeout. Default is system default.                                            |
+| `speed_degs_per_sec` | float   | Optional  | Default movement speed in degrees per second. Range: 3-180. Default is `50`.                 |
+| `acceleration_degs_per_sec_per_sec` | float | Optional | Default acceleration in degrees per second². Range: 10-500. Default is `100`. |
+
+### Speed and Acceleration
+
+Movement speed and acceleration can be configured at three levels (in order of precedence):
+
+1. **Per-call parameters** via `extra` map in `MoveToJointPositions` or `MoveThroughJointPositions`:
+   - `speed_d`: Speed in degrees/second
+   - `speed_r`: Speed in radians/second
+   - `acceleration_d`: Acceleration in degrees/second²
+   - `acceleration_r`: Acceleration in radians/second²
+
+2. **MoveOptions** (for `MoveThroughJointPositions`):
+   - `MaxVelRads`: Maximum velocity in radians/second
+   - `MaxAccRads`: Maximum acceleration in radians/second²
+
+3. **Component configuration** via attributes above
+
+Example using extra parameters:
+```python
+arm.move_to_joint_positions(positions, extra={"speed_d": 30, "acceleration_d": 50})
+```
+
+You can also dynamically adjust defaults using DoCommand:
+```json
+{
+    "set_speed": 60,
+    "set_acceleration": 120
+}
+```
+
+To query current motion parameters:
+```json
+{
+    "get_motion_params": true
+}
+```
 
 **If you're building and setting up an arm for the first time, please see the [calibration sensor component](#model-devrelso101calibration) for setup instructions.**
 
@@ -221,6 +259,38 @@ Follow the [end-to-end tutorial](https://codelabs.viam.com/guide/so101/index.htm
 | `baudrate`         | int      | Optional  | The baud rate for serial communication. Default is `1000000`.                                |
 | `servo_id`         | int      | Optional  | The servo ID for the gripper. Default is `6`.                                                |
 | `timeout`          | duration | Optional  | Communication timeout. Default is system default.                                            |
+| `speed_percent_per_sec` | float | Optional | Default gripper speed in percent per second. Range: 0-100. Default is `100`.               |
+| `acceleration_percent_per_sec_per_sec` | float | Optional | Default acceleration in percent per second². Range: 10-200. Default is `50`. |
+
+### Speed and Acceleration
+
+The gripper uses percentage-based speed and acceleration (0-100% of full travel). Parameters can be set via:
+
+1. **Per-call parameters** via `extra` map in `Open` or `Grab`:
+   - `speed_percent`: Speed in percent/second
+   - `acceleration_percent`: Acceleration in percent/second²
+
+2. **Component configuration** via attributes above
+
+Example using extra parameters:
+```python
+gripper.open(extra={"speed_percent": 50, "acceleration_percent": 30})
+```
+
+Use DoCommand to dynamically adjust or query motion parameters:
+```json
+{
+    "command": "set_motion_params",
+    "speed": 50,
+    "acceleration": 30
+}
+```
+
+```json
+{
+    "command": "get_motion_params"
+}
+```
 
 ### Communication
 

arm.go

@@ -111,6 +111,72 @@ type so101 struct {
 	initCtx    context.Context // Context for initialization operations
 }
 
+// moveOptions holds movement parameters for arm operations
+type moveOptions struct {
+	speedDegsPerSec        float64
+	accelerationDegsPerSec float64
+}
+
+// buildMoveOptions constructs moveOptions from defaults, MoveOptions, and extra params
+// 1. Start with configured defaults
+// 2. Override with arm.MoveOptions if provided
+// 3. Override with extra map parameters if provided
+func (s *so101) buildMoveOptions(options *arm.MoveOptions, extra map[string]interface{}) moveOptions {
+	s.mu.RLock()
+	speed := float64(s.defaultSpeed)
+	acc := float64(s.defaultAcc)
+	s.mu.RUnlock()
+
+	// Apply arm.MoveOptions if provided
+	if options != nil {
+		if options.MaxVelRads != 0 {
+			// Convert radians/sec to degrees/sec
+			speed = options.MaxVelRads * 180.0 / math.Pi
+		}
+		if options.MaxAccRads != 0 {
+			// Convert radians/sec² to degrees/sec²
+			acc = options.MaxAccRads * 180.0 / math.Pi
+		}
+	}
+
+	// Apply extra map parameters (highest priority)
+	// Speed in radians/sec
+	if speedR, ok := extra["speed_r"].(float64); ok && speedR > 0 {
+		speed = speedR * 180.0 / math.Pi
+	}
+	// Speed in degrees/sec
+	if speedD, ok := extra["speed_d"].(float64); ok && speedD > 0 {
+		speed = speedD
+	}
+	// Acceleration in radians/sec²
+	if accR, ok := extra["acceleration_r"].(float64); ok && accR > 0 {
+		acc = accR * 180.0 / math.Pi
+	}
+	// Acceleration in degrees/sec²
+	if accD, ok := extra["acceleration_d"].(float64); ok && accD > 0 {
+		acc = accD
+	}
+
+	// Clamp to valid ranges
+	if speed < 3 {
+		speed = 3
+	}
+	if speed > 180 {
+		speed = 180
+	}
+	if acc < 10 {
+		acc = 10
+	}
+	if acc > 500 {
+		acc = 500
+	}
+
+	return moveOptions{
+		speedDegsPerSec:        speed,
+		accelerationDegsPerSec: acc,
+	}
+}
+
 func makeSO101ModelFrame() (referenceframe.Model, error) {
 	m := &referenceframe.ModelConfigJSON{
 		OriginalFile: &referenceframe.ModelFile{
@@ -325,6 +391,9 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef
 		return fmt.Errorf("expected %d joint positions for SO-101 arm, got %d", len(s.armServoIDs), len(positions))
 	}
 
+	// Build move options from defaults and extra parameters
+	opts := s.buildMoveOptions(nil, extra)
+
 	values := make([]float64, len(positions))
 	copy(values, positions)
 
@@ -344,7 +413,10 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef
 		clampedPositions[i] = math.Max(min, math.Min(max, pos))
 	}
 
-	if err := s.controller.MoveServosToPositions(ctx, s.armServoIDs, clampedPositions, 0, 0); err != nil {
+	// Pass speed and acceleration to controller (currently 0, 0 means use servo defaults)
+	speed := int(opts.speedDegsPerSec)
+	acc := int(opts.accelerationDegsPerSec)
+	if err := s.controller.MoveServosToPositions(ctx, s.armServoIDs, clampedPositions, speed, acc); err != nil {
 		return fmt.Errorf("failed to move SO-101 arm: %w", err)
 	}
 
@@ -364,7 +436,8 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef
 		}
 	}
 
-	speedRadPerSec := float64(s.defaultSpeed) * math.Pi / 180.0
+	// Use the configured speed from moveOptions
+	speedRadPerSec := opts.speedDegsPerSec * math.Pi / 180.0
 	moveTimeSeconds := maxMovement / speedRadPerSec
 	if moveTimeSeconds < 0.1 {
 		moveTimeSeconds = 0.1 // Minimum move time
@@ -379,8 +452,20 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef
 }
 
 func (s *so101) MoveThroughJointPositions(ctx context.Context, positions [][]referenceframe.Input, options *arm.MoveOptions, extra map[string]interface{}) error {
+	// Build move options once for all positions
+	opts := s.buildMoveOptions(options, extra)
+
+	// Create a new extra map that includes the speed/acc from moveOptions
+	// This ensures MoveToJointPositions uses the same options
+	mergedExtra := make(map[string]interface{})
+	for k, v := range extra {
+		mergedExtra[k] = v
+	}
+	mergedExtra["speed_d"] = opts.speedDegsPerSec
+	mergedExtra["acceleration_d"] = opts.accelerationDegsPerSec
+
 	for _, jointPositions := range positions {
-		if err := s.MoveToJointPositions(ctx, jointPositions, extra); err != nil {
+		if err := s.MoveToJointPositions(ctx, jointPositions, mergedExtra); err != nil {
 			return err
 		}
 

calibrated_servo.go

@@ -255,6 +255,14 @@ func (cs *CalibratedServo) Moving(ctx context.Context) (bool, error) {
 	return cs.servo.Moving(ctx)
 }
 
+// Load reads the current load on the servo
+// Returns signed value: positive = clockwise load, negative = counter-clockwise
+func (cs *CalibratedServo) Load(ctx context.Context) (int, error) {
+	cs.mu.RLock()
+	defer cs.mu.RUnlock()
+	return cs.servo.Load(ctx)
+}
+
 // Ping pings the servo
 func (cs *CalibratedServo) Ping(ctx context.Context) (int, error) {
 	cs.mu.RLock()

go.mod

@@ -4,7 +4,7 @@ go 1.25.1
 
 require (
 	github.com/golang/geo v0.0.0-20230421003525-6adc56603217
-	github.com/hipsterbrown/feetech-servo v0.4.0
+	github.com/hipsterbrown/feetech-servo v0.4.2
 	github.com/pkg/errors v0.9.1
 	github.com/stretchr/testify v1.10.0
 	go.bug.st/serial v1.6.4

go.sum

@@ -465,8 +465,8 @@ github.com/hashicorp/logutils v1.0.0/go.mod h1:QIAnNjmIWmVIIkWDTG1z5v++HQmx9WQRO
 github.com/hashicorp/mdns v1.0.0/go.mod h1:tL+uN++7HEJ6SQLQ2/p+z2pH24WQKWjBPkE0mNTz8vQ=
 github.com/hashicorp/memberlist v0.1.3/go.mod h1:ajVTdAv/9Im8oMAAj5G31PhhMCZJV2pPBoIllUwCN7I=
 github.com/hashicorp/serf v0.8.2/go.mod h1:6hOLApaqBFA1NXqRQAsxw9QxuDEvNxSQRwA/JwenrHc=
-github.com/hipsterbrown/feetech-servo v0.4.0 h1:idHWF0PcUYAlHjIKW6LqczcIg2RYpfYxSk9AptRU5Hk=
-github.com/hipsterbrown/feetech-servo v0.4.0/go.mod h1:jyxvkJTDDDy6ApD3kxnbOLXvpG0L/7Qm4x9MIOAkTUw=
+github.com/hipsterbrown/feetech-servo v0.4.2 h1:y0tfg15JHKCA9x0Y4my0OSHvD+PPad7Zv4pLtmWMEzo=
+github.com/hipsterbrown/feetech-servo v0.4.2/go.mod h1:jyxvkJTDDDy6ApD3kxnbOLXvpG0L/7Qm4x9MIOAkTUw=
 github.com/hpcloud/tail v1.0.0/go.mod h1:ab1qPbhIpdTxEkNHXyeSf5vhxWSCs/tWer42PpOxQnU=
 github.com/hudl/fargo v1.3.0/go.mod h1:y3CKSmjA+wD2gak7sUSXTAoopbhU08POFhmITJgmKTg=
 github.com/iancoleman/orderedmap v0.0.0-20190318233801-ac98e3ecb4b0 h1:i462o439ZjprVSFSZLZxcsoAe592sZB1rci2Z8j4wdk=