Manual TCP teaching/adjustment via FT sensor feedback

[PeterBowman]

Collaborative manipulators usually feature a magic button near the TCP which, if pressed, enables a human operator to manually translate and/or reorient the arm. This is performed in gravity compensation mode so that the arm doesn't fall by itself, yet the operator doesn't need to exert full force to rotate the joints.

TEO is able to operate in gravity compensation mode, but it's rather hard in the strict sense - joint friction is quite high. I hereby propose to implement it using FT sensor feedback: once active and whenever an external force is exerted on the TCP by a human operator, sensor data is used to compute IK in the direction of said force and thus perform motion through position commands.

[PeterBowman]

For the record, this has been blocked for a longer while on https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/49, we are currently expecting some repairs regarding the connector of the FT sensors.

[PeterBowman]

Today, I learned that the Jr3 assumes itz XYZ axes are levorotary, i.e. one of them must be reversed to follow the usual, dextrorotary system. In order to not break compatibility, I added a new --dextrorotary configuration parameter which will adjust the sign of the X values (linear force and torque) accordingly: roboticslab-uc3m/yarp-devices@6cb1f8e.

[PeterBowman]

With regard to the wrist flange coordinates (no tool, i.e. R_0_6, see diagram), the coordinate system's rotation of the (dextrorotary) sensor axes (i.e. R_6_sensor) is obtained with the following RPY transformation: --sensorFrameRPY (90 0 -90). Sequence (old axes): 1. R_x (roll), 2. R_y (pitch), 3. R_z (yaw).

No transformation is needed in case the LacqueyFetch TCP is used, i.e. R_0_f.

[PeterBowman]

Commands:

• (JR3 PC) yarpdev --device multipleanalogsensorsserver --subdevice Jr3 --name /jr3 --period 5 --filter 2 --dextrorotary
  • note the period must be less than half the command period (syncPeriod in manipulation-leftArm.ini)
• (manipulation PC) launchCanBus --from manipulation-leftArm.ini
• yarp rpc /teo/leftArm/rpc:i + set ivar mvar all (csv (enable 1))
• yarpdev --device BasicCartesianControl --name /teo/leftArm/CartesianControl --kinematics teo-fixedTrunk-leftArm.ini --local /BasicCartesianControl/teo/leftArm --remote /teo/leftArm
  • teo-fixedTrunk-leftArm.ini is the same as teo-fixedTrunk-leftArm-fetch.ini, but no HN
• ftCompensation --cartesianRemote /teo/leftArm/CartesianControl --sensorRemote /jr3 --sensorName ch0 --sensorFrameRPY "(90 0 -90)" --linGain 0.01 --rotGain 0.02 --forceDeadband 1.0 --torqueDeadband 1.0

[PeterBowman]

I was told this application might be of interest for @elisabeth-ms and @JuanHernandez1993:

More videos: https://drive.google.com/drive/folders/1HbvVDbmZ3rE-VOZoXUkqnoRfV2xmF7ha.

[PeterBowman]

Merged at bb15c49. I didn't put too much effort into testing tool weight compensation, but it seems to do the trick (it's actually hard to pick the right values). Also, I noticed a tiny residual displacement even though no force were being applied on the TCP. This is due to having CSV mode disabled (it's easy to overlook this step) and performing velocity instead of position control. This also means that velocity control is usually good enough.

See application and context files for notes on how to run this demo:

• ftCompensation-stump.ini
• ftCompensation-lacquey.ini
• ftCompensation.xml

Maybe ftCompensation is not the best name, but at least it reflects the intention of creating an alternative for classical gravity compensation.