@@ -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