# 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


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

edit retag close merge delete

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.

( 2019-11-25 14:55:58 -0600 )edit

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.

( 2019-11-25 15:04:11 -0600 )edit

Thanks gvdhoorn for opening a ticket and the background info. Learning more all the time.

( 2019-11-25 16:46:47 -0600 )edit

I've opened ros-industrial/staubli_experimental#42 to track this on the staubli_val3_driver side.

( 2019-11-26 02:11:20 -0600 )edit

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)

( 2019-11-26 07:32:36 -0600 )edit

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.

( 2019-11-26 22:37:01 -0600 )edit

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 ...)

( 2019-11-27 03:20:17 -0600 )edit
( 2019-11-27 03:22:39 -0600 )edit

Sort by » oldest newest most voted

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.

more

I've converted my comment to an answer, as I believe this is now the answer to your question.

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.

Summarising: it's not really a trajectory point, as much as it is a segment. Segments cannot have zero velocity, as there would be no motion (of course it's valid to have a zero-velocity segment, that would (should!) just cause the robot to pause).

( 2019-11-27 02:05:39 -0600 )edit