trajectory relays: velocity calculation final segment incorrect?

[G.A. vd. Hoorn]

This is triggered by Moveit executor sends wrong velocity on last waypoint? on ROS Answers (note that the title is most likely incorrect: it's not MoveIt related).

I seem to remember @simonschmeisser reporting a similar observation, but using fanuc_driver(_exp). I believe he added some commits to their fork of ros-industrial/fanuc (these: ros-industrial/fanuc@82757ce...c5f8189) to work around/fix this.

But I'm not entirely sure what the cause was.

@simonschmeisser: could you perhaps clarify?

[Simon Schmeisser]

I think this code was mainly written to support velocity in "sparse" trajectories (only start and end point)

But this is a general issue I have been planning to think about for some time now ... how to best translate from velocities in rad/s as given by MoveIt (or its message format) back to % as used by some industrial controllers

[G.A. vd. Hoorn]

Code is here:

industrial_core/industrial_robot_client/src/joint_trajectory_interface.cpp

Lines 229 to 280 in a85789b

 // default velocity calculation computes the %-of-max-velocity for the "critical joint" (closest to velocity-limit) 
 // such that 0.2 = 20% of maximum joint speed. 
 // 
 // NOTE: this calculation uses the maximum joint speeds from the URDF file, which may differ from those defined on 
 // the physical robot.  These differences could lead to different actual movement velocities than intended. 
 // Behavior should be verified on a physical robot if movement velocity is critical. 
 bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity) 
 { 
   std::vector<double> vel_ratios; 
  
   ROS_ASSERT(all_joint_names_.size() == pt.positions.size()); 
  
   // check for empty velocities in ROS topic 
   if (pt.velocities.empty()) 
   { 
     ROS_WARN("Joint velocities unspecified.  Using default/safe speed."); 
     *rbt_velocity = default_vel_ratio_; 
     return true; 
   } 
  
   for (size_t i=0; i<all_joint_names_.size(); ++i) 
   { 
     const std::string &jnt_name = all_joint_names_[i]; 
  
     // update vel_ratios 
     if (jnt_name.empty())                             // ignore "dummy joints" in velocity calcs 
       vel_ratios.push_back(-1); 
     else if (joint_vel_limits_.count(jnt_name) == 0)  // no velocity limit specified for this joint 
       vel_ratios.push_back(-1); 
     else 
       vel_ratios.push_back( fabs(pt.velocities[i] / joint_vel_limits_[jnt_name]) );  // calculate expected duration for this joint 
   } 
  
   // find largest velocity-ratio (closest to max joint-speed) 
   int max_idx = std::max_element(vel_ratios.begin(), vel_ratios.end()) - vel_ratios.begin(); 
    
   if (vel_ratios[max_idx] > 0) 
     *rbt_velocity = vel_ratios[max_idx]; 
   else 
   { 
     ROS_WARN_ONCE("Joint velocity-limits unspecified.  Using default velocity-ratio."); 
     *rbt_velocity = default_vel_ratio_; 
   } 
  
   if ( (*rbt_velocity < 0) || (*rbt_velocity > 1) ) 
   { 
     ROS_WARN("computed velocity (%.1f %%) is out-of-range.  Clipping to [0-100%%]", *rbt_velocity * 100); 
     *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity));  // clip to [0,1] 
   } 
    
   return true; 
 } 

in essence: perc = target_vel_in_rad / max_joint_vel, for all joints, then take lowest one.

[G.A. vd. Hoorn]

There will always be a discrepancy between how ROS encodes joint state in JointTrajectoryPoint, while industrial robot controllers expect maximum velocity / duration for the next segment. Those two are completely different.

[G.A. vd. Hoorn]

I think this code was mainly written to support velocity in "sparse" trajectories (only start and end point)

I seem to remember (from our private email conversation, teaches me to not insist on discussing things on the tracker here ;) ) that the same behaviour is seen on trajectories with more than 1 segment (ie: incorrect or too low velocities on the last segment).

The last edit by the OP of the ROS Answers question shows this for the trajectory:

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

That 0.1 is most likely the default velocity ratio assigned to segments (default_vel_ratio_).