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