DSRTController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package DSController::ArmarXObjects::DSRTController
17  * @author Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "DSRTController.h"
24 
25 #include <VirtualRobot/RobotNodeSet.h>
26 #include <VirtualRobot/Tools/Gravity.h>
27 #include <VirtualRobot/IK/DifferentialIK.h>
28 
29 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
30 #include <ArmarXCore/core/time/CycleUtil.h>
31 
32 namespace armarx::control::ds_controller
33 {
34  NJointControllerRegistration<DSRTController>
35  registrationControllerDSRTController("DSRTController");
36 
37  void
38  DSRTController::onInitNJointController()
39  {
40  ARMARX_INFO << "init ...";
41 
42  runTask("DSRTControllerTask",
43  [&]
44  {
45  CycleUtil c(1);
46  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
47  while (getState() == eManagedIceObjectStarted)
48  {
49  if (isControllerActive())
50  {
51  controllerRun();
52  }
53  c.waitForCycleDuration();
54  }
55  });
56  }
57 
58  void
59  DSRTController::onDisconnectNJointController()
60  {
61  }
62 
63  DSRTController::DSRTController(const RobotUnitPtr& robotUnit,
64  const armarx::NJointControllerConfigPtr& config,
65  const VirtualRobot::RobotPtr&)
66  {
67  DSControllerConfigPtr cfg = DSControllerConfigPtr::dynamicCast(config);
68  useSynchronizedRtRobot();
69  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
70 
71  jointNames.clear();
72  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
73  for (size_t i = 0; i < rns->getSize(); ++i)
74  {
75  std::string jointName = rns->getNode(i)->getName();
76  jointNames.push_back(jointName);
77  ControlTargetBase* ct = useControlTarget(
78  jointName,
79  ControlModes::
80  Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
81  ARMARX_CHECK_EXPRESSION(ct);
82  const SensorValueBase* sv = useSensorValue(jointName);
83  ARMARX_CHECK_EXPRESSION(sv);
84  auto casted_ct = ct->asA<
85  ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
86  ARMARX_CHECK_EXPRESSION(casted_ct);
87  targets.push_back(casted_ct);
88 
89  const SensorValue1DoFActuatorTorque* torqueSensor =
90  sv->asA<SensorValue1DoFActuatorTorque>();
91  const SensorValue1DoFActuatorVelocity* velocitySensor =
92  sv->asA<SensorValue1DoFActuatorVelocity>();
93  const SensorValue1DoFGravityTorque* gravityTorqueSensor =
94  sv->asA<SensorValue1DoFGravityTorque>();
95  const SensorValue1DoFActuatorPosition* positionSensor =
96  sv->asA<SensorValue1DoFActuatorPosition>();
97  if (!torqueSensor)
98  {
99  ARMARX_WARNING << "No Torque sensor available for " << jointName;
100  }
101  if (!gravityTorqueSensor)
102  {
103  ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
104  }
105 
106  torqueSensors.push_back(torqueSensor);
107  gravityTorqueSensors.push_back(gravityTorqueSensor);
108  velocitySensors.push_back(velocitySensor);
109  positionSensors.push_back(positionSensor);
110  };
111  ARMARX_INFO << "Initialized " << targets.size() << " targets";
112  tcp = rns->getTCP();
113  ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
114 
115 
116  ik.reset(new VirtualRobot::DifferentialIK(
117  rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
118 
119  DSRTControllerSensorData initSensorData;
120  initSensorData.tcpPose = tcp->getPoseInRootFrame();
121  initSensorData.currentTime = 0;
122  controllerSensorData.reinitAllBuffers(initSensorData);
123 
124 
125  oldPosition = tcp->getPositionInRootFrame();
126  oldOrientation = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
127 
128  std::vector<float> desiredPositionVec = cfg->desiredPosition;
129  for (size_t i = 0; i < 3; ++i)
130  {
131  desiredPosition(i) = desiredPositionVec[i];
132  }
133  ARMARX_INFO << "ik reseted ";
134 
135  std::vector<float> desiredQuaternionVec = cfg->desiredQuaternion;
136  ARMARX_CHECK_EQUAL(desiredQuaternionVec.size(), 4);
137 
138  desiredQuaternion.x() = desiredQuaternionVec.at(0);
139  desiredQuaternion.y() = desiredQuaternionVec.at(1);
140  desiredQuaternion.z() = desiredQuaternionVec.at(2);
141  desiredQuaternion.w() = desiredQuaternionVec.at(3);
142 
143 
144  DSRTControllerControlData initData;
145  for (size_t i = 0; i < 3; ++i)
146  {
147  initData.tcpDesiredLinearVelocity(i) = 0;
148  }
149 
150  for (size_t i = 0; i < 3; ++i)
151  {
152  initData.tcpDesiredAngularError(i) = 0;
153  }
154  reinitTripleBuffer(initData);
155 
156  // initial filter
157  currentTCPLinearVelocity_filtered.setZero();
158  currentTCPAngularVelocity_filtered.setZero();
159  filterTimeConstant = cfg->filterTimeConstant;
160  posiKp = cfg->posiKp;
161  v_max = cfg->v_max;
162  posiDamping = cfg->posiDamping;
163  torqueLimit = cfg->torqueLimit;
164  oriKp = cfg->oriKp;
165  oriDamping = cfg->oriDamping;
166 
167 
168  std::vector<float> qnullspaceVec = cfg->qnullspaceVec;
169 
170  qnullspace.resize(qnullspaceVec.size());
171 
172  for (size_t i = 0; i < qnullspaceVec.size(); ++i)
173  {
174  qnullspace(i) = qnullspaceVec[i];
175  }
176 
177  nullspaceKp = cfg->nullspaceKp;
178  nullspaceDamping = cfg->nullspaceDamping;
179 
180 
181  //set GMM parameters ...
182  std::vector<std::string> gmmParamsFiles = cfg->gmmParamsFiles;
183  if (gmmParamsFiles.size() == 0)
184  {
185  ARMARX_ERROR << "no gmm found ... ";
186  }
187 
188  gmmMotionGenList.clear();
189  float sumBelief = 0;
190  for (size_t i = 0; i < gmmParamsFiles.size(); ++i)
191  {
192  gmmMotionGenList.push_back(GMMMotionGenPtr(new GMMMotionGen(gmmParamsFiles.at(i))));
193  sumBelief += gmmMotionGenList[i]->belief;
194  }
195  ARMARX_CHECK_EQUAL(gmmMotionGenList.size(), 2);
196 
197  dsAdaptorPtr.reset(new DSAdaptor(gmmMotionGenList, cfg->dsAdaptorEpsilon));
198  positionErrorTolerance = cfg->positionErrorTolerance;
199 
200 
201  ARMARX_INFO << "Initialization done";
202  }
203 
204  void
205  DSRTController::controllerRun()
206  {
207  if (!controllerSensorData.updateReadBuffer())
208  {
209  return;
210  }
211 
212 
213  Eigen::Matrix4f currentTCPPose = controllerSensorData.getReadBuffer().tcpPose;
214  Eigen::Vector3f realVelocity = controllerSensorData.getReadBuffer().linearVelocity;
215 
216  Eigen::Vector3f currentTCPPositionInMeter;
217  currentTCPPositionInMeter << currentTCPPose(0, 3), currentTCPPose(1, 3),
218  currentTCPPose(2, 3);
219  currentTCPPositionInMeter = 0.001 * currentTCPPositionInMeter;
220 
221  dsAdaptorPtr->updateDesiredVelocity(currentTCPPositionInMeter, positionErrorTolerance);
222  Eigen::Vector3f tcpDesiredLinearVelocity = dsAdaptorPtr->totalDesiredVelocity;
223  dsAdaptorPtr->updateBelief(realVelocity);
224 
225 
226  float vecLen = tcpDesiredLinearVelocity.norm();
227  if (vecLen > v_max)
228  {
229 
230  tcpDesiredLinearVelocity = tcpDesiredLinearVelocity / vecLen * v_max;
231  }
232 
233  ARMARX_INFO << "tcpDesiredLinearVelocity: " << tcpDesiredLinearVelocity;
234 
235  debugDataInfo.getWriteBuffer().belief0 = dsAdaptorPtr->task0_belief;
236  debugDataInfo.getWriteBuffer().belief1 = gmmMotionGenList[0]->belief;
237  debugDataInfo.getWriteBuffer().belief2 = gmmMotionGenList[1]->belief;
238  debugDataInfo.commitWrite();
239 
240  Eigen::Vector3f tcpDesiredAngularError;
241  tcpDesiredAngularError << 0, 0, 0;
242 
243  Eigen::Matrix3f currentOrientation = currentTCPPose.block<3, 3>(0, 0);
244  Eigen::Matrix3f desiredOrientation = desiredQuaternion.normalized().toRotationMatrix();
245  Eigen::Matrix3f orientationError = currentOrientation * desiredOrientation.inverse();
246  Eigen::Quaternionf diffQuaternion(orientationError);
247  Eigen::AngleAxisf angleAxis(diffQuaternion);
248  tcpDesiredAngularError = angleAxis.angle() * angleAxis.axis();
249 
250  getWriterControlStruct().tcpDesiredLinearVelocity = tcpDesiredLinearVelocity;
251  getWriterControlStruct().tcpDesiredAngularError = tcpDesiredAngularError;
252 
253  writeControlStruct();
254  }
255 
256  void
257  DSRTController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
258  const IceUtil::Time& timeSinceLastIteration)
259  {
260  double deltaT = timeSinceLastIteration.toSecondsDouble();
261  if (deltaT != 0)
262  {
263 
264  Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
265 
266 
267  Eigen::MatrixXf jacobi =
268  ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
269 
270  Eigen::VectorXf qpos;
271  Eigen::VectorXf qvel;
272  qpos.resize(positionSensors.size());
273  qvel.resize(velocitySensors.size());
274 
275  int jointDim = positionSensors.size();
276 
277  for (size_t i = 0; i < velocitySensors.size(); ++i)
278  {
279  qpos(i) = positionSensors[i]->position;
280  qvel(i) = velocitySensors[i]->velocity;
281  }
282 
283  Eigen::VectorXf tcptwist = jacobi * qvel;
284 
285  Eigen::Vector3f currentTCPLinearVelocity;
286  currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1),
287  0.001 * tcptwist(2);
288  double filterFactor = deltaT / (filterTimeConstant + deltaT);
289  currentTCPLinearVelocity_filtered =
290  (1 - filterFactor) * currentTCPLinearVelocity_filtered +
291  filterFactor * currentTCPLinearVelocity;
292 
293  controllerSensorData.getWriteBuffer().tcpPose = currentTCPPose;
294  controllerSensorData.getWriteBuffer().currentTime += deltaT;
295  controllerSensorData.getWriteBuffer().linearVelocity =
296  currentTCPLinearVelocity_filtered;
297  controllerSensorData.commitWrite();
298 
299 
300  Eigen::Vector3f currentTCPAngularVelocity;
301  currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
302  // // calculate linear velocity
303  // Eigen::Vector3f currentTCPPosition;
304  // currentTCPPosition << currentTCPPose(0, 3), currentTCPPose(1, 3), currentTCPPose(2, 3);
305  // Eigen::Vector3f currentTCPLinearVelocity_raw = (currentTCPPosition - oldPosition) / deltaT;
306  // oldPosition = currentTCPPosition;
307 
308  // // calculate angular velocity
309  // Eigen::Matrix3f currentTCPOrientation = currentTCPPose.block<3, 3>(0, 0);
310  // Eigen::Matrix3f currentTCPDiff = currentTCPOrientation * oldOrientation.inverse();
311  // Eigen::AngleAxisf currentAngleAxisDiff(currentTCPDiff);
312  // Eigen::Vector3f currentTCPAngularVelocity_raw = currentAngleAxisDiff.angle() * currentAngleAxisDiff.axis();
313  // oldOrientation = currentTCPOrientation;
314  // currentTCPAngularVelocity_filtered = (1 - filterFactor) * currentTCPAngularVelocity_filtered + filterFactor * currentTCPAngularVelocity_raw;
315 
316 
317  Eigen::Vector3f tcpDesiredLinearVelocity =
318  rtGetControlStruct().tcpDesiredLinearVelocity;
319  Eigen::Vector3f tcpDesiredAngularError = rtGetControlStruct().tcpDesiredAngularError;
320 
321  // calculate desired tcp force
322  Eigen::Vector3f tcpDesiredForce =
323  -posiDamping * (currentTCPLinearVelocity_filtered - tcpDesiredLinearVelocity);
324 
325  // calculate desired tcp torque
326  Eigen::Vector3f tcpDesiredTorque =
327  -oriKp * tcpDesiredAngularError - oriDamping * currentTCPAngularVelocity;
328 
329  Eigen::Vector6f tcpDesiredWrench;
330  tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
331 
332  // calculate desired joint torque
333  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
334 
335  float lambda = 2;
336  Eigen::MatrixXf jtpinv =
337  ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
338 
339  Eigen::VectorXf nullqerror = qpos - qnullspace;
340 
341  for (int i = 0; i < nullqerror.rows(); ++i)
342  {
343  if (fabs(nullqerror(i)) < 0.09)
344  {
345  nullqerror(i) = 0;
346  }
347  }
348  Eigen::VectorXf nullspaceTorque = -nullspaceKp * nullqerror - nullspaceDamping * qvel;
349 
350  Eigen::VectorXf jointDesiredTorques =
351  jacobi.transpose() * tcpDesiredWrench +
352  (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
353 
354  for (size_t i = 0; i < targets.size(); ++i)
355  {
356  float desiredTorque = jointDesiredTorques(i);
357 
358  desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
359  desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
360 
361  debugDataInfo.getWriteBuffer().desired_torques[jointNames[i]] =
362  jointDesiredTorques(i);
363 
364  targets.at(i)->torque = desiredTorque; // targets.at(i)->velocity = desiredVelocity;
365  if (!targets.at(i)->isValid())
366  {
367  ARMARX_INFO
368  << deactivateSpam(1)
369  << "Torque controller target is invalid - setting to zero! set value: "
370  << targets.at(i)->torque;
371  targets.at(i)->torque = 0.0f;
372  }
373  }
374 
375 
376  debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
377  debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
378  debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
379 
380 
381  debugDataInfo.getWriteBuffer().tcpDesiredTorque_x = tcpDesiredTorque(0);
382  debugDataInfo.getWriteBuffer().tcpDesiredTorque_y = tcpDesiredTorque(1);
383  debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
384 
385  debugDataInfo.getWriteBuffer().tcpDesiredAngularError_x = tcpDesiredAngularError(0);
386  debugDataInfo.getWriteBuffer().tcpDesiredAngularError_y = tcpDesiredAngularError(1);
387  debugDataInfo.getWriteBuffer().tcpDesiredAngularError_z = tcpDesiredAngularError(2);
388 
389  debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_x =
390  currentTCPAngularVelocity(0);
391  debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_y =
392  currentTCPAngularVelocity(1);
393  debugDataInfo.getWriteBuffer().currentTCPAngularVelocity_z =
394  currentTCPAngularVelocity(2);
395 
396  debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_x =
397  currentTCPLinearVelocity_filtered(0);
398  debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_y =
399  currentTCPLinearVelocity_filtered(1);
400  debugDataInfo.getWriteBuffer().currentTCPLinearVelocity_z =
401  currentTCPLinearVelocity_filtered(2);
402 
403  debugDataInfo.commitWrite();
404  }
405  else
406  {
407  for (size_t i = 0; i < targets.size(); ++i)
408  {
409  targets.at(i)->torque = 0;
410  }
411  }
412  }
413 
414  void
415  DSRTController::onPublish(const SensorAndControl&,
416  const DebugDrawerInterfacePrx&,
417  const DebugObserverInterfacePrx& debugObs)
418  {
419 
420  StringVariantBaseMap datafields;
421  auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
422  for (auto& pair : values)
423  {
424  datafields[pair.first] = new Variant(pair.second);
425  }
426 
427  // std::string nameJacobi = "jacobiori";
428  // std::string nameJacobipos = "jacobipos";
429 
430  // std::vector<float> jacobiVec = debugDataInfo.getUpToDateReadBuffer().jacobiVec;
431  // std::vector<float> jacobiPos = debugDataInfo.getUpToDateReadBuffer().jacobiPos;
432 
433  // for (size_t i = 0; i < jacobiVec.size(); ++i)
434  // {
435  // std::stringstream ss;
436  // ss << i;
437  // std::string name = nameJacobi + ss.str();
438  // datafields[name] = new Variant(jacobiVec[i]);
439  // std::string namepos = nameJacobipos + ss.str();
440  // datafields[namepos] = new Variant(jacobiPos[i]);
441 
442  // }
443 
444 
445  datafields["desiredForce_x"] =
446  new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
447  datafields["desiredForce_y"] =
448  new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
449  datafields["desiredForce_z"] =
450  new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
451 
452  datafields["tcpDesiredTorque_x"] =
453  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
454  datafields["tcpDesiredTorque_y"] =
455  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
456  datafields["tcpDesiredTorque_z"] =
457  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
458 
459  datafields["tcpDesiredAngularError_x"] =
460  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_x);
461  datafields["tcpDesiredAngularError_y"] =
462  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_y);
463  datafields["tcpDesiredAngularError_z"] =
464  new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredAngularError_z);
465 
466  datafields["currentTCPAngularVelocity_x"] =
467  new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_x);
468  datafields["currentTCPAngularVelocity_y"] =
469  new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_y);
470  datafields["currentTCPAngularVelocity_z"] =
471  new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPAngularVelocity_z);
472 
473 
474  datafields["currentTCPLinearVelocity_x"] =
475  new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_x);
476  datafields["currentTCPLinearVelocity_y"] =
477  new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_y);
478  datafields["currentTCPLinearVelocity_z"] =
479  new Variant(debugDataInfo.getUpToDateReadBuffer().currentTCPLinearVelocity_z);
480 
481  datafields["belief_0"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief0);
482  datafields["belief_1"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief1);
483  datafields["belief_2"] = new Variant(debugDataInfo.getUpToDateReadBuffer().belief2);
484 
485  debugObs->setDebugChannel("DSControllerOutput", datafields);
486  }
487 
488  void
489  DSRTController::rtPreActivateController()
490  {
491  }
492 
493  void
494  DSRTController::rtPostDeactivateController()
495  {
496  }
497 } // namespace armarx::control::ds_controller
armarx::NJointControllerWithTripleBuffer< DSRTControllerControlData >::reinitTripleBuffer
void reinitTripleBuffer(const DSRTControllerControlData &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::ds_controller::DSRTController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: DSRTController.cpp:59
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:92
armarx::control::ds_controller::GMMMotionGen
Definition: DSRTController.h:60
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< DSRTControllerControlData >::rtGetControlStruct
const DSRTControllerControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::control::ds_controller::DSRTController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: DSRTController.cpp:489
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:163
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
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::control::ds_controller::DSRTControllerControlData
Definition: DSRTController.h:42
armarx::control::ds_controller::DSRTController::controllerRun
void controllerRun()
Definition: DSRTController.cpp:205
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
DSRTController.h
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
armarx::control::ds_controller::DSRTController::rtPostDeactivateController
void rtPostDeactivateController()
This function is called after the controller is deactivated.
Definition: DSRTController.cpp:494
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::control::ds_controller
Definition: DSJointCarryController.cpp:32
armarx::NJointControllerWithTripleBuffer< DSRTControllerControlData >::getWriterControlStruct
DSRTControllerControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:846
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:182
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::control::ds_controller::DSRTController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: DSRTController.cpp:257
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:778
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::control::ds_controller::DSRTControllerControlData::tcpDesiredAngularError
Eigen::Vector3f tcpDesiredAngularError
Definition: DSRTController.h:46
armarx::NJointControllerWithTripleBuffer< DSRTControllerControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:108
ArmarXObjectScheduler.h
armarx::control::ds_controller::DSRTController::DSRTController
DSRTController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: DSRTController.cpp:63
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::control::ds_controller::GMMMotionGenPtr
boost::shared_ptr< GMMMotionGen > GMMMotionGenPtr
Definition: DSRTController.h:164
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::ds_controller::DSRTControllerControlData::tcpDesiredLinearVelocity
Eigen::Vector3f tcpDesiredLinearVelocity
Definition: DSRTController.h:45
CycleUtil.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
armarx::control::ds_controller::DSRTController::onInitNJointController
void onInitNJointController()
Definition: DSRTController.cpp:38
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::control::ds_controller::DSRTController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: DSRTController.cpp:415
armarx::Quaternion< float, 0 >
IceUtil::Handle< class RobotUnit >
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
Eigen::Matrix< float, 6, 1 >
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::control::ds_controller::DSAdaptor
Definition: DSRTController.h:166
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:171
armarx::control::ds_controller::registrationControllerDSRTController
NJointControllerRegistration< DSRTController > registrationControllerDSRTController("DSRTController")
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:776
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::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:143