trajectory relays: velocity calculation final segment incorrect?

[gavanderhoorn]

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?

[simonschmeisser]

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

[gavanderhoorn]

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.

[gavanderhoorn]

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.

[gavanderhoorn]

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