We want to control the robot with speedj commands and occasionally use force_mode to push against things with a certain force.
The way we (want to) do this:
- We connect via the RTDE interface and send over F/T data.
- Via the realtime client interface (port 30003) we:
- a) send over a script that runs a thread which connects via a socket back to our architecture and pulls data.
- b) run force_mode with this data.
- We send over speedj commands via the real time client interface (30003) continuously to control the robot.
However as soon as a speedj command is sent in step 3, the socket connection established in step 2 closes and force_mode is not active anymore (although the log does not show that it went out of force_mode).
The script we send over in step 2 is the following:
def force_test():
textmsg("Starting")
socket_open("192.168.100.1", 7654, "stream")
enable_external_ft_sensor(True, sensor_mass = 0.2, sensor_measuring_offset = [0.0, 0.0, 0.0], sensor_cog = [0.0, 0.0, 0.0175])
thread Force_properties_calculation_thread_1():
while(True):
f = socket_read_ascii_float(6, "stream")
s = socket_read_ascii_float(6, "stream")
w = socket_read_ascii_float(6, "stream")
t = socket_read_ascii_float(1, "stream")
l = socket_read_ascii_float(6, "stream")
force_mode(p[f[1],f[2],f[3],f[4],f[5],f[6]], [s[1],s[2],s[3],s[4],s[5],s[6]], [w[1],w[2],w[3],w[4],w[5],w[6]], t[1], [l[1],l[2],l[3],l[4],l[5],l[6]])
sync()
end
end
global thread_handler_1 = run Force_properties_calculation_thread_1()
while(True):
sync()
end
kill thread_handler_1
end_force_mode()
end
Whenever I send out just one speedj command after this connection has been established the connection is lost.
Both elements work individually: We can send out speedj very reliably at 125 Hz and we can enable force_mode and control its parameters from outside. It is only that if we combine those two it breaks.
So the question is: How can we continuously send out speedj commands while controlling force_mode parameters from outside?
There cannot be two different threads (which these programs would be) that control the robot at the same time.
Hence any running program on the controller will stop immediately when a new script command or script program is received.
You have the option for a secondary program, however this cannot take any physical time (sleep, sync or motion).
The best option would be to have your program above also include a thread or loop that gets speedj data over e.g. RTDE, and executes these while also operating in and controlling the force mode settings.
Hence, in stead of just a sync() in the while loop, you could have the speedj command here.
Thank you @jbm I am now sending over all data via RTDE and read it out in the script I send over.
This way I can execute both speedj commands and force_mode while varying the data over time however I want.
However I noticed a behaviour which seems strange to me: My speedj movement results in a circular movement in the X-Y tool plane whereas I test different forces here (think about a polishing movement). As in an earlier example I only want to apply the force in the tools Z direction. So I send over as a task_frame its tool frame, as selection frame a 1 in the linear z position, as wrench a force zForce in the z direction, as type 2 and some limits.
When I set the zForce to 0 I expect normal behaviour as if force_mode was deactivated (so circular movement in tool X-Y plane while keeping Z position). However what happens is the wrist drifting up and down (in Z) quite a lot. Why is that so? //Edit: I wanna add that this is most prominent when pointing down with the tool.
Also it seems that the wrench force is pretty insensitive when applying small forces (<5N).
Bonus question: I regulary get the error: “Force mode: Maximum orientation deviation exceeded” sometimes when applying a small force but sometimes in normal execution.
It is a little cryptic. What exactly does it mean and how to overcome it?
Note that the external force torque data are slightly filtered with internal force measurements as well. Eg. if you pushed the wrist joins, the FT sendornwould not notice it, since it is prior to the sensor ik the kinematic chain, but the force is still seen by calling e.g. force() or get_tcp_force(). You might want to read out these in a thread while debugging.
The force_mode() command takes a number of limitations (the limits in respectively linear and rotational space). If you exceed any of these limits, the robot will fire the “deviation exceeded” error. Try upping this limit a bit, if you encounter this error often. E.g. if you allow the robot to deviate 100 mm in X, but it has to travel more than 100 mm in X to acheive the desired force.
Obviously in force mode the positional control is replaced by force control, but actually the speed limits given in the force mode command have quite high weight in the algorithm. So also pay a bit attention to the speed limit, which will be favored in many instances until the force it met. Lowering it will give a more smooth motion, since it filters out noise and fluctation in the force measurements.
Thank you!
Hm it seems though that the wobbliness is not due to the sensor data. When I send a zero vector as FT data (with no force applied) and after verifying the internal tcp_force is close to 0, I still experience wobbliness.
Could the problem be, that I accounted for the mounted FT sensor (optoforce) both as a custom TCP in Polyscope as well as in the enable_external_ft_sensor(...) function? Both are set to have a 35mm long 200g thing/sensor attached. If this is a problem, which of the two should I set back to normal?