Moveit time parametrization slow down trajectory execution of doosan robot

[BenjaminTT]

Maybe it is more a question than an issue. We are not sure if the following is intended behavior. We are currently working with an A0912 and A0509, and using ROS2 and moveit to interact with the robot. We are using OMPL to generate the trajectories. By default the time parametrization is on and set to 100ms, we noticed that every trajectories that we are sending would have a reduced speed when fed to move spline joint (here is an example) :

_[dsr_control_node2-6] [INFO] [1638523300.265757751] [dsr_hw_interface2]: msg->trajectory[0].joint_trajectory.points.size() =24 [dsr_control_node2-6] [INFO] [1638523300.265769856] [dsr_hw_interface2]: msg->trajectory[0].joint_trajectory.joint_names.size() =6 [dsr_control_node2-6] [INFO] [1638523300.265781289] [dsr_hw_interface2]: [trajectory] targetTime = 2219436544.000000 [dsr_control_node2-6] [INFO] [1638523300.265804263] [dsr_hw_interface2]: [trajectory] [00 : 0.000] -112.139 28.845 -98.519 -1.701 -104.654 250.471 [dsr_control_node2-6] [INFO] [1638523300.265840287] [dsr_hw_interface2]: [trajectory] [01 : 0.100] -112.415 28.728 -98.484 -1.694 -104.594 250.185 [dsr_control_node2-6] [INFO] [1638523300.265871113] [dsr_hw_interface2]: [trajectory] [02 : 0.200] -113.240 28.376 -98.380 -1.673 -104.415 249.325 [dsr_control_node2-6] [INFO] [1638523300.265899870] [dsr_hw_interface2]: [trajectory] [03 : 0.300] -114.616 27.791 -98.206 -1.639 -104.117 247.893 [dsr_control_node2-6] [INFO] [1638523300.265928016] [dsr_hw_interface2]: [trajectory] [04 : 0.400] -116.542 26.971 -97.963 -1.591 -103.699 245.887 [dsr_control_node2-6] [INFO] [1638523300.265955666] [dsr_hw_interface2]: [trajectory] [05 : 0.500] -119.018 25.917 -97.650 -1.529 -103.162 243.309 [dsr_control_node2-6] [INFO] [1638523300.265982635] [dsr_hw_interface2]: [trajectory] [06 : 0.600] -122.045 24.629 -97.267 -1.453 -102.506 240.158 [dsr_control_node2-6] [INFO] [1638523300.266009926] [dsr_hw_interface2]: [trajectory] [07 : 0.700] -125.621 23.107 -96.816 -1.363 -101.730 236.434 [dsr_control_node2-6] [INFO] [1638523300.266037500] [dsr_hw_interface2]: [trajectory] [08 : 0.800] -129.749 21.350 -96.294 -1.260 -100.835 232.136 [dsr_control_node2-6] [INFO] [1638523300.266065693] [dsr_hw_interface2]: [trajectory] [09 : 0.900] -134.426 19.360 -95.704 -1.143 -99.821 227.266 [dsr_control_node2-6] [INFO] [1638523300.266140256] [dsr_hw_interface2]: [trajectory] [10 : 1.000] -139.654 17.135 -95.043 -1.012 -98.687 221.823 [dsr_control_node2-6] [INFO] [1638523300.266170592] [dsr_hw_interface2]: [trajectory] [11 : 1.100] -145.432 14.676 -94.314 -0.868 -97.434 215.807 [dsr_control_node2-6] [INFO] [1638523300.266198497] [dsr_hw_interface2]: [trajectory] [12 : 1.200] -151.312 12.174 -93.571 -0.720 -96.159 209.685 [dsr_control_node2-6] [INFO] [1638523300.266225901] [dsr_hw_interface2]: [trajectory] [13 : 1.300] -156.646 9.903 -92.897 -0.587 -95.003 204.131 [dsr_control_node2-6] [INFO] [1638523300.266252022] [dsr_hw_interface2]: [trajectory] [14 : 1.400] -161.431 7.867 -92.293 -0.467 -93.965 199.149 [dsr_control_node2-6] [INFO] [1638523300.266277962] [dsr_hw_interface2]: [trajectory] [15 : 1.500] -165.665 6.065 -91.758 -0.361 -93.047 194.741 [dsr_control_node2-6] [INFO] [1638523300.266303888] [dsr_hw_interface2]: [trajectory] [16 : 1.600] -169.348 4.497 -91.293 -0.269 -92.248 190.905 [dsr_control_node2-6] [INFO] [1638523300.266329883] [dsr_hw_interface2]: [trajectory] [17 : 1.700] -172.482 3.164 -90.897 -0.191 -91.569 187.642 [dsr_control_node2-6] [INFO] [1638523300.266356050] [dsr_hw_interface2]: [trajectory] [18 : 1.800] -175.065 2.064 -90.571 -0.126 -91.008 184.953 [dsr_control_node2-6] [INFO] [1638523300.266381302] [dsr_hw_interface2]: [trajectory] [19 : 1.900] -177.098 1.199 -90.314 -0.075 -90.568 182.836 [dsr_control_node2-6] [INFO] [1638523300.266407169] [dsr_hw_interface2]: [trajectory] [20 : 2.000] -178.581 0.568 -90.127 -0.038 -90.246 181.292 [dsr_control_node2-6] [INFO] [1638523300.266433003] [dsr_hw_interface2]: [trajectory] [21 : 2.100] -179.513 0.171 -90.009 -0.015 -90.044 180.321 [dsr_control_node2-6] [INFO] [1638523300.266458272] [dsr_hw_interface2]: [trajectory] [22 : 2.200] -179.895 0.009 -89.961 -0.005 -89.961 179.924 [dsr_control_node2-6] [INFO] [1638523300.266484750] [dsr_hw_interface2]: [trajectory] [23 : 2.219] -179.906 0.004 -89.959 -0.005 -89.959 179.913 [dsr_control_node2-6] [INFO] [1638523300.266511302] [dsr_hw_interface2]: [trajectory] last targetTime = 2.219436 [dsr_control_node2-6] [INFO] [1638523300.291285371] [dsr_hw_interface2]: On Monitor State [dsr_control_node2-6] [callback OnMonitoringStateCB] current state: (2) MOVING [ditha_state_machine-9] [INFO] [1638523300.376343423] [smach_ros]: State machine transitioning 'FAULT':'stay'-->'FAULT' [dsr_control_node2-6] [INFO] [1638523300.852483320] [dsr_hw_interface2]: [callback OnLogAlarm] [dsr_control_node2-6] [INFO] [1638523300.852538780] [dsr_hw_interface2]: level : 1 [dsr_control_node2-6] [INFO] [1638523300.852552341] [dsr_hw_interface2]: group : 2 [dsr_control_node2-6] [INFO] [1638523300.852562826] [dsr_hw_interface2]: index : 1214 [dsr_control_node2-6] [INFO] [1638523300.852573727] [dsr_hw_interface2]: param : [INFO]finalTime(3.599) is adjusted above User Input(2.219) [dsr_control_node2-6] [INFO] [1638523300.852584747] [dsr_hw_interface2]: param : [dsr_control_node2-6] [INFO] [1638523300.852594874] [dsr_hwinterface2]: param :

We can see here that the time of execution is adjusted to 3.6 sec instead of 2.2. When we change the step of the downsampling to reduce the number of points provided we notice that this issue disappear :

_[dsr_control_node2-6] [INFO] [1638524979.426379027] [dsr_hw_interface2]: callback: Trajectory received [dsr_control_node2-6] [INFO] [1638524979.426389477] [dsr_hw_interface2]: msg->trajectory[0].joint_trajectory.points.size() =5 [dsr_control_node2-6] [INFO] [1638524979.426400081] [dsr_hw_interface2]: msg->trajectory[0].joint_trajectory.joint_names.size() =6 [dsr_control_node2-6] [INFO] [1638524979.426411142] [dsr_hw_interface2]: [trajectory] targetTime = 3005128448.000000 [dsr_control_node2-6] [INFO] [1638524979.426432086] [dsr_hw_interface2]: [trajectory] [00 : 0.000] -155.337 4.121 -82.141 -2.135 -100.028 206.948 [dsr_control_node2-6] [INFO] [1638524979.426465177] [dsr_hw_interface2]: [trajectory] [01 : 0.800] -137.450 6.106 -81.477 -2.235 -103.462 225.283 [dsr_control_node2-6] [INFO] [1638524979.426491976] [dsr_hw_interface2]: [trajectory] [02 : 1.600] -84.319 12.001 -79.503 -2.532 -113.662 279.743 [dsr_control_node2-6] [INFO] [1638524979.426519107] [dsr_hw_interface2]: [trajectory] [03 : 2.400] -39.372 16.988 -77.833 -2.783 -122.291 325.815 [dsr_control_node2-6] [INFO] [1638524979.426546122] [dsr_hw_interface2]: [trajectory] [04 : 3.005] -29.138 18.123 -77.453 -2.840 -124.256 336.305 [dsr_control_node2-6] [INFO] [1638524979.426572423] [dsr_hwinterface2]: [trajectory] last targetTime = 3.005128

In the end it seems to be that, for a specific trajectory, the more points we give to move spline joints the slower it goes. Why specifying more points with the correct time stamps slow down the realization of the movement? Is there a way to prevent this from happening?

[mpavezb]

@BenjaminTT I think the issue is related to: https://github.com/doosan-robotics/doosan-robot/issues/46#issuecomment-943178819 I have not tried yet with the updated driver.

[BenjaminTT]

Sorry for the late update, I got caught up on other matters.

I checked and the robot we are using is already up to date so the problem remains. I also tried calling the move_js "manually" instead of letting the dsr_node listen to the topic where the trajectory is published and build the call to move_js. The objective behind this is to remove the Time Parametrization from the moveit pipeline and use only the state given by the planner (without interpolation either)

I am not sure if this should not be new issue since I got some unexpected behavior , I tried the following : request->acc = {20.0}; // acc max per axis request->vel = {20.0}; // vel max per axis request->time= 0.0;

request->sync_type = 1; std::vector<std_msgs::msg::Float64MultiArray> JointTraj; for( auto & point : my_plan_.trajectory_.joint_trajectory.points) { std_msgs::msg::Float64MultiArray state_point; for(auto & joint_val : point.positions) { state_point.data.push_back(joint_val); } JointTraj.push_back(state_point); } request->pos_cnt = JointTraj.size(); request->pos = JointTraj;

doosan_move_spline_joint_->async_send_request(request);

That yielded the following error :

[callback OnLogAlarm] level : 2 group : 2 index : 1208 param : ## JNT(1) VEL LIMIT ERROR - INPUT( 0.000) < 0 [deg/sec]

The same happened with request->acc = {200.0 , 200.0, 200.0, 200.0, 200.0, 200.0}; // acc max per axis request->vel = {200.0 , 200.0, 200.0, 200.0, 200.0, 200.0}; // vel max per axis

And when I tried with : request->time= 0.5; (just an arbitrary value that gets adjusted) the first execution goes smoothly but the second one yield the following error :

level : 2 group : 2 index : 1203 param : state[JOINT_MOTION] rejected event[eMoveJX]

Eventhough I waited for the previous execution to be finished. and Also tried calling the service Move_Wait in between just to make sure.