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