Interrupting trajectories makes it difficult to get goal start position right.

[ros-industrial]

Interrupting trajectories makes it difficult to get goal start position right.

[ThomasTimm]

No input?

[gavanderhoorn]

I think "checking whether the start state of a trajectory corresponds to the current state" and "allowing trajectory replacement at arbitrary points in time (and thus at arbitrary trajectory points)" are fundamentally incompatible, unless there is a strict synchronisation between the executing party (ie: the controller) and the requesting party (ie: the entity that generated the trajectory) and there is some way to "look back" in time to see what the state of the robot was at the point where the trajectory was supposed to start.

That last part is doable, but seems to require common clocks (or at least, synchronised to a certain degree).

I think part of the difficulty comes from the fact that the whole FollowJointTrajectory pipe-line is rather loosely coupled, which is a good thing, but also means that there are no direct links between most of the components involved. This makes it hard to do something like trajectory replacement, because synchronisation of state and control flow is hard to achieve.

The oft referenced joint_trajectory_controller in ros_control (which is capable of on-line trajectory replacement) does at least part of the things I described above, iirc. But it can do that because it is in complete control of the state of the system under control (or at least, as much as that is possible).

[gavanderhoorn]

• Insert the current pose as starting pose if the first point in the trajectory has a time_since_start value higher than 0
  • This would allow the easiest transition from the old driver and make it possible to have the safety check in place for the critical trajectories (by specifying a start pose) and have "sloppy" safety when not needed

I don't know if this is a good idea, unless 'sloppy safety' means implementing some system that can, with certain levels of certainty, guarantee that the driver is able to transition to the new trajectory with a sane motion.

The ur_driver implementation has no such system, so it accepts whatever comes in. The ros_control controller tries to intelligently splice trajectories. I'm not sure how much time we (you) want to invest in adding functionality that is already present in ros_control, and which can be used with the modern driver in its current form?

[gavanderhoorn]

Note btw that this driver isn't the only one that struggles with this: the motoman_driver also requires the start state to be very close to the current state, making trajectory splicing difficult. But that driver is using relative motion on the robot controller side, so it really needs strict checking. Otherwise it'll end up somewhere different from what the trajectory specified.

[facontidavide]

Hi,

I am joining the discussion because I am struggling with #35 and #43...

My instinct teels me that a user friendly solution was the one you described as

Insert the current pose as starting pose if the first point in the trajectory has a time_since_start value higher than 0

With a difference, though. If time_since_start is equal to zero, use a interpolation time consistent with the maximum velocity and acceleration of the joints of the robot

On the other hand, as @gavanderhoorn noticed, this trajectory generated by the driver would not check collisions and self collisions and is potentially dangerous.
But isn't this what you expect from a "driver"?
A driver, IMHO, is not supposed to be intelligent, only a bridge between your planning and the hardware.

In fact, an "actual" driver would not even worry of large jumps if the trajectory is discontinuous.

Summarizing, my suggestion is to let the user the possibility to enable or disable the automatic insertion of an interpolated trajectory between the current position of the robot and the first position of the sent trajectory.
The default behavior should be the current one (i.e., to reject the discontinuous trajectory, with an informative warning message).

[gavanderhoorn]

@facontidavide wrote:

Summarizing, my suggestion is to let the user the possibility to enable or disable the automatic insertion of an interpolated trajectory between the current position of the robot and the first position of the sent trajectory.

if we go along with the "a driver should be stupid" (which does make sense), then it should also not "insert [..] an interpolated trajectory between the current position of the robot and the first position of the sent trajectory".

The joint_trajectory_controller of ros_control is already capable of this, so ideally this driver doesn't duplicate that functionality.

@ThomasTimm: would perhaps make sense to add a parameter that allows disabling the start-state check completely?

[gavanderhoorn]

The ros_control side of things already supports trajectory replacement.

The non-ros_control side of the driver doesn't, but I believe it makes sense to keep it that way.