trajectory profile

[philip long]

Hi everybody,

I am controlling a UR using moveit by sending joint positions. I define a target configuration and set it using the move group. Movegroup then communicates with the robot driver via  follow_joint_trajectory . 

I have noticed that for the last trajectory point where the position is equal to the desired position, the velocity is zero however  the acceleration is non-zero. This discontinuity leads to quite an abrupt  stop of the physical robot. 

Does anyone have any information about this? 

Kind regards

Philip

[Daniel Solomon]

Philip,
If you are using the driver from the http://wiki.ros.org/universal_robot package, the acceleration values aren't even used. In fact, I'm pretty sure velocity isn't even currently used in the actual control loop, it is only sanity-checked when a new trajectory is sent to the driver [1]. The commands sent to the robot are only position and time using the send_servoj method [2].

Of course, the timestamps used are calculated to match the velocities and accelerations you are seeing in the trajectory, and moveit tries to create the minimum time path, so you will see very strong accelerations.  If you are just trying to smooth out the stop, I recommend spreading out the last few timestamps a bit so that the interpolator in the UR control cabinet can use lower acceleration values.


[1] http://docs.ros.org/indigo/api/ur_driver/html/driver_8py_source.html#l00741
[2] http://docs.ros.org/indigo/api/ur_driver/html/classur__driver_1_1driver_1_1CommanderTCPHandler.html#abd2f5b3741f1da1efc4ec5444227a520

[philip long]

Hi Daniel,

Thanks for getting back to me. 

I am using the ur_driver so only positions are eventually sent to the robot but the generation of these position is such that I have a discontinuity in acceleration as you can see below from the list of waypoints sent to the driver (non-zero acceleration for the last point). What I would like to do is to change the acceleration to zero for the last waypoint so the robot smoothly stops.

Also, your idea of changing the timestamps is very interesting, do you think this be used to change the execution speed of the trajectory on-line i.e. the first half of the trajectory is at one speed and the second half at an alternative speed? 

I am relatively new to moveit so I would very mech appreciate it if you have any tips for the practical implementation of these changes?  

Thanks very much

Philip

header: 

  seq: 19

  stamp: 

    secs: 1423727357

    nsecs: 31230602

  frame_id: ''

goal_id: 

  stamp: 

    secs: 1423727357

    nsecs: 31231264

  id: /move_group-20-1423727357.31231264

goal: 

  trajectory: 

    header: 

      seq: 0

      stamp: 

        secs: 0

        nsecs: 0

      frame_id: /world

    joint_names: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']

    points: 

      - 

        positions: [-0.7694681326495569, -2.29511005083193, -0.42692834535707647, -1.5764692465411585, 1.5913084745407104, 1.5688050985336304]

        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        accelerations: [0.0, 0.0, -0.4587561848065269, 0.0, 0.0, 0.0]

        effort: []

        time_from_start: 

          secs: 0

          nsecs: 0

      - 

        positions: [-0.9011669489587687, -2.1846932011309046, -0.5470397211843068, -1.564592749174104, 1.57584782757301, 1.6424558229664876]

        velocities: [-0.31100930320609554, 0.2607515272354517, -0.2836453382803828, 0.028046578353326232, -0.036510616991807424, 0.17392761095670464]

        accelerations: [-0.5044787002747231, 0.4229570954899079, -0.4600924477807298, 0.04549349890496102, -0.05922275770011273, 0.28212267032790916]

        effort: []

        time_from_start: 

          secs: 0

          nsecs: 723629232

      - 

        positions: [-1.0328657652679805, -2.0742763514298788, -0.6671510970115371, -1.5527162518070496, 1.5603871806053091, 1.7161065473993449]

        velocities: [-0.5057902336437553, 0.42405669066429463, -0.46128858668168937, 0.04561177196952333, -0.05937672380985529, 0.28285612705481183]

        accelerations: [-0.49663296194579626, 0.41637919498831455, -0.452937011980492, 0.04478597629224975, -0.05830171532551596, 0.2777350506982896]

        effort: []

        time_from_start: 

          secs: 1

          nsecs: 22930465

      - 

        positions: [-1.1645645815771921, -1.9638595017288534, -0.7872624728387673, -1.5408397544399952, 1.5449265336376086, 1.7897572718322021]

        velocities: [-0.6199681577185664, 0.5197839495345904, -0.5654206353519198, 0.055908248829774215, -0.07278052366999152, 0.34670853710689364]

        accelerations: [-0.45298576031501225, 0.3797851948492346, -0.41313008291460357, 0.040849905416485, -0.05317779702121658, 0.2533259625252633]

        effort: []

        time_from_start: 

          secs: 1

          nsecs: 253350596

      - 

        positions: [-1.296263397886404, -1.8534426520278278, -0.9073738486659976, -1.5289632570729408, 1.529465886669908, 1.8634079962650594]

        velocities: [-0.7122919374210671, 0.5971886004868601, -0.6496213632886205, 0.06423393585780923, -0.08361877874855564, 0.3983393220146617]

        accelerations: [-0.4732235549069284, 0.39675264821259054, -0.4315872674229093, 0.042674934075973524, -0.05555359208422454, 0.2646436666200204]

        effort: []

        time_from_start: 

          secs: 1

          nsecs: 450393363

      - 

        positions: [-1.4279622141956159, -1.7430258023268022, -1.027485224493228, -1.5170867597058866, 1.5140052397022072, 1.9370587206979164]

        velocities: [-0.7149346955810476, 0.5994043002078178, -0.6520315999746413, 0.06447225774412818, -0.08392902262224812, 0.39981724762127135]

        accelerations: [0.44660558484054586, -0.37443602850003227, 0.4073112633098919, -0.04027454612816047, 0.052428802889254715, -0.2497579384619171]

        effort: []

        time_from_start: 

          secs: 1

          nsecs: 624550446

      - 

        positions: [-1.5596610305048273, -1.6326089526257768, -1.147596600320458, -1.5052102623388322, 1.4985445927345067, 2.010709445130774]

        velocities: [-0.620302001710189, 0.5200638457620566, -0.5657251062824056, 0.055938354622663125, -0.07281971494172365, 0.34689523469856287]

        accelerations: [0.498935083422883, -0.4183093034604566, 0.4550365826955779, -0.04499357982603678, 0.05857197050648076, -0.2790224799954801]

        effort: []

        time_from_start: 

          secs: 1

          nsecs: 820047227

      - 

        positions: [-1.6913598468140392, -1.522192102924751, -1.2677079761476886, -1.4933337649717777, 1.4830839457668061, 2.084360169563631]

        velocities: [-0.5003405956919422, 0.4194876909457432, -0.45631843182004117, 0.04512032783509656, -0.05873696917244876, 0.27980849311020983]

        accelerations: [0.497085438537383, -0.4167585532939374, 0.45334967769358375, -0.04482678028122378, 0.05835483334919791, -0.27798809191530266]

        effort: []

        time_from_start: 

          secs: 2

          nsecs: 52344166

      - 

        positions: [-1.8230586631232508, -1.4117752532237255, -1.3878193519749187, -1.4814572676047233, 1.4676232987991054, 2.1580108939964884]

        velocities: [-0.30836249416469935, 0.25853243123822534, -0.2812314071272913, 0.027807891161655263, -0.03619989756906132, 0.17244741995121726]

        accelerations: [0.49006097594985165, -0.41086921387924275, 0.4469432581867079, -0.04419331967949852, 0.05753020379483621, -0.27405975927860626]

        effort: []

        time_from_start: 

          secs: 2

          nsecs: 355979902

      - 

        positions: [-1.9547574794324627, -1.3013584035227, -1.507930727802149, -1.4695807702376689, 1.4521626518314048, 2.2316616184293454]

        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        accelerations: [0.5084887612815299, -0.42631914775346524, 0.463749686001822, -0.04585512310420399, 0.059693514683982825, -0.28436524096330273]

        effort: []

        time_from_start: 

          secs: 3

          nsecs: 75702691

  path_tolerance: []

  goal_tolerance: []

  goal_time_tolerance: 

    secs: 0

    nsecs: 0

[Daniel Solomon]

Hey Philip.

I would first point out that it may be ok to have accelerations at the endpoints, and I agree that if they were closer to zero you would see smoother deceleration at the stop point.

Just to be clear, changing the velocity and acceleration values in the trajectory alone will not change how the robot executes the path, only adjusting the timestamps will have any effect (you could change joint positions as well, but that would be a much different problem... ).

You can try to just change the last time stamp, but I suspect you will have to smoothly change the last few in order to have the controller behave properly. Note: the trajectory uses "time_from_start", but you need to modify the difference between adjacent points, so be careful that after you figure out a new delta_t that you convert it to an equivalent time_from_start.

As to changing path speeds "on-line", do you mean that you decide to change the speed while it is executing? That is not currently implemented in the driver and would be a bit difficult to accomplish. If you want to change the entire second half of the trajectory, I would just modify the second half of that trajectory (as above) and send the new trajectory to the controller.

[G.A. vd. Hoorn - 3ME]

On 15-2-2015 17:16, Daniel Solomon wrote:
> Hey Philip.
[..]

> As to changing path speeds "on-line", do you mean that you decide to change
> the speed while it is executing? That is not currently implemented in the
> driver and would be a bit difficult to accomplish. If you want to change
> the entire second half of the trajectory, I would just modify the second
> half of that trajectory (as above) and send the new trajectory to the
> controller.

[..]

Just thought I'd mention PR#157 on the UR repository [1].


Gijs

[1] https://github.com/ros-industrial/universal_robot/pull/157

[Adolfo Rodríguez Tsouroukdissian]

On Sun, Feb 15, 2015 at 6:23 PM, G.A. vd. Hoorn - 3ME <g.a.van...@tudelft.nl> wrote:

On 15-2-2015 17:16, Daniel Solomon wrote:

Hey Philip.

[..]

As to changing path speeds "on-line", do you mean that you decide to change
the speed while it is executing? That is not currently implemented in the
driver and would be a bit difficult to accomplish. If you want to change
the entire second half of the trajectory, I would just modify the second
half of that trajectory (as above) and send the new trajectory to the
controller.

[..]

Just thought I'd mention PR#157 on the UR repository [1].

Hey Gijs,

To not get confused, I think these are two different problems. The original post is about MoveIt! generating trajectories with the last waypoint having non-zero acceleration, which is indeed unexpected. #157 above is specific to the UR robot driver, and is about an incorrect implementation of the trajectory replacement logic.

It turns out that the current Python incantation of the UR driver is in fact not only a driver, but also a FollowJointTrajectory action server (which requires trajectory replacement logic). A qualitative description of trajectory replacement can be found here:

http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement

Adolfo.

Gijs

[1] https://github.com/ros-industrial/universal_robot/pull/157

--

Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com

PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel

AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.

CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed.  If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited.  If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.

[G.A. vd. Hoorn - 3ME]

On 17-2-2015 10:59, Adolfo Rodríguez Tsouroukdissian wrote:
> On Sun, Feb 15, 2015 at 6:23 PM, G.A. vd. Hoorn - 3ME<
> g.a.van...@tudelft.nl> wrote:
>
>> On 15-2-2015 17:16, Daniel Solomon wrote:
>>
>>> Hey Philip.
>>>
>> [..]
>>
>>> As to changing path speeds "on-line", do you mean that you decide to
>>> change
>>> the speed while it is executing? That is not currently implemented in the
>>> driver and would be a bit difficult to accomplish. If you want to change
>>> the entire second half of the trajectory, I would just modify the second
>>> half of that trajectory (as above) and send the new trajectory to the
>>> controller.
>>>
>> [..]
>>
>> Just thought I'd mention PR#157 on the UR repository [1].
>>
>
> Hey Gijs,
>
> To not get confused, I think these are two different problems. The original
> post is about MoveIt! generating trajectories with the last waypoint having
> non-zero acceleration, which is indeed unexpected. #157 above is specific
> to the UR robot driver, and is about an incorrect implementation of the
> trajectory replacement logic.

Hi Adolfo,


thanks for your clarification. I understand the difference, but I
thought I'd mention PR#157, as Daniel suggested to Philip to perhaps
just send a new trajectory with updated velocities, while still
executing the old one. That is trajectory replacement, and, If I'm not
mistaken, that bit of functionality is actually affected by the bug
described in PR#157.

[Adolfo Rodríguez Tsouroukdissian]

On Tue, Feb 17, 2015 at 1:02 PM, G.A. vd. Hoorn - 3ME <g.a.van...@tudelft.nl> wrote:

On 17-2-2015 10:59, Adolfo Rodríguez Tsouroukdissian wrote:

On Sun, Feb 15, 2015 at 6:23 PM, G.A. vd. Hoorn - 3ME<
g.a.van...@tudelft.nl>  wrote:

On 15-2-2015 17:16, Daniel Solomon wrote:

Hey Philip.

[..]

As to changing path speeds "on-line", do you mean that you decide to
change
the speed while it is executing? That is not currently implemented in the
driver and would be a bit difficult to accomplish. If you want to change
the entire second half of the trajectory, I would just modify the second
half of that trajectory (as above) and send the new trajectory to the
controller.

[..]

Just thought I'd mention PR#157 on the UR repository [1].

Hey Gijs,

To not get confused, I think these are two different problems. The original
post is about MoveIt! generating trajectories with the last waypoint having
non-zero acceleration, which is indeed unexpected. #157 above is specific
to the UR robot driver, and is about an incorrect implementation of the
trajectory replacement logic.

Hi Adolfo,


thanks for your clarification. I understand the difference, but I thought I'd mention PR#157, as Daniel suggested to Philip to perhaps just send a new trajectory with updated velocities, while still executing the old one. That is trajectory replacement, and, If I'm not mistaken, that bit of functionality is actually affected by the bug described in PR#157.

I get your point now. You could hack your way around by sending a new endpoint that has the expected boundary condition, and similar end time. Hopefully there is a clener fix to this.

Adolfo.

[philip long]

Hi everybody,

After investigating a little bit more I don't think the jerk at the end is caused by the acceleration at the end point since by running the test move file which directly uses the FollowJointTrajectoryGoal action I have the same response.  Therefore its not caused by moveit, so I guess this isn't the right forum. Anyway to resolve this thread I "fixed" this by appending an endpoint to the trajectory, inserting in the driver.py file:

        with self.following_lock:

            if self.goal_handle:

                # Cancels the existing goal

                self.goal_handle.set_canceled()

                self.first_waypoint_id += len(self.goal_handle.get_goal().trajectory.points)

                self.goal_handle = None

            # Inserts the current setpoint at the head of the trajectory

            now = time.time()

            point0 = sample_traj(self.traj, now)

            point0.time_from_start = rospy.Duration(0.0)

            goal_handle.get_goal().trajectory.points.insert(0, point0)

            self.traj_t0 = now

            # Replaces the goal

            self.goal_handle = goal_handle

            self.traj = goal_handle.get_goal().trajectory

            self.goal_handle.set_accepted()

            #add another point at the end of the trajectory in the same position but a little further in time

            self.traj.points.append(JointTrajectoryPoint(

            self.traj.points[-1].positions,

            self.traj.points[-1].velocities,

            [0] * 6,

            [0] * 6,

            self.traj.points[-1].time_from_start+rospy.Duration(0.2)))

Philip

[G.A. vd. Hoorn - 3ME]

On 2-3-2015 9:26, philip long wrote:
> Hi everybody,
>
> After investigating a little bit more I don't think the jerk at the end is
> caused by the acceleration at the end point since by running the test move
> file

> <https://github.com/ros-industrial/universal_robot/blob/hydro-devel/ur_driver/test_move.py> which

> directly uses the FollowJointTrajectoryGoal action I have the same
> response. Therefore its not caused by moveit, so I guess this isn't the
> right forum. Anyway to resolve this thread I "fixed" this by appending an
> endpoint to the trajectory, inserting in the driver.py file:

[..]

Philip,


if you feel this is an issue with the driver itself (and it is not
solved by PR 157), I'd recommend you to file a bug report against the
universal_robot repository.

An MWE would help in that case (even if it is just 'run test_move.py and
observe').


Thanks,

Gijs

[Daniel Solomon]

This doesn't sound like the driver is doing anything "wrong". It is simply executing the path that is sent to it.  As far as I understand, the robot is not faulting, it is just stopping suddenly.

Philip, I understand why you added the code to the driver (so that it will automatically effect every path you send it), but if that change were made public the trajectory sent to the robot would be different from that requested by the user, and they would have no knowledge that it happened.  I think the appropriate place for your change is directly where you generate the trajectory in the first place.  Have you played with the final timestep to find the tradeoff between additional time and smooth stopping? 

Now, it would be nice to let other users know to watch out for this issue, and that this is a simple method to assist in trajectory smoothness.  Gijs, any ideas on that?

-Dan

[Adolfo Rodríguez Tsouroukdissian]

Just for the record, I can confirm that for robot/MoveIt configurations we're using we also get nonzero accelerations in the last waypoint. This looks like a bug in the trajectory time paramterization code. I suggest opening an issue in the moveit_core repo:

https://github.com/ros-planning/moveit_core/issues

Adolfo.

-Dan

[philip long]

Gijs, I tried the fixed proposed in PR 157, but it didn't help. What Daniel wrote is correct the robot is not faulting, it is stopping suddenly with a jerky motion, especially apparent at increased velocities/accelerations. 

Daniel,  I completely agree that changing the driver is not a good strategy but I don't see any other practical way to do it. For instance if I use the test file's follow joint trajectory action its trivial to append an extra point , but how to do this when the trajectory is generated by moveit (moveit publishes a trajectory to the followjointtrajectory topic, which is then read directly by the driver.py file) ?

Incidentally, I could not smooth out the ending by changing the timestamp of the final point, however even if possible wouldn't this also require a modification of the driver?

Philip