NJointAnomalyDetectionAdaptiveWipingController.cpp
Go to the documentation of this file.
1 #include "NJointAnomalyDetectionAdaptiveWipingController.h"
2 
3 #include <VirtualRobot/IK/DifferentialIK.h>
4 #include <VirtualRobot/MathTools.h>
5 #include <VirtualRobot/Nodes/RobotNode.h>
6 #include <VirtualRobot/Robot.h>
7 #include <VirtualRobot/RobotNodeSet.h>
8 
9 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
10 
11 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
12 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
13 #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
14 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
15 #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
16 #include <RobotAPI/libraries/core/CartesianVelocityController.h>
17 #include <RobotAPI/libraries/core/PIDController.h>
18 #include <RobotAPI/libraries/core/Trajectory.h>
19 
20 namespace armarx::control::deprecated_njoint_mp_controller::adaptive
21 {
22  NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController>
23  registrationControllerNJointAnomalyDetectionAdaptiveWipingController(
24  "NJointAnomalyDetectionAdaptiveWipingController");
25 
26  NJointAnomalyDetectionAdaptiveWipingController::NJointAnomalyDetectionAdaptiveWipingController(
27  const RobotUnitPtr& robUnit,
28  const armarx::NJointControllerConfigPtr& config,
29  const VirtualRobot::RobotPtr&)
30  {
31  useSynchronizedRtRobot();
32  cfg = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr::dynamicCast(config);
33  ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
34  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
35 
36  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
37  for (size_t i = 0; i < rns->getSize(); ++i)
38  {
39  std::string jointName = rns->getNode(i)->getName();
40  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
41  const SensorValueBase* sv = useSensorValue(jointName);
42  targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
43 
44  const SensorValue1DoFActuatorVelocity* velocitySensor =
45  sv->asA<SensorValue1DoFActuatorVelocity>();
46  const SensorValue1DoFActuatorPosition* positionSensor =
47  sv->asA<SensorValue1DoFActuatorPosition>();
48 
49  velocitySensors.push_back(velocitySensor);
50  positionSensors.push_back(positionSensor);
51  };
52 
53  useDMPInGlobalFrame = cfg->useDMPInGlobalFrame;
54 
55  tcp = rns->getTCP();
56  // set tcp controller
57  nodeSetName = cfg->nodeSetName;
58  ik.reset(new VirtualRobot::DifferentialIK(
59  rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
60 
61  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
62  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
63  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
64  taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
65  taskSpaceDMPConfig.DMPMode = "Linear";
66  taskSpaceDMPConfig.DMPStyle = "Periodic";
67  taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
68  taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
69  taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
70  taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
71  taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
72  taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
73  taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
74  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
75  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
76 
77  lastCanVal = cfg->timeDuration;
78 
79  dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
80 
81  NJointAnomalyDetectionAdaptiveWipingControllerControlData initData;
82  initData.targetTSVel.resize(6);
83  for (size_t i = 0; i < 6; ++i)
84  {
85  initData.targetTSVel(i) = 0;
86  }
87  initData.targetTSPose = tcp->getPoseInRootFrame();
88  reinitTripleBuffer(initData);
89 
90  firstRun = true;
91  dmpRunning = false;
92 
93  // anomaly detection
94  velocityHorizon = cfg->velocityHorizon;
95 
96  // friction estimation
97  frictionHorizon = cfg->frictionHorizon;
98  estimatedFriction << 0.0, 0.0;
99  lastForceInToolXY.setZero();
100 
101  ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
102  ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
103  ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
104  ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
105 
106  kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
107  dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
108  kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
109  dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
110 
111  isForceCtrlInForceDir = cfg->isForceCtrlInForceDir;
112  isForceControlEnabled = cfg->isForceControlEnabled;
113  isRotControlEnabled = cfg->isRotControlEnabled;
114  isTorqueControlEnabled = cfg->isTorqueControlEnabled;
115  isLCRControlEnabled = cfg->isLCRControlEnabled;
116  forcePID.reset(new PIDController(
117  cfg->pidForce[0], cfg->pidForce[1], cfg->pidForce[2], cfg->pidForce[3]));
118  rotPID.reset(
119  new PIDController(cfg->pidRot[0], cfg->pidRot[1], cfg->pidRot[2], cfg->pidRot[3]));
120  torquePID.reset(new PIDController(
121  cfg->pidTorque[0], cfg->pidTorque[1], cfg->pidTorque[2], cfg->pidTorque[3]));
122  lcrPID.reset(
123  new PIDController(cfg->pidLCR[0], cfg->pidLCR[1], cfg->pidLCR[2], cfg->pidLCR[3]));
124  adaptKpForce = cfg->pidForce[0];
125  adaptKpRot = cfg->pidRot[0];
126 
127  knull.setZero(targets.size());
128  dnull.setZero(targets.size());
129 
130  for (size_t i = 0; i < targets.size(); ++i)
131  {
132  knull(i) = cfg->Knull.at(i);
133  dnull(i) = cfg->Dnull.at(i);
134  }
135 
136  nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
137  for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
138  {
139  nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
140  }
141 
142 
143  const SensorValueBase* svlf =
144  robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
145  forceSensor = svlf->asA<SensorValueForceTorque>();
146 
147 
148  ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
149 
150  currentForceOffset.setZero();
151  forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
152  torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
153 
154  handMass = cfg->handMass;
155  handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
156 
157 
158  filteredForce.setZero();
159  filteredTorque.setZero();
160  filteredFTCommand.setZero();
161  filteredForceInRoot.setZero();
162  filteredTorqueInRoot.setZero();
163  targetFTInToolFrame.setZero();
164 
165  UserToRTData initUserData;
166  initUserData.targetForce = 0;
167  user2rtData.reinitAllBuffers(initUserData);
168 
169  oriToolDir << 0, 0, 1;
170  gravityInRoot << 0, 0, -9.8;
171 
172  qvel_filtered.setZero(targets.size());
173 
174  ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
175  ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
176  ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
177  // only for ARMAR-6 (safe-guard)
178  ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
179  ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
180  ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
181 
182  ARMARX_CHECK_LESS(cfg->ws_y[0], cfg->ws_y[1]);
183  ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
184  ARMARX_CHECK_LESS(0, cfg->ws_y[1]);
185 
186  ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
187  ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
188  ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
189 
190  adaptK = kpos;
191  adaptD = dpos;
192  adaptKOri = kori;
193  adaptDOri = dori;
194  adaptKNull = knull;
195  adaptDNull = dnull;
196  lastDiff = 0;
197  changeTimer = 0;
198 
199  abnormalFlag = false;
200  lastAbnormalFlag = false;
201  positionOffset.setZero();
202 
203  // toolToFTSensorLink =
204  // rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3,1>(0, 3) -
205  // tcp->getPoseInRootFrame().block<3,1>(0,3)
206  }
207 
208  void
209  NJointAnomalyDetectionAdaptiveWipingController::onInitNJointController()
210  {
211  ARMARX_INFO << "init ...";
212 
213 
214  RTToControllerData initSensorData;
215  initSensorData.deltaT = 0;
216  initSensorData.currentTime = 0;
217  initSensorData.currentPose = tcp->getPoseInRootFrame();
218  initSensorData.currentTwist.setZero();
219  initSensorData.isPhaseStop = false;
220  rt2CtrlData.reinitAllBuffers(initSensorData);
221 
222  RTToUserData initInterfaceData;
223  initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
224  initInterfaceData.waitTimeForCalibration = 0;
225  rt2UserData.reinitAllBuffers(initInterfaceData);
226 
227  started = false;
228 
229  runTask("NJointAnomalyDetectionAdaptiveWipingController",
230  [&]
231  {
232  CycleUtil c(1);
233  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
234  while (getState() == eManagedIceObjectStarted)
235  {
236  if (isControllerActive())
237  {
238  controllerRun();
239  }
240  c.waitForCycleDuration();
241  }
242  });
243  }
244 
245  std::string
246  NJointAnomalyDetectionAdaptiveWipingController::getClassName(const Ice::Current&) const
247  {
248  return "NJointAnomalyDetectionAdaptiveWipingController";
249  }
250 
251  void
252  NJointAnomalyDetectionAdaptiveWipingController::controllerRun()
253  {
254  if (!started)
255  {
256  return;
257  }
258 
259  if (!dmpCtrl)
260  {
261  return;
262  }
263 
264  Eigen::VectorXf targetVels(6);
265  Eigen::Matrix4f targetDMPPose;
266  bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
267  if (isPhaseStop)
268  {
269  targetVels.setZero();
270  targetDMPPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
271  }
272  else
273  {
274 
275  double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
276  Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
277  Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
278 
279  LockGuardType guard{controllerMutex};
280  dmpCtrl->flow(deltaT, currentPose, currentTwist);
281 
282  targetVels = dmpCtrl->getTargetVelocity();
283  targetDMPPose = dmpCtrl->getTargetPoseMat();
284 
285  debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
286  debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
287  debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
288  debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
289  debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
290  debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
291  debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
292  debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
293  debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
294  VirtualRobot::MathTools::Quaternion currentQ =
295  VirtualRobot::MathTools::eigen4f2quat(currentPose);
296  debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
297  debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
298  debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
299  debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
300  debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
301  debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
302  debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
303  debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
304  debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
305  debugOutputData.getWriteBuffer().deltaT = deltaT;
306  debugOutputData.commitWrite();
307  }
308  getWriterControlStruct().canVal = dmpCtrl->canVal;
309  getWriterControlStruct().targetTSVel = targetVels;
310  getWriterControlStruct().targetTSPose = targetDMPPose;
311  writeControlStruct();
312 
313  dmpRunning = true;
314  }
315 
316  void
317  NJointAnomalyDetectionAdaptiveWipingController::rtRun(
318  const IceUtil::Time& sensorValuesTimestamp,
319  const IceUtil::Time& timeSinceLastIteration)
320  {
321  double deltaT = timeSinceLastIteration.toSecondsDouble();
322  float dTf = (float)deltaT;
323 
324  Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
325 
326 
327  Eigen::Vector3f currentToolDir;
328  currentToolDir.setZero();
329  Eigen::MatrixXf jacobi =
330  ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
331 
332  Eigen::VectorXf qpos(positionSensors.size());
333  Eigen::VectorXf qvel(velocitySensors.size());
334  for (size_t i = 0; i < positionSensors.size(); ++i)
335  {
336  qpos(i) = positionSensors[i]->position;
337  qvel(i) = velocitySensors[i]->velocity;
338  }
339 
340  qvel_filtered = (1 - cfg->velFilter) * qvel_filtered + cfg->velFilter * qvel;
341  Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
342 
343  velocityHorizonList.push_back(currentTwist);
344  if (velocityHorizonList.size() > velocityHorizon)
345  {
346  velocityHorizonList.pop_front();
347  }
348 
349  Eigen::VectorXf targetVel(6);
350  Eigen::Vector3f axis;
351  Eigen::Vector3f forceInToolFrame;
352  Eigen::Vector3f torqueInToolFrame;
353  Eigen::Vector6f targetFTInRootFrame;
354  Eigen::Vector3f velPInToolFrame;
355  targetVel.setZero();
356  axis.setZero();
357  forceInToolFrame.setZero();
358  torqueInToolFrame.setZero();
359  targetFTInRootFrame.setZero();
360  velPInToolFrame.setZero();
361  float angle = 0;
362  bool isPhaseStop = false;
363 
364  if (firstRun || !dmpRunning)
365  {
366  initHandPose = currentPose;
367  lastPosition = currentPose.block<2, 1>(0, 3);
368  targetPose = currentPose;
369  firstRun = false;
370  filteredForce.setZero();
371  Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
372 
373  Eigen::Matrix3f forceFrameOri = rtGetRobot()
374  ->getRobotNode(cfg->forceFrameName)
375  ->getPoseInRootFrame()
376  .block<3, 3>(0, 0);
377  Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
378  Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
379  currentForce = currentForce - handGravity;
380 
381  currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
382  origHandOri = currentPose.block<3, 3>(0, 0);
383  toolTransform = origHandOri.transpose();
384  targetVel.setZero();
385  }
386  else
387  {
388  rtUpdateControlStruct();
389  targetVel = rtGetControlStruct().targetTSVel;
390 
391 
392  Eigen::Matrix3f currentToolOri = currentPose.block<3, 3>(0, 0) * toolTransform;
393 
394  /* -------------------------- get target vel from dmp thread --------------------------------- */
395  targetVel(2) = 0;
396  targetVel.head<3>() = currentToolOri * targetVel.head<3>();
397  targetVel.tail<3>() = currentToolOri * targetVel.tail<3>();
398 
399  double canVal = rtGetControlStruct().canVal;
400  if (canVal - lastCanVal > 0.9 * cfg->timeDuration)
401  {
402  wipingCounter++;
403  mu = 1.0;
404  }
405  lastCanVal = canVal;
406 
407  /* -------------------------- force feedback, filter and transform frame --------------------------------- */
408  Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
409 
410  Eigen::Matrix3f forceFrameOri = rtGetRobot()
411  ->getRobotNode(cfg->forceFrameName)
412  ->getPoseInRootFrame()
413  .block<3, 3>(0, 0);
414  Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
415  Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
416 
417  currentForce = currentForce - handGravity;
418  filteredForce =
419  (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
420 
421  Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
422  Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
423  currentTorque = currentTorque - handTorque;
424  filteredTorque =
425  (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
426 
427  Eigen::Vector3f forceInRootForFricEst = forceFrameOri * filteredForce;
428  Eigen::Vector3f forceInToolForFricEst =
429  currentToolOri.transpose() * forceInRootForFricEst;
430 
431  for (size_t i = 0; i < 3; ++i)
432  {
433  if (fabs(filteredForce(i)) > cfg->forceDeadZone)
434  {
435  filteredForce(i) -=
436  (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
437  }
438  else
439  {
440  filteredForce(i) = 0;
441  }
442  }
443 
444  filteredForceInRoot = forceFrameOri * filteredForce;
445  filteredTorqueInRoot = forceFrameOri * filteredTorque;
446  float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
447 
448  forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
449  // TODO this is wrong
450  torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
451  velPInToolFrame = currentToolOri.transpose() * currentTwist.head<3>();
452  // Eigen::Vector3f velRInToolFrame = currentToolOri.transpose() * currentTwist.tail<3>();
453 
454 
455  /* -------------------------- Drag Force Adaptation --------------------------------- */
456  // Eigen::Vector3f dragForce = filteredForceInRoot - cfg->dragForceDeadZone * filteredForceInRoot / filteredForceInRoot.norm();
457  if (abnormalFlag == true && filteredForceInRoot.norm() > cfg->dragForceDeadZone)
458  {
459  // adaptKpForce = fmax(adaptKpForce - dTf * cfg->decreaseKpForceCoeff, 0);
460  // adaptKpRot = fmax(adaptKpRot - dTf * cfg->decreaseKpRotCoeff, 0);
461  adaptKpForce *= cfg->adaptRateDecrease;
462  adaptKpRot *= cfg->adaptRateDecreaseRot;
463  forcePID->Kp = adaptKpForce;
464  torquePID->Kp = adaptKpRot;
465  // adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
466  // adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
467  // adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
468  // adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff, 0);
469  // adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff, 0);
470  // adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff, 0);
471  adaptK *= cfg->adaptRateDecrease;
472  adaptD *= cfg->adaptRateDecrease;
473  if (cfg->isAdaptOriImpEnabled)
474  {
475  adaptKOri *= cfg->adaptRateDecrease;
476  adaptDOri *= cfg->adaptRateDecrease;
477  }
478  adaptKNull *= cfg->adaptRateDecrease;
479  adaptDNull *= cfg->adaptRateDecrease;
480 
481  positionOffset.setZero();
482  forcePID->reset();
483 
484  // lastDiff = diff;
485  }
486  else
487  {
488  adaptKpForce =
489  fmin(adaptKpForce + dTf * cfg->increaseKpForceCoeff, cfg->pidForce[0]);
490  adaptKpRot = fmin(adaptKpRot + dTf * cfg->increaseKpRotCoeff, cfg->pidRot[0]);
491  // adaptKpForce *= cfg->adaptRateIncrease;
492  // adaptKpRot *= cfg->adaptRateIncrease;
493  forcePID->Kp = adaptKpForce;
494  torquePID->Kp = adaptKpRot;
495  for (int i = 0; i < 3; i++)
496  {
497  adaptK(i) = fmin(adaptK(i) + fabs(dTf * cfg->adaptCoeff), kpos(i));
498  adaptD(i) = fmin(adaptD(i) + fabs(dTf * cfg->adaptCoeffKdImpIncrease), dpos(i));
499  if (cfg->isAdaptOriImpEnabled)
500  {
501  adaptKOri(i) =
502  fmin(adaptKOri(i) + fabs(dTf * cfg->increaseKpOriCoeff), kori(i));
503  adaptDOri(i) =
504  fmin(adaptDOri(i) + fabs(dTf * cfg->increaseKdOriCoeff), dori(i));
505  }
506  }
507  for (size_t i = 0; i < targets.size(); i++)
508  {
509  adaptKNull(i) =
510  fmin(adaptKNull(i) + fabs(dTf * cfg->increaseKpNullCoeff), knull(i));
511  adaptDNull(i) =
512  fmin(adaptDNull(i) + fabs(dTf * cfg->increaseKdNullCoeff), dnull(i));
513  }
514 
515  // adaptK *= cfg->adaptRateIncrease;
516  // adaptD *= cfg->adaptRateIncrease;
517  }
518 
519  if (!isContactedOnce && fabs(forceInToolFrame(2)) > targetForce)
520  {
521  isContactedOnce = true;
522  }
523  if (cfg->loseContactDetectionEnabled && isContactedOnce)
524  {
525  if (abnormalFlag && !lastAbnormalFlag)
526  {
527  startLoseContactDetection = true;
528  loseContactCounter = 0;
529  forceIntegral = 0;
530  }
531  if (startLoseContactDetection && loseContactCounter < cfg->loseContactCounterMax)
532  {
533  forceIntegral += forceInToolFrame.norm() * deltaT;
534  loseContactCounter++;
535  }
536  if (loseContactCounter >= cfg->loseContactCounterMax &&
537  forceIntegral < cfg->loseContactForceIntThreshold)
538  {
539  isLoseContact = true;
540  }
541  lastAbnormalFlag = abnormalFlag;
542  if (isLoseContact)
543  {
544  adaptK *= 0.0;
545  adaptD *= 0.0;
546  adaptKOri *= 0.0;
547  adaptDOri *= 0.0;
548  adaptKNull *= 0.0;
549  adaptDNull *= 0.0;
550  }
551  }
552 
553  // adaptK(2) = kpos(2);
554  /* -------------------------- friction estimation --------------------------------- */
555 
556  Eigen::Vector2f v_xy;
557  Eigen::Vector2f f_xy;
558  v_xy << velPInToolFrame(0), velPInToolFrame(1);
559  f_xy << forceInToolForFricEst(0), forceInToolForFricEst(1);
560  f_xy = cfg->fricEstiFilter * f_xy + (1 - cfg->fricEstiFilter) * lastForceInToolXY;
561  lastForceInToolXY = f_xy;
562 
563  if (wipingCounter > 0)
564  {
565  if (v_xy.norm() > cfg->velNormThreshold &&
566  fabs(forceInToolForFricEst(2) - targetForce) < 0.5 * targetForce)
567  {
568  recordFrictionNorm.push_back(f_xy.norm());
569  recordForceNormToSurface.push_back(forceInToolForFricEst(2));
570  }
571  if (recordFrictionNorm.size() > frictionHorizon)
572  {
573  recordFrictionNorm.pop_front();
574  recordForceNormToSurface.pop_front();
575  float dotProduct = 0.0;
576  float normSquare = 0.0;
577  for (size_t i = 0; i < recordFrictionNorm.size(); i++)
578  {
579  dotProduct += (recordFrictionNorm[i] * recordForceNormToSurface[i]);
580  normSquare += (recordForceNormToSurface[i] * recordForceNormToSurface[i]);
581  }
582  if (normSquare > 0)
583  {
584  float mu_tmp = dotProduct / normSquare;
585  if (mu_tmp > 0)
586  {
587  mu = fmax(fmin(mu, mu_tmp), safeFrictionConeLowerLimit);
588  }
589  }
590  if (v_xy.norm() > cfg->velNormThreshold)
591  {
592  estimatedFriction = -v_xy * mu * forceInToolForFricEst(2) / v_xy.norm();
593  }
594  }
595  }
596 
597  /* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
598 
599  if (isForceCtrlInForceDir)
600  {
601  forcePID->update(deltaT, forceInToolFrame.norm(), targetForce);
602  }
603  else
604  {
605  forcePID->update(deltaT, forceInToolFrame(2), targetForce);
606  }
607  torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
608 
609  /* -------------------------- Rotation PID Controller --------------------------------- */
610 
611  currentToolDir = currentToolOri * oriToolDir;
612  for (int i = 3; i < 6; ++i)
613  {
614  targetVel(i) = 0;
615  }
616  float frictionCone;
617  if (cfg->frictionCone < 0.0)
618  {
619  frictionCone = atan(mu);
620  }
621  else
622  {
623  frictionCone = cfg->frictionCone;
624  }
625  float adaptedAngularKp = 0.0;
626  Eigen::Vector3f angularDiff;
627  angularDiff.setZero();
628  // rotation changes
629  if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
630  {
631  Eigen::Vector3f toolYDir;
632  toolYDir << 0, 1.0, 0;
633  Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
634  Eigen::Vector3f projectedFilteredForceInRoot =
635  filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
636  Eigen::Vector3f desiredToolDir =
637  projectedFilteredForceInRoot
638  .normalized(); // / projectedFilteredForceInRoot.norm();
639  currentToolDir.normalize();
640 
641  axis = currentToolDir.cross(desiredToolDir);
642  axis = axis.normalized();
643  angle = acosf(currentToolDir.dot(desiredToolDir));
644 
645  int sign = 1;
646  if (axis(1) < 0)
647  {
648  sign = -1;
649  }
650 
651  if (fabs(angle) < M_PI / 2 && fabs(angle) > frictionCone)
652  {
653  // sigmoid function
654  adaptedAngularKp =
655  cfg->pidRot[0] / (1 + exp(10 * (angle - cfg->rotAngleSigmoid)));
656  adaptedAngularKp = fmin(adaptedAngularKp, cfg->pidRot[0]);
657  rotPID->Kp = adaptedAngularKp;
658  angle -= frictionCone;
659  angle *= sign;
660  }
661  else
662  {
663  angle = 0.0;
664  rotPID->Kp = cfg->pidRot[0];
665  }
666  }
667  rotPID->update(deltaT, angle, 0.0);
668 
669  /* -------------------------- Lose Contact Recover PID Controller --------------------------------- */
670 
671  // if (forceInToolFrame(2) > loseContactRatio * targetForce)
672  // {
673  // makingContactCounter++;
674  // if (makingContactCounter > 100)
675  // {
676  // isMakingContact = true;
677  // isLCREnabled = false;
678  // }
679  // else
680  // {
681  // isMakingContact = false;
682  // }
683  // }
684  // if (!isContactedOnce && isMakingContact)
685  // {
686  // isContactedOnce = true;
687  // }
688  // Eigen::Vector3f compensationAxis;
689  // compensationAxis.setZero();
690  // if (isContactedOnce && fabs(velPInToolFrame(2)) > 10 && frictionCone < 1.0)
691  // {
692  // makingContactCounter = 0;
693  // Eigen::Vector3f v;
694  // v << velPInToolFrame(0), 0.0, 0.0;
695  // Eigen::Vector3f f;
696  // f << 0.0, 0.0, targetForce;
697  // compensationAxis = f.cross(v);
698  // if (compensationAxis.norm() > 0)
699  // {
700  // compensationAxis.normalized();
701  // }
702  // else
703  // {
704  // compensationAxis.setZero();
705  // }
706  // forceControlGate *= 0.5;
707  // isLCREnabled = true;
708  // lcrCounter -= 1;
709  // }
710  // else
711  // {
712  // forceControlGate = 1.0;
713  // // TODO: restart vmp controller
714  // }
715  // float velInForceDir = 0.0;
716  // if (lcrCounter < 500)
717  // {
718  // velInForceDir = fabs(velPInToolFrame(2));
719  // lcrCounter -= 1;
720  // if (lcrCounter == 0)
721  // {
722  // lcrCounter = 500;
723  // }
724  // }
725  // lcrPID->update(deltaT, velInForceDir, 0.0);
726 
727  /* -------------------------- VMP Phase Stop --------------------------------- */
728 
729  float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
730  if (diff > cfg->phaseDist0)
731  {
732  isPhaseStop = true;
733  }
734 
735 
736  /* -------------------------- get PID control commands --------------------------------- */
737  // if (isLCRControlEnabled)
738  // {
739  // // targetFTInToolFrame.tail<3>() += (float)lcrPID->getControlValue() * compensationAxis;
740  //// targetVel.tail<3>() += (float)lcrPID->getControlValue() * compensationAxis;
741  // }
742 
743  if (isForceControlEnabled)
744  {
745  if (isForceCtrlInForceDir)
746  {
747  float forcePIDVel = -(float)forcePID->getControlValue();
748  Eigen::Vector3f targetVelInTool;
749  if (forceInToolFrame.norm() < 1e-4)
750  {
751  targetVelInTool << 0, 0, forcePIDVel;
752  }
753  else
754  {
755  targetVelInTool = forceInToolFrame / forceInToolFrame.norm() * forcePIDVel;
756  }
757  targetVel.head<3>() += currentToolOri * targetVelInTool;
758  }
759  else
760  {
761  // targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
762  Eigen::Vector3f localVel;
763  localVel << 0, 0, -(float)forcePID->getControlValue();
764  positionOffset += currentToolOri * localVel * deltaT;
765 
766  targetVel(2) -= (float)forcePID->getControlValue();
767  targetVel.head<3>() = currentToolOri * targetVel.head<3>();
768  }
769  }
770  else
771  {
772  positionOffset.setZero();
773  }
774 
775  if (isRotControlEnabled)
776  {
777  // targetFTInToolFrame(4) -= (float)rotPID->getControlValue();
778  // targetVel.tail<3>() = adaptedAngularKp * angularDiff;
779  targetVel(4) -= (float)rotPID->getControlValue();
780  }
781  if (isTorqueControlEnabled)
782  {
783  // targetFTInToolFrame(4) -= (float)torquePID->getControlValue();
784  targetVel(4) += (float)torquePID->getControlValue();
785  }
786 
787  // targetFTInRootFrame.head<3>() = currentToolOri * targetFTInToolFrame.head<3>();
788  // targetFTInRootFrame.tail<3>() = currentToolOri * targetFTInToolFrame.tail<3>();
789  }
790 
791 
792  rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
793  rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head<3>();
794  rt2UserData.getWriteBuffer().forceOutput = filteredForceInRoot;
795  rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
796  rt2UserData.commitWrite();
797 
798  /* -------------------------- Integrate Target Velocity 2 Target Pose --------------------------------- */
799 
800  if (useDMPInGlobalFrame)
801  {
802  targetPose = rtGetControlStruct().targetTSPose;
803  targetPose.block<3, 1>(0, 3) += positionOffset;
804  }
805  else
806  {
807  targetPose.block<3, 1>(0, 3) =
808  targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
809  Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(
810  dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
811  targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
812 
813  if (isPhaseStop)
814  {
815  Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
816  if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
817  {
818  changeTimer += deltaT;
819  }
820  else
821  {
822  lastPosition = currentPose.block<2, 1>(0, 3);
823  changeTimer = 0;
824  }
825 
826  if (changeTimer > cfg->changeTimerThreshold)
827  {
828  targetPose(0, 3) = currentPose(0, 3);
829  targetPose(1, 3) = currentPose(1, 3);
830  isPhaseStop = false;
831  changeTimer = 0;
832  }
833  }
834  else
835  {
836  changeTimer = 0;
837  }
838  }
839 
840  targetPose(0, 3) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
841  targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
842 
843  targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
844  targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
845 
846  targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
847  targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
848 
849  /* -------------------------- Force/Torque Impedance Controller --------------------------------- */
850 
851  // inverse dynamic controller
852  jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
853 
854  Eigen::Matrix3f diffMat =
855  targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
856  Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
857  Eigen::Vector6f taskFTControlCommand;
858 
859  // for (int i = 0; i < 3; i++)
860  // {
861  // adaptD[i] = (float)2 * sqrt(adaptK[i]);
862  // }
863  taskFTControlCommand.head<3>() =
864  adaptK.cwiseProduct(targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) *
865  0.001 +
866  adaptD.cwiseProduct(-currentTwist.head<3>()) * 0.001 + targetFTInRootFrame.head<3>();
867  taskFTControlCommand.tail<3>() = kori.cwiseProduct(rpy) +
868  dori.cwiseProduct(-currentTwist.tail<3>()) +
869  targetFTInRootFrame.tail<3>();
870 
871  filteredFTCommand = cfg->ftCommandFilter * taskFTControlCommand +
872  (1 - cfg->ftCommandFilter) * filteredFTCommand;
873 
874  /* -------------------------- NullSpace and Joint Torque Controller --------------------------------- */
875 
876  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
877  Eigen::VectorXf nullspaceTorque =
878  knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
879  Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
880  Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * filteredFTCommand +
881  (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
882  // if (forceInToolFrame.norm() > cfg->maxInteractionForce)
883  // {
884  // jointDesiredTorques.setZero();
885  // }
886 
887  for (size_t i = 0; i < targets.size(); ++i)
888  {
889  targets.at(i)->torque = jointDesiredTorques(i);
890 
891  if (!targets.at(i)->isValid())
892  {
893  targets.at(i)->torque = 0.0f;
894  }
895  else
896  {
897  if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
898  {
899  targets.at(i)->torque = fabs(cfg->maxJointTorque) *
900  (targets.at(i)->torque / fabs(targets.at(i)->torque));
901  }
902  }
903  }
904 
905  /* -------------------------- Communication --------------------------------- */
906 
907  debugRT.getWriteBuffer().currentToolDir =
908  tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
909  debugRT.getWriteBuffer().targetPose = targetPose;
910  debugRT.getWriteBuffer().globalPose =
911  tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
912  debugRT.getWriteBuffer().currentPose = currentPose;
913  debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
914  debugRT.getWriteBuffer().rotationAxis = axis;
915  debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
916  debugRT.getWriteBuffer().globalFilteredForce =
917  tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
918  debugRT.getWriteBuffer().targetVel = targetVel;
919  debugRT.getWriteBuffer().adaptK = adaptK;
920  debugRT.getWriteBuffer().adaptD = adaptD;
921  debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
922  debugRT.getWriteBuffer().rotAngle = angle;
923  debugRT.getWriteBuffer().currentTwist = currentTwist;
924  debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
925  debugRT.getWriteBuffer().wipingCounter = wipingCounter;
926  debugRT.getWriteBuffer().mu = mu;
927  debugRT.getWriteBuffer().estimatedFriction = estimatedFriction;
928  debugRT.getWriteBuffer().frictionInToolXY = lastForceInToolXY;
929  debugRT.getWriteBuffer().velPInTool = velPInToolFrame;
930  debugRT.getWriteBuffer().kpForcePID = adaptKpForce;
931  debugRT.getWriteBuffer().kpRotPID = adaptKpRot;
932  debugRT.getWriteBuffer().loseContactForceIntegral = forceIntegral;
933 
934 
935  debugRT.commitWrite();
936 
937  rt2CtrlData.getWriteBuffer().currentPose = currentPose;
938  rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
939  rt2CtrlData.getWriteBuffer().deltaT = deltaT;
940  rt2CtrlData.getWriteBuffer().currentTime += deltaT;
941  rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
942  rt2CtrlData.commitWrite();
943  }
944 
945  void
946  NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromFiles(
947  const Ice::StringSeq& fileNames,
948  const Ice::Current&)
949  {
950  ARMARX_INFO << "Learning DMP ... ";
951 
952  LockGuardType guard{controllerMutex};
953  dmpCtrl->learnDMPFromFiles(fileNames);
954  }
955 
956  void
957  NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromTrajectory(
958  const TrajectoryBasePtr& trajectory,
959  const Ice::Current&)
960  {
961  ARMARX_INFO << "Learning DMP ... ";
962  ARMARX_CHECK_EXPRESSION(trajectory);
963  TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
964  ARMARX_CHECK_EXPRESSION(dmpTraj);
965 
966  LockGuardType guard{controllerMutex};
967  dmpCtrl->learnDMPFromTrajectory(dmpTraj);
968  }
969 
970  void
971  NJointAnomalyDetectionAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
972  {
973  LockGuardType guard{controllerMutex};
974  dmpCtrl->setSpeed(times);
975  }
976 
977  void
978  NJointAnomalyDetectionAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals,
979  const Ice::Current& ice)
980  {
981  LockGuardType guard{controllerMutex};
982  dmpCtrl->setGoalPoseVec(goals);
983  }
984 
985  void
986  NJointAnomalyDetectionAdaptiveWipingController::setAmplitude(Ice::Double amp,
987  const Ice::Current&)
988  {
989  LockGuardType guard{controllerMutex};
990  dmpCtrl->setAmplitude(amp);
991  }
992 
993  void
994  NJointAnomalyDetectionAdaptiveWipingController::setTargetForceInRootFrame(float targetForce,
995  const Ice::Current&)
996  {
997  LockGuardType guard{controlDataMutex};
998  user2rtData.getWriteBuffer().targetForce = targetForce;
999  user2rtData.commitWrite();
1000  }
1001 
1002  void
1003  NJointAnomalyDetectionAdaptiveWipingController::runDMP(const Ice::DoubleSeq& goals,
1004  Ice::Double tau,
1005  const Ice::Current&)
1006  {
1007  firstRun = true;
1008  while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration <
1009  cfg->waitTimeForCalibration)
1010  {
1011  usleep(100);
1012  }
1013 
1014 
1015  Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
1016 
1017  LockGuardType guard{controllerMutex};
1018  dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
1019  dmpCtrl->setSpeed(tau);
1020 
1021  ARMARX_IMPORTANT << "run DMP";
1022  started = true;
1023  dmpRunning = false;
1024  }
1025 
1026  void
1027  NJointAnomalyDetectionAdaptiveWipingController::setTrigerAbnormalEvent(bool abnormal,
1028  const Ice::Current&)
1029  {
1030  abnormalFlag = abnormal;
1031  }
1032 
1033  std::vector<float>
1034  NJointAnomalyDetectionAdaptiveWipingController::getAnomalyInput(const Ice::Current&)
1035  {
1036  Eigen::Matrix4f tpos = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
1037  Eigen::Vector3f tvel = rt2UserData.getUpToDateReadBuffer().tcpTranslVel;
1038  std::vector<float> tvelvec = {
1039  tvel(0), tvel(1), tvel(2), tpos(0, 3) / 1000, tpos(1, 3) / 1000, tpos(2, 3) / 1000};
1040  return tvelvec;
1041  }
1042 
1043  std::vector<float>
1044  NJointAnomalyDetectionAdaptiveWipingController::getAnomalyOutput(const Ice::Current&)
1045  {
1046  Eigen::Vector3f force = rt2UserData.getUpToDateReadBuffer().forceOutput;
1047  std::vector<float> forceVec = {force(0), force(1), force(2)};
1048  return forceVec;
1049  }
1050 
1051  void
1052  NJointAnomalyDetectionAdaptiveWipingController::onPublish(
1053  const SensorAndControl&,
1054  const DebugDrawerInterfacePrx& debugDrawer,
1055  const DebugObserverInterfacePrx& debugObs)
1056  {
1057  std::string datafieldName;
1058  std::string debugName = "Periodic";
1059  StringVariantBaseMap datafields;
1060 
1061  Eigen::Matrix4f targetPoseDebug = debugRT.getUpToDateReadBuffer().targetPose;
1062  datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
1063  datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
1064  datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
1065 
1066  VirtualRobot::MathTools::Quaternion qtarget =
1067  VirtualRobot::MathTools::eigen4f2quat(targetPoseDebug);
1068  datafields["target_qw"] = new Variant(qtarget.w);
1069  datafields["target_qx"] = new Variant(qtarget.x);
1070  datafields["target_qy"] = new Variant(qtarget.y);
1071  datafields["target_qz"] = new Variant(qtarget.z);
1072 
1073 
1074  Eigen::Matrix4f currentPoseDebug = debugRT.getUpToDateReadBuffer().currentPose;
1075  datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
1076  datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
1077  datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
1078 
1079 
1080  VirtualRobot::MathTools::Quaternion qcurrent =
1081  VirtualRobot::MathTools::eigen4f2quat(currentPoseDebug);
1082  datafields["current_qw"] = new Variant(qcurrent.w);
1083  datafields["current_qx"] = new Variant(qcurrent.x);
1084  datafields["current_qy"] = new Variant(qcurrent.y);
1085  datafields["current_qz"] = new Variant(qcurrent.z);
1086 
1087 
1088  Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
1089  datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
1090  datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
1091  datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
1092 
1093  Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
1094  datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
1095  datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
1096  datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
1097 
1098 
1099  Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
1100  datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
1101  datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
1102  datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
1103 
1104  Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
1105  datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
1106  datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
1107  datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
1108 
1109  Eigen::Vector3f reactForce = debugRT.getUpToDateReadBuffer().reactForce;
1110  datafields["reactForce_x"] = new Variant(reactForce(0));
1111  datafields["reactForce_y"] = new Variant(reactForce(1));
1112  datafields["reactForce_z"] = new Variant(reactForce(2));
1113 
1114  Eigen::VectorXf targetVel = debugRT.getUpToDateReadBuffer().targetVel;
1115  datafields["targetVel_x"] = new Variant(targetVel(0));
1116  datafields["targetVel_y"] = new Variant(targetVel(1));
1117  datafields["targetVel_z"] = new Variant(targetVel(2));
1118  datafields["targetVel_ro"] = new Variant(targetVel(3));
1119  datafields["targetVel_pi"] = new Variant(targetVel(4));
1120  datafields["targetVel_ya"] = new Variant(targetVel(5));
1121 
1122  Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
1123  datafields["currentTwist_x"] = new Variant(currentTwist(0));
1124  datafields["currentTwist_y"] = new Variant(currentTwist(1));
1125  datafields["currentTwist_z"] = new Variant(currentTwist(2));
1126  datafields["currentTwist_ro"] = new Variant(currentTwist(3));
1127  datafields["currentTwist_pi"] = new Variant(currentTwist(4));
1128  datafields["currentTwist_ya"] = new Variant(currentTwist(5));
1129 
1130 
1131  Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
1132  datafields["adaptK_x"] = new Variant(adaptK(0));
1133  datafields["adaptK_y"] = new Variant(adaptK(1));
1134  datafields["adaptK_z"] = new Variant(adaptK(2));
1135 
1136  Eigen::Vector3f adaptD = debugRT.getUpToDateReadBuffer().adaptD;
1137  datafields["adaptD_x"] = new Variant(adaptD(0));
1138  datafields["adaptD_y"] = new Variant(adaptD(1));
1139  datafields["adaptD_z"] = new Variant(adaptD(2));
1140 
1141  datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
1142  datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
1143 
1144  datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
1145  datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
1146  datafields["wipingCounter"] = new Variant(debugRT.getUpToDateReadBuffer().wipingCounter);
1147  datafields["mu"] = new Variant(debugRT.getUpToDateReadBuffer().mu);
1148  datafields["loseContactForceIntegral"] =
1149  new Variant(debugRT.getUpToDateReadBuffer().loseContactForceIntegral);
1150 
1151  Eigen::VectorXf estFri = debugRT.getUpToDateReadBuffer().estimatedFriction;
1152  datafields["estimatedFriction_x"] = new Variant(estFri(0));
1153  datafields["estimatedFriction_y"] = new Variant(estFri(1));
1154 
1155  Eigen::VectorXf frictionInToolXY = debugRT.getUpToDateReadBuffer().frictionInToolXY;
1156  datafields["frictionInToolXY_x"] = new Variant(frictionInToolXY(0));
1157  datafields["frictionInToolXY_y"] = new Variant(frictionInToolXY(1));
1158 
1159  Eigen::VectorXf velPInTool = debugRT.getUpToDateReadBuffer().velPInTool;
1160  datafields["velPInTool_x"] = new Variant(velPInTool(0));
1161  datafields["velPInTool_y"] = new Variant(velPInTool(1));
1162  datafields["velPInTool_z"] = new Variant(velPInTool(2));
1163 
1164  datafields["kp_force_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpForcePID);
1165  datafields["kp_rot_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpRotPID);
1166 
1167  // datafields["targetVel_rx"] = new Variant(targetVel(3));
1168  // datafields["targetVel_ry"] = new Variant(targetVel(4));
1169  // datafields["targetVel_rz"] = new Variant(targetVel(5));
1170 
1171  // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
1172  // for (auto& pair : values)
1173  // {
1174  // datafieldName = pair.first + "_" + debugName;
1175  // datafields[datafieldName] = new Variant(pair.second);
1176  // }
1177 
1178  // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
1179  // for (auto& pair : currentPose)
1180  // {
1181  // datafieldName = pair.first + "_" + debugName;
1182  // datafields[datafieldName] = new Variant(pair.second);
1183  // }
1184 
1185  // datafieldName = "canVal_" + debugName;
1186  // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
1187  // datafieldName = "mpcFactor_" + debugName;
1188  // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
1189  // datafieldName = "error_" + debugName;
1190  // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
1191  // datafieldName = "posError_" + debugName;
1192  // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
1193  // datafieldName = "oriError_" + debugName;
1194  // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
1195  // datafieldName = "deltaT_" + debugName;
1196  // datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
1197 
1198  datafieldName = "PeriodicDMP";
1199  debugObs->setDebugChannel(datafieldName, datafields);
1200 
1201 
1202  // draw force;
1203  Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
1204  Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
1205  Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
1206 
1207  debugDrawer->setArrowVisu("Force",
1208  "currentForce",
1209  new Vector3(handPosition),
1210  new Vector3(forceDir),
1211  DrawColor{0, 0, 1, 1},
1212  10 * forceDir.norm(),
1213  3);
1214 
1215  // draw direction of the tool
1216  Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
1217  debugDrawer->setArrowVisu("Tool",
1218  "Tool",
1219  new Vector3(handPosition),
1220  new Vector3(currentToolDir),
1221  DrawColor{1, 0, 0, 1},
1222  100,
1223  3);
1224  debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
1225  }
1226 
1227  void
1228  NJointAnomalyDetectionAdaptiveWipingController::pauseDMP(const Ice::Current&)
1229  {
1230  dmpCtrl->pauseController();
1231  }
1232 
1233  void
1234  NJointAnomalyDetectionAdaptiveWipingController::resumeDMP(const Ice::Current&)
1235  {
1236  dmpCtrl->resumeController();
1237  }
1238 
1239  void
1240  NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController()
1241  {
1242  ARMARX_INFO << "stopped ...";
1243  }
1244 
1245 
1246 } // namespace armarx::control::deprecated_njoint_mp_controller::adaptive
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointAnomalyDetectionAdaptiveWipingControllerControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:223
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:54
armarx::NJointAnomalyDetectionAdaptiveWipingControllerInterface::pauseDMP
void pauseDMP()
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:53
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::rtGetControlStruct
const NJointAnomalyDetectionAdaptiveWipingControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition: TaskSpaceVMP.h:61
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::runDMP
void runDMP(const Ice::DoubleSeq &goals, Ice::Double tau, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:1003
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingControllerControlData::targetTSPose
Eigen::Matrix4f targetTSPose
Definition: NJointAnomalyDetectionAdaptiveWipingController.h:34
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
RobotUnit.h
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::controllerRun
void controllerRun()
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:252
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
armarx::NJointAnomalyDetectionAdaptiveWipingControllerInterface::resumeDMP
void resumeDMP()
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::NJointAnomalyDetectionAdaptiveWipingController
NJointAnomalyDetectionAdaptiveWipingController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:26
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition: NJointController.cpp:410
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:769
armarx::sign
T sign(T t)
Definition: algorithm.h:214
SensorValueForceTorque.h
ARMARX_CHECK_LESS
#define ARMARX_CHECK_LESS(lhs, rhs)
This macro evaluates whether lhs is less (<) than rhs and if it turns out to be false it will throw a...
Definition: ExpressionException.h:102
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::setTrigerAbnormalEvent
void setTrigerAbnormalEvent(bool abnormal, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:1027
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:59
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:946
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:67
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::getWriterControlStruct
NJointAnomalyDetectionAdaptiveWipingControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:48
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:246
armarx::SensorValueForceTorque::force
Eigen::Vector3f force
Definition: SensorValueForceTorque.h:36
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:66
IceInternal::Handle< Trajectory >
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition: TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:50
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:920
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::setSpeed
void setSpeed(Ice::Double times, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:971
armarx::control::deprecated_njoint_mp_controller::adaptive
Definition: NJointAdaptiveWipingController.cpp:21
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:55
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::onInitNJointController
void onInitNJointController()
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:209
M_PI
#define M_PI
Definition: MathTools.h:17
NJointAnomalyDetectionAdaptiveWipingController.h
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:778
armarx::PIDController
Definition: PIDController.h:43
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:1240
armarx::NJointControllerBase::runTask
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
Definition: NJointControllerBase.h:754
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::rtUpdateControlStruct
bool rtUpdateControlStruct()
Definition: NJointControllerWithTripleBuffer.h:38
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:1052
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::SensorValueForceTorque::torque
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque
Definition: SensorValueForceTorque.h:35
ArmarXObjectScheduler.h
ControlTarget1DoFActuator.h
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:978
armarx::NJointControllerWithTripleBuffer< NJointAnomalyDetectionAdaptiveWipingControllerControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromTrajectory
void learnDMPFromTrajectory(const TrajectoryBasePtr &trajectory, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:957
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:64
armarx::NJointAnomalyDetectionAdaptiveWipingControllerInterface::getAnomalyInput
Ice::FloatSeq getAnomalyInput()
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:51
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:52
CartesianVelocityController.h
PIDController.h
NJointController.h
Trajectory.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::setTargetForceInRootFrame
void setTargetForceInRootFrame(Ice::Float force, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:994
IceUtil::Handle< class RobotUnit >
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition: TaskSpaceVMP.h:63
armarx::control::deprecated_njoint_mp_controller::adaptive::registrationControllerNJointAnomalyDetectionAdaptiveWipingController
NJointControllerRegistration< NJointAnomalyDetectionAdaptiveWipingController > registrationControllerNJointAnomalyDetectionAdaptiveWipingController("NJointAnomalyDetectionAdaptiveWipingController")
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::setAmplitude
void setAmplitude(Ice::Double amp, const Ice::Current &)
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:986
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:109
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:56
armarx::NJointAnomalyDetectionAdaptiveWipingControllerInterface::getAnomalyOutput
Ice::FloatSeq getAnomalyOutput()
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:116
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::SensorValueForceTorque
Definition: SensorValueForceTorque.h:32
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingControllerControlData
Definition: NJointAnomalyDetectionAdaptiveWipingController.h:30
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:171
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:90
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
SensorValue1DoFActuator.h
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingControllerControlData::targetTSVel
Eigen::VectorXf targetTSVel
Definition: NJointAnomalyDetectionAdaptiveWipingController.h:33
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition: NJointController.cpp:383
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointAnomalyDetectionAdaptiveWipingController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: NJointAnomalyDetectionAdaptiveWipingController.cpp:317
norm
double norm(const Point &a)
Definition: point.hpp:102