Smooth motion with robotiq force torque sensor and speedl()?

Technical Questions URScript
1

Hi,

my supervisor and me are trying to program a feature like the active drive with a own urp-script so we can use digital outputs to en-/disable the use of the roboter as a balancer.

We invested a lot of time for this but we met problems while realizing this. Now it’s basically working but the motion is not smooth at all. However we change the parameters the motion stutters which is not suitable for our application.

I wrote a script-file “fromTemplateDefinitionen.script” for clarity which holds several methods.
In a thread we receive the values from the Robotiq FT-300 Force Torque Sensor, multiply them with a value < 1 to get a value to pass it to the speedl method (otherwise the forces would be to big and the motion to fast). After that we transform them to the tcp-frame with the “baseToTool” method so the robot moves as expected.

As I said fundamentally it works but we don’t know what else to try to get it running smoothly.

Any advices and hints are highly appreciated!
Thank you in advance.

Frederik

  $ 6 "Script: fromTemplateDefinitionen.script"
  def tolerance(value, tolerance):
	  if (norm(value) < tolerance):
		return 0
	  else:
		return value
	  end
  end
  
  def forceToVel(force, multiplyer):
	return force * multiplyer
  end
  
  def baseToTool(vec):
	  tool = get_actual_tcp_pose()
	  tcp = p[0,0,0,tool[3],tool[4],tool[5]]
	  tempt = p[vec[0],vec[1],vec[2],0,0,0]
	  tempr = p[vec[3],vec[4],vec[5],0,0,0]
	  t = pose_trans(tcp, tempt)
	  r = pose_trans(tcp, tempr)
	  return [t[0],t[1],t[2],r[0],r[1],r[2]]
  end

 Programm
   Init Variablen
   VorStart
     socket_close("stream")
     socket_open("127.0.0.1",63351,"stream")
     Script: accessor_capt.script
     Script: airbus-fromTemplateDefinitionen.script
     speed_Vec≔[0.0,0.0,0.0,0.0,0.0,0.0]
     tol_Vec≔[10.0,10.0,10.0,5.0,5.0,5.0]
     forceToSpeedFac≔[0.004,0.004,0.004,0,0,0]
     timer≔0.0
     Einstellen DO[0]=Aus
     Einstellen DO[2]=Aus
   Roboterprogramm
     rq_set_zero()
     Schleife timer<0.3
       If digital_out[0]≟ True 
         speedl(speed_Vec, 10, 0.008)
       Else
         stopl(10)
         Warten: 0.1
   Thread_1
     Schleife
       data≔socket_read_ascii_float(6,"stream")
       If data[0]≠0
         Fx = tolerance(data[1], tol_Vec[0])
         Fy = tolerance(data[2], tol_Vec[1])
         Fz = tolerance(data[3], tol_Vec[2])
         Mx = tolerance(data[4], tol_Vec[3])
         My = tolerance(data[5], tol_Vec[4])
         Mz = tolerance(data[6], tol_Vec[5])
          
         SpeedX = forceToVel(Fx, forceToSpeedFac[0])
         SpeedY = forceToVel(Fy, forceToSpeedFac[1])
         SpeedZ = forceToVel(Fz, forceToSpeedFac[2])
         SpeedMX = forceToVel(Mx, forceToSpeedFac[3])
         SpeedMY = forceToVel(My, forceToSpeedFac[4])
         SpeedMZ = forceToVel(Mz, forceToSpeedFac[5])
		  
         speed_Vec≔baseToTool([SpeedX,SpeedY,SpeedZ,SpeedMX,SpeedMY,SpeedMZ])
         timer≔0.0
       Else
         speed_Vec≔[0.0,0.0,0.0,0.0,0.0,0.0]
         timer≔2
   Thread_3
     timer≔timer + 0.008
     sync()
2

For anything involving a constantly changing stream of positions, recommend you look into using the servoj command, it’s possible to get a lot smoother movements than with move or speed commands.

3

In this case, I would recommend you add a proper control algorithm here like simple PID control. That could solve your issue.

4

So here I have also tested force servo with the force sensor mounted on the robot end using speedl command. the parameters of speedl: a=1.0, t=0.1
but I found it is not smooth, it seems that within each control loop, the robot is trying to stop and continue for another control command.
By the way, I have already used the PID controller for generating the j with force sensor input data.

So my question is how can I eliminate the “stop” within each control loop. Any one can help?