NJointBimanualCCDMPVelocityController.cpp
Go to the documentation of this file.
1#include "NJointBimanualCCDMPVelocityController.h"
2
3#include <ArmarXCore/core/ArmarXObjectScheduler.h>
4#include <ArmarXCore/core/system/ArmarXDataPath.h>
5#include <ArmarXCore/core/time/CycleUtil.h>
6
7// Simox
8#include <VirtualRobot/IK/DifferentialIK.h>
9#include <VirtualRobot/MathTools.h>
10#include <VirtualRobot/Nodes/RobotNode.h>
11#include <VirtualRobot/Robot.h>
12#include <VirtualRobot/RobotNodeSet.h>
13
14// DMP
15#include <dmp/general/simoxhelpers.h>
16#include <dmp/representation/dmp/umidmp.h>
17#include <dmp/representation/dmp/umitsmp.h>
18
19// armarx
20#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
21#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
22#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
23#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
24#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
25#include <RobotAPI/libraries/core/CartesianVelocityController.h>
26#include <RobotAPI/libraries/core/PIDController.h>
27#include <RobotAPI/libraries/core/math/MathUtils.h>
28
29namespace armarx::control::deprecated_njoint_mp_controller::bimanual
30{
31 NJointControllerRegistration<NJointBimanualCCDMPVelocityController>
32 registrationControllerNJointBimanualCCDMPVelocityController(
33 "NJointBimanualCCDMPVelocityController");
34
35 NJointBimanualCCDMPVelocityController::NJointBimanualCCDMPVelocityController(
36 const RobotUnitPtr& robotUnit,
37 const armarx::NJointControllerConfigPtr& config,
38 const VirtualRobot::RobotPtr&)
39 {
40 ARMARX_INFO << "Preparing ... ";
41 cfg = NJointBimanualCCDMPVelocityControllerConfigPtr::dynamicCast(config);
42
43 ARMARX_CHECK_EXPRESSION(robotUnit);
44 useSynchronizedRtRobot();
45
46 leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
47 leftNullSpaceCoefs.resize(leftRNS->getSize());
48 leftNullSpaceCoefs.setOnes();
49
50 for (size_t i = 0; i < leftRNS->getSize(); ++i)
51 {
52 std::string jointName = leftRNS->getNode(i)->getName();
53
54 if (leftRNS->getNode(i)->isLimitless())
55 {
56 leftNullSpaceCoefs(i) = 1;
57 }
58
59 leftJointNames.push_back(jointName);
60 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
61 const SensorValueBase* sv = useSensorValue(jointName);
62 leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
63 const SensorValue1DoFActuatorVelocity* velocitySensor =
64 sv->asA<SensorValue1DoFActuatorVelocity>();
65 const SensorValue1DoFActuatorPosition* positionSensor =
66 sv->asA<SensorValue1DoFActuatorPosition>();
67 const SensorValue1DoFActuatorAcceleration* accelerationSensor =
68 sv->asA<SensorValue1DoFActuatorAcceleration>();
69
70 if (!velocitySensor)
71 {
72 ARMARX_WARNING << "No velocitySensor available for " << jointName;
73 }
74 if (!positionSensor)
75 {
76 ARMARX_WARNING << "No positionSensor available for " << jointName;
77 }
78
79 if (!accelerationSensor)
80 {
81 ARMARX_WARNING << "No accelerationSensor available for " << jointName;
82 }
83
84
85 leftVelocitySensors.push_back(velocitySensor);
86 leftPositionSensors.push_back(positionSensor);
87 leftAccelerationSensors.push_back(accelerationSensor);
88 };
89 rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
90
91 rightNullSpaceCoefs.resize(rightRNS->getSize());
92 rightNullSpaceCoefs.setOnes();
93 for (size_t i = 0; i < rightRNS->getSize(); ++i)
94 {
95 std::string jointName = rightRNS->getNode(i)->getName();
96
97 if (rightRNS->getNode(i)->isLimitless())
98 {
99 rightNullSpaceCoefs(i) = 1;
100 }
101
102 rightJointNames.push_back(jointName);
103 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
104 const SensorValueBase* sv = useSensorValue(jointName);
105 rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
106 const SensorValue1DoFActuatorVelocity* velocitySensor =
107 sv->asA<SensorValue1DoFActuatorVelocity>();
108 const SensorValue1DoFActuatorPosition* positionSensor =
109 sv->asA<SensorValue1DoFActuatorPosition>();
110 const SensorValue1DoFActuatorAcceleration* accelerationSensor =
111 sv->asA<SensorValue1DoFActuatorAcceleration>();
112
113
114 if (!velocitySensor)
115 {
116 ARMARX_WARNING << "No velocitySensor available for " << jointName;
117 }
118 if (!positionSensor)
119 {
120 ARMARX_WARNING << "No positionSensor available for " << jointName;
121 }
122 if (!accelerationSensor)
123 {
124 ARMARX_WARNING << "No accelerationSensor available for " << jointName;
125 }
126
127 rightVelocitySensors.push_back(velocitySensor);
128 rightPositionSensors.push_back(positionSensor);
129 rightAccelerationSensors.push_back(accelerationSensor);
130 };
131
132
133 const SensorValueBase* svlf = useSensorValue("FT L");
134 leftForceTorque = svlf->asA<SensorValueForceTorque>();
135 const SensorValueBase* svrf = useSensorValue("FT R");
136 rightForceTorque = svrf->asA<SensorValueForceTorque>();
137
138 leftIK.reset(new VirtualRobot::DifferentialIK(
139 leftRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
140 rightIK.reset(new VirtualRobot::DifferentialIK(
141 rightRNS, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
142
143
144 leftCtrl.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
145 rightCtrl.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
146
147 tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
148 taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
149 taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
150 taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
151 taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
152 taskSpaceDMPConfig.DMPAmplitude = 1.0;
153
154
155 tsvmp::TaskSpaceDMPControllerPtr leftLeader(
156 new tsvmp::TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig, false));
157 tsvmp::TaskSpaceDMPControllerPtr leftFollower(
158 new tsvmp::TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig, false));
159 tsvmp::TaskSpaceDMPControllerPtr rightLeader(
160 new tsvmp::TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig, false));
161 tsvmp::TaskSpaceDMPControllerPtr rightFollower(
162 new tsvmp::TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig, false));
163 leftJointDMP.reset(new DMP::UMIDMP(10));
164 rightJointDMP.reset(new DMP::UMIDMP(10));
165 isLeftJointLearned = false;
166 isRightJointLearned = false;
167
168 started = false;
169 isDMPRun = false;
170
171 leftGroup.push_back(leftLeader);
172 leftGroup.push_back(rightFollower);
173
174 rightGroup.push_back(rightLeader);
175 rightGroup.push_back(leftFollower);
176
177 bothLeaderGroup.push_back(leftLeader);
178 bothLeaderGroup.push_back(rightLeader);
179
180
181 tcpLeft = leftRNS->getTCP();
182 tcpRight = rightRNS->getTCP();
183
184
185 leaderName = cfg->defautLeader;
186
187
188 leftDesiredJointValues.resize(leftTargets.size());
189 ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
190
191 for (size_t i = 0; i < leftTargets.size(); ++i)
192 {
193 leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
194 }
195
196 rightDesiredJointValues.resize(rightTargets.size());
197 ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
198
199 for (size_t i = 0; i < rightTargets.size(); ++i)
200 {
201 rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
202 }
203
204
205 leftKpos << cfg->leftKpos[0], cfg->leftKpos[1], cfg->leftKpos[2];
206 leftDpos << cfg->leftDpos[0], cfg->leftDpos[1], cfg->leftDpos[2];
207 leftKori << cfg->leftKori[0], cfg->leftKori[1], cfg->leftKori[2];
208 leftDori << cfg->leftDori[0], cfg->leftDori[1], cfg->leftDori[2];
209
210 rightKpos << cfg->rightKpos[0], cfg->rightKpos[1], cfg->rightKpos[2];
211 rightDpos << cfg->rightDpos[0], cfg->rightDpos[1], cfg->rightDpos[2];
212 rightKori << cfg->rightKori[0], cfg->rightKori[1], cfg->rightKori[2];
213 rightDori << cfg->rightDori[0], cfg->rightDori[1], cfg->rightDori[2];
214
215 knull = cfg->knull;
216 dnull = cfg->dnull;
217
218
219 timeDuration = cfg->timeDuration;
220
221 maxLinearVel = cfg->maxLinearVel;
222 maxAngularVel = cfg->maxAngularVel;
223
224 NJointBimanualCCDMPVelocityControllerInterfaceData initInterfaceData;
225 initInterfaceData.currentLeftPose = Eigen::Matrix4f::Identity();
226 initInterfaceData.currentRightPose = Eigen::Matrix4f::Identity();
227 initInterfaceData.currentLeftJointVals.setZero(leftTargets.size());
228 initInterfaceData.currentRightJointVals.setZero(rightTargets.size());
229 interfaceData.reinitAllBuffers(initInterfaceData);
230
231 NJointBimanualCCDMPVelocityControllerSensorData initSensorData;
232 initSensorData.deltaT = 0;
233 initSensorData.currentTime = 0;
234 initSensorData.currentLeftPose = Eigen::Matrix4f::Identity();
235 initSensorData.currentRightPose = Eigen::Matrix4f::Identity();
236 controllerSensorData.reinitAllBuffers(initSensorData);
237 }
238
239 std::string
240 NJointBimanualCCDMPVelocityController::getClassName(const Ice::Current&) const
241 {
242 return "NJointBimanualCCDMPVelocityController";
243 }
244
245 void
246 NJointBimanualCCDMPVelocityController::rtPreActivateController()
247 {
248 NJointBimanualCCDMPVelocityControllerControlData initData;
249 initData.leftTargetVel.resize(6);
250 initData.leftTargetVel.setZero();
251 initData.rightTargetVel.resize(6);
252 initData.rightTargetVel.setZero();
253 initData.leftTargetPose = tcpLeft->getPoseInRootFrame();
254 initData.rightTargetPose = tcpRight->getPoseInRootFrame();
255 initData.virtualTime = cfg->timeDuration;
256 reinitTripleBuffer(initData);
257 }
258
259 void
260 NJointBimanualCCDMPVelocityController::controllerRun()
261 {
262 if (!started)
263 {
264 return;
265 }
266
267 if (!controllerSensorData.updateReadBuffer())
268 {
269 return;
270 }
271 double deltaT = controllerSensorData.getReadBuffer().deltaT;
272
273
274 Eigen::VectorXf leftTargetVel;
275 leftTargetVel.resize(6);
276 leftTargetVel.setZero();
277 Eigen::VectorXf rightTargetVel;
278 rightTargetVel.resize(6);
279 rightTargetVel.setZero();
280 Eigen::Matrix4f leftTargetPose;
281 Eigen::Matrix4f rightTargetPose;
282
283 std::vector<tsvmp::TaskSpaceDMPControllerPtr> currentControlGroup;
284 Eigen::Matrix4f currentLeaderPose;
285 Eigen::Matrix4f currentFollowerPose;
286 Eigen::VectorXf currentLeaderTwist;
287 Eigen::VectorXf currentFollowerTwist;
288
289
290 // get desired joint values
291 if (leaderName == "Both")
292 {
293 currentControlGroup = bothLeaderGroup;
294
295 tsvmp::TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
296 tsvmp::TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
297 leaderDMPleft->flow(deltaT,
298 controllerSensorData.getReadBuffer().currentLeftPose,
299 controllerSensorData.getReadBuffer().currentLeftTwist);
300 leaderDMPright->flow(deltaT,
301 controllerSensorData.getReadBuffer().currentRightPose,
302 controllerSensorData.getReadBuffer().currentRightTwist);
303
304 leftTargetVel = leaderDMPleft->getTargetVelocity();
305 leftTargetPose = leaderDMPleft->getTargetPoseMat();
306 rightTargetVel = leaderDMPright->getTargetVelocity();
307 rightTargetPose = leaderDMPright->getTargetPoseMat();
308
309 virtualtimer = leaderDMPleft->canVal;
310 }
311 else
312 {
313 if (leaderName == "Left")
314 {
315 currentControlGroup = leftGroup;
316 currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
317 currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
318 currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
319 currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
320 }
321 else if (leaderName == "Right")
322 {
323 currentControlGroup = rightGroup;
324 currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
325 currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
326 currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
327 currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
328 }
329
330 tsvmp::TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
331 tsvmp::TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
332 virtualtimer = leaderDMP->canVal;
333
334 if (virtualtimer < 1e-8)
335 {
336 finished = true;
337 started = false;
338 isDMPRun = false;
339 }
340
341
342 leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
343
344 Eigen::Matrix4f currentFollowerLocalPose;
345 currentFollowerLocalPose.block<3, 3>(0, 0) =
346 currentLeaderPose.block<3, 3>(0, 0).inverse() *
347 currentFollowerPose.block<3, 3>(0, 0);
348 currentFollowerLocalPose.block<3, 1>(0, 3) =
349 currentLeaderPose.block<3, 3>(0, 0).inverse() *
350 (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
351 followerDMP->flow(deltaT, currentFollowerLocalPose, currentFollowerTwist);
352
353 Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
354 Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
355 Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
356 std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
357
358 Eigen::Matrix4f followerTargetPose;
359 followerTargetPose.block<3, 3>(0, 0) =
360 currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 3>(0, 0);
361 followerTargetPose.block<3, 1>(0, 3) =
362 currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) +
363 currentLeaderPose.block<3, 1>(0, 3);
364
365
366 Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
367 Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
368 followerTargetVel.setZero();
369
370 std::vector<double> leftDMPTarget;
371 std::vector<double> rightDMPTarget;
372
373 if (leaderName == "Left")
374 {
375 leftTargetVel = leaderTargetVel;
376 rightTargetVel = followerTargetVel;
377 leftDMPTarget = leaderDMP->getTargetPose();
378 rightDMPTarget = followerLocalTargetPoseVec;
379 leftTargetPose = leaderTargetPose;
380 rightTargetPose = followerTargetPose;
381 }
382 else if (leaderName == "Right")
383 {
384 rightTargetVel = leaderTargetVel;
385 leftTargetVel = followerTargetVel;
386 rightDMPTarget = leaderDMP->getTargetPose();
387 leftDMPTarget = followerLocalTargetPoseVec;
388 rightTargetPose = leaderTargetPose;
389 leftTargetPose = followerTargetPose;
390 }
391 }
392
393
394 Eigen::VectorXf leftDesiredJoint = leftDesiredJointValues;
395 Eigen::VectorXf rightDesiredJoint = rightDesiredJointValues;
396 if (isLeftJointLearned)
397 {
398 DMP::DVec targetJointState;
399 currentLeftJointState =
400 leftJointDMP->calculateDirectlyVelocity(currentLeftJointState,
401 virtualtimer / timeDuration,
402 deltaT / timeDuration,
403 targetJointState);
404 for (size_t i = 0; i < targetJointState.size(); ++i)
405 {
406 leftDesiredJoint(i) = targetJointState[i];
407 }
408 }
409
410 if (isRightJointLearned)
411 {
412 DMP::DVec targetJointState;
413 currentRightJointState =
414 rightJointDMP->calculateDirectlyVelocity(currentRightJointState,
415 virtualtimer / timeDuration,
416 deltaT / timeDuration,
417 targetJointState);
418 for (size_t i = 0; i < targetJointState.size(); ++i)
419 {
420 rightDesiredJoint(i) = targetJointState[i];
421 }
422 }
423
424 LockGuardType guard{controlDataMutex};
425 getWriterControlStruct().leftTargetVel = leftTargetVel;
426 getWriterControlStruct().rightTargetVel = rightTargetVel;
427 getWriterControlStruct().leftTargetPose = leftTargetPose;
428 getWriterControlStruct().rightTargetPose = rightTargetPose;
429 getWriterControlStruct().leftDesiredJoint = leftDesiredJoint;
430 getWriterControlStruct().rightDesiredJoint = rightDesiredJoint;
431 getWriterControlStruct().virtualTime = virtualtimer;
432 writeControlStruct();
433 isDMPRun = true;
434 }
435
436 Eigen::VectorXf
437 NJointBimanualCCDMPVelocityController::getControlWrench(const Eigen::VectorXf& tcptwist,
438 const Eigen::Matrix4f& currentPose,
439 const Eigen::Matrix4f& targetPose)
440 {
441 return Eigen::Vector6f::Zero();
442 }
443
444 void
445 NJointBimanualCCDMPVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
446 const IceUtil::Time& timeSinceLastIteration)
447 {
448 double deltaT = timeSinceLastIteration.toSecondsDouble();
449
450 controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
451 controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
452 controllerSensorData.getWriteBuffer().deltaT = deltaT;
453 controllerSensorData.getWriteBuffer().currentTime += deltaT;
454
455 // cartesian vel controller
456 Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
457
458 Eigen::MatrixXf jacobiL =
459 leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
460
461 Eigen::VectorXf leftqpos;
462 Eigen::VectorXf leftqvel;
463 leftqpos.resize(leftPositionSensors.size());
464 leftqvel.resize(leftVelocitySensors.size());
465 for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
466 {
467 leftqpos(i) = leftPositionSensors[i]->position;
468 leftqvel(i) = leftVelocitySensors[i]->velocity;
469 }
470
471 Eigen::MatrixXf jacobiR =
472 rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
473 Eigen::VectorXf rightqpos;
474 Eigen::VectorXf rightqvel;
475 rightqpos.resize(rightPositionSensors.size());
476 rightqvel.resize(rightVelocitySensors.size());
477
478 for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
479 {
480 rightqpos(i) = rightPositionSensors[i]->position;
481 rightqvel(i) = rightVelocitySensors[i]->velocity;
482 }
483
484 Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
485 Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
486
487 controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
488 controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
489 controllerSensorData.commitWrite();
490
491
492 interfaceData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
493 interfaceData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
494 interfaceData.getWriteBuffer().currentLeftJointVals = leftqpos;
495 interfaceData.getWriteBuffer().currentRightJointVals = rightqpos;
496 interfaceData.commitWrite();
497
498 if (!isDMPRun)
499 {
500 for (size_t i = 0; i < leftTargets.size(); ++i)
501 {
502 leftTargets.at(i)->velocity = 0;
503 }
504 for (size_t i = 0; i < rightTargets.size(); ++i)
505 {
506 rightTargets.at(i)->velocity = 0;
507 }
508 }
509 else
510 {
511 Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
512 Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
513 double virtualtime = rtGetControlStruct().virtualTime;
514 Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
515 Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
516 Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
517 Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
518 Eigen::VectorXf leftDesiredJoint = rtGetControlStruct().leftDesiredJoint;
519 Eigen::VectorXf rightDesiredJoint = rtGetControlStruct().rightDesiredJoint;
520
521 // generate joint control signals
522 Eigen::VectorXf leftCartesianTarget(6);
523 {
524 Eigen::Vector3f targetTCPLinearVelocity;
525 targetTCPLinearVelocity << leftTargetVel(0), leftTargetVel(1), leftTargetVel(2);
526 Eigen::Vector3f currentTCPLinearVelocity;
527 currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1),
528 currentLeftTwist(2);
529 Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
530 Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
531 Eigen::Vector3f posVel =
532 leftKpos.cwiseProduct(desiredPosition - currentTCPPosition) +
533 leftDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
534
535
536 Eigen::Vector3f currentTCPAngularVelocity;
537 currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4),
538 currentLeftTwist(5);
539 Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
540 Eigen::Matrix3f diffMat =
541 leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
542 Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
543 Eigen::Vector3f oriVel =
544 leftKori.cwiseProduct(rpy) - leftDori.cwiseProduct(currentTCPAngularVelocity);
545
546 float normLinearVelocity = posVel.norm();
547 if (normLinearVelocity > maxLinearVel)
548 {
549 posVel = maxLinearVel * posVel / normLinearVelocity;
550 }
551 float normAngularVelocity = oriVel.norm();
552 if (normAngularVelocity > maxAngularVel)
553 {
554 oriVel = maxAngularVel * oriVel / normAngularVelocity;
555 }
556
557 leftCartesianTarget << posVel, oriVel;
558 }
559
560 Eigen::VectorXf rightCartesianTarget(6);
561 {
562 Eigen::Vector3f targetTCPLinearVelocity;
563 targetTCPLinearVelocity << rightTargetVel(0), rightTargetVel(1), rightTargetVel(2);
564 Eigen::Vector3f currentTCPLinearVelocity;
565 currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1),
566 currentRightTwist(2);
567 Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
568 Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
569 Eigen::Vector3f posVel =
570 rightKpos.cwiseProduct(desiredPosition - currentTCPPosition) +
571 rightDpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
572
573
574 Eigen::Vector3f currentTCPAngularVelocity;
575 currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4),
576 currentRightTwist(5);
577 Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
578 Eigen::Matrix3f diffMat =
579 rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
580 Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
581 Eigen::Vector3f oriVel =
582 rightKori.cwiseProduct(rpy) - rightDori.cwiseProduct(currentTCPAngularVelocity);
583
584 float normLinearVelocity = posVel.norm();
585 if (normLinearVelocity > maxLinearVel)
586 {
587 posVel = maxLinearVel * posVel / normLinearVelocity;
588 }
589 float normAngularVelocity = oriVel.norm();
590 if (normAngularVelocity > maxAngularVel)
591 {
592 oriVel = maxAngularVel * oriVel / normAngularVelocity;
593 }
594
595 rightCartesianTarget << posVel, oriVel;
596 }
597
598 Eigen::VectorXf leftNullspaceVel =
599 knull * (leftDesiredJoint - leftqpos) - dnull * leftqvel;
600 Eigen::VectorXf rightNullspaceVel =
601 knull * (rightDesiredJoint - rightqpos) - dnull * rightqvel;
602 Eigen::VectorXf desiredLeftVels =
603 leftCtrl->calculate(leftCartesianTarget,
604 leftNullspaceVel,
605 VirtualRobot::IKSolver::CartesianSelection::All);
606 Eigen::VectorXf desiredRightVels =
607 rightCtrl->calculate(rightCartesianTarget,
608 rightNullspaceVel,
609 VirtualRobot::IKSolver::CartesianSelection::All);
610
611 // torque limit
612 for (size_t i = 0; i < leftTargets.size(); ++i)
613 {
614
615 float desiredVel = desiredLeftVels[i];
616
617 if (fabs(desiredVel) > cfg->maxJointVel)
618 {
619 desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
620 }
621
622 leftTargets.at(i)->velocity = desiredVel;
623 debugDataInfo.getWriteBuffer().desired_velocities[leftJointNames[i]] = desiredVel;
624 }
625
626 for (size_t i = 0; i < rightTargets.size(); ++i)
627 {
628 float desiredVel = desiredRightVels[i];
629
630 if (fabs(desiredVel) > cfg->maxJointVel)
631 {
632 desiredVel = cfg->maxJointVel * desiredVel / fabs(desiredVel);
633 }
634
635 rightTargets.at(i)->velocity = desiredVel;
636 debugDataInfo.getWriteBuffer().desired_velocities[rightJointNames[i]] = desiredVel;
637 }
638
639
640 debugDataInfo.getWriteBuffer().leftControlSignal_x = leftCartesianTarget(0);
641 debugDataInfo.getWriteBuffer().leftControlSignal_y = leftCartesianTarget(1);
642 debugDataInfo.getWriteBuffer().leftControlSignal_z = leftCartesianTarget(2);
643 debugDataInfo.getWriteBuffer().leftControlSignal_ro = leftCartesianTarget(3);
644 debugDataInfo.getWriteBuffer().leftControlSignal_pi = leftCartesianTarget(4);
645 debugDataInfo.getWriteBuffer().leftControlSignal_ya = leftCartesianTarget(5);
646
647 debugDataInfo.getWriteBuffer().rightControlSignal_x = rightCartesianTarget(0);
648 debugDataInfo.getWriteBuffer().rightControlSignal_y = rightCartesianTarget(1);
649 debugDataInfo.getWriteBuffer().rightControlSignal_z = rightCartesianTarget(2);
650 debugDataInfo.getWriteBuffer().rightControlSignal_ro = rightCartesianTarget(3);
651 debugDataInfo.getWriteBuffer().rightControlSignal_pi = rightCartesianTarget(4);
652 debugDataInfo.getWriteBuffer().rightControlSignal_ya = rightCartesianTarget(5);
653
654 debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
655 debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
656 debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
657 debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
658 debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
659 debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
660
661
662 debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
663 debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
664 debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
665
666 debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
667 debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
668 debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
669 debugDataInfo.getWriteBuffer().virtualTime = virtualtime;
670
671 debugDataInfo.commitWrite();
672 }
673 }
674
675 void
676 NJointBimanualCCDMPVelocityController::learnDMPFromFiles(const std::string& name,
677 const Ice::StringSeq& fileNames,
678 const Ice::Current&)
679 {
680 if (name == "LeftLeader")
681 {
682 leftGroup.at(0)->learnDMPFromFiles(fileNames);
683 }
684 else if (name == "LeftFollower")
685 {
686 rightGroup.at(1)->learnDMPFromFiles(fileNames);
687 }
688 else if (name == "RightLeader")
689 {
690 rightGroup.at(0)->learnDMPFromFiles(fileNames);
691 }
692 else if (name == "RightFollower")
693 {
694 leftGroup.at(1)->learnDMPFromFiles(fileNames);
695 }
696 else if (name == "LeftJoint")
697 {
698 DMP::Vec<DMP::SampledTrajectoryV2> trajs;
699 DMP::SampledTrajectoryV2 traj;
700 std::string absPath;
701 ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
702 traj.readFromCSVFile(absPath);
703 traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
704 trajs.push_back(traj);
705 leftJointDMP->learnFromTrajectories(trajs);
706 }
707 else if (name == "RightJoint")
708 {
709 DMP::Vec<DMP::SampledTrajectoryV2> trajs;
710 DMP::SampledTrajectoryV2 traj;
711 std::string absPath;
712 ArmarXDataPath::getAbsolutePath(fileNames.at(0), absPath);
713 traj.readFromCSVFile(absPath);
714 traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
715 trajs.push_back(traj);
716 rightJointDMP->learnFromTrajectories(trajs);
717 }
718 else
719 {
720 ARMARX_ERROR
721 << "The given name is not supported by CCDMP, the supported names contain ";
722 }
723 }
724
725 void
726 NJointBimanualCCDMPVelocityController::learnDMPFromBothFiles(const Ice::StringSeq& leftFiles,
727 const Ice::StringSeq& rightFiles,
728 const Ice::Current&)
729 {
730 ARMARX_CHECK_EQUAL(leftFiles.size(), rightFiles.size());
731
732 DMP::Vec<DMP::SampledTrajectoryV2> leftFollowerTrajs;
733 DMP::Vec<DMP::SampledTrajectoryV2> rightFollowerTrajs;
734
735 for (size_t i = 0; i < leftFiles.size(); ++i)
736 {
737 DMP::SampledTrajectoryV2 leftFollowerTraj =
738 DMP::RobotHelpers::getRelativeTrajFromFiles(leftFiles[i], rightFiles[i]);
739 leftFollowerTrajs.push_back(leftFollowerTraj);
740 DMP::SampledTrajectoryV2 rightFollowerTraj =
741 DMP::RobotHelpers::getRelativeTrajFromFiles(rightFiles[i], leftFiles[i]);
742 rightFollowerTrajs.push_back(rightFollowerTraj);
743 }
744
745 leftGroup.at(0)->learnDMPFromFiles(leftFiles, cfg->initRatio);
746 leftGroup.at(1)->learnDMPFromSampledTrajectory(leftFollowerTrajs, cfg->initRatio);
747 rightGroup.at(0)->learnDMPFromFiles(rightFiles, cfg->initRatio);
748 rightGroup.at(1)->learnDMPFromSampledTrajectory(rightFollowerTrajs, cfg->initRatio);
749 }
750
751 void
752 NJointBimanualCCDMPVelocityController::setRatios(const Ice::DoubleSeq& ratios,
753 const Ice::Current&)
754 {
755 leftGroup.at(0)->setRatios(ratios);
756 leftGroup.at(1)->setRatios(ratios);
757 rightGroup.at(0)->setRatios(ratios);
758 rightGroup.at(1)->setRatios(ratios);
759 }
760
761 void
762 NJointBimanualCCDMPVelocityController::setViaPoints(Ice::Double u,
763 const Ice::DoubleSeq& viapoint,
764 const Ice::Current&)
765 {
766 LockGuardType guard(controllerMutex);
767 if (leaderName == "Left")
768 {
769 leftGroup.at(0)->setViaPose(u, viapoint);
770 }
771 else
772 {
773 rightGroup.at(0)->setViaPose(u, viapoint);
774 }
775 }
776
777 void
778 NJointBimanualCCDMPVelocityController::setGoals(const Ice::DoubleSeq& goals,
779 const Ice::Current& ice)
780 {
781 LockGuardType guard(controllerMutex);
782 if (leaderName == "Left")
783 {
784 leftGroup.at(0)->setGoalPoseVec(goals);
785 }
786 else
787 {
788 rightGroup.at(0)->setGoalPoseVec(goals);
789 }
790 }
791
792 void
793 NJointBimanualCCDMPVelocityController::changeLeader(const Ice::Current&)
794 {
795 LockGuardType guard(controllerMutex);
796
797 if (leaderName == "Left")
798 {
799 leaderName = "Right";
800 rightGroup.at(0)->canVal = virtualtimer;
801 rightGroup.at(1)->canVal = virtualtimer;
802 }
803 else
804 {
805 leaderName = "Left";
806 leftGroup.at(0)->canVal = virtualtimer;
807 leftGroup.at(1)->canVal = virtualtimer;
808 }
809 }
810
811 void
812 NJointBimanualCCDMPVelocityController::runDMP(const Ice::DoubleSeq& leftGoals,
813 const Ice::DoubleSeq& rightGoals,
814 const Ice::Current&)
815 {
816 ARMARX_INFO << "start running ccdmp";
817 while (!interfaceData.updateReadBuffer())
818 {
819 usleep(100);
820 }
821 Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
822 Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
823 Eigen::VectorXf leftJointVals = interfaceData.getReadBuffer().currentLeftJointVals;
824 Eigen::VectorXf rightJointVals = interfaceData.getReadBuffer().currentRightJointVals;
825
826 for (size_t i = 0; i < leftTargets.size(); ++i)
827 {
828 DMP::DMPState jstate;
829 jstate.pos = leftJointVals(i);
830 jstate.vel = 0;
831 currentLeftJointState.push_back(jstate);
832 }
833
834 for (size_t i = 0; i < rightTargets.size(); ++i)
835 {
836 DMP::DMPState jstate;
837 jstate.pos = rightJointVals(i);
838 jstate.vel = 0;
839 currentRightJointState.push_back(jstate);
840 }
841
842 virtualtimer = cfg->timeDuration;
843 leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
844 rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
845
846
847 ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
848
849 leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose),
850 getLocalPose(leftGoals, rightGoals));
851 rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose),
852 getLocalPose(rightGoals, leftGoals));
853
854 finished = false;
855 started = true;
856 }
857
858 Eigen::Matrix4f
859 NJointBimanualCCDMPVelocityController::getLocalPose(const Eigen::Matrix4f& newCoordinate,
860 const Eigen::Matrix4f& globalTargetPose)
861 {
862 Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
863
864 localPose.block<3, 3>(0, 0) =
865 newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
866 localPose.block<3, 1>(0, 3) =
867 newCoordinate.block<3, 3>(0, 0).inverse() *
868 (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
869
870
871 return localPose;
872 }
873
874 void
875 NJointBimanualCCDMPVelocityController::onPublish(const SensorAndControl&,
876 const DebugDrawerInterfacePrx&,
877 const DebugObserverInterfacePrx& debugObs)
878 {
879
880 StringVariantBaseMap datafields;
881 auto values = debugDataInfo.getUpToDateReadBuffer().desired_velocities;
882 for (auto& pair : values)
883 {
884 datafields[pair.first] = new Variant(pair.second);
885 }
886
887 auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
888 for (auto& pair : constrained_force)
889 {
890 datafields[pair.first] = new Variant(pair.second);
891 }
892
893
894 datafields["leftTargetPose_x"] =
895 new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
896 datafields["leftTargetPose_y"] =
897 new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
898 datafields["leftTargetPose_z"] =
899 new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
900 datafields["rightTargetPose_x"] =
901 new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
902 datafields["rightTargetPose_y"] =
903 new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
904 datafields["rightTargetPose_z"] =
905 new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
906
907
908 datafields["leftCurrentPose_x"] =
909 new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
910 datafields["leftCurrentPose_y"] =
911 new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
912 datafields["leftCurrentPose_z"] =
913 new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
914 datafields["rightCurrentPose_x"] =
915 new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
916 datafields["rightCurrentPose_y"] =
917 new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
918 datafields["rightCurrentPose_z"] =
919 new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
920
921 datafields["leftControlSignal_x"] =
922 new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_x);
923 datafields["leftControlSignal_y"] =
924 new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_y);
925 datafields["leftControlSignal_z"] =
926 new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_z);
927 datafields["leftControlSignal_ro"] =
928 new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ro);
929 datafields["leftControlSignal_pi"] =
930 new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_pi);
931 datafields["leftControlSignal_ya"] =
932 new Variant(debugDataInfo.getUpToDateReadBuffer().leftControlSignal_ya);
933
934
935 datafields["rightControlSignal_x"] =
936 new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_x);
937 datafields["rightControlSignal_y"] =
938 new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_y);
939 datafields["rightControlSignal_z"] =
940 new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_z);
941 datafields["rightControlSignal_ro"] =
942 new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ro);
943 datafields["rightControlSignal_pi"] =
944 new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_pi);
945 datafields["rightControlSignal_ya"] =
946 new Variant(debugDataInfo.getUpToDateReadBuffer().rightControlSignal_ya);
947
948 datafields["virtual_timer"] =
949 new Variant(debugDataInfo.getUpToDateReadBuffer().virtualTime);
950
951
952 debugObs->setDebugChannel("CCDMPVelocityController", datafields);
953 }
954
955 void
956 NJointBimanualCCDMPVelocityController::onInitNJointController()
957 {
958 ARMARX_INFO << "init ...";
959 started = false;
960 runTask("NJointBimanualCCDMPVelocityController",
961 [&]
962 {
963 CycleUtil c(1);
964 getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
965 while (getState() == eManagedIceObjectStarted)
966 {
967 if (isControllerActive())
968 {
969 controllerRun();
970 }
971 c.waitForCycleDuration();
972 }
973 });
974 ARMARX_INFO << "Finished init ...";
975 }
976
977 void
982
983 Eigen::Matrix4f
984 NJointBimanualCCDMPVelocityController::getLocalPose(
985 const std::vector<double>& newCoordinateVec,
986 const std::vector<double>& globalTargetPoseVec)
987 {
988 Eigen::Matrix4f newCoordinate =
989 VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4),
990 newCoordinateVec.at(5),
991 newCoordinateVec.at(6),
992 newCoordinateVec.at(3));
993 newCoordinate(0, 3) = newCoordinateVec.at(0);
994 newCoordinate(1, 3) = newCoordinateVec.at(1);
995 newCoordinate(2, 3) = newCoordinateVec.at(2);
996
997 Eigen::Matrix4f globalTargetPose =
998 VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4),
999 globalTargetPoseVec.at(5),
1000 globalTargetPoseVec.at(6),
1001 globalTargetPoseVec.at(3));
1002 globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
1003 globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
1004 globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
1005
1006 return getLocalPose(newCoordinate, globalTargetPose);
1007 }
1008
1009 std::string
1010 NJointBimanualCCDMPVelocityController::getLeaderName(const Ice::Current&)
1011 {
1012 return leaderName;
1013 }
1014
1015 double
1016 NJointBimanualCCDMPVelocityController::getVirtualTime(const Ice::Current&)
1017 {
1018 return virtualtimer;
1019 }
1020
1021 bool
1022 NJointBimanualCCDMPVelocityController::isFinished(const Ice::Current&)
1023 {
1024 return finished;
1025 }
1026} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition ArmarXDataPath.cpp:109
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::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::bimanual::NJointBimanualCCDMPVelocityControllerControlData
Definition NJointBimanualCCDMPVelocityController.h:43
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::leftTargetVel
Eigen::VectorXf leftTargetVel
Definition NJointBimanualCCDMPVelocityController.h:45
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::rightTargetVel
Eigen::VectorXf rightTargetVel
Definition NJointBimanualCCDMPVelocityController.h:46
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::rightTargetPose
Eigen::Matrix4f rightTargetPose
Definition NJointBimanualCCDMPVelocityController.h:49
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::leftTargetPose
Eigen::Matrix4f leftTargetPose
Definition NJointBimanualCCDMPVelocityController.h:48
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityControllerControlData::virtualTime
double virtualTime
Definition NJointBimanualCCDMPVelocityController.h:51
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition NJointBimanualCCDMPVelocityController.cpp:875
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::onInitNJointController
void onInitNJointController() override
Definition NJointBimanualCCDMPVelocityController.cpp:956
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::learnDMPFromBothFiles
void learnDMPFromBothFiles(const Ice::StringSeq &, const Ice::StringSeq &, const Ice::Current &) override
Definition NJointBimanualCCDMPVelocityController.cpp:726
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &) override
Definition NJointBimanualCCDMPVelocityController.cpp:778
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::NJointBimanualCCDMPVelocityController
NJointBimanualCCDMPVelocityController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition NJointBimanualCCDMPVelocityController.cpp:35
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::onDisconnectNJointController
void onDisconnectNJointController() override
Definition NJointBimanualCCDMPVelocityController.cpp:978
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::learnDMPFromFiles
void learnDMPFromFiles(const std::string &, const Ice::StringSeq &, const Ice::Current &) override
Definition NJointBimanualCCDMPVelocityController.cpp:676
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition NJointBimanualCCDMPVelocityController.cpp:445
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::controllerRun
void controllerRun()
Definition NJointBimanualCCDMPVelocityController.cpp:260
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::runDMP
void runDMP(const Ice::DoubleSeq &leftGoals, const Ice::DoubleSeq &rightGoals, const Ice::Current &) override
Definition NJointBimanualCCDMPVelocityController.cpp:812
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::setRatios
void setRatios(const Ice::DoubleSeq &ratios, const Ice::Current &) override
Definition NJointBimanualCCDMPVelocityController.cpp:752
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &) override
Definition NJointBimanualCCDMPVelocityController.cpp:762
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition NJointBimanualCCDMPVelocityController.cpp:240
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCCDMPVelocityController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition NJointBimanualCCDMPVelocityController.cpp:246
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_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_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition NJointBimanualCartesianAdmittanceController.cpp:31
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualCCDMPVelocityController
NJointControllerRegistration< NJointBimanualCCDMPVelocityController > registrationControllerNJointBimanualCCDMPVelocityController("NJointBimanualCCDMPVelocityController")
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerPtr
std::shared_ptr< TaskSpaceDMPController > TaskSpaceDMPControllerPtr
Definition NJointTaskSpaceImpedanceDMPController.h:31
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::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::DMPStyle
std::string DMPStyle
Definition TaskSpaceVMP.h:63