NJointTaskSpaceAdaptiveDMPController.cpp
Go to the documentation of this file.
1#include "NJointTaskSpaceAdaptiveDMPController.h"
2
3#include <VirtualRobot/IK/DifferentialIK.h>
4#include <VirtualRobot/MathTools.h>
5#include <VirtualRobot/Nodes/RobotNode.h>
6#include <VirtualRobot/Robot.h>
7#include <VirtualRobot/RobotNodeSet.h>
8
9#include <ArmarXCore/core/ArmarXObjectScheduler.h>
10
11#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
12#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
13#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
14#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
15#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
16#include <RobotAPI/libraries/core/CartesianVelocityController.h>
17#include <RobotAPI/libraries/core/PIDController.h>
18#include <RobotAPI/libraries/core/Trajectory.h>
19
20#include <dmp/representation/dmp/umidmp.h>
21
22namespace armarx::control::deprecated_njoint_mp_controller::adaptive
23{
24 NJointControllerRegistration<NJointTaskSpaceAdaptiveDMPController>
25 registrationControllerNJointTaskSpaceAdaptiveDMPController(
26 "NJointTaskSpaceAdaptiveDMPController");
27
28 NJointTaskSpaceAdaptiveDMPController::NJointTaskSpaceAdaptiveDMPController(
29 const RobotUnitPtr& robotUnit,
30 const armarx::NJointControllerConfigPtr& config,
31 const VirtualRobot::RobotPtr&)
32 {
33 ARMARX_INFO << "creating impedance dmp controller";
34 cfg = NJointTaskSpaceAdaptiveDMPControllerConfigPtr::dynamicCast(config);
35 useSynchronizedRtRobot();
36 rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
37 ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
38
39 for (size_t i = 0; i < rns->getSize(); ++i)
40 {
41 std::string jointName = rns->getNode(i)->getName();
42 jointNames.push_back(jointName);
43 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
44 const SensorValueBase* sv = useSensorValue(jointName);
45 targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
46 const SensorValue1DoFActuatorVelocity* velocitySensor =
47 sv->asA<SensorValue1DoFActuatorVelocity>();
48 const SensorValue1DoFActuatorPosition* positionSensor =
49 sv->asA<SensorValue1DoFActuatorPosition>();
50
51 if (!velocitySensor)
52 {
53 ARMARX_WARNING << "No velocitySensor available for " << jointName;
54 }
55 if (!positionSensor)
56 {
57 ARMARX_WARNING << "No positionSensor available for " << jointName;
58 }
59
60 velocitySensors.push_back(velocitySensor);
61 positionSensors.push_back(positionSensor);
62 };
63
64
65 const SensorValueBase* svlf =
66 robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
67 forceSensor = svlf->asA<SensorValueForceTorque>();
68
69 tcp = rns->getTCP();
70 ik.reset(new VirtualRobot::DifferentialIK(
71 rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
72 numOfJoints = targets.size();
73 // set DMP
74 double phaseL = 1000;
75 double phaseK = 1000;
76 double phaseDist0 = 10000;
77 double phaseDist1 = 10000;
78 double posToOriRatio = 10;
79
80 tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
81 taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
82 taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
83 taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
84 taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
85 taskSpaceDMPConfig.DMPAmplitude = 1.0;
86 taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
87 taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
88 taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
89 taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
90 taskSpaceDMPConfig.phaseStopParas.Kori = 0;
91 taskSpaceDMPConfig.phaseStopParas.Dori = 0;
92 taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
93 taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
94 taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
95
96 dmpCtrl.reset(
97 new tsvmp::TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
98 finished = false;
99
100 useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
101 nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
102
103 isNullSpaceJointDMPLearned = false;
104
105 defaultNullSpaceJointValues.resize(targets.size());
106 ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
107
108 for (size_t i = 0; i < targets.size(); ++i)
109 {
110 defaultNullSpaceJointValues(i) = cfg->defaultNullSpaceJointValues.at(i);
111 }
112
113
114 kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
115 dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
116 kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
117 dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
118
119
120 ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
121 ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
122
123 knull.setZero(targets.size());
124 dnull.setZero(targets.size());
125
126 for (size_t i = 0; i < targets.size(); ++i)
127 {
128 knull(i) = cfg->Knull.at(i);
129 dnull(i) = cfg->Dnull.at(i);
130 }
131
132 torqueLimit = cfg->torqueLimit;
133 timeDuration = cfg->timeDuration;
134
135 NJointTaskSpaceAdaptiveDMPControllerInterfaceData initInterfaceData;
136 initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
137 initInterfaceData.currentForce = Eigen::Vector3f::Zero();
138 initInterfaceData.currentVel.setZero(6);
139 interfaceData.reinitAllBuffers(initInterfaceData);
140
141 NJointTaskSpaceAdaptiveDMPControllerSensorData initControllerSensorData;
142 initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
143 initControllerSensorData.currentTime = 0;
144 initControllerSensorData.deltaT = 0;
145 initControllerSensorData.currentTwist.setZero();
146 controllerSensorData.reinitAllBuffers(initControllerSensorData);
147
148 //initialize control parameters
149 Eigen::VectorXf KpImpedance;
150 KpImpedance.setZero(6);
151
152 for (int i = 0; i < 3; ++i)
153 {
154 KpImpedance(i) = cfg->Kpos.at(i);
155 KpImpedance(i + 3) = cfg->Kori.at(i);
156 }
157
158 Eigen::VectorXf KdImpedance;
159 KdImpedance.setZero(6);
160
161 for (int i = 0; i < 3; ++i)
162 {
163 KdImpedance(i) = cfg->Dpos.at(i);
164 KdImpedance(i + 3) = cfg->Dori.at(i);
165 }
166
167 Inferface2rtData initInt2rtData;
168 initInt2rtData.KpImpedance = KpImpedance;
169 initInt2rtData.KdImpedance = KdImpedance;
170 initInt2rtData.Knull = knull;
171 initInt2rtData.Dnull = dnull;
172 interface2rtBuffer.reinitAllBuffers(initInt2rtData);
173
174
175 Interface2CtrlData initInt2CtrlData;
176 initInt2CtrlData.canVal = 1.0;
177 interface2CtrlBuffer.reinitAllBuffers(initInt2CtrlData);
178
179 firstRun = true;
180 forceOffset.setZero(3);
181 filteredForce.setZero(3);
182
183 ARMARX_INFO << "Finished controller constructor ";
184 }
185
186 std::string
187 NJointTaskSpaceAdaptiveDMPController::getClassName(const Ice::Current&) const
188 {
189 return "NJointTaskSpaceAdaptiveDMPController";
190 }
191
192 void
193 NJointTaskSpaceAdaptiveDMPController::rtPreActivateController()
194 {
195 NJointTaskSpaceAdaptiveDMPControllerControlData initData;
196 initData.targetPose = tcp->getPoseInRootFrame();
197 initData.targetVel.resize(6);
198 initData.targetVel.setZero();
199 initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues;
200 reinitTripleBuffer(initData);
201 }
202
203 void
204 NJointTaskSpaceAdaptiveDMPController::controllerRun()
205 {
206
207
208 if (!dmpCtrl)
209 {
210 return;
211 }
212
213 if (!controllerSensorData.updateReadBuffer())
214 {
215 return;
216 }
217
218
219 double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
220 Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
221 Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
222
223 if (interface2CtrlBuffer.updateReadBuffer())
224 {
225 dmpCtrl->canVal = interface2CtrlBuffer.getUpToDateReadBuffer().canVal;
226 }
227
228 if (!started)
229 {
230 LockGuardType guard{controlDataMutex};
231 getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
232 getWriterControlStruct().targetVel.setZero(6);
233 getWriterControlStruct().targetPose = currentPose;
234 getWriterControlStruct().canVal = 1.0;
235 getWriterControlStruct().mpcFactor = 0.0;
236 writeControlStruct();
237 }
238 else
239 {
240 if (stopped)
241 {
242
243 LockGuardType guard{controlDataMutex};
244 getWriterControlStruct().desiredNullSpaceJointValues = defaultNullSpaceJointValues;
245 getWriterControlStruct().targetVel.setZero(6);
246 getWriterControlStruct().targetPose = oldPose;
247 getWriterControlStruct().canVal = dmpCtrl->canVal;
248 getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
249 writeControlStruct();
250 }
251 else
252 {
253 if (dmpCtrl->canVal < 1e-8)
254 {
255 finished = true;
256 LockGuardType guard{controlDataMutex};
257 getWriterControlStruct().targetVel.setZero();
258 writeControlStruct();
259 return;
260 }
261
262 dmpCtrl->flow(deltaT, currentPose, currentTwist);
263
264 Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
265 if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
266 {
267 DMP::DVec targetJointState;
268 currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(
269 currentJointState,
270 dmpCtrl->canVal / timeDuration,
271 deltaT / timeDuration,
272 targetJointState);
273
274 if (targetJointState.size() == jointNames.size())
275 {
276 for (size_t i = 0; i < targetJointState.size(); ++i)
277 {
278 desiredNullSpaceJointValues(i) = targetJointState[i];
279 }
280 }
281 else
282 {
283 desiredNullSpaceJointValues = defaultNullSpaceJointValues;
284 }
285 }
286 else
287 {
288 desiredNullSpaceJointValues = defaultNullSpaceJointValues;
289 }
290
291 LockGuardType guard{controlDataMutex};
292 getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
293 getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
294 getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
295 getWriterControlStruct().canVal = dmpCtrl->canVal;
296 getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
297
298 writeControlStruct();
299 }
300 }
301 }
302
303 void
304 NJointTaskSpaceAdaptiveDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
305 const IceUtil::Time& timeSinceLastIteration)
306 {
307 double deltaT = timeSinceLastIteration.toSecondsDouble();
308 Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
309
310 Eigen::MatrixXf jacobi =
311 ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
312
313 Eigen::VectorXf qpos(positionSensors.size());
314 Eigen::VectorXf qvel(velocitySensors.size());
315 for (size_t i = 0; i < positionSensors.size(); ++i)
316 {
317 qpos(i) = positionSensors[i]->position;
318 qvel(i) = velocitySensors[i]->velocity;
319 }
320
321 Eigen::VectorXf currentTwist = jacobi * qvel;
322
323 controllerSensorData.getWriteBuffer().currentPose = currentPose;
324 controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
325 controllerSensorData.getWriteBuffer().deltaT = deltaT;
326 controllerSensorData.getWriteBuffer().currentTime += deltaT;
327 controllerSensorData.commitWrite();
328
329
330 Eigen::Matrix4f targetPose;
331 Eigen::VectorXf targetVel;
332 Eigen::VectorXf desiredNullSpaceJointValues;
333 if (firstRun || !started)
334 {
335 firstRun = false;
336 targetPose = currentPose;
337 targetVel.setZero(6);
338 desiredNullSpaceJointValues = defaultNullSpaceJointValues;
339 forceOffset = 0.1 * forceOffset + 0.9 * forceSensor->force;
340 }
341 else
342 {
343 filteredForce = (1 - cfg->filterCoeff) * filteredForce +
344 cfg->filterCoeff * (forceSensor->force - forceOffset);
345 targetPose = rtGetControlStruct().targetPose;
346 targetVel = rtGetControlStruct().targetVel;
347 desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
348 }
349
350 interfaceData.getWriteBuffer().currentTcpPose = currentPose;
351 interfaceData.getWriteBuffer().currentForce = filteredForce;
352 interfaceData.getWriteBuffer().currentVel = currentTwist;
353 interfaceData.commitWrite();
354
355
356 kpos = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.head<3>();
357 kori = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance.tail<3>();
358 dpos = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.head<3>();
359 dori = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance.tail<3>();
360 knull = interface2rtBuffer.getUpToDateReadBuffer().Knull;
361 dnull = interface2rtBuffer.getUpToDateReadBuffer().Dnull;
362
363 /* calculate torques in meter */
364 jacobi.block(0, 0, 3, numOfJoints) =
365 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
366 Eigen::Vector6f jointControlWrench;
367 {
368 Eigen::Vector3f targetTCPLinearVelocity;
369 targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1),
370 0.001 * targetVel(2);
371 Eigen::Vector3f currentTCPLinearVelocity;
372 currentTCPLinearVelocity << 0.001 * currentTwist(0), 0.001 * currentTwist(1),
373 0.001 * currentTwist(2);
374 Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
375 Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
376 Eigen::Vector3f tcpDesiredForce =
377 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
378 dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
379
380 Eigen::Vector3f currentTCPAngularVelocity;
381 currentTCPAngularVelocity << currentTwist(3), currentTwist(4), currentTwist(5);
382 Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
383 Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
384 Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
385 Eigen::Vector3f tcpDesiredTorque =
386 kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
387 jointControlWrench << tcpDesiredForce, tcpDesiredTorque;
388 }
389
390 Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
391
392 Eigen::VectorXf nullspaceTorque =
393 knull.cwiseProduct(desiredNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
394 Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
395 Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * jointControlWrench +
396 (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
397
398 // torque limit
399 ARMARX_CHECK_EXPRESSION(!targets.empty());
400 ARMARX_CHECK_LESS(targets.size(), 1000);
401 for (size_t i = 0; i < targets.size(); ++i)
402 {
403 float desiredTorque = jointDesiredTorques(i);
404
405 if (isnan(desiredTorque))
406 {
407 desiredTorque = 0;
408 }
409
410 desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
411 desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
412
413 debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] =
414 jointDesiredTorques(i);
415 debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] =
416 desiredNullSpaceJointValues(i);
417
418 targets.at(i)->torque = desiredTorque;
419 if (!targets.at(i)->isValid())
420 {
421 ARMARX_INFO << deactivateSpam(1)
422 << "Torque controller target is invalid - setting to zero! set value: "
423 << targets.at(i)->torque;
424 targets.at(i)->torque = 0.0f;
425 }
426 }
427
428
429 debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
430 debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
431 debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
432 debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
433 debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
434 debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
435
436 debugOutputData.getWriteBuffer().impedanceKp_x = kpos(0);
437 debugOutputData.getWriteBuffer().impedanceKp_y = kpos(1);
438 debugOutputData.getWriteBuffer().impedanceKp_z = kpos(2);
439 debugOutputData.getWriteBuffer().impedanceKp_rx = kori(0);
440 debugOutputData.getWriteBuffer().impedanceKp_ry = kori(1);
441 debugOutputData.getWriteBuffer().impedanceKp_rz = kori(2);
442
443 debugOutputData.getWriteBuffer().forceInRoot_x = filteredForce(0);
444 debugOutputData.getWriteBuffer().forceInRoot_y = filteredForce(1);
445 debugOutputData.getWriteBuffer().forceInRoot_z = filteredForce(2);
446 // debugOutputData.getWriteBuffer().torqueInRoot_x = filteredForce(3);
447 // debugOutputData.getWriteBuffer().torqueInRoot_y = filteredForce(4);
448 // debugOutputData.getWriteBuffer().torqueInRoot_z = filteredForce(5);
449
450 debugOutputData.getWriteBuffer().vel_x = currentTwist(0);
451 debugOutputData.getWriteBuffer().vel_y = currentTwist(1);
452 debugOutputData.getWriteBuffer().vel_z = currentTwist(2);
453
454 // debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
455 // debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
456
457 debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
458 debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
459 debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
460 VirtualRobot::MathTools::Quaternion targetQuat =
461 VirtualRobot::MathTools::eigen4f2quat(targetPose);
462 debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
463 debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
464 debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
465 debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
466
467 debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
468 debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
469 debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
470 VirtualRobot::MathTools::Quaternion currentQuat =
471 VirtualRobot::MathTools::eigen4f2quat(currentPose);
472 debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
473 debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
474 debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
475 debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
476 debugOutputData.getWriteBuffer().deltaT = deltaT;
477
478 debugOutputData.commitWrite();
479 }
480
481 void
482 NJointTaskSpaceAdaptiveDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
483 const Ice::Current&)
484 {
485 dmpCtrl->learnDMPFromFiles(fileNames);
486 ARMARX_INFO << "Learned DMP ... ";
487 }
488
489 void
490 NJointTaskSpaceAdaptiveDMPController::setViaPoints(Ice::Double u,
491 const Ice::DoubleSeq& viapoint,
492 const Ice::Current&)
493 {
494 LockGuardType guard(controllerMutex);
495 ARMARX_INFO << "setting via points ";
496 dmpCtrl->setViaPose(u, viapoint);
497 }
498
499 void
500 NJointTaskSpaceAdaptiveDMPController::setGoals(const Ice::DoubleSeq& goals,
501 const Ice::Current& ice)
502 {
503 dmpCtrl->setGoalPoseVec(goals);
504 }
505
506 void
507 NJointTaskSpaceAdaptiveDMPController::setCanVal(double canVal, const Ice::Current&)
508 {
509 LockGuardType guard(int2ctrlMutex);
510 interface2CtrlBuffer.getWriteBuffer().canVal = canVal;
511 interface2CtrlBuffer.commitWrite();
512 }
513
514 void
515 NJointTaskSpaceAdaptiveDMPController::setUseNullSpaceJointDMP(bool useJointDMP,
516 const Ice::Current&)
517 {
518 useNullSpaceJointDMP = useJointDMP;
519 }
520
521 void
522 NJointTaskSpaceAdaptiveDMPController::setDefaultJointValues(
523 const Ice::FloatSeq& desiredJointVals,
524 const Ice::Current&)
525 {
526 ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
527
528 for (size_t i = 0; i < targets.size(); ++i)
529 {
530 defaultNullSpaceJointValues(i) = desiredJointVals.at(i);
531 }
532 }
533
534 void
535 NJointTaskSpaceAdaptiveDMPController::learnJointDMPFromFiles(const std::string& fileName,
536 const Ice::FloatSeq& currentJVS,
537 const Ice::Current&)
538 {
539 DMP::Vec<DMP::SampledTrajectoryV2> trajs;
540 DMP::DVec ratios;
541 DMP::SampledTrajectoryV2 traj;
542 traj.readFromCSVFile(fileName);
543 traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
544 if (traj.dim() != jointNames.size())
545 {
546 isNullSpaceJointDMPLearned = false;
547 return;
548 }
549
550 DMP::DVec goal;
551 goal.resize(traj.dim());
552 currentJointState.resize(traj.dim());
553
554 for (size_t i = 0; i < goal.size(); ++i)
555 {
556 goal.at(i) = traj.rbegin()->getPosition(i);
557 currentJointState.at(i).pos = currentJVS.at(i);
558 currentJointState.at(i).vel = 0;
559 }
560
561 trajs.push_back(traj);
562 nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
563
564 // prepare exeuction of joint dmp
565 nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
566 ARMARX_INFO << "prepared nullspace joint dmp";
567 isNullSpaceJointDMPLearned = true;
568 }
569
570 void
571 NJointTaskSpaceAdaptiveDMPController::resetDMP(const Ice::Current&)
572 {
573 if (started)
574 {
575 ARMARX_INFO << "Cannot reset running DMP";
576 }
577 firstRun = true;
578 }
579
580 void
581 NJointTaskSpaceAdaptiveDMPController::setKdImpedance(const Ice::FloatSeq& value,
582 const Ice::Current&)
583 {
584 Eigen::VectorXf setpoint;
585 setpoint.setZero(value.size());
586 ARMARX_CHECK_EQUAL(value.size(), 6);
587
588 for (size_t i = 0; i < value.size(); ++i)
589 {
590 setpoint(i) = value.at(i);
591 }
592
593 LockGuardType guard{interfaceDataMutex};
594 interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
595 interface2rtBuffer.commitWrite();
596 }
597
598 void
599 NJointTaskSpaceAdaptiveDMPController::setKpImpedance(const Ice::FloatSeq& value,
600 const Ice::Current&)
601 {
602 Eigen::VectorXf setpoint;
603 setpoint.setZero(value.size());
604 ARMARX_CHECK_EQUAL(value.size(), 6);
605
606 for (size_t i = 0; i < value.size(); ++i)
607 {
608 setpoint(i) = value.at(i);
609 }
610
611 LockGuardType guard{interfaceDataMutex};
612 interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
613 interface2rtBuffer.commitWrite();
614 }
615
616 void
617 NJointTaskSpaceAdaptiveDMPController::setKpNull(const Ice::FloatSeq& value, const Ice::Current&)
618 {
619 Eigen::VectorXf setpoint;
620 setpoint.setZero(value.size());
621 ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
622
623 for (size_t i = 0; i < value.size(); ++i)
624 {
625 setpoint(i) = value.at(i);
626 }
627
628 LockGuardType guard{interfaceDataMutex};
629 interface2rtBuffer.getWriteBuffer().Knull = setpoint;
630 interface2rtBuffer.commitWrite();
631 }
632
633 void
634 NJointTaskSpaceAdaptiveDMPController::setKdNull(const Ice::FloatSeq& value, const Ice::Current&)
635 {
636 Eigen::VectorXf setpoint;
637 setpoint.setZero(value.size());
638 ARMARX_CHECK_EQUAL(value.size(), rns->getSize());
639
640 for (size_t i = 0; i < value.size(); ++i)
641 {
642 setpoint(i) = value.at(i);
643 }
644
645 LockGuardType guard{interfaceDataMutex};
646 interface2rtBuffer.getWriteBuffer().Dnull = setpoint;
647 interface2rtBuffer.commitWrite();
648 }
649
650 Ice::FloatSeq
651 NJointTaskSpaceAdaptiveDMPController::getForce(const Ice::Current&)
652 {
653 Eigen::Vector3f force = interfaceData.getUpToDateReadBuffer().currentForce;
654 std::vector<float> forceVec = {force(0), force(1), force(2)};
655 return forceVec;
656 }
657
658 Ice::FloatSeq
659 NJointTaskSpaceAdaptiveDMPController::getVelocityInMM(const Ice::Current&)
660 {
661 Eigen::VectorXf currentVel = interfaceData.getUpToDateReadBuffer().currentVel;
662 std::vector<float> tvelvec = {currentVel(0),
663 currentVel(1),
664 currentVel(2),
665 currentVel(3),
666 currentVel(4),
667 currentVel(5)};
668 return tvelvec;
669 }
670
671 void
672 NJointTaskSpaceAdaptiveDMPController::stopDMP(const Ice::Current&)
673 {
674 oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
675 stopped = true;
676 }
677
678 void
679 NJointTaskSpaceAdaptiveDMPController::resumeDMP(const Ice::Current&)
680 {
681 stopped = false;
682 }
683
684 void
685 NJointTaskSpaceAdaptiveDMPController::removeAllViaPoints(const Ice::Current&)
686 {
687 LockGuardType guard{controllerMutex};
688 dmpCtrl->removeAllViaPoints();
689 }
690
691 void
692 NJointTaskSpaceAdaptiveDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
693 Ice::Double timeDuration,
694 const Ice::Current&)
695 {
696 firstRun = true;
697 while (firstRun)
698 {
699 usleep(100);
700 }
701
702 Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
703 dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
704
705 dmpCtrl->canVal = timeDuration;
706 dmpCtrl->config.motionTimeDuration = timeDuration;
707
708 finished = false;
709
710 if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
711 {
712 ARMARX_INFO << "Using Null Space Joint DMP";
713 }
714
715 started = true;
716 stopped = false;
717 // controllerTask->start();
718 }
719
720 void
721 NJointTaskSpaceAdaptiveDMPController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
722 {
723 firstRun = true;
724 while (firstRun)
725 {
726 usleep(100);
727 }
728
729 Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
730 dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
731
732 finished = false;
733
734 if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
735 {
736 ARMARX_INFO << "Using Null Space Joint DMP";
737 }
738
739 started = true;
740 stopped = false;
741 // controllerTask->start();
742 }
743
744 void
745 NJointTaskSpaceAdaptiveDMPController::onPublish(const SensorAndControl&,
746 const DebugDrawerInterfacePrx&,
747 const DebugObserverInterfacePrx& debugObs)
748 {
749 StringVariantBaseMap datafields;
750 auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
751 for (auto& pair : values)
752 {
753 datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
754 }
755
756 auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
757 for (auto& pair : values_null)
758 {
759 datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
760 }
761
762 datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
763 datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
764 datafields["targetPose_x"] =
765 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
766 datafields["targetPose_y"] =
767 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
768 datafields["targetPose_z"] =
769 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
770 datafields["targetPose_qw"] =
771 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
772 datafields["targetPose_qx"] =
773 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
774 datafields["targetPose_qy"] =
775 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
776 datafields["targetPose_qz"] =
777 new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
778
779 datafields["impedanceKp_x"] =
780 new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_x);
781 datafields["impedanceKp_y"] =
782 new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_y);
783 datafields["impedanceKp_z"] =
784 new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_z);
785 datafields["impedanceKp_rx"] =
786 new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rx);
787 datafields["impedanceKp_ry"] =
788 new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_ry);
789 datafields["impedanceKp_rz"] =
790 new Variant(debugOutputData.getUpToDateReadBuffer().impedanceKp_rz);
791
792 datafields["currentPose_x"] =
793 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
794 datafields["currentPose_y"] =
795 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
796 datafields["currentPose_z"] =
797 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
798 datafields["currentPose_qw"] =
799 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
800 datafields["currentPose_qx"] =
801 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
802 datafields["currentPose_qy"] =
803 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
804 datafields["currentPose_qz"] =
805 new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
806
807 datafields["forceDesired_x"] =
808 new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
809 datafields["forceDesired_y"] =
810 new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
811 datafields["forceDesired_z"] =
812 new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
813 datafields["forceDesired_rx"] =
814 new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
815 datafields["forceDesired_ry"] =
816 new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
817 datafields["forceDesired_rz"] =
818 new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
819
820 datafields["forceInRoot_x"] =
821 new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_x);
822 datafields["forceInRoot_y"] =
823 new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_y);
824 datafields["forceInRoot_z"] =
825 new Variant(debugOutputData.getUpToDateReadBuffer().forceInRoot_z);
826
827 datafields["vel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_x);
828 datafields["vel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_y);
829 datafields["vel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().vel_z);
830
831 datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
832
833 std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
834 debugObs->setDebugChannel(channelName, datafields);
835 }
836
837 void
838 NJointTaskSpaceAdaptiveDMPController::onInitNJointController()
839 {
840 ARMARX_INFO << "init ...";
841 // controllerTask = new PeriodicTask<NJointTaskSpaceAdaptiveDMPController>(this, &NJointTaskSpaceAdaptiveDMPController::controllerRun, 1);
842 runTask("NJointTaskSpaceAdaptiveDMPController",
843 [&]
844 {
845 CycleUtil c(1);
846 getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
847 while (getState() == eManagedIceObjectStarted)
848 {
849 if (isControllerActive())
850 {
851 controllerRun();
852 }
853 c.waitForCycleDuration();
854 }
855 });
856 }
857
858 void
859 NJointTaskSpaceAdaptiveDMPController::onDisconnectNJointController()
860 {
861 // controllerTask->stop();
862 }
863
864
865} // namespace armarx::control::deprecated_njoint_mp_controller::adaptive
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition CycleUtil.h:41
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
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition ManagedIceObject.cpp:776
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition ManagedIceObject.cpp:769
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition NJointControllerBase.h:778
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
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::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition NJointController.cpp:293
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition NJointControllerBase.h:846
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::SensorValueBase
The SensorValueBase class.
Definition SensorValueBase.h:41
armarx::SensorValueBase::asA
const T * asA() const
Definition SensorValueBase.h:82
armarx::Variant
The Variant class is described here: Variants.
Definition Variant.h:224
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPControllerControlData
Definition NJointTaskSpaceAdaptiveDMPController.h:38
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPControllerControlData::targetVel
Eigen::VectorXf targetVel
Definition NJointTaskSpaceAdaptiveDMPController.h:40
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPControllerControlData::desiredNullSpaceJointValues
Eigen::VectorXf desiredNullSpaceJointValues
Definition NJointTaskSpaceAdaptiveDMPController.h:42
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPControllerControlData::targetPose
Eigen::Matrix4f targetPose
Definition NJointTaskSpaceAdaptiveDMPController.h:41
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::onDisconnectNJointController
void onDisconnectNJointController()
Definition NJointTaskSpaceAdaptiveDMPController.cpp:859
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::learnJointDMPFromFiles
void learnJointDMPFromFiles(const std::string &fileName, const Ice::FloatSeq &currentJVS, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:535
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:482
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition NJointTaskSpaceAdaptiveDMPController.cpp:193
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:500
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::runDMPWithTime
void runDMPWithTime(const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:692
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setUseNullSpaceJointDMP
void setUseNullSpaceJointDMP(bool useJointDMP, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:515
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition NJointTaskSpaceAdaptiveDMPController.cpp:304
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::getClassName
std::string getClassName(const Ice::Current &) const
Definition NJointTaskSpaceAdaptiveDMPController.cpp:187
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::controllerRun
void controllerRun()
Definition NJointTaskSpaceAdaptiveDMPController.cpp:204
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setDefaultJointValues
void setDefaultJointValues(const Ice::FloatSeq &desiredJointVals, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:522
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setCanVal
void setCanVal(double canVal, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:507
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::onInitNJointController
void onInitNJointController()
Definition NJointTaskSpaceAdaptiveDMPController.cpp:838
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setKpImpedance
void setKpImpedance(const Ice::FloatSeq &stiffness, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:599
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setKdNull
void setKdNull(const Ice::FloatSeq &dnull, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:634
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setKdImpedance
void setKdImpedance(const Ice::FloatSeq &dampings, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:581
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::NJointTaskSpaceAdaptiveDMPController
NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:28
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:745
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:490
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::setKpNull
void setKpNull(const Ice::FloatSeq &knull, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:617
armarx::control::deprecated_njoint_mp_controller::adaptive::NJointTaskSpaceAdaptiveDMPController::runDMP
void runDMP(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition NJointTaskSpaceAdaptiveDMPController.cpp:721
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition TaskSpaceVMP.h:91
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_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
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_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
Eigen::Vector6f
Matrix< float, 6, 1 > Vector6f
Definition CartesianNaturalPositionController.h:40
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::deprecated_njoint_mp_controller::adaptive
Definition NJointAdaptiveWipingController.cpp:22
armarx::control::deprecated_njoint_mp_controller::adaptive::registrationControllerNJointTaskSpaceAdaptiveDMPController
NJointControllerRegistration< NJointTaskSpaceAdaptiveDMPController > registrationControllerNJointTaskSpaceAdaptiveDMPController("NJointTaskSpaceAdaptiveDMPController")
armarx::DebugObserverInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition JointController.h:44
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition ManagedIceObject.h:110
armarx::RobotUnitPtr
IceUtil::Handle< class RobotUnit > RobotUnitPtr
Definition FTSensor.h:34
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition JointController.h:42
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition NJointControllerBase.h:73
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition TaskSpaceVMP.h:52
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition TaskSpaceVMP.h:51
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition TaskSpaceVMP.h:53
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition TaskSpaceVMP.h:48
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition TaskSpaceVMP.h:50
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition TaskSpaceVMP.h:55
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition TaskSpaceVMP.h:56
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition TaskSpaceVMP.h:60
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition TaskSpaceVMP.h:64
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition TaskSpaceVMP.h:66
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition TaskSpaceVMP.h:61
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition TaskSpaceVMP.h:67
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition TaskSpaceVMP.h:63