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