# 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:

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 plan velocity is in fact 0.75. The scaling with joint velocity limit suggests that the problem is not a physical capability limit of the robot, but some scaling factor somewhere. This is taken with the pendant/MCP velocity set to 100% and the Velocity Overwrite disabled in the val3 driver.

Analysis of the Staubli Val3 motion server suggests it is looking only at velocity which might explain the behavior.

The Staubli Val3 packet receiving logic in pushMotion.pgx copies velocity and duration into local structure and enqueues it

  l_mTrajPt.mDesc.vel  = limit((rosTrajPtMsg.jointTrajPt.nVelocity * 100), 0, 100)

l_mTrajPt.nDuration  = rosTrajPtMsg.jointTrajPt.nDuration


The Staubli Val3 waypoint execution logic in motionControl.pgx dequeues waypoint and calls a Staubli val3 motion primitive with only the velocity (which would be in the mDesc component). There is no mechanism to communicate the duration to the Staubli controller.

  movej(l_motion.jJointRx, tTool, l_motion.mDesc)


ros-planning/moveit#416 (Jan 16, 2017 Moveit! trajectory velocity has weird behavior on kinetic).

This answer seems to describe erratic behavior throughout the trajectory caused by the path simplifier, however, I think the problem described here might be happening after the path simplifier because times have already been applied? So this is probably not the planner or simplifier?

Originally posted by bprice on ROS Answers with karma: 13 on 2019-11-25

Post score: 0

Comment by gvdhoorn on 2019-11-25:
Observation: just about all of what you write happens in the industrial_robot_client and the VAL3 parts of staubli_val3_driver. Not MoveIt. The "trajectory execution manager" in MoveIt (when configured with a MoveItSimpleControllerManager (the default)) forwards the JointTrajectory wholesale to the FollowJointTrajectory action server without any changes. The action server in this case would be the industrial_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, val3 and industrial_robot_client.

Comment by gvdhoorn on 2019-11-25:
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.

I have a plot of this, but not enough karma to attach it.

you do now, so please attach it.

Comment by bprice on 2019-11-25:
Thanks gvdhoorn for opening a ticket and the background info. Learning more all the time.

Comment by gvdhoorn on 2019-11-26:
I've opened ros-industrial/staubli_experimental#42 to track this on the staubli_val3_driver side.

Comment by Simon Schmeisser on 2019-11-26:
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)

Comment by bprice on 2019-11-26:
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.

Comment by Simon Schmeisser on 2019-11-27:
wrt the factor, please have a look at rostopic echo /joint_trajectory_action/goal to 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_states can show you what the robot actually did. Can be surprising (UR ...)

Comment by Simon Schmeisser on 2019-11-27:

Comment by gvdhoorn on 2019-11-27:
@Simon Schmeisser: that last issue is also linked to in ros-industrial/staubli_experimental#42.

Comment by bprice on 2019-11-27:
Reran the experiment and captured plan with more decimals and wrote a script to computationally parse out wireshark dissection logs. Updated the data tables. The last velocity coming out of the industrial robot client as reported by wireshark seems high. Will follow up on Simon's suggestions to check additional topics.

The shift in values between plan and observed packets seems suspicious.

this is probably due to the fact that a JointTrajectory encodes the state of the system under control at specific times, while a typical industrial robot trajectory (or really: program) specifies constraints on segments (ie: between points).

A segment is defined by two points: start and end. So in the JointTrajectory, start and end velocity could be 0, as in the first list of points you show. But if we translate that into segments, no velocity can be 0 any more, as that would lead the industrial robot controller to not move at all.

Without using a real-time, external motion control interface (with which you typically are able to specify the desired state of the system at any point in time (ie: specify position, velocity and/or effort setpoints)), I don't know of any industrial robot controller that allows you to specify state of the system at specific points in time.

They always only let you (implicitly) define segments and attach constraints to those segments (ie: please use a maximum of 0.511 seconds to move along this segment, please execute this motion (ie: travel along this segment) with 50% of maximum joint velocity, etc). Important to realise is that those constraints (or: motion settings or modifiers) typically really only specify a desired maximum velocity or time. The industrial robot controller then uses those constraints to generate a velocity profile with the desired characteristics (ie: leads to motion that takes 0.511 seconds from start to end, or leads to a velocity profile with its maximum (ie: constant phase) velocity at 50% of maximum joint velocity, etc). But you never control the state of the system (ie: the robot and its joints) directly.

That is a fundamentally different way of encoding robot motion and the industrial_robot_client needs to translate between the two.

It is this discrepancy which is what I believe causes what you observe.

Edit:

The last velocity coming out of the industrial robot client as reported by wireshark seems high

This may be ros-industrial/industrial_core#247.

Originally posted by gvdhoorn with karma: 86574 on 2019-11-27

This answer was ACCEPTED on the original site

Post score: 0

The initial question asked what caused the "jerk" the end of trajectory execution on a Stäubli robot. If that is still a problem, then I would recommend continuing at ros-industrial/staubli_experimental#42, as that may be a problem in staubli_val3_driver.
After your edits, you seem to ask why there is a non-zero velocity encoded in the last traj pt which the industrial_robot_client sends. The rationale for that is in my answer.