Industrial Robot Client sends wrong velocity on last waypoint?
Abstract:
Created a plan to rotate single joint from 0 to pi/2 radians. Examined plan looks fine with smoothly tapering velocity, however, actual robot motion ends with accelerated jerk at end. Wireshark packet sniffing reveals velocity in broadcast packet for the last waypoint is wrong.
Details:
Ubuntu 16.04 (kernel 4.15.0-70-generic) ROS Kinetic Used apt dist-upgrade to get to latest versions
Using Staubli Experimental Package with Staubli TX60
Reduced joint speeds using joint_limits.yaml to set all max joint velocities to 1 In particular, joint_3 had velocity 9.424777960769999 and we set it to 1 (approx 1/10th).
I think I am using OMPL RRT Connect
Setup problem where we move single joint, joint_3 from 0 to -pi/2 radians.
Start: -0.000
Goal: -1.571
Use moveit to construct a plan:
group.set_joint_value_target(joint_goal)
plan = group.plan()
Plan has 13 steps with these way points (updated table with full precision values):
Pt Time Pos Vel
0 0.000 0.00000 0.000000
1 510.895 -0.13089 -0.435163
2 724.038 -0.26179 -0.700843
3 890.239 -0.39268 -0.855525
4 1031.980 -0.52358 -0.961742
5 1162.875 -0.65447 -1.000000
6 1293.770 -0.78537 -1.000000
7 1424.665 -0.91626 -1.000000
8 1555.560 -1.04716 -0.961742
9 1697.300 -1.17805 -0.855525
10 1863.502 -1.30895 -0.700843
11 2076.645 -1.43984 -0.435163
12 2587.540 -1.57074 0.000000
Basically, velocity follows a bell curve and position looks like a cumulative normal.
Use wireshark to capture outgoing "simplemessage" to robot controller. Joint Trajectory Point (rounded to 3 decimals).
The broadcast also has 13 waypoints.
It looks like the scaling factor for max velocity has been applied in executor?? For instance, 0.435 * (1.0/9.424) = 0.0461 ...
Velocity and Duration from Trajectory Point Messages (updated by extracting directly by script from wireshark json log of dissected messages):
Pt Velocity Duration
0 0.10000000 10.00000000
1 0.04617230 0.51089500
2 0.07436170 0.21314300
3 0.09077400 0.16620200
4 0.10204400 0.14174000
5 0.10610300 0.13089500
6 0.10610300 0.13089500
7 0.10610300 0.13089500
8 0.10204400 0.13089500
9 0.09077400 0.14174000
10 0.07436170 0.16620200
11 0.04617230 0.21314300
12 0.10000000 0.51089500 <- odd high velocity of 0.1 on last point
In a separate run inspired by Simon's comment, we were able to capture the position coming back from the robot. It seems like the robot controller is falling behind the plan. If we change the joint velocity limit to a lower value, the plan velocities adjust as expected, but the robot still seems to fall behind. The sudden velocity increase at the end looks like an attempt by the robot to catch up to the desired position. In the graph below we set the joint limit to 0.75 and the peak ...
Observation: just about all of what you write happens in the
industrial_robot_clientand the VAL3 parts ofstaubli_val3_driver. Not MoveIt. The "trajectory execution manager" in MoveIt (when configured with aMoveItSimpleControllerManager(the default)) forwards theJointTrajectorywholesale to theFollowJointTrajectoryaction server without any changes. The action server in this case would be theindustrial_robot_client.MoveIt issue 416 is also most likely not related.
Suggestion: change the title of your question and add at least the tags
staubli,val3andindustrial_robot_client.As to the behaviour your describing: it would appear that perhaps either duration should be used instead of velocity (on the VAL3 side), or the calculation of the velocity should be fixed.
For the latter: I've opened ros-industrial/industrial_core#247.
you do now, so please attach it.
Thanks gvdhoorn for opening a ticket and the background info. Learning more all the time.
I've opened ros-industrial/staubli_experimental#42 to track this on the
staubli_val3_driverside.You wrote: <-- Velocity is wrong on last point, but duration correct
but I'm not sure why that would be wrong? I think the first data point is always the current position (so the velocity does indeed not matter) and the last one is the "command to move to the last position" so it should not have zero velocity (you would never reach the goal)
I see that a velocity of zero would be problematic... It seemed like seq 1 and seq 12 could not both be consistent velocity-duration pairs and I made the assumption it was velocity that was wrong. The shift in values between plan and observed packets seems suspicious. I will rerun this experiment tomorrow with more decimal places under the assumption that the zero in the last step of the plan is just a very small value and double check the values in the packet sequence.
wrt the factor, please have a look at
rostopic echo /joint_trajectory_action/goalto see what actually gets transferred to industrial_robot_controller from MoveIt!. You can also send hand crafted packets to this topic and compare that. Also plotting/joint_statescan show you what the robot actually did. Can be surprising (UR ...)please also check this https://github.com/ros-industrial/sta...