NJointBimanualDMPForceController.cpp
Go to the documentation of this file.
1#include <VirtualRobot/IK/DifferentialIK.h>
2#include <VirtualRobot/Robot.h>
3
4#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
5#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
6#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
7#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
8#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
9#include <RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.h>
10#include <RobotAPI/libraries/DMPController/TaskSpaceDMPController.h>
11#include <RobotAPI/libraries/core/CartesianVelocityController.h>
12#include <RobotAPI/libraries/core/PIDController.h>
13
14#include "NJointBimanualCCDMPController.h"
15#include <dmp/representation/dmp/umitsmp.h>
16
17namespace armarx
18{
19 NJointControllerRegistration<NJointBimanualCCDMPController>
20 registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController");
21
22 NJointBimanualCCDMPController::NJointBimanualCCDMPController(
23 armarx::NJointControllerDescriptionProviderInterfacePtr prov,
24 const armarx::NJointControllerConfigPtr& config,
25 const VirtualRobot::RobotPtr&)
26 {
27 ARMARX_INFO << "Preparing ... ";
28 cfg = NJointBimanualCCDMPControllerConfigPtr::dynamicCast(config);
29 ARMARX_CHECK_EXPRESSION(prov);
30 RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
31 ARMARX_CHECK_EXPRESSION(robotUnit);
32
33 leftRNS = robotUnit->getRtRobot()->getRobotNodeSet("LeftArm");
34 rnsLeftBody = robotUnit->getRtRobot()->getRobotNodeSet("LeftArmCol");
35 for (size_t i = 0; i < leftRNS->getSize(); ++i)
36 {
37 std::string jointName = leftRNS->getNode(i)->getName();
38
39 leftJointNames.push_back(jointName);
40 ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
41 const SensorValueBase* sv = prov->getSensorValue(jointName);
42 leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
43 const SensorValue1DoFActuatorVelocity* velocitySensor =
44 sv->asA<SensorValue1DoFActuatorVelocity>();
45 const SensorValue1DoFActuatorPosition* positionSensor =
46 sv->asA<SensorValue1DoFActuatorPosition>();
47 const SensorValue1DoFActuatorAcceleration* accelerationSensor =
48 sv->asA<SensorValue1DoFActuatorAcceleration>();
49
50 if (!velocitySensor)
51 {
52 ARMARX_WARNING << "No velocitySensor available for " << jointName;
53 }
54 if (!positionSensor)
55 {
56 ARMARX_WARNING << "No positionSensor available for " << jointName;
57 }
58
59 if (!accelerationSensor)
60 {
61 ARMARX_WARNING << "No accelerationSensor available for " << jointName;
62 }
63
64
65 leftVelocitySensors.push_back(velocitySensor);
66 leftPositionSensors.push_back(positionSensor);
67 leftAccelerationSensors.push_back(accelerationSensor);
68 };
69
70
71 rnsRightBody = robotUnit->getRtRobot()->getRobotNodeSet("RightArmCol");
72
73 rightRNS = robotUnit->getRtRobot()->getRobotNodeSet("RightArm");
74 for (size_t i = 0; i < rightRNS->getSize(); ++i)
75 {
76 std::string jointName = rightRNS->getNode(i)->getName();
77 rightJointNames.push_back(jointName);
78 ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Torque1DoF);
79 const SensorValueBase* sv = prov->getSensorValue(jointName);
80 rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
81 const SensorValue1DoFActuatorVelocity* velocitySensor =
82 sv->asA<SensorValue1DoFActuatorVelocity>();
83 const SensorValue1DoFActuatorPosition* positionSensor =
84 sv->asA<SensorValue1DoFActuatorPosition>();
85 const SensorValue1DoFActuatorAcceleration* accelerationSensor =
86 sv->asA<SensorValue1DoFActuatorAcceleration>();
87
88
89 if (!velocitySensor)
90 {
91 ARMARX_WARNING << "No velocitySensor available for " << jointName;
92 }
93 if (!positionSensor)
94 {
95 ARMARX_WARNING << "No positionSensor available for " << jointName;
96 }
97 if (!accelerationSensor)
98 {
99 ARMARX_WARNING << "No accelerationSensor available for " << jointName;
100 }
101
102 rightVelocitySensors.push_back(velocitySensor);
103 rightPositionSensors.push_back(positionSensor);
104 rightAccelerationSensors.push_back(accelerationSensor);
105 };
106
107
108 const SensorValueBase* svlf = prov->getSensorValue("FT L");
109 leftForceTorque = svlf->asA<SensorValueForceTorque>();
110 const SensorValueBase* svrf = prov->getSensorValue("FT R");
111 rightForceTorque = svrf->asA<SensorValueForceTorque>();
112
113 leftIK.reset(new VirtualRobot::DifferentialIK(
114 leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
115 rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
116 rightRNS->getRobot()->getRootNode(),
117 VirtualRobot::JacobiProvider::eSVDDamped));
118
119
120 TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
121 taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
122 taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
123 taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
124 taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
125 taskSpaceDMPConfig.DMPAmplitude = 1.0;
126 taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
127 taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
128 taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
129 taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
130 taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
131 taskSpaceDMPConfig.phaseStopParas.Dori = 0;
132 taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
133 taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
134 taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
135
136
137 TaskSpaceDMPControllerPtr leftLeader(
138 new TaskSpaceDMPController("leftLeader", taskSpaceDMPConfig));
139 TaskSpaceDMPControllerPtr leftFollower(
140 new TaskSpaceDMPController("leftFollower", taskSpaceDMPConfig));
141 TaskSpaceDMPControllerPtr rightLeader(
142 new TaskSpaceDMPController("rightLeader", taskSpaceDMPConfig));
143 TaskSpaceDMPControllerPtr rightFollower(
144 new TaskSpaceDMPController("rightFollower", taskSpaceDMPConfig));
145
146 leftGroup.push_back(leftLeader);
147 leftGroup.push_back(rightFollower);
148
149 rightGroup.push_back(rightLeader);
150 rightGroup.push_back(leftFollower);
151
152 bothLeaderGroup.push_back(leftLeader);
153 bothLeaderGroup.push_back(rightLeader);
154
155
156 tcpLeft = leftRNS->getTCP();
157 tcpRight = rightRNS->getTCP();
158 NJointBimanualCCDMPControllerSensorData initSensorData;
159 initSensorData.deltaT = 0;
160 initSensorData.currentTime = 0;
161 initSensorData.currentLeftPose = leftRNS->getTCP()->getPoseInRootFrame();
162 initSensorData.currentRightPose = rightRNS->getTCP()->getPoseInRootFrame();
163 controllerSensorData.reinitAllBuffers(initSensorData);
164
165 leaderName = "both";
166
167 NJointBimanualCCDMPControllerControlData initData;
168 initData.leftTargetVel.resize(6);
169 initData.leftTargetVel.setZero();
170 initData.rightTargetVel.resize(6);
171 initData.rightTargetVel.setZero();
172 reinitTripleBuffer(initData);
173
174 leftDesiredJointValues.resize(leftTargets.size());
175 ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
176
177 for (size_t i = 0; i < leftTargets.size(); ++i)
178 {
179 leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
180 }
181
182 rightDesiredJointValues.resize(rightTargets.size());
183 ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
184
185 for (size_t i = 0; i < rightTargets.size(); ++i)
186 {
187 rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
188 }
189
190 KoriFollower = cfg->KoriFollower;
191 KposFollower = cfg->KposFollower;
192
193 kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
194 dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
195 kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
196 dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
197
198
199 knull = cfg->knull;
200 dnull = cfg->dnull;
201
202 torqueLimit = cfg->torqueLimit;
203
204 kpf.resize(12);
205 kif.resize(12);
206 forceC_des.resize(12);
207
208 ARMARX_CHECK_EQUAL(cfg->Kpf.size(), kpf.rows());
209 ARMARX_CHECK_EQUAL(cfg->Kif.size(), kif.rows());
210 ARMARX_CHECK_EQUAL(cfg->DesiredForce.size(), forceC_des.rows());
211
212
213 for (size_t i = 0; i < 12; i++)
214 {
215 kpf(i) = cfg->Kpf.at(i);
216 kif(i) = cfg->Kif.at(i);
217 forceC_des(i) = cfg->DesiredForce.at(i);
218 }
219
220 boxWidth = cfg->BoxWidth;
221
222 filtered_leftForce.setZero();
223 filtered_leftTorque.setZero();
224 filtered_rightForce.setZero();
225 filtered_rightTorque.setZero();
226
227 // offset_leftForce.setZero();
228 // offset_leftTorque.setZero();
229 // offset_rightForce.setZero();
230 // offset_rightTorque.setZero();
231
232 offset_leftForce(0) = -4.34;
233 offset_leftForce(1) = -8.21;
234 offset_leftForce(2) = 15.59;
235
236 offset_leftTorque(0) = 1.42;
237 offset_leftTorque(1) = 0.29;
238 offset_leftTorque(2) = 0.09;
239
240 offset_rightForce(0) = 2.88;
241 offset_rightForce(1) = 11.15;
242 offset_rightForce(2) = -20.18;
243
244 offset_rightTorque(0) = -1.83;
245 offset_rightTorque(1) = 0.34;
246 offset_rightTorque(2) = -0.01;
247
248
249 filterTimeConstant = cfg->FilterTimeConstant;
250
251 ftPIDController.resize(12);
252 for (size_t i = 0; i < ftPIDController.size(); i++)
253 {
254 ftPIDController[i].reset(new PIDController(kpf(i), kif(i), 0.0, 10, 10));
255 }
256
257 isForceCompensateDone = false;
258 }
259
260 std::string
261 NJointBimanualCCDMPController::getClassName(const Ice::Current&) const
262 {
263 return "NJointBimanualCCDMPController";
264 }
265
266 void
267 NJointBimanualCCDMPController::controllerRun()
268 {
269 if (!controllerSensorData.updateReadBuffer())
270 {
271 return;
272 }
273
274 double deltaT = controllerSensorData.getReadBuffer().deltaT;
275
276
277 Eigen::VectorXf leftTargetVel;
278 leftTargetVel.resize(6);
279 leftTargetVel.setZero();
280 Eigen::VectorXf rightTargetVel;
281 rightTargetVel.resize(6);
282 rightTargetVel.setZero();
283
284 std::vector<TaskSpaceDMPControllerPtr> currentControlGroup;
285 Eigen::Matrix4f currentLeaderPose;
286 Eigen::Matrix4f currentFollowerPose;
287 Eigen::VectorXf currentLeaderTwist;
288 Eigen::VectorXf currentFollowerTwist;
289 if (leaderName == "Left")
290 {
291 currentControlGroup = leftGroup;
292 currentLeaderPose = controllerSensorData.getReadBuffer().currentLeftPose;
293 currentFollowerPose = controllerSensorData.getReadBuffer().currentRightPose;
294 currentLeaderTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
295 currentFollowerTwist = controllerSensorData.getReadBuffer().currentRightTwist;
296 }
297 else if (leaderName == "right")
298 {
299 currentControlGroup = rightGroup;
300 currentLeaderPose = controllerSensorData.getReadBuffer().currentRightPose;
301 currentFollowerPose = controllerSensorData.getReadBuffer().currentLeftPose;
302 currentLeaderTwist = controllerSensorData.getReadBuffer().currentRightTwist;
303 currentFollowerTwist = controllerSensorData.getReadBuffer().currentLeftTwist;
304 }
305 else
306 {
307 currentControlGroup = bothLeaderGroup;
308
309 TaskSpaceDMPControllerPtr leaderDMPleft = currentControlGroup[0];
310 TaskSpaceDMPControllerPtr leaderDMPright = currentControlGroup[1];
311 leaderDMPleft->flow(deltaT,
312 controllerSensorData.getReadBuffer().currentLeftPose,
313 controllerSensorData.getReadBuffer().currentLeftTwist);
314 leaderDMPright->flow(deltaT,
315 controllerSensorData.getReadBuffer().currentRightPose,
316 controllerSensorData.getReadBuffer().currentRightTwist);
317
318 Eigen::VectorXf leftTargetVel = leaderDMPleft->getTargetVelocity();
319 Eigen::Matrix4f leftTargetPose = leaderDMPleft->getTargetPoseMat();
320 Eigen::VectorXf rightTargetVel = leaderDMPright->getTargetVelocity();
321 Eigen::Matrix4f rightTargetPose = leaderDMPright->getTargetPoseMat();
322
323 virtualtimer = leaderDMPleft->canVal;
324 LockGuardType guard{controlDataMutex};
325 getWriterControlStruct().leftTargetVel = leftTargetVel;
326 getWriterControlStruct().rightTargetVel = rightTargetVel;
327 getWriterControlStruct().leftTargetPose = leftTargetPose;
328 getWriterControlStruct().rightTargetPose = rightTargetPose;
329 writeControlStruct();
330
331 return;
332 }
333
334 TaskSpaceDMPControllerPtr leaderDMP = currentControlGroup[0];
335 TaskSpaceDMPControllerPtr followerDMP = currentControlGroup[1];
336
337
338 leaderDMP->flow(deltaT, currentLeaderPose, currentLeaderTwist);
339
340 Eigen::Matrix4f currentFollowerLocalPose;
341 currentFollowerLocalPose.block<3, 3>(0, 0) =
342 currentFollowerPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0).inverse();
343 currentFollowerLocalPose.block<3, 1>(0, 3) =
344 currentFollowerLocalPose.block<3, 3>(0, 0) *
345 (currentFollowerPose.block<3, 1>(0, 3) - currentLeaderPose.block<3, 1>(0, 3));
346 followerDMP->flow(deltaT, currentFollowerPose, currentFollowerTwist);
347
348 Eigen::VectorXf leaderTargetVel = leaderDMP->getTargetVelocity();
349 Eigen::Matrix4f leaderTargetPose = leaderDMP->getTargetPoseMat();
350 Eigen::Matrix4f followerLocalTargetPose = followerDMP->getTargetPoseMat();
351 std::vector<double> followerLocalTargetPoseVec = followerDMP->getTargetPose();
352
353 Eigen::Matrix4f followerTargetPose;
354 followerTargetPose.block<3, 3>(0, 0) =
355 followerLocalTargetPose.block<3, 3>(0, 0) * currentLeaderPose.block<3, 3>(0, 0);
356 followerTargetPose.block<3, 1>(0, 3) =
357 currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetPose.block<3, 1>(0, 3) +
358 currentLeaderPose.block<3, 1>(0, 3);
359
360
361 Eigen::VectorXf followerLocalTargetVel = followerDMP->getTargetVelocity();
362 Eigen::VectorXf followerTargetVel = followerLocalTargetVel;
363 // followerTargetVel.block<3, 1>(0, 0) = currentLeaderPose.block<3, 3>(0, 0) * followerLocalTargetVel.block<3, 1>(0, 0) + leaderTargetVel.block<3, 1>(0, 0);
364 followerTargetVel.block<3, 1>(0, 0) =
365 KposFollower *
366 (followerTargetPose.block<3, 1>(0, 3) - currentFollowerPose.block<3, 1>(0, 3));
367
368
369 Eigen::Matrix3f followerDiffMat =
370 followerTargetPose.block<3, 3>(0, 0) * currentFollowerPose.block<3, 3>(0, 0).inverse();
371 Eigen::Vector3f followerDiffRPY =
372 KoriFollower * VirtualRobot::MathTools::eigen3f2rpy(followerDiffMat);
373 followerTargetVel(3) = followerDiffRPY(0);
374 followerTargetVel(4) = followerDiffRPY(1);
375 followerTargetVel(5) = followerDiffRPY(2);
376
377 virtualtimer = leaderDMP->canVal;
378
379
380 std::vector<double> leftDMPTarget;
381 std::vector<double> rightDMPTarget;
382
383 Eigen::Matrix4f leftTargetPose;
384 Eigen::Matrix4f rightTargetPose;
385
386 if (leaderName == "Left")
387 {
388 leftTargetVel = leaderTargetVel;
389 rightTargetVel = followerTargetVel;
390
391 leftDMPTarget = leaderDMP->getTargetPose();
392 rightDMPTarget = followerLocalTargetPoseVec;
393
394
395 leftTargetPose = leaderTargetPose;
396 rightTargetPose = followerLocalTargetPose;
397 }
398 else if (leaderName == "right")
399 {
400 rightTargetVel = leaderTargetVel;
401 leftTargetVel = followerTargetVel;
402
403 rightDMPTarget = leaderDMP->getTargetPose();
404 leftDMPTarget = followerLocalTargetPoseVec;
405
406 rightTargetPose = leaderTargetPose;
407 leftTargetPose = followerLocalTargetPose;
408 }
409
410
411 // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_xvel"] = leftTargetVel(0);
412 // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yvel"] = leftTargetVel(1);
413 // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_zvel"] = leftTargetVel(2);
414 // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_rollvel"] = leftTargetVel(3);
415 // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_pitchvel"] = leftTargetVel(4);
416 // debugOutputData.getWriteBuffer().latestTargetVelocities["LeftArm_yawvel"] = leftTargetVel(5);
417
418 // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_xvel"] = rightTargetVel(0);
419 // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yvel"] = rightTargetVel(1);
420 // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_zvel"] = rightTargetVel(2);
421 // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_rollvel"] = rightTargetVel(3);
422 // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_pitchvel"] = rightTargetVel(4);
423 // debugOutputData.getWriteBuffer().latestTargetVelocities["RightArm_yawvel"] = rightTargetVel(5);
424
425
426 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_x"] = leftDMPTarget[0];
427 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_y"] = leftDMPTarget[1];
428 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_z"] = leftDMPTarget[2];
429 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qw"] = leftDMPTarget[3];
430 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qx"] = leftDMPTarget[4];
431 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qy"] = leftDMPTarget[5];
432 // debugOutputData.getWriteBuffer().dmpTargets["LeftArm_dmp_qz"] = leftDMPTarget[6];
433
434 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_x"] = rightDMPTarget[0];
435 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_y"] = rightDMPTarget[1];
436 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_z"] = rightDMPTarget[2];
437 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qw"] = rightDMPTarget[3];
438 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qx"] = rightDMPTarget[4];
439 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qy"] = rightDMPTarget[5];
440 // debugOutputData.getWriteBuffer().dmpTargets["RightArm_dmp_qz"] = rightDMPTarget[6];
441
442 // std::vector<double> currentLeftPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentLeftPose);
443 // std::vector<double> currentRightPoseVec = leaderDMP->eigen4f2vec(controllerSensorData.getReadBuffer().currentRightPose);
444
445
446 // debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = currentLeftPoseVec[0];
447 // debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = currentLeftPoseVec[1];
448 // debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = currentLeftPoseVec[2];
449 // debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = currentLeftPoseVec[3];
450 // debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = currentLeftPoseVec[4];
451 // debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = currentLeftPoseVec[5];
452 // debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = currentLeftPoseVec[6];
453
454 // debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = currentRightPoseVec[0];
455 // debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = currentRightPoseVec[1];
456 // debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = currentRightPoseVec[2];
457 // debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = currentRightPoseVec[3];
458 // debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = currentRightPoseVec[4];
459 // debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = currentRightPoseVec[5];
460 // debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = currentRightPoseVec[6];
461
462
463 // debugOutputData.getWriteBuffer().leaderCanVal = leaderDMP->debugData.canVal;
464 // debugOutputData.getWriteBuffer().leadermpcFactor = leaderDMP->debugData.mpcFactor;
465 // debugOutputData.getWriteBuffer().leadererror = leaderDMP->debugData.poseError;
466 // debugOutputData.getWriteBuffer().leaderposError = leaderDMP->debugData.posiError;
467 // debugOutputData.getWriteBuffer().leaderoriError = leaderDMP->debugData.oriError;
468
469 // debugOutputData.getWriteBuffer().followerCanVal = followerDMP->debugData.canVal;
470 // debugOutputData.getWriteBuffer().followermpcFactor = followerDMP->debugData.mpcFactor;
471 // debugOutputData.getWriteBuffer().followererror = followerDMP->debugData.poseError;
472 // debugOutputData.getWriteBuffer().followerposError = followerDMP->debugData.posiError;
473 // debugOutputData.getWriteBuffer().followeroriError = followerDMP->debugData.oriError;
474
475 // debugOutputData.commitWrite();
476
477
478 LockGuardType guard{controlDataMutex};
479 getWriterControlStruct().leftTargetVel = leftTargetVel;
480 getWriterControlStruct().rightTargetVel = rightTargetVel;
481 getWriterControlStruct().leftTargetPose = leftTargetPose;
482 getWriterControlStruct().rightTargetPose = rightTargetPose;
483 writeControlStruct();
484 }
485
486 Eigen::VectorXf
487 NJointBimanualCCDMPController::getControlWrench(const Eigen::VectorXf& tcptwist,
488 const Eigen::Matrix4f& currentPose,
489 const Eigen::Matrix4f& targetPose)
490 {
491 // Eigen::Vector3f currentTCPLinearVelocity;
492 // currentTCPLinearVelocity << 0.001 * tcptwist(0), 0.001 * tcptwist(1), 0.001 * tcptwist(2);
493 // Eigen::Vector3f currentTCPAngularVelocity;
494 // currentTCPAngularVelocity << tcptwist(3), tcptwist(4), tcptwist(5);
495
496 // Eigen::Vector3f currentTCPPosition = currentPose.block<3,1>(0,3);
497 // Eigen::Vector3f desiredPosition = targetPose.block<3,1>(0,3);
498 // Eigen::Vector3f tcpForces = 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition);
499 // Eigen::Vector3f tcpDesiredForce = tcpForces - dpos.cwiseProduct(currentTCPLinearVelocity);
500
501 // Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
502 // Eigen::Matrix3f diffMat = targetPose.block<3,3>(0,0) * currentRotMat.inverse();
503 // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
504
505 // Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
506 // Eigen::Vector6f tcpDesiredWrench;
507 // tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
508
509 // return tcpDesiredWrench;
510 }
511
512 void
513 NJointBimanualCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
514 const IceUtil::Time& timeSinceLastIteration)
515 {
516 double deltaT = timeSinceLastIteration.toSecondsDouble();
517 controllerSensorData.getWriteBuffer().currentLeftPose = tcpLeft->getPoseInRootFrame();
518 controllerSensorData.getWriteBuffer().currentRightPose = tcpRight->getPoseInRootFrame();
519 controllerSensorData.getWriteBuffer().deltaT = deltaT;
520 controllerSensorData.getWriteBuffer().currentTime += deltaT;
521
522
523 // if(controllerSensorData.getWriteBuffer().currentTime > 0.3 && !isForceCompensateDone)
524 // {
525 // offset_leftForce = filtered_leftForce;
526 // offset_leftTorque = filtered_leftTorque;
527 // offset_rightForce = filtered_rightForce;
528 // offset_rightTorque = filtered_rightTorque;
529 // isForceCompensateDone = true;
530
531
532 // filtered_leftForce.setZero();
533 // filtered_leftTorque.setZero();
534 // filtered_rightForce.setZero();
535 // filtered_rightTorque.setZero();
536 // }
537
538 // if(controllerSensorData.getWriteBuffer().currentTime <= 0.3)
539 // {
540 // // torque limit
541 // for (size_t i = 0; i < leftTargets.size(); ++i)
542 // {
543 // leftTargets.at(i)->torque = 0;
544 // }
545
546 // for (size_t i = 0; i < rightTargets.size(); ++i)
547 // {
548 // rightTargets.at(i)->torque = 0;
549 // }
550
551 // return;
552 // }
553
554 // cartesian vel controller
555 Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
556
557 Eigen::MatrixXf jacobiL =
558 leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
559 jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
560
561 Eigen::VectorXf leftqpos;
562 Eigen::VectorXf leftqvel;
563 leftqpos.resize(leftPositionSensors.size());
564 leftqvel.resize(leftVelocitySensors.size());
565 for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
566 {
567 leftqpos(i) = leftPositionSensors[i]->position;
568 leftqvel(i) = leftVelocitySensors[i]->velocity;
569 }
570
571 Eigen::MatrixXf jacobiR =
572 rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
573 jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
574
575
576 Eigen::VectorXf rightqpos;
577 Eigen::VectorXf rightqvel;
578 rightqpos.resize(rightPositionSensors.size());
579 rightqvel.resize(rightVelocitySensors.size());
580
581 for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
582 {
583 rightqpos(i) = rightPositionSensors[i]->position;
584 rightqvel(i) = rightVelocitySensors[i]->velocity;
585 }
586
587 Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
588 Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
589
590
591 controllerSensorData.getWriteBuffer().currentLeftTwist = currentLeftTwist;
592 controllerSensorData.getWriteBuffer().currentRightTwist = currentRightTwist;
593 controllerSensorData.commitWrite();
594
595
596 Eigen::Matrix4f leftTargetPose = rtGetControlStruct().leftTargetPose;
597 Eigen::Matrix4f rightTargetPose = rtGetControlStruct().rightTargetPose;
598 Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
599 Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
600 Eigen::Matrix4f leftCurrentPose = tcpLeft->getPoseInRootFrame();
601 Eigen::Matrix4f rightCurrentPose = tcpRight->getPoseInRootFrame();
602
603 // Todo: force control
604 int nl = leftRNS->getSize();
605 int nr = rightRNS->getSize();
606 int n = nl + nr;
607 // GraspMatrix
608
609 // Eigen::Matrix4f poseLeftInRightCoordinate = tcpRight->toLocalCoordinateSystem(tcpLeft->getGlobalPose());
610 // Eigen::Vector3f positionLeftInRightCoordinate;
611 // positionLeftInRightCoordinate << poseLeftInRightCoordinate(0,3), poseLeftInRightCoordinate(1,3), poseLeftInRightCoordinate(2,3);
612 // boxWidth = 0.001 * 0.5 * positionLeftInRightCoordinate.norm();
613
614
615 Eigen::Vector3f localDistanceCoM;
616 localDistanceCoM << 0.5 * boxWidth, 0, 0;
617
618
619 Eigen::Vector3f rl =
620 -leftCurrentPose.block<3, 3>(0, 0) *
621 localDistanceCoM; //tcpLeft->getRobot()->getRootNode()->toLocalCoordinateSystem(tcpLeft->toGlobalCoordinateSystemVec(localDistanceCoM));
622 Eigen::Vector3f rr = rightCurrentPose.block<3, 3>(0, 0) * localDistanceCoM;
623 Eigen::MatrixXf leftGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
624 Eigen::MatrixXf rightGraspMatrix = Eigen::MatrixXf::Identity(6, 6);
625 leftGraspMatrix.block<3, 3>(3, 0) << 0, -rl(2), rl(1), rl(2), 0, -rl(0), -rl(1), rl(0), 0;
626 rightGraspMatrix.block<3, 3>(3, 0) << 0, -rr(2), rr(1), rr(2), 0, -rr(0), -rr(1), rr(0), 0;
627 Eigen::MatrixXf graspMatrix;
628 graspMatrix.resize(6, 12);
629 graspMatrix << leftGraspMatrix, rightGraspMatrix;
630
631 // constraint Jacobain and projection matrix
632 Eigen::MatrixXf jacobi;
633 jacobi.resize(12, n);
634 jacobi.setZero(12, n);
635 jacobi.block<6, 8>(0, 0) = jacobiL;
636 jacobi.block<6, 8>(6, 8) = jacobiR;
637
638 Eigen::MatrixXf pinvGraspMatrix =
639 leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 1);
640 Eigen::MatrixXf proj2GraspNullspace =
641 Eigen::MatrixXf::Identity(12, 12) - pinvGraspMatrix * graspMatrix;
642 Eigen::MatrixXf jacobiConstrained = proj2GraspNullspace * jacobi;
643
644 Eigen::MatrixXf pinvJacobiC =
645 leftIK->computePseudoInverseJacobianMatrix(jacobiConstrained, 1);
646 Eigen::MatrixXf projection =
647 Eigen::MatrixXf::Identity(n, n) - pinvJacobiC * jacobiConstrained;
648
649
650 // calculate inertia matrix
651 Eigen::MatrixXf leftInertialMatrix;
652 Eigen::MatrixXf rightInertialMatrix;
653 leftInertialMatrix.setZero(nl, nl);
654 rightInertialMatrix.setZero(nr, nr);
655
656 for (size_t i = 0; i < rnsLeftBody->getNodeNames().size(); ++i)
657 {
658 VirtualRobot::RobotNodePtr rnbody = rnsLeftBody->getNode(i);
659 VirtualRobot::RobotNodePtr rnjoint = leftRNS->getNode(i + 1);
660
661 Eigen::MatrixXf jacobipos_rn =
662 0.001 * leftIK->getJacobianMatrix(
663 rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
664 Eigen::MatrixXf jacobiori_rn = leftIK->getJacobianMatrix(
665 rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
666
667
668 float m = rnbody->getMass();
669 Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
670 leftInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn +
671 jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
672 }
673
674 for (size_t i = 0; i < rnsRightBody->getNodeNames().size(); ++i)
675 {
676 VirtualRobot::RobotNodePtr rnbody = rnsRightBody->getNode(i);
677 VirtualRobot::RobotNodePtr rnjoint = rightRNS->getNode(i + 1);
678
679 Eigen::MatrixXf jacobipos_rn =
680 0.001 * rightIK->getJacobianMatrix(
681 rnjoint, VirtualRobot::IKSolver::CartesianSelection::Position);
682 Eigen::MatrixXf jacobiori_rn = rightIK->getJacobianMatrix(
683 rnjoint, VirtualRobot::IKSolver::CartesianSelection::Orientation);
684
685
686 float m = rnbody->getMass();
687 Eigen::Matrix3f InertiaMatrix = rnbody->getInertiaMatrix();
688
689 rightInertialMatrix += m * jacobipos_rn.transpose() * jacobipos_rn +
690 jacobiori_rn.transpose() * InertiaMatrix * jacobiori_rn;
691 }
692 Eigen::MatrixXf M = Eigen::MatrixXf::Zero(n, n);
693 M.topLeftCorner(nl, nl) = leftInertialMatrix;
694 M.bottomRightCorner(nr, nr) = rightInertialMatrix;
695
696 Eigen::MatrixXf Mc = M + projection * M - M * projection;
697
698
699 // force filter and measure
700 double filterFactor = 1 / ((filterTimeConstant / deltaT) + 1);
701
702 filtered_leftForce = (1 - filterFactor) * filtered_leftForce +
703 filterFactor * (leftForceTorque->force - offset_leftForce);
704 filtered_leftTorque = (1 - filterFactor) * filtered_leftTorque +
705 filterFactor * (leftForceTorque->torque - offset_leftTorque);
706 filtered_rightForce = (1 - filterFactor) * filtered_rightForce +
707 filterFactor * (rightForceTorque->force - offset_rightForce);
708 filtered_rightTorque = (1 - filterFactor) * filtered_rightForce +
709 filterFactor * (rightForceTorque->torque - offset_rightTorque);
710
711 Eigen::VectorXf filteredForce;
712 filteredForce.resize(12);
713 filteredForce << filtered_leftForce, filtered_leftTorque, filtered_rightForce,
714 filtered_rightTorque;
715 filteredForce.block<3, 1>(0, 0) =
716 leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(0, 0);
717 filteredForce.block<3, 1>(3, 0) =
718 leftCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(3, 0);
719 filteredForce.block<3, 1>(6, 0) =
720 rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(6, 0);
721 filteredForce.block<3, 1>(9, 0) =
722 rightCurrentPose.block<3, 3>(0, 0) * filteredForce.block<3, 1>(9, 0);
723
724
725 // calculate force control part
726 Eigen::VectorXf constrainedForceMeasure = proj2GraspNullspace * filteredForce;
727
728 constrainedForceMeasure.block<3, 1>(0, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() *
729 constrainedForceMeasure.block<3, 1>(0, 0);
730 constrainedForceMeasure.block<3, 1>(3, 0) = leftCurrentPose.block<3, 3>(0, 0).transpose() *
731 constrainedForceMeasure.block<3, 1>(3, 0);
732 constrainedForceMeasure.block<3, 1>(6, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() *
733 constrainedForceMeasure.block<3, 1>(6, 0);
734 constrainedForceMeasure.block<3, 1>(9, 0) = rightCurrentPose.block<3, 3>(0, 0).transpose() *
735 constrainedForceMeasure.block<3, 1>(9, 0);
736 Eigen::VectorXf forceConstrained = Eigen::VectorXf::Zero(12);
737 for (size_t i = 0; i < 12; i++)
738 {
739 ftPIDController[i]->update(constrainedForceMeasure(i), forceC_des(i));
740 forceConstrained(i) = ftPIDController[i]->getControlValue() + forceC_des(i);
741 // forceConstrained(i) = forceC_des(i) + kpf(i) * (forceC_des(i) - filtered_leftForce(i)); // TODO: add PID controller
742 // forceConstrained(i + 3) = forceC_des(i + 3) + kpf(i + 3) * (forceC_des(i + 3) - filtered_leftTorque(i)); // TODO: add PID controller
743 // forceConstrained(i + 6) = forceC_des(i + 6) + kpf(i + 6) * (forceC_des(i + 6) - filtered_rightForce(i)); // TODO: add PID controller
744 // forceConstrained(i + 9) = forceC_des(i + 9) + kpf(i + 9) * (forceC_des(i + 9) - filtered_rightTorque(i)); // TODO: add PID controller
745 }
746 forceConstrained.block<3, 1>(0, 0) =
747 leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(0, 0);
748 forceConstrained.block<3, 1>(3, 0) =
749 leftCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(3, 0);
750 forceConstrained.block<3, 1>(6, 0) =
751 rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(6, 0);
752 forceConstrained.block<3, 1>(9, 0) =
753 rightCurrentPose.block<3, 3>(0, 0) * forceConstrained.block<3, 1>(9, 0);
754
755
756 // unconstrained space controller
757 Eigen::Vector6f leftJointControlWrench;
758 {
759 Eigen::Vector3f targetTCPLinearVelocity;
760 targetTCPLinearVelocity << 0.001 * leftTargetVel(0), 0.001 * leftTargetVel(1),
761 0.001 * leftTargetVel(2);
762
763 Eigen::Vector3f currentTCPLinearVelocity;
764 currentTCPLinearVelocity << currentLeftTwist(0), currentLeftTwist(1),
765 currentLeftTwist(2);
766 Eigen::Vector3f currentTCPAngularVelocity;
767 currentTCPAngularVelocity << currentLeftTwist(3), currentLeftTwist(4),
768 currentLeftTwist(5);
769 Eigen::Vector3f currentTCPPosition = leftCurrentPose.block<3, 1>(0, 3);
770 Eigen::Vector3f desiredPosition = leftTargetPose.block<3, 1>(0, 3);
771
772 Eigen::Vector3f tcpDesiredForce =
773 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
774 dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
775 Eigen::Matrix3f currentRotMat = leftCurrentPose.block<3, 3>(0, 0);
776 Eigen::Matrix3f diffMat = leftTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
777 Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
778 Eigen::Vector3f tcpDesiredTorque =
779 kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
780 leftJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
781 }
782
783
784 Eigen::Vector6f rightJointControlWrench;
785 {
786 Eigen::Vector3f targetTCPLinearVelocity;
787 targetTCPLinearVelocity << 0.001 * rightTargetVel(0), 0.001 * rightTargetVel(1),
788 0.001 * rightTargetVel(2);
789
790 Eigen::Vector3f currentTCPLinearVelocity;
791 currentTCPLinearVelocity << currentRightTwist(0), currentRightTwist(1),
792 currentRightTwist(2);
793 Eigen::Vector3f currentTCPAngularVelocity;
794 currentTCPAngularVelocity << currentRightTwist(3), currentRightTwist(4),
795 currentRightTwist(5);
796 Eigen::Vector3f currentTCPPosition = rightCurrentPose.block<3, 1>(0, 3);
797 Eigen::Vector3f desiredPosition = rightTargetPose.block<3, 1>(0, 3);
798
799 Eigen::Vector3f tcpDesiredForce =
800 0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) -
801 dpos.cwiseProduct(targetTCPLinearVelocity - currentTCPLinearVelocity);
802 Eigen::Matrix3f currentRotMat = rightCurrentPose.block<3, 3>(0, 0);
803 Eigen::Matrix3f diffMat = rightTargetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
804 Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
805 Eigen::Vector3f tcpDesiredTorque =
806 kori.cwiseProduct(rpy) - dori.cwiseProduct(currentTCPAngularVelocity);
807 rightJointControlWrench << tcpDesiredForce, tcpDesiredTorque;
808 }
809
810 Eigen::VectorXf forceUnconstrained(12);
811 forceUnconstrained << leftJointControlWrench, rightJointControlWrench;
812
813 Eigen::VectorXf unConstrainedForceMeasure = pinvGraspMatrix * graspMatrix * filteredForce;
814
815 Eigen::MatrixXf taskSpaceInertia = (jacobi * M.inverse() * jacobi.transpose()).inverse();
816
817
818 forceUnconstrained =
819 taskSpaceInertia *
820 forceUnconstrained; //+ unConstrainedForceMeasure) - unConstrainedForceMeasure;
821
822 Eigen::VectorXf leftNullspaceTorque =
823 knull * (leftDesiredJointValues - leftqpos) - dnull * leftqvel;
824 Eigen::VectorXf rightNullspaceTorque =
825 knull * (rightDesiredJointValues - rightqpos) - dnull * rightqvel;
826 float lambda = 2;
827
828 Eigen::MatrixXf jtpinvL =
829 leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
830 Eigen::MatrixXf jtpinvR =
831 leftIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
832 forceUnconstrained.head<8>()) =
833 forceUnconstrained.head<8>()) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
834 forceUnconstrained.tail<8>() = forceUnconstrained.tail<8>() +
835 (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
836
837
838 //
839 //// Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * leftJointControlWrench + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
840 // Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceUnconstrained.head<6>() + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
841
842 //
843 //// Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * rightJointControlWrench + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
844 // Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceUnconstrained.tail<6>() + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
845
846 // Eigen::VectorXf torqueUnconstrained(16);
847 // torqueUnconstrained << leftJointDesiredTorques, rightJointDesiredTorques;
848
849 // constrained space control
850
851 // Eigen::MatrixXf mu = (Eigen::MatrixXf::Identity(n, n) - projection) * M * Mc.inverse();
852 // Eigen::VectorXf qacc;
853 // qacc.resize(n);
854 // for (size_t i = 0; i < leftAccelerationSensors.size(); ++i)
855 // {
856 // qacc(i) = 0.001 * leftAccelerationSensors[i]->acceleration;
857 // }
858
859 // for (size_t i = 0; i < rightAccelerationSensors.size(); ++i)
860 // {
861 // qacc(i + leftAccelerationSensors.size()) = 0.001 * rightAccelerationSensors[i]->acceleration;
862 // }
863
864
865 // Eigen::VectorXf torqueConstrained = mu * (torqueUnconstrained + (Eigen::MatrixXf::Identity(n, n) - projection) * qacc) - jacobiConstrained.transpose() * forceConstrained;
866
867
868 // all torques
869
870
871 // Eigen::VectorXf torqueInput = projection * torqueUnconstrained + projection * torqueConstrained;
872 // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * (Eigen::MatrixXf::Identity(12,12) - proj2GraspNullspace) * forceConstrained;
873 // Eigen::VectorXf torqueInput = torqueUnconstrained + jacobi.transpose() * forceConstrained;
874 Eigen::VectorXf torqueInput = jacobi.transpose() * (forceUnconstrained + forceConstrained);
875
876 Eigen::VectorXf leftJointDesiredTorques = torqueInput.head(nl);
877 Eigen::VectorXf rightJointDesiredTorques = torqueInput.block<8, 1>(8, 0);
878
879 // torque limit
880 for (size_t i = 0; i < leftTargets.size(); ++i)
881 {
882 float desiredTorque = leftJointDesiredTorques(i);
883
884 if (isnan(desiredTorque))
885 {
886 desiredTorque = 0;
887 }
888
889 desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
890 desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
891
892 debugDataInfo.getWriteBuffer().desired_torques[leftJointNames[i]] =
893 leftJointDesiredTorques(i);
894
895 leftTargets.at(i)->torque = desiredTorque;
896 }
897
898
899 for (size_t i = 0; i < rightTargets.size(); ++i)
900 {
901 float desiredTorque = rightJointDesiredTorques(i);
902
903 if (isnan(desiredTorque))
904 {
905 desiredTorque = 0;
906 }
907
908 desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
909 desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
910
911 debugDataInfo.getWriteBuffer().desired_torques[rightJointNames[i]] =
912 rightJointDesiredTorques(i);
913
914 rightTargets.at(i)->torque = desiredTorque;
915 }
916
917 // debugDataInfo.getWriteBuffer().desiredForce_x = tcpDesiredForce(0);
918 // debugDataInfo.getWriteBuffer().desiredForce_y = tcpDesiredForce(1);
919 // debugDataInfo.getWriteBuffer().desiredForce_z = tcpDesiredForce(2);
920 // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_x = tcpDesiredTorque(0);
921 // debugDataInfo.getWriteBuffer().leftTcpDesiredTorque_y = tcpDesiredTorque(1);
922 // debugDataInfo.getWriteBuffer().tcpDesiredTorque_z = tcpDesiredTorque(2);
923 // debugDataInfo.getWriteBuffer().quatError = errorAngle;
924 // debugDataInfo.getWriteBuffer().posiError = posiError;
925 debugDataInfo.getWriteBuffer().leftTargetPose_x = leftTargetPose(0, 3);
926 debugDataInfo.getWriteBuffer().leftTargetPose_y = leftTargetPose(1, 3);
927 debugDataInfo.getWriteBuffer().leftTargetPose_z = leftTargetPose(2, 3);
928 debugDataInfo.getWriteBuffer().leftCurrentPose_x = leftCurrentPose(0, 3);
929 debugDataInfo.getWriteBuffer().leftCurrentPose_y = leftCurrentPose(1, 3);
930 debugDataInfo.getWriteBuffer().leftCurrentPose_z = leftCurrentPose(2, 3);
931
932
933 debugDataInfo.getWriteBuffer().rightTargetPose_x = rightTargetPose(0, 3);
934 debugDataInfo.getWriteBuffer().rightTargetPose_y = rightTargetPose(1, 3);
935 debugDataInfo.getWriteBuffer().rightTargetPose_z = rightTargetPose(2, 3);
936
937 debugDataInfo.getWriteBuffer().rightCurrentPose_x = rightCurrentPose(0, 3);
938 debugDataInfo.getWriteBuffer().rightCurrentPose_y = rightCurrentPose(1, 3);
939 debugDataInfo.getWriteBuffer().rightCurrentPose_z = rightCurrentPose(2, 3);
940
941
942 for (size_t i = 0; i < 12; ++i)
943 {
944 std::stringstream ss;
945 ss << i;
946 std::string name = "forceConstrained_" + ss.str();
947 debugDataInfo.getWriteBuffer().constrained_force[name] = filteredForce(i);
948 }
949
950
951 debugDataInfo.commitWrite();
952
953
954 // Eigen::VectorXf leftTargetVel = rtGetControlStruct().leftTargetVel;
955 // float normLinearVelocity = leftTargetVel.block<3, 1>(0, 0).norm();
956 // if (normLinearVelocity > cfg->maxLinearVel)
957 // {
958 // leftTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * leftTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
959 // }
960 // float normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
961 // if (normAngularVelocity > cfg->maxAngularVel)
962 // {
963 // leftTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * leftTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
964 // }
965
966 // Eigen::VectorXf leftNullSpaceJointVelocity = cfg->knull * (leftDesiredJointValues - leftqpos);
967 // Eigen::MatrixXf jtpinvL = leftIK->computePseudoInverseJacobianMatrix(jacobiL, leftIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
968 // Eigen::VectorXf jnvL = jtpinvL * leftTargetVel + (I - jtpinvL * jacobiL) * leftNullSpaceJointVelocity;
969
970 // for (size_t i = 0; i < leftTargets.size(); ++i)
971 // {
972 // leftTargets.at(i)->velocity = jnvL(i);
973 // }
974
975 // Eigen::VectorXf rightTargetVel = rtGetControlStruct().rightTargetVel;
976 // normLinearVelocity = rightTargetVel.block<3, 1>(0, 0).norm();
977 // if (normLinearVelocity > cfg->maxLinearVel)
978 // {
979 // rightTargetVel.block<3, 1>(0, 0) = cfg->maxLinearVel * rightTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
980 // }
981 // normAngularVelocity = leftTargetVel.block<3, 1>(3, 0).norm();
982 // if (normAngularVelocity > cfg->maxAngularVel)
983 // {
984 // rightTargetVel.block<3, 1>(3, 0) = cfg->maxAngularVel * rightTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
985 // }
986 // Eigen::VectorXf rightNullSpaceJointVelocity = cfg->knull * (rightDesiredJointValues - rightqpos);
987 // Eigen::MatrixXf jtpinvR = rightIK->computePseudoInverseJacobianMatrix(jacobiR, rightIK->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
988 // Eigen::VectorXf jnvR = jtpinvR * rightTargetVel + (I - jtpinvR * jacobiR) * rightNullSpaceJointVelocity;
989
990 // for (size_t i = 0; i < rightTargets.size(); ++i)
991 // {
992 // rightTargets.at(i)->velocity = jnvR(i);
993 // }
994 }
995
996 void
997 NJointBimanualCCDMPController::learnDMPFromFiles(const std::string& name,
998 const Ice::StringSeq& fileNames,
999 const Ice::Current&)
1000 {
1001 if (name == "LeftLeader")
1002 {
1003 leftGroup.at(0)->learnDMPFromFiles(fileNames);
1004 }
1005 else if (name == "LeftFollower")
1006 {
1007 rightGroup.at(1)->learnDMPFromFiles(fileNames);
1008 }
1009 else if (name == "RightLeader")
1010 {
1011 rightGroup.at(0)->learnDMPFromFiles(fileNames);
1012 }
1013 else
1014 {
1015 leftGroup.at(1)->learnDMPFromFiles(fileNames);
1016 }
1017 }
1018
1019 void
1020 NJointBimanualCCDMPController::setViaPoints(Ice::Double u,
1021 const Ice::DoubleSeq& viapoint,
1022 const Ice::Current&)
1023 {
1024 LockGuardType guard(controllerMutex);
1025 if (leaderName == "Left")
1026 {
1027 leftGroup.at(0)->setViaPose(u, viapoint);
1028 }
1029 else
1030 {
1031 rightGroup.at(0)->setViaPose(u, viapoint);
1032 }
1033 }
1034
1035 void
1036 NJointBimanualCCDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
1037 {
1038 LockGuardType guard(controllerMutex);
1039 if (leaderName == "Left")
1040 {
1041 leftGroup.at(0)->setGoalPoseVec(goals);
1042 }
1043 else
1044 {
1045 rightGroup.at(0)->setGoalPoseVec(goals);
1046 }
1047 }
1048
1049 void
1050 NJointBimanualCCDMPController::changeLeader(const Ice::Current&)
1051 {
1052 LockGuardType guard(controllerMutex);
1053
1054 if (leaderName == "Left")
1055 {
1056 leaderName = "Right";
1057 rightGroup.at(0)->canVal = virtualtimer;
1058 rightGroup.at(1)->canVal = virtualtimer;
1059 }
1060 else
1061 {
1062 leaderName = "Left";
1063 leftGroup.at(0)->canVal = virtualtimer;
1064 leftGroup.at(1)->canVal = virtualtimer;
1065 }
1066 }
1067
1068 void
1069 NJointBimanualCCDMPController::runDMP(const Ice::DoubleSeq& leftGoals,
1070 const Ice::DoubleSeq& rightGoals,
1071 const Ice::Current&)
1072 {
1073 virtualtimer = cfg->timeDuration;
1074
1075 Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
1076 Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
1077
1078 leftGroup.at(0)->prepareExecution(leftGroup.at(0)->eigen4f2vec(leftPose), leftGoals);
1079 rightGroup.at(0)->prepareExecution(rightGroup.at(0)->eigen4f2vec(rightPose), rightGoals);
1080
1081
1082 ARMARX_INFO << "leftgroup goal local pose: " << getLocalPose(leftGoals, rightGoals);
1083
1084 leftGroup.at(1)->prepareExecution(getLocalPose(leftPose, rightPose),
1085 getLocalPose(leftGoals, rightGoals));
1086 rightGroup.at(1)->prepareExecution(getLocalPose(rightPose, leftPose),
1087 getLocalPose(rightGoals, leftGoals));
1088
1089 controllerTask->start();
1090 }
1091
1092 Eigen::Matrix4f
1093 NJointBimanualCCDMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate,
1094 const Eigen::Matrix4f& globalTargetPose)
1095 {
1096 Eigen::Matrix4f localPose;
1097 localPose.block<3, 3>(0, 0) =
1098 globalTargetPose.block<3, 3>(0, 0) * newCoordinate.block<3, 3>(0, 0).inverse();
1099 localPose.block<3, 1>(0, 3) =
1100 localPose.block<3, 3>(0, 0) *
1101 (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
1102
1103 return localPose;
1104 }
1105
1106 void
1107 NJointBimanualCCDMPController::onPublish(const SensorAndControl&,
1108 const DebugDrawerInterfacePrx&,
1109 const DebugObserverInterfacePrx& debugObs)
1110 {
1111
1112 StringVariantBaseMap datafields;
1113 auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
1114 for (auto& pair : values)
1115 {
1116 datafields[pair.first] = new Variant(pair.second);
1117 }
1118
1119 auto constrained_force = debugDataInfo.getUpToDateReadBuffer().constrained_force;
1120 for (auto& pair : constrained_force)
1121 {
1122 datafields[pair.first] = new Variant(pair.second);
1123 }
1124
1125
1126 datafields["leftTargetPose_x"] =
1127 new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_x);
1128 datafields["leftTargetPose_y"] =
1129 new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_y);
1130 datafields["leftTargetPose_z"] =
1131 new Variant(debugDataInfo.getUpToDateReadBuffer().leftTargetPose_z);
1132 datafields["rightTargetPose_x"] =
1133 new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_x);
1134 datafields["rightTargetPose_y"] =
1135 new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_y);
1136 datafields["rightTargetPose_z"] =
1137 new Variant(debugDataInfo.getUpToDateReadBuffer().rightTargetPose_z);
1138
1139
1140 datafields["leftCurrentPose_x"] =
1141 new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_x);
1142 datafields["leftCurrentPose_y"] =
1143 new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_y);
1144 datafields["leftCurrentPose_z"] =
1145 new Variant(debugDataInfo.getUpToDateReadBuffer().leftCurrentPose_z);
1146 datafields["rightCurrentPose_x"] =
1147 new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_x);
1148 datafields["rightCurrentPose_y"] =
1149 new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_y);
1150 datafields["rightCurrentPose_z"] =
1151 new Variant(debugDataInfo.getUpToDateReadBuffer().rightCurrentPose_z);
1152
1153 // StringVariantBaseMap datafields;
1154 // auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
1155 // for (auto& pair : values)
1156 // {
1157 // datafields[pair.first] = new Variant(pair.second);
1158 // }
1159
1160 // auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
1161 // for (auto& pair : dmpTargets)
1162 // {
1163 // datafields[pair.first] = new Variant(pair.second);
1164 // }
1165
1166 // auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
1167 // for (auto& pair : currentPose)
1168 // {
1169 // datafields[pair.first] = new Variant(pair.second);
1170 // }
1171
1172 // datafields["leaderCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderCanVal);
1173 // datafields["leadermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadermpcFactor);
1174 // datafields["leaderError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leadererror);
1175 // datafields["leaderposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderposError);
1176 // datafields["leaderoriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().leaderoriError);
1177
1178 // datafields["followerCanVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerCanVal);
1179 // datafields["followermpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().followermpcFactor);
1180 // datafields["followerError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followererror);
1181 // datafields["followerposError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followerposError);
1182 // datafields["followeroriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().followeroriError);
1183
1184 debugObs->setDebugChannel("DMPController", datafields);
1185 }
1186
1187 void
1194
1195 void
1196 NJointBimanualCCDMPController::onDisconnectComponent()
1197 {
1198 controllerTask->stop();
1199 ARMARX_INFO << "stopped ...";
1200 }
1201
1202
1203} // namespace armarx
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
armarx::NJointBimanualCCDMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition NJointBimanualDMPForceController.cpp:1036
armarx::NJointBimanualCCDMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition NJointBimanualDMPForceController.cpp:513
armarx::NJointBimanualCCDMPController::getClassName
std::string getClassName(const Ice::Current &) const
Definition NJointBimanualDMPForceController.cpp:261
armarx::NJointBimanualCCDMPController::runDMP
void runDMP(const Ice::DoubleSeq &leftGoals, const Ice::DoubleSeq &rightGoals, const Ice::Current &)
Definition NJointBimanualDMPForceController.cpp:1069
armarx::NJointBimanualCCDMPController::onInitComponent
void onInitComponent()
Pure virtual hook for the subclass.
Definition NJointBimanualDMPForceController.cpp:1188
armarx::NJointBimanualCCDMPController::NJointBimanualCCDMPController
NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
armarx::NJointBimanualCCDMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointBimanualDMPForceController.cpp:1107
armarx::NJointBimanualCCDMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition NJointBimanualDMPForceController.cpp:1020
armarx::NJointBimanualCCDMPController::learnDMPFromFiles
void learnDMPFromFiles(const std::string &, const Ice::StringSeq &, const Ice::Current &)
Definition NJointBimanualDMPForceController.cpp:997
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_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_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::tsvmp::TaskSpaceDMPControllerPtr
std::shared_ptr< TaskSpaceDMPController > TaskSpaceDMPControllerPtr
Definition NJointTaskSpaceImpedanceDMPController.h:31
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
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::registrationControllerNJointBimanualCCDMPController
NJointControllerRegistration< NJointBimanualCCDMPController > registrationControllerNJointBimanualCCDMPController("NJointBimanualCCDMPController")
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition JointController.h:42
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition NJointControllerBase.h:73