UR5 stops during intermediate points of the trajectory using movej (ROS + Moveit)

Technical Questions URScript
1

Overview

I’m currently working on a project using ROS, MoveIt (noetic), and URSim 3.14.1. I use moveit for generating the trajectories between desired points. The communication between the ROS and the robot is via TCP sockets. I’m NOT using the Universal_Robots_ROS_Driver either for communication or for motor control.

The trajectories generated by the planner are passed to the robot, which is running a URScript program that receives the waypoints in joint space and then calls the function movej.

The Issue

The problem I’m experiencing is that for each point in the trajectory the robot will stop. So far I have tried adding a blend radius value depending on the euclidean distance between the points in the trajectory. With this approach, the robot doesn’t stop on each intermediary point of the trajectory, but the overall speed of the robot is affected and the robot seems to accelerate and deaccelerate between the points in the trajectory.

  • I also tried to use the servoj function without luck. When I use this function, it seems the robot doesn’t wait until the motors reach that position before trying to apply the next servoj command. Is this function only meant to be used with RTDE communication?
  • The movep function throws a protective stop, “C204A1: Path sanity check failed: Sudden change in target position.”

Note:
So far I’ve only been working with URsim, and not with real hardware.

Here is a video showing the issue.

Code

This is a code snipped of how my execute trajectory function looks like

...

while i < end_idx:

	pose = [0, 0, 0, 0, 0, 0]          
	is_last_point = (i + 5) == end_idx

	if (are_joint_values):
	  pose =  [traj_buff[i], traj_buff[i + 1], traj_buff[i + 2], traj_buff[i + 3], traj_buff[i + 4], traj_buff[i + 5]]
	else:
	  pose = p[traj_buff[i], traj_buff[i + 1], traj_buff[i + 2], traj_buff[i + 3], traj_buff[i + 4], traj_buff[i + 5]]
	  
	end

	
	if (is_last_point and stop_at_point == 1):
		radius = 0
	end

	# The last movement of the trajectory must be a precise movement
	if (is_last_point):
	  if (are_joint_values):
 
		  movel(get_forward_kin(pose), accel, vel)
	  else:
		  movel(pose, accel, vel)
	  end
	elif (move_type == MOVEJ):
		if (are_joint_values):
			movej(pose, accel, vel, 0, radius)
		end
	elif (move_type == MOVEP):
		if (are_joint_values):
			movep(get_forward_kin(pose), accel, vel)
		end
	end

	i = i + NUM_JOINTS
end

...

I’ll appreciate any information or advice that points me in the correct direction.

Thanks!

2

I’m running into this problem as well. Bumping this for more visibility.