Discretization of joint trajectory

[HS]

Hello everyone,

I'm working in my master thesis on optimizing a joint space trajectory which is then executed via ROS (incl. Kuka Experimential package) and passed to a Kuka KR6 robot via RSI. I'm not yet completely familiar with the ROS industrial structure as specified here. My assumptions from what I got so far are:

• the Kuka RSI interface allows only for positions to be passed and not velocities or acceleration limits?
• the controller runs with 50Hz = 20 ms
• RSI runs with 250 Hz = 4ms
• Kuka tries to reach the passed point with unkown/max velocity and acceleration settings?

My main question is: Which time discretization (time_from_start) and which joint space displacement is favorable from one joint space point to another to generate a smooth path?

Besides that it's not quite clear to me which controllers are actually active. I looked into the code but didn't quite get which controller processes my trajectory on it's way to the robot.

Thank you in advance! I'm thankful for any advice shedding some light on this.

ps:

Here is the code/system I use (mostly a legacy from former projects in the institute):

ROS Indigo on Ubuntu 14.04

calling via Service (I'm aware that there is a new Action - not sure if that'd change something for my case).


  moveit_msgs::ExecuteKnownTrajectory srv; /** Execute trajectory service */
  srv.request.trajectory  = moveit_traj; /** hand over trajectory to service request */

  srv.request.wait_for_execution = true;

  ROS_INFO_STREAM("Robot path sent for execution");
  if(moveit_run_path_client_.call(srv)) /** send trajectory execution request */
  {
    ROS_INFO_STREAM("Robot path execution completed");

  }

with a moveit trajectory generated by passing a csv-file

  out_traj.header.stamp = ros::Time::now(); /** time stamp of trajectory point */
  out_traj.header.frame_id = config_.world_frame; /** frame id is world frame name */
  out_traj.joint_names = config_.joint_names; /** supply joint names */

 [...csv stuff]

  std::vector<double> J(6, 0.0); /** array to store 6 joint positions wit init value 0.0 */
  double time_offset = 0.0; /** increasing trajectory time offset */
  char* ctimeoffset; char* cj1; char* cj2; char* cj3; char* cj4; char* cj5; char* cj6;/** variables to store input characters of each column */

  while (csv.read_row(ctimeoffset,cj1,cj2,cj3,cj4,cj5,cj6) ) {
          time_offset = strtod(ctimeoffset, 0);
          J[0] = strtod(cj1, 0);
          J[1] = strtod(cj2, 0);
          J[2] = strtod(cj3, 0);
          J[3] = strtod(cj4, 0);
          J[4] = strtod(cj5, 0);
          J[5] = strtod(cj6, 0);

         // build trajectory
          trajectory_msgs::JointTrajectoryPoint pt; /** ROS joint trajectory point */
          pt.positions = J; /** copy joint solution */

          pt.velocities.resize(J.size(), 0.0); /** trajectory point velocity values set to 0, controller will calculate them */
          pt.accelerations.resize(J.size(), 0.0); /** trajectory point acceleration values set to 0, controller will calculate them */
          pt.effort.resize(J.size(), 0.0); /** trajectory point effort values set to 0, controller will calculate them */

          pt.time_from_start = ros::Duration(time_offset); /** set ROS time of trajectory point (including offset increment along trajectory) */

          out_traj.points.push_back(pt); /** append trajectory point */
  }
}

[Hemes]

I am not sure what controller you are referring to (50Hz?) but the Kuka is indeed at 250Hz and will require position updates every 4ms.  The Kuka Experimental RSI interface uses position controller via ros_contol.  In this setup the position controller will sample your provided joint_trajectory at 250Hz via interpolation to provide positional set points to the Kuka controller (via RSI).

Here is the kicker, ros_control will interpolate as best it can given the information provided in the joint_trajectory.  Time and position will result in linear interpolation; time, position, and velocity will result in cubic interpolation; and finally time, position, velocity, and acceleration will result in quintic interpolation.  I would suggest in providing at least enough to get cubic interpolation which results in smooth velocities and piecewise linear joint accelerations (I have found this fine in practice with modest trajectory demands).

Long story short you need to fill in your joint trajectory with appropriate dynamic information.  To do this you can use MoveIt!'s iterativeParabolicTimeParameterization.  There is also code that provides optimal (versus iterative approximate) solutions such as Tobias Kuntz's work (see https://github.com/tobiaskunz/trajectories and/or http://www.tobiaskunz.net/software.htm).

Hope that helps,

Brett

Despite the fact that only positions are being sent, the 

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

On 5-10-2017 16:00, Hemes wrote:
> I am not sure what controller you are referring to (50Hz?) but the Kuka is
> indeed at 250Hz and will require position updates every 4ms. The Kuka
> Experimental RSI interface uses position controller via ros_contol. In
> this setup the position controller will sample your provided
> joint_trajectory at 250Hz via interpolation to provide positional set
> points to the Kuka controller (via RSI).
>
> Here is the kicker, ros_control will interpolate as best it can given the
> information provided in the joint_trajectory. Time and position will

> result in *linear* interpolation; time, position, and velocity will result

> in cubic interpolation; and finally time, position, velocity, and
> acceleration will result in quintic interpolation. I would suggest in
> providing at least enough to get cubic interpolation which results in
> smooth velocities and piecewise linear joint accelerations (I have found
> this fine in practice with modest trajectory demands).
>
> Long story short you need to fill in your joint trajectory with appropriate
> dynamic information. To do this you can use MoveIt!'s
> iterativeParabolicTimeParameterization. There is also code that provides
> optimal (versus iterative approximate) solutions such as Tobias Kuntz's
> work (see https://github.com/tobiaskunz/trajectories and/or
> http://www.tobiaskunz.net/software.htm).

Nice reply Brett, thanks.

There are some packages available that wrap work like Tobias' such as
moveit_topp [1, from 2] and moveit_totg. The latter is not yet
open-source, but I could ask Ruben whether he would be ok with pushing
that to his public account.

That would allow you to use TOTP and TOTG as replacements for the IPTP.


Gijs

[1] https://github.com/rbbg/moveit_topp
[2] https://github.com/davetcoleman/moveit_topp

> On Thursday, October 5, 2017 at 8:30:15 AM UTC-5, HS wrote:
>>
>> Hello everyone,
>>
>> I'm working in my master thesis on optimizing a joint space trajectory
>> which is then executed via ROS (incl. Kuka Experimential package) and
>> passed to a Kuka KR6 robot via RSI. I'm not yet completely familiar with
>> the ROS industrial structure as specified here.

>> <http://wiki.ros.org/ros_control#Overview> My assumptions from what I got
>> so far are:
>>
>> - the Kuka RSI interface allows only for positions to be passed and

>> not velocities or acceleration limits?

>> - the controller runs with 50Hz = 20 ms
>> - RSI runs with 250 Hz = 4ms
>> - Kuka tries to reach the passed point with unkown/max velocity and
>> acceleration settings?
>>
>> *My main question is:* Which time discretization (time_from_start) and

>> which joint space displacement is favorable from one joint space point to
>> another to generate a smooth path?
>>
>>

>> *Besides that it's not quite clear to me which controllers are actually

>> active. I looked into the code but didn't quite get which controller

>> processes my trajectory on it's way to the robot.*

>>
>>
>>
>> Thank you in advance! I'm thankful for any advice shedding some light on
>> this.
>>
>>
>>
>> ps:
>>

>> *Here is the code/system I use (mostly a legacy from former projects in
>> the institute):*

[HS]

Thank you very much!

On Thursday, October 5, 2017 at 4:00:36 PM UTC+2, Hemes wrote:

I am not sure what controller you are referring to (50Hz?) but the Kuka is indeed at 250Hz and will require position updates every 4ms.  The Kuka Experimental RSI interface uses position controller via ros_contol.  In this setup the position controller will sample your provided joint_trajectory at 250Hz via interpolation to provide positional set points to the Kuka controller (via RSI).

 

With 50Hz controller, I was referring to the controller specified in kuka_experimental/kuka_rsi_hw_interface/config/hardware_controllers.yaml (see below). Where can I find the position controller used by kuka_expermental or how do I find that out?

 

Here is the kicker, ros_control will interpolate as best it can given the information provided in the joint_trajectory.  Time and position will result in linear interpolation; time, position, and velocity will result in cubic interpolation; and finally time, position, velocity, and acceleration will result in quintic interpolation.  I would suggest in providing at least enough to get cubic interpolation which results in smooth velocities and piecewise linear joint accelerations (I have found this fine in practice with modest trajectory demands).

 

Long story short you need to fill in your joint trajectory with appropriate dynamic information.  To do this you can use MoveIt!'s iterativeParabolicTimeParameterization.  There is also code that provides optimal (versus iterative approximate) solutions such as Tobias Kuntz's work (see https://github.com/tobiaskunz/trajectories and/or http://www.tobiaskunz.net/software.htm).

Hope that helps,

Brett

Despite the fact that only positions are being sent, the

That makes sense to me, thanks. Since I develop my trajectories in Matlab for faster development and testing of new ideas I want to handle time parameterization there. But Tobias' implementation looks helpful to gather some ideas on how to do the velocity part. If I got it right, the discretization of the moveit trajectory depends on my task but is not so relevant for the smoothness of the robot path since it is interpolated by the controller (when using velocities and accelerations).

 kuka_experimental/kuka_rsi_hw_interface/config/hardware_controllers.yaml

#Publish all joint states
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

# Joint trajectory controller
position_trajectory_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - joint_a1
    - joint_a2
    - joint_a3
    - joint_a4
    - joint_a5
    - joint_a6

  state_publish_rate: 50 # Defaults to 50
  action_monitor_rate: 20 # Defaults to 20

[Hemes]

Gijs, I would be interested in seeing a moveit_totg repo made public.  I was actually thinking about putting the same together myself but if Ruben will share that would be great!

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

On 6-10-2017 22:17, Hemes wrote:
> Gijs, I would be interested in seeing a moveit_totg repo made public. I
> was actually thinking about putting the same together myself but if Ruben
> will share that would be great!

Brett,


see [3] for the code.

it's not too sophisticated, but shows how this could work.

Afaik, we've not actually used this for anything other than for
comparison when working on/looking at [4, 5].

One outstanding issue is the 'sub sampling' of the TOTG output: the
2.5ms step size is 'arbitrary' at the moment, and the for-loop doing the
work most likely skips the last point in the TOTG output.


Gijs

[3] https://github.com/tud-cor/moveit_totg_planning_adapter
[4] https://github.com/ros-planning/moveit/issues/160
[5] https://github.com/ros-planning/moveit/pull/441