Hi,

we try to program a solution which is simular to the Active Drive Toolbar but we want to use it programmically in a program. We know that we cannot embed the Active Drive so we have to do our own solution but are struggling. We took the "template_sensor.urp" and modified it so it looks pretty much like the code below. It should do nothing when you press play and just acts like the Active Drive when the digital_out[0] gets true. So we can use the digital outputs to regulate the Active Drive-Feature.

We receive the forces and torques from the sensor in the Thread_1, multiply with values < 1 so we can use that values as velocities and transform them with baseToTool() to the tcp frame. These transformed values we pass to the speedl() method.

When the program is like this the robot behaves shaky. We don't know why and changed all parameters, tried to change the parameters, added round-functions but nothing really helps.

We thought of using the force mode somehow to achieve this but didn't try it and want to get some opinions here first.

Do you have an idea how to realize this?

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_2
	timer≔timer + 0.008
    sync()