FTSensor.cpp
Go to the documentation of this file.
1 #include <boost/algorithm/clamp.hpp>
2 
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 <RobotAPI/libraries/core/json_conversions.h>
8 #include <VirtualRobot/MathTools.h>
9 
10 #include <ArmarXCore/core/system/ArmarXDataPath.h>
11 
12 #include "../utils.h"
13 #include "FTSensor.h"
14 
15 namespace armarx::control::common::ft
16 {
17 
18  void
19  FTSensor::initialize(const VirtualRobot::RobotNodeSetPtr& rns,
20  const RobotUnitPtr& robotUnit,
21  const FTConfig& c)
22  {
23  ARMARX_INFO << "Initializing force torque sensor " << c.sensorName;
24  const SensorValueBase* svlf = robotUnit->getSensorDevice(c.sensorName)->getSensorValue();
25  forceSensor = svlf->asA<SensorValueForceTorque>();
26 
27  ARMARX_CHECK_NOT_NULL(rns);
28  sensorNode = rns->getRobot()->getSensor(c.sensorName)->getRobotNode();
29 
30  if (c.sensorName == "FT R")
31  {
32  tcpMass = calibDefaults.tcpMassRight;
33  tcpCoMInFTSensorFrame = calibDefaults.tcpCoMInFTFrameRight;
34  }
35  else if (c.sensorName == "FT L")
36  {
37  tcpMass = calibDefaults.tcpMassLeft;
38  tcpCoMInFTSensorFrame = calibDefaults.tcpCoMInFTFrameLeft;
39  }
40 
41  rawForce.setZero();
42  rawTorque.setZero();
43  filteredForce.setZero();
44  filteredTorque.setZero();
45  filteredForceInRoot.setZero();
46  filteredTorqueInRoot.setZero();
47 
48  tcpGravityCompensation.setZero();
49  tcpGravForceVec.setZero();
50  tcpGravTorqueVec.setZero();
51 
52  sensorOriInRoot = Eigen::Matrix3f::Identity();
53  reset();
54  ARMARX_INFO << "Force torque sensor initialized.";
55  }
56 
57  //FTSensor::FTConfig FTSensor::reconfigure(const std::string &configFileName)
58  //{
59  // nlohmann::json userConfig;
60  // std::ifstream ifs{configFileName};
61  // ifs >> userConfig;
62  // auto c = userConfig["force_torque"];
63  // FTConfig config = FTConfig();
64  // std::string name = getValueWithDefault<std::string>(c, "", "sensor_name");
65  // if (name != sensorName)
66  // {
67  // ARMARX_ERROR << "the sensor name: " << name << " is not the same as initialized: " << sensorName << ", please consider to switch controllers instead of reuse";
68  // }
69  // config.ftFilter = getValueWithDefault<float>(c, s.ftFilter, "force_filter_coeff");
70  // config.deadZoneForce = getValueWithDefault<float>(c, s.deadZoneForce, "force_dead_zone");
71  // config.deadZoneTorque = getValueWithDefault<float>(c, s.deadZoneTorque, "torque_dead_zone");
72  // config.timeLimit = getValueWithDefault<float>(c, s.timeLimit, "wait_time_for_calibration");
73  // s.timeLimit = config.timeLimit;
74 
75  // config.enableTCPGravityCompensation = getValueWithDefault<bool>(c, s.enableTCPGravityCompensation, "enable_tcp_gravity_compensation");
76  // nlohmann::json compCfg;
77  // if (c.find(sensorName) != c.end())
78  // auto compCfg = c[sensorName];
79  // config.tcpMass = getValueWithDefault<float>(compCfg, s.tcpMass, "tcp_mass");
80  // config.tcpCoMInFTSensorFrame = getEigenVecWithDefault(compCfg, s.tcpCoMInFTSensorFrame, "tcp_CoM_in_FT_frame");
81  // config.forceBaseline.setZero();
82  // config.torqueBaseline.setZero();
83  // return config;
84  //}
85 
86  void
87  FTSensor::reset()
88  {
89  /// you have to call this method **only** in the first loop of rtRun
90  // s.tcpMass = 0.0f;
91  // s.tcpCoMInFTSensorFrame.setZero();
92 
93  forceOffset.setZero();
94  torqueOffset.setZero();
95  ft.setZero();
96 
97  calibrated.store(false);
98  timeForCalibration = 0.0f;
99 
100  safeGuardForceOffset.setZero();
101  safeGuardTorqueOffset.setZero();
102 
103  // Eigen::Matrix4f forceFrameInRoot = sensorNode->getPoseInRootFrame();
104  // forceFrameInTCP.block<3, 3>(0, 0) = currentPose.block<3, 3>(0, 0).transpose() * forceFrameInRoot.block<3, 3>(0, 0);
105  // tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * tcpCoMInFTSensorFrame;
106  }
107 
108  void
109  FTSensor::updateStatus()
110  {
111  sensorOriInRoot = sensorNode->getOrientationInRootFrame();
112  rawForce = forceSensor->force;
113  rawTorque = forceSensor->torque;
114  }
115 
116  void
117  FTSensor::resetSafeGuardOffset()
118  {
119  safeGuardForceOffset = filteredForceInRoot;
120  safeGuardTorqueOffset = filteredTorqueInRoot;
121  }
122 
123  bool
124  FTSensor::isSafe(const FTConfig& c)
125  {
126  auto unsafeFlag =
127  (c.enableSafeGuardForce and
128  (filteredForceInRoot - safeGuardForceOffset).norm() > c.safeGuardForceThreshold) or
129  (c.enableSafeGuardTorque and
130  (filteredTorqueInRoot - safeGuardTorqueOffset).norm() > c.safeGuardTorqueThreshold);
131  ftSafe.store(not unsafeFlag);
132 
133  return true;
134  }
135 
136  bool
137  FTSensor::calibrate(const FTConfig& c, double deltaT)
138  {
139  /// this will calibrate the force torque value in FTSensorFrame until timeLimit is reached
140  // sensorOriInRoot = sensorNode->getOrientationInRootFrame();
141  forceOffset = (1 - c.ftFilter) * forceOffset +
142  c.ftFilter * (forceSensor->force -
143  sensorOriInRoot.transpose() * tcpGravityCompensation.head(3));
144  torqueOffset = (1 - c.ftFilter) * torqueOffset +
145  c.ftFilter * (forceSensor->torque -
146  sensorOriInRoot.transpose() * tcpGravityCompensation.tail(3));
147 
148  timeForCalibration = timeForCalibration + deltaT;
149  if (timeForCalibration > c.timeLimit)
150  {
151  calibrated.store(true);
152  /// rt is ready only if the FT sensor is calibrated, and the rt2interface buffer is updated (see above)
153  ARMARX_RT_LOGF_IMPORTANT(
154  "FT calibration done \n"
155  "-- force offset: [%.2f, %.2f, %.2f] \n"
156  "-- torque offset: [%.2f, %.2f, %.2f] \n"
157  "-- gravity compensation: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]",
158  forceOffset(0),
159  forceOffset(1),
160  forceOffset(2),
161  torqueOffset(0),
162  torqueOffset(1),
163  torqueOffset(2),
164  tcpGravityCompensation(0),
165  tcpGravityCompensation(1),
166  tcpGravityCompensation(2),
167  tcpGravityCompensation(3),
168  tcpGravityCompensation(4),
169  tcpGravityCompensation(5));
170  }
171  return calibrated.load();
172  }
173 
174  //void FTSensor::enableGravityCompensation(const float mass, const Eigen::Vector3f& tcpCoMInFTFrame)
175  //{
176  // enableTCPGravityCompensation.store(true);
177  // tcpMass = mass;
178  // tcpCoMInFTSensorFrame = tcpCoMInFTFrame;
179  //}
180 
181  //void FTSensor::disableGravityCompensation()
182  //{
183  // enableTCPGravityCompensation.store(false);
184  // tcpMass = 0.0f;
185  // tcpCoMInFTSensorFrame.setZero();
186  // tcpGravityCompensation.setZero();
187  //}
188 
189  Eigen::Vector6f&
190  FTSensor::compensateTCPGravity(const FTConfig& c)
191  {
192  // /// update variables from rt buffer
193  // tcpMass = data.tcpMass;
194  // tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * data.tcpCoMInFTSensorFrame;
195 
196  /// static compensation in root frame
197  if (c.enableTCPGravityCompensation)
198  {
199 
200  tcpGravForceVec = tcpMass * gravity.head(3);
201  tcpGravTorqueVec = (sensorOriInRoot * tcpCoMInFTSensorFrame).cross(tcpGravForceVec);
202  // tcpGravityCompensation << forceVec, torqueVec;
203  // Eigen::Vector3f localGravity = currentPose.block<3, 3>(0, 0).transpose() * gravity;
204  // Eigen::Vector3f localForceVec = tcpMass * localGravity;
205  // Eigen::Vector3f localTorqueVec = tcpCoMInTCPFrame.cross(localForceVec);
206 
207  // tcpGravityCompensation << localForceVec, localTorqueVec;
208  tcpGravityCompensation.head(3) = tcpGravForceVec;
209  tcpGravityCompensation.tail(3) = tcpGravTorqueVec;
210  }
211  else
212  {
213  tcpGravityCompensation.setZero();
214  }
215  return tcpGravityCompensation;
216  }
217 
218  Eigen::Vector6f&
219  FTSensor::getFilteredForceTorque(const FTConfig& c)
220  {
221  filteredForce = (1 - c.ftFilter) * filteredForce + c.ftFilter * rawForce;
222  filteredTorque = (1 - c.ftFilter) * filteredTorque + c.ftFilter * rawTorque;
223 
224  filteredForceInRoot = sensorOriInRoot * (filteredForce - forceOffset) -
225  tcpGravityCompensation.head(3) - c.forceBaseline;
226  filteredTorqueInRoot = sensorOriInRoot * (filteredTorque - torqueOffset) -
227  tcpGravityCompensation.tail(3) - c.torqueBaseline;
228 
229  for (size_t i = 0; i < 3; ++i)
230  {
231  if (fabs(filteredForceInRoot(i)) > c.deadZoneForce)
232  {
233  filteredForceInRoot(i) -=
234  (filteredForceInRoot(i) / fabs(filteredForceInRoot(i))) * c.deadZoneForce;
235  }
236  else
237  {
238  filteredForceInRoot(i) = 0;
239  }
240 
241  if (fabs(filteredTorqueInRoot(i)) > c.deadZoneTorque)
242  {
243  filteredTorqueInRoot(i) -=
244  (filteredTorqueInRoot(i) / fabs(filteredTorqueInRoot(i))) * c.deadZoneTorque;
245  }
246  else
247  {
248  filteredTorqueInRoot(i) = 0;
249  }
250  }
251 
252  ft.head(3) = filteredForceInRoot;
253  ft.tail(3) = filteredTorqueInRoot;
254  return ft;
255  }
256 } // namespace armarx::control::common::ft
armarx::control::common::ft::FTSensor::initialize
void initialize(const VirtualRobot::RobotNodeSetPtr &rns, const armarx::RobotUnitPtr &robotUnit, const FTConfig &c)
Definition: FTSensor.cpp:19
armarx::control::common::ft::FTSensor::isSafe
bool isSafe(const FTConfig &c)
Definition: FTSensor.cpp:124
armarx::control::common::ft::FTCalibrateDefaultValue::tcpMassRight
float tcpMassRight
Definition: FTSensor.h:39
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::control::common::ft::FTSensor::FTConfig
arondto::FTConfig FTConfig
Definition: FTSensor.h:48
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
FTSensor.h
armarx::control::common::ft
This file is part of ArmarX.
Definition: aron_conversions.cpp:29
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::control::common::ft::FTSensor::updateStatus
void updateStatus()
Definition: FTSensor.cpp:109
armarx::control::common::ft::FTCalibrateDefaultValue::tcpCoMInFTFrameRight
Eigen::Vector3f tcpCoMInFTFrameRight
Definition: FTSensor.h:40
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:35
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::control::common::ft::FTSensor::ftSafe
std::atomic_bool ftSafe
Definition: FTSensor.h:65
armarx::control::common::ft::FTSensor::calibrated
std::atomic_bool calibrated
Definition: FTSensor.h:64
cross
Point cross(const Point &x, const Point &y)
Definition: point.hpp:33
armarx::control::common::ft::FTCalibrateDefaultValue::tcpCoMInFTFrameLeft
Eigen::Vector3f tcpCoMInFTFrameLeft
Definition: FTSensor.h:42
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:34
ARMARX_RT_LOGF_IMPORTANT
#define ARMARX_RT_LOGF_IMPORTANT(...)
Definition: ControlThreadOutputBuffer.h:341
armarx::control::common::ft::FTSensor::reset
void reset()
Definition: FTSensor.cpp:87
armarx::control::common::ft::FTSensor::getFilteredForceTorque
Eigen::Vector6f & getFilteredForceTorque(const FTConfig &data)
Definition: FTSensor.cpp:219
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::common::ft::FTSensor::calibrate
bool calibrate(const FTConfig &c, double deltaT)
Definition: FTSensor.cpp:137
armarx::control::common::ft::FTCalibrateDefaultValue::tcpMassLeft
float tcpMassLeft
Definition: FTSensor.h:41
Eigen::Matrix< float, 6, 1 >
armarx::control::common::ft::FTSensor::resetSafeGuardOffset
void resetSafeGuardOffset()
Definition: FTSensor.cpp:117
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:31
ArmarXDataPath.h
armarx::control::common::ft::FTSensor::compensateTCPGravity
Eigen::Vector6f & compensateTCPGravity(const FTConfig &data)
Definition: FTSensor.cpp:190