Skip to content

Commit b6a9b1f

Browse files
committed
removed code i didnt add
1 parent 120049c commit b6a9b1f

File tree

1 file changed

+0
-44
lines changed

1 file changed

+0
-44
lines changed

services/motion/builtin/builtin_test.go

Lines changed: 0 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -1422,50 +1422,6 @@ func TestMultiWaypointPlanning(t *testing.T) {
14221422
test.That(t, spatialmath.PoseAlmostEqualEps(plannedPose, finalPose.Pose(), 1e-3), test.ShouldBeTrue)
14231423
})
14241424

1425-
t.Run("plan with pose start state", func(t *testing.T) {
1426-
// Define waypoints as poses relative to world frame
1427-
start := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -180, Z: 30}))
1428-
waypoint := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -190, Z: 30}))
1429-
finalPose := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: -800, Y: -200, Z: 30}))
1430-
1431-
startState := armplanning.NewPlanState(referenceframe.FrameSystemPoses{"pieceGripper": start}, nil)
1432-
wpState := armplanning.NewPlanState(referenceframe.FrameSystemPoses{"pieceGripper": waypoint}, nil)
1433-
1434-
moveReq := motion.MoveReq{
1435-
ComponentName: "pieceGripper",
1436-
Destination: finalPose,
1437-
Extra: map[string]interface{}{
1438-
"start_state": startState.Serialize(),
1439-
"waypoints": []interface{}{wpState.Serialize()},
1440-
"smooth_iter": 5,
1441-
},
1442-
}
1443-
1444-
plan := getPlanFromMove(t, moveReq)
1445-
test.That(t, len(plan), test.ShouldBeGreaterThan, 0)
1446-
1447-
frameSys, err := framesystem.NewFromService(ctx, ms.(*builtIn).fsService, nil)
1448-
test.That(t, err, test.ShouldBeNil)
1449-
1450-
// Verify start configuration matches start pose
1451-
firstConfig := plan[0]
1452-
firstPoseInWorld, err := frameSys.Transform(firstConfig,
1453-
referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewZeroPose()),
1454-
"world")
1455-
test.That(t, err, test.ShouldBeNil)
1456-
plannedPose := firstPoseInWorld.(*referenceframe.PoseInFrame).Pose()
1457-
test.That(t, spatialmath.PoseAlmostEqualEps(plannedPose, start.Pose(), 1e-3), test.ShouldBeTrue)
1458-
1459-
// Verify final pose
1460-
finalConfig := plan[len(plan)-1]
1461-
finalPoseInWorld, err := frameSys.Transform(finalConfig,
1462-
referenceframe.NewPoseInFrame("pieceGripper", spatialmath.NewZeroPose()),
1463-
"world")
1464-
test.That(t, err, test.ShouldBeNil)
1465-
plannedPose = finalPoseInWorld.(*referenceframe.PoseInFrame).Pose()
1466-
test.That(t, spatialmath.PoseAlmostEqualEps(plannedPose, finalPose.Pose(), 1e-3), test.ShouldBeTrue)
1467-
})
1468-
14691425
t.Run("plan through mixed pose and configuration waypoints", func(t *testing.T) {
14701426
// Define specific arm configuration for first waypoint
14711427
armConfig := []float64{0.2, 0.3, 0.4, 0.5, 0.6, 0.7}

0 commit comments

Comments
 (0)