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.
$ 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,tool,tool] tempt = p[vec,vec,vec,0,0,0] tempr = p[vec,vec,vec,0,0,0] t = pose_trans(tcp, tempt) r = pose_trans(tcp, tempr) return [t,t,t,r,r,r] 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=Aus Einstellen DO=Aus Roboterprogramm rq_set_zero() Schleife timer<0.3 If digital_out≟ 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 Fx = tolerance(data, tol_Vec) Fy = tolerance(data, tol_Vec) Fz = tolerance(data, tol_Vec) Mx = tolerance(data, tol_Vec) My = tolerance(data, tol_Vec) Mz = tolerance(data, tol_Vec) SpeedX = forceToVel(Fx, forceToSpeedFac) SpeedY = forceToVel(Fy, forceToSpeedFac) SpeedZ = forceToVel(Fz, forceToSpeedFac) SpeedMX = forceToVel(Mx, forceToSpeedFac) SpeedMY = forceToVel(My, forceToSpeedFac) SpeedMZ = forceToVel(Mz, forceToSpeedFac) 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()
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.
In this case, I would recommend you add a proper control algorithm here like simple PID control. That could solve your issue.