RSDK-13274 motion planning failed to avoid obstacles and created large arm movement (viamrobotics#5689)

Author: biotinker

motionplan/armplanning/data/sanding-collision-with-wall.json

@@ -411,6 +411,62 @@ func TestOrbPlanTooManySteps(t *testing.T) {
 	test.That(t, zeros, test.ShouldBeLessThanOrEqualTo, 0)
 }
 
+func TestSandingWallCollision(t *testing.T) {
+	if IsTooSmallForCache() {
+		t.Skip()
+		return
+	}
+
+	logger := logging.NewTestLogger(t).Sublogger("mp")
+	ctx := context.Background()
+
+	start := time.Now()
+	req, err := ReadRequestFromFile("data/sanding-collision-with-wall.json")
+	test.That(t, err, test.ShouldBeNil)
+
+	logger.Infof("time to ReadRequestFromFile %v", time.Since(start))
+	plan, _, err := PlanMotion(ctx, logger, req)
+	test.That(t, err, test.ShouldBeNil)
+
+	t.Run("check trajectory length", func(t *testing.T) {
+		test.That(t, len(plan.Trajectory()), test.ShouldBeGreaterThan, 3)
+	})
+
+	t.Run("check collision checks pass with smaller resolution", func(t *testing.T) {
+		// Create plan context to validate the path
+		pc, err := newPlanContext(ctx, logger, req, &PlanMeta{})
+		test.That(t, err, test.ShouldBeNil)
+
+		psc, err := newPlanSegmentContext(ctx, pc, req.StartState.LinearConfiguration(), req.Goals[0].Poses())
+		test.That(t, err, test.ShouldBeNil)
+
+		trajectory := plan.Trajectory()
+		smallResolution := 0.001
+
+		for j := 0; j < len(trajectory)-1; j++ {
+			start := trajectory[j].ToLinearInputs()
+			end := trajectory[j+1].ToLinearInputs()
+
+			// Default resolution passes
+			err := psc.checkPath(ctx, start, end, false)
+			test.That(t, err, test.ShouldBeNil)
+
+			// Small resolution noticed the collision when we had large jumps
+			_, err = psc.checker.CheckStateConstraintsAcrossSegmentFS(
+				ctx,
+				&motionplan.SegmentFS{
+					StartConfiguration: start,
+					EndConfiguration:   end,
+					FS:                 pc.fs,
+				},
+				smallResolution,
+				true,
+			)
+			test.That(t, err, test.ShouldBeNil)
+		}
+	})
+}
+
 func BenchmarkBigPlanRequest(b *testing.B) {
 	if IsTooSmallForCache() {
 		b.Skip()

motionplan/armplanning/real_test.go

@@ -291,38 +291,9 @@ func (c *ConstraintChecker) CheckStateFSConstraints(ctx context.Context, state *
 // configuration of a segment at a given resolution value.
 func InterpolateSegmentFS(ci *SegmentFS, resolution float64) ([]*referenceframe.LinearInputs, error) {
 	// Find the frame with the most steps by calculating steps for each frame
-	maxSteps := defaultMinStepCount
-	for frameName, startConfig := range ci.StartConfiguration.Items() {
-		if len(startConfig) == 0 {
-			// No need to interpolate 0dof frames
-			continue
-		}
-		endConfig := ci.EndConfiguration.Get(frameName)
-		if endConfig == nil {
-			return nil, fmt.Errorf("frame %s exists in start config but not in end config", frameName)
-		}
-
-		// Get frame from FrameSystem
-		frame := ci.FS.Frame(frameName)
-		if frame == nil {
-			return nil, fmt.Errorf("frame %s exists in start config but not in framesystem", frameName)
-		}
-
-		// Calculate positions for this frame's start and end configs
-		startPos, err := frame.Transform(startConfig)
-		if err != nil {
-			return nil, err
-		}
-		endPos, err := frame.Transform(endConfig)
-		if err != nil {
-			return nil, err
-		}
-
-		// Calculate steps needed for this frame
-		steps := CalculateStepCount(startPos, endPos, resolution)
-		if steps > maxSteps {
-			maxSteps = steps
-		}
+	maxSteps, err := segmentStepCount(ci, resolution)
+	if err != nil {
+		return nil, err
 	}
 
 	// Create interpolated configurations for all frames
@@ -521,3 +492,53 @@ func collisionCheckFinish(fs *referenceframe.FrameSystem, internalGeoms, static
 	}
 	return cg.minDistance, nil
 }
+
+// Computes the quantity of intermediate constraint check steps that should be performed across a segment
+func segmentStepCount(ci *SegmentFS, resolution float64) (int, error) {
+	// Find the frame with the most steps by calculating steps for each frame
+	maxSteps := defaultMinStepCount
+	for frameName, startConfig := range ci.StartConfiguration.Items() {
+		if len(startConfig) == 0 {
+			// No need to interpolate 0dof frames
+			continue
+		}
+		endConfig := ci.EndConfiguration.Get(frameName)
+		if endConfig == nil {
+			return -1, fmt.Errorf("frame %s exists in start config but not in end config", frameName)
+		}
+
+		// Get frame from FrameSystem
+		frame := ci.FS.Frame(frameName)
+		if frame == nil {
+			return -1, fmt.Errorf("frame %s exists in start config but not in framesystem", frameName)
+		}
+
+		// Calculate positions for this frame's start and end configs
+		startPos, err := frame.Transform(startConfig)
+		if err != nil {
+			return -1, err
+		}
+		endPos, err := frame.Transform(endConfig)
+		if err != nil {
+			return -1, err
+		}
+
+		// Compute joint step size from the largest limit range, divided by 1000
+		jointStepSize := jointStepSizeFromLimits(frame.DoF())
+
+		maxSteps = max(maxSteps, CalculateStepCount(startPos, endPos, resolution))
+		maxSteps = max(maxSteps, calculateJointStepCount(startConfig, endConfig, jointStepSize))
+	}
+	return maxSteps, nil
+}
+
+// jointStepSizeFromLimits computes the joint step size as 1/1000 of the largest limit range.
+func jointStepSizeFromLimits(limits []referenceframe.Limit) float64 {
+	var maxRange float64
+
+	for _, limit := range limits {
+		_, _, r := limit.GoodLimits()
+		maxRange = max(maxRange, r)
+	}
+	return maxRange / 1000
+}

motionplan/constraint_checker.go

@@ -342,6 +342,97 @@ func TestCollisionConstraints(t *testing.T) {
 	}
 }
 
+func TestCalculateJointStepCount(t *testing.T) {
+	t.Run("no movement", func(t *testing.T) {
+		start := []float64{0, 0, 0}
+		end := []float64{0, 0, 0}
+		test.That(t, calculateJointStepCount(start, end, 0.05), test.ShouldEqual, 0)
+	})
+
+	t.Run("small movement under step size", func(t *testing.T) {
+		start := []float64{0, 0, 0}
+		end := []float64{0.04, 0, 0} // 0.04 rad < defaultJointStepSizeRadians
+		test.That(t, calculateJointStepCount(start, end, 0.05), test.ShouldEqual, 1)
+	})
+
+	t.Run("one radian movement", func(t *testing.T) {
+		start := []float64{0, 0, 0}
+		end := []float64{1.0, 0, 0} // 1 rad / defaultJointStepSizeRadians = 20 steps
+		test.That(t, calculateJointStepCount(start, end, 0.05), test.ShouldEqual, 20)
+	})
+
+	t.Run("large joint movement from sanding collision bug", func(t *testing.T) {
+		// Joint 1 from TestSandingWallCollision moves ~6.7 radians
+		start := []float64{1.06, -3.336, -0.18, 1.90, -1.53, 4.20}
+		end := []float64{1.06, 3.379, -1.26, -0.59, 1.53, 1.06}
+		// Joint 1 moves 6.715 rad / 0.05 = 135 steps
+		steps := calculateJointStepCount(start, end, 0.05)
+		test.That(t, steps, test.ShouldEqual, 135)
+	})
+}
+
+// TestSegmentStepCount tests that segmentStepCount correctly emits step count from either joint or cartesian excursion
+func TestSegmentStepCount(t *testing.T) {
+	model, err := referenceframe.ParseModelJSONFile(utils.ResolveFile("components/arm/fake/kinematics/ur20.json"), "")
+	test.That(t, err, test.ShouldBeNil)
+	jointStepSize := jointStepSizeFromLimits(model.DoF())
+
+	fs := referenceframe.NewEmptyFrameSystem("test")
+	err = fs.AddFrame(model, fs.World())
+	test.That(t, err, test.ShouldBeNil)
+	startConfig := []referenceframe.Input{1.06, -3.336, -0.18, 1.90, -1.53, 4.20}
+	startPos, err := model.Transform(startConfig)
+	test.That(t, err, test.ShouldBeNil)
+
+	t.Run("joint steps dominate when cartesian distance is small", func(t *testing.T) {
+		// For a ur20 this config is very close to startConfig in cartesian space but far away in joint space
+		endConfig := []referenceframe.Input{1.06, 3.379, -1.26, -0.59, 1.53, 1.06}
+		segment := &SegmentFS{
+			StartConfiguration: referenceframe.FrameSystemInputs{model.Name(): startConfig}.ToLinearInputs(),
+			EndConfiguration:   referenceframe.FrameSystemInputs{model.Name(): endConfig}.ToLinearInputs(),
+			FS:                 fs,
+		}
+		endPos, err := model.Transform(endConfig)
+		test.That(t, err, test.ShouldBeNil)
+
+		cartesianSteps := CalculateStepCount(startPos, endPos, 1.0)
+		jointSteps := calculateJointStepCount(startConfig, endConfig, jointStepSize)
+
+		totalSteps, err := segmentStepCount(segment, 1.0)
+		test.That(t, err, test.ShouldBeNil)
+
+		// Joint steps should dominate over cartesian for this trajectory
+		test.That(t, jointSteps, test.ShouldBeGreaterThan, cartesianSteps)
+
+		// segmentStepCount should return the joint step count
+		test.That(t, totalSteps, test.ShouldEqual, jointSteps)
+	})
+
+	t.Run("cartesian steps dominate when joint distance is small", func(t *testing.T) {
+		// Joints are close to startConfig, but quite far in cartesian space
+		endConfig := []referenceframe.Input{1.06, -3.379, -1.26, -0.59, 1.53, 1.06}
+		segment := &SegmentFS{
+			StartConfiguration: referenceframe.FrameSystemInputs{model.Name(): startConfig}.ToLinearInputs(),
+			EndConfiguration:   referenceframe.FrameSystemInputs{model.Name(): endConfig}.ToLinearInputs(),
+			FS:                 fs,
+		}
+		endPos, err := model.Transform(endConfig)
+		test.That(t, err, test.ShouldBeNil)
+
+		cartesianSteps := CalculateStepCount(startPos, endPos, 1.0)
+		jointSteps := calculateJointStepCount(startConfig, endConfig, jointStepSize)
+
+		totalSteps, err := segmentStepCount(segment, 1.0)
+		test.That(t, err, test.ShouldBeNil)
+
+		// Cartesian steps should dominate over joint for this trajectory
+		test.That(t, cartesianSteps, test.ShouldBeGreaterThan, jointSteps)
+
+		// segmentStepCount should return the cartesian step count
+		test.That(t, totalSteps, test.ShouldEqual, cartesianSteps)
+	})
+}
+
 func BenchmarkCollisionConstraints(b *testing.B) {
 	// define external obstacles
 	bc, err := spatial.NewBox(spatial.NewZeroPose(), r3.Vector{2, 2, 2}, "")

motionplan/constraint_test.go

@@ -22,3 +22,15 @@ func CalculateStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int
 	nSteps := math.Max(math.Abs(mmDist/stepSize), math.Abs(utils.RadToDeg(rDist.Theta)/stepSize))
 	return int(nSteps) + 1
 }
+
+func calculateJointStepCount(start, end []float64, stepSize float64) int {
+	steps := 0
+	for idx, s := range start {
+		if idx >= len(end) {
+			break
+		}
+		mySteps := int(math.Ceil(math.Abs(end[idx]-s) / stepSize))
+		steps = max(mySteps, steps)
+	}
+	return steps
+}