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