@@ -49,40 +49,62 @@ public async UniTaskVoid ExecuteSelectedPath()
4949 {
5050 IsExecuting = true ;
5151
52- var pathPoses = pathSelector . SelectedPath . pointsPose ;
53-
54- Debug . Log ( $ "Starting executing '{ pathSelector . SelectedPath } '...") ;
55-
56- for ( var i = 0 ; i < pathPoses . Count ; i ++ )
52+ try
5753 {
58- var pose = pathPoses [ i ] ;
54+ var pathPoses = pathSelector . SelectedPath . pointsPose ;
5955
60- // Convert pose back to world space before using it in the request
61- var worldPose = new Pose
62- {
63- position = pathCoordinateSpaceOrigin . TransformPoint ( pose . position ) ,
64- rotation = pathCoordinateSpaceOrigin . rotation * pose . rotation
65- } ;
56+ Debug . Log ( $ "Starting executing '{ pathSelector . SelectedPath } '...") ;
6657
67- Debug . Log ( $ "Starting motion to point { i + 1 } /{ pathPoses . Count } ...") ;
68-
69- bool motionSuccessful = await robotController . MoveCartesian ( worldPose ) ;
70-
71- if ( ! motionSuccessful )
58+ for ( var i = 0 ; i < pathPoses . Count ; i ++ )
7259 {
73- Debug . LogError ( $ "Motion to point { i + 1 } failed. Please check the logs to identify the issue.\n " +
74- $ "Aborting the rest of the trajectory execution.") ;
75- break ;
60+ var pose = pathPoses [ i ] ;
61+
62+ // Convert pose back to world space before using it in the request
63+ var worldPose = new Pose
64+ {
65+ position = pathCoordinateSpaceOrigin . TransformPoint ( pose . position ) ,
66+ rotation = pathCoordinateSpaceOrigin . rotation * pose . rotation
67+ } ;
68+
69+ Debug . Log ( $ "PathExecutor point { i + 1 } /{ pathPoses . Count } : savedLocalPos={ pose . position } savedLocalRot={ pose . rotation . eulerAngles } " +
70+ $ "origin={ pathCoordinateSpaceOrigin . name } originWorldPos={ pathCoordinateSpaceOrigin . position } originWorldRot={ pathCoordinateSpaceOrigin . rotation . eulerAngles } " +
71+ $ "executedWorldPos={ worldPose . position } executedWorldRot={ worldPose . rotation . eulerAngles } ", this ) ;
72+
73+ Debug . Log ( $ "Starting motion to point { i + 1 } /{ pathPoses . Count } ...") ;
74+
75+ bool motionSuccessful ;
76+
77+ if ( i == 0 )
78+ {
79+ // The first move is an approach from the robot's arbitrary current pose to the start of the
80+ // programmed path. It is not a visible path segment in the VR program, so do not force it to be
81+ // a Cartesian LIN move. LIN is preserved for all programmed point-to-point segments below.
82+ motionSuccessful = await robotController . MoveCartesianPtp ( worldPose ) ;
83+ }
84+ else
85+ {
86+ motionSuccessful = await robotController . MoveCartesian ( worldPose ) ;
87+ }
88+
89+ if ( ! motionSuccessful )
90+ {
91+ Debug . LogError ( $ "Motion to point { i + 1 } failed. Please check the logs to identify the issue.\n " +
92+ $ "Aborting the rest of the trajectory execution.") ;
93+ break ;
94+ }
95+
96+ Debug . Log ( $ "Moved to point { i + 1 } /{ pathPoses . Count } .") ;
7697 }
77-
78- Debug . Log ( $ "Moved to point { i + 1 } /{ pathPoses . Count } .") ;
79- }
8098
81- Debug . Log ( $ "Finished executing path '{ pathSelector . SelectedPath } '.") ;
82-
83- IsExecuting = false ;
99+ Debug . Log ( $ "Finished executing path '{ pathSelector . SelectedPath } '.") ;
100+ }
101+ finally
102+ {
103+ IsExecuting = false ;
104+ }
84105 }
85106
107+
86108 #endregion
87109 }
88110}
0 commit comments