3 #include <SimoxUtility/json.h>
4 #include <SimoxUtility/math/compare/is_equal.h>
5 #include <SimoxUtility/math/convert/mat4f_to_pos.h>
6 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
7 #include <VirtualRobot/MathTools.h>
8 #include <VirtualRobot/Nodes/RobotNode.h>
9 #include <VirtualRobot/Nodes/Sensor.h>
10 #include <VirtualRobot/RobotNodeSet.h>
27 ARMARX_INFO <<
"Initializing force torque sensor " <<
c.sensorName;
28 const SensorValueBase* svlf = robotUnit->getSensorDevice(
c.sensorName)->getSensorValue();
32 sensorNode = rns->getRobot()->getSensor(
c.sensorName)->getRobotNode();
34 if (
c.sensorName ==
"FT R")
39 else if (
c.sensorName ==
"FT L")
47 filteredForce.setZero();
48 filteredTorque.setZero();
49 filteredForceInRoot.setZero();
50 filteredTorqueInRoot.setZero();
52 tcpGravityCompensation.setZero();
53 tcpGravForceVec.setZero();
54 tcpGravTorqueVec.setZero();
97 forceOffset.setZero();
98 torqueOffset.setZero();
102 timeForCalibration = 0.0f;
104 safeGuardForceOffset.setZero();
105 safeGuardTorqueOffset.setZero();
107 safeGuardEnabledForce.store(
false);
108 safeGuardEnabledTorque.store(
false);
110 resetForceGuard.store(
true);
111 resetTorqueGuard.store(
true);
129 sensorOriInRoot = sensorNode->getOrientationInRootFrame();
130 rawForce = forceSensor->
force;
131 rawTorque = forceSensor->
torque;
156 if (
c.enableSafeGuardForce and resetForceGuard.load())
158 safeGuardForceOffset = filteredForceInRoot;
159 ARMARX_INFO <<
"Resetting force safe guard with baseline "
160 <<
VAROUT(filteredForceInRoot);
161 safeGuardEnabledForce.store(
true);
162 resetForceGuard.store(
false);
164 if (
c.enableSafeGuardTorque and resetTorqueGuard.load())
166 safeGuardTorqueOffset = filteredTorqueInRoot;
167 safeGuardEnabledTorque.store(
true);
168 resetTorqueGuard.store(
false);
175 if (enableForceGuard)
177 resetForceGuard.store(
true);
179 if (enableTorqueGuard)
181 resetTorqueGuard.store(
true);
188 return safeGuardEnabledForce.load();
194 return safeGuardEnabledTorque.load();
201 (
c.enableSafeGuardForce and
202 (filteredForceInRoot - safeGuardForceOffset).norm() >
c.safeGuardForceThreshold) or
203 (
c.enableSafeGuardTorque and
204 (filteredTorqueInRoot - safeGuardTorqueOffset).norm() >
c.safeGuardTorqueThreshold);
205 ftSafe.store(not unsafeFlag);
216 forceOffset = (1 -
c.ftFilter) * forceOffset +
217 c.ftFilter * (forceSensor->
force -
218 sensorOriInRoot.transpose() * tcpGravityCompensation.head<3>());
219 torqueOffset = (1 -
c.ftFilter) * torqueOffset +
220 c.ftFilter * (forceSensor->
torque - sensorOriInRoot.transpose() *
221 tcpGravityCompensation.tail<3>());
223 timeForCalibration = timeForCalibration + deltaT;
224 if (timeForCalibration >
c.timeLimit)
229 "FT calibration done \n"
230 "-- force offset: [%.2f, %.2f, %.2f] \n"
231 "-- torque offset: [%.2f, %.2f, %.2f] \n"
232 "-- gravity compensation: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]",
239 tcpGravityCompensation(0),
240 tcpGravityCompensation(1),
241 tcpGravityCompensation(2),
242 tcpGravityCompensation(3),
243 tcpGravityCompensation(4),
244 tcpGravityCompensation(5));
272 if (
c.enableTCPGravityCompensation)
275 tcpGravForceVec = tcpMass * gravity.head<3>();
276 tcpGravTorqueVec = (sensorOriInRoot * tcpCoMInFTSensorFrame).
cross(tcpGravForceVec);
283 tcpGravityCompensation.head<3>() = tcpGravForceVec;
284 tcpGravityCompensation.tail<3>() = tcpGravTorqueVec;
288 tcpGravityCompensation.setZero();
290 return tcpGravityCompensation;
296 filteredForce = (1 -
c.ftFilter) * filteredForce +
c.ftFilter * rawForce;
297 filteredTorque = (1 -
c.ftFilter) * filteredTorque +
c.ftFilter * rawTorque;
299 filteredForceInRoot = sensorOriInRoot * (filteredForce - forceOffset) -
300 tcpGravityCompensation.head<3>() -
c.forceBaseline;
301 filteredTorqueInRoot = sensorOriInRoot * (filteredTorque - torqueOffset) -
302 tcpGravityCompensation.tail<3>() -
c.torqueBaseline;
304 for (
size_t i = 0; i < 3; ++i)
306 if (fabs(filteredForceInRoot(i)) >
c.deadZoneForce)
308 filteredForceInRoot(i) -=
309 (filteredForceInRoot(i) / fabs(filteredForceInRoot(i))) *
c.deadZoneForce;
313 filteredForceInRoot(i) = 0;
316 if (fabs(filteredTorqueInRoot(i)) >
c.deadZoneTorque)
318 filteredTorqueInRoot(i) -=
319 (filteredTorqueInRoot(i) / fabs(filteredTorqueInRoot(i))) *
c.deadZoneTorque;
323 filteredTorqueInRoot(i) = 0;
327 ft.head<3>() = filteredForceInRoot;
328 ft.tail<3>() = filteredTorqueInRoot;
335 return safeGuardForceOffset;