NJointBimanualForceController.cpp
Go to the documentation of this file.
1#include "NJointBimanualForceController.h"
2
3#include <ArmarXCore/core/ArmarXObjectScheduler.h>
4#include <ArmarXCore/core/time/CycleUtil.h>
5
6// Simox
7#include <VirtualRobot/IK/DifferentialIK.h>
8#include <VirtualRobot/MathTools.h>
9#include <VirtualRobot/Nodes/RobotNode.h>
10#include <VirtualRobot/Nodes/Sensor.h>
11#include <VirtualRobot/Robot.h>
12#include <VirtualRobot/RobotNodeSet.h>
13
14// RobotAPI
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/PIDController.h>
21#include <RobotAPI/libraries/core/math/MathUtils.h>
22
23namespace armarx::control::deprecated_njoint_mp_controller::bimanual
24{
25 NJointControllerRegistration<NJointBimanualForceController>
26 registrationControllerNJointBimanualForceController("NJointBimanualForceController");
27
28 NJointBimanualForceController::NJointBimanualForceController(
29 const RobotUnitPtr& robUnit,
30 const armarx::NJointControllerConfigPtr& config,
31 const VirtualRobot::RobotPtr&)
32 {
33 ARMARX_INFO << "Preparing ... bimanual ";
34 useSynchronizedRtRobot();
35 cfg = NJointBimanualForceControllerConfigPtr::dynamicCast(config);
36 // ARMARX_CHECK_EXPRESSION(prov);
37 // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
38 // ARMARX_CHECK_EXPRESSION(robotUnit);
39 leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
40
41 for (size_t i = 0; i < leftRNS->getSize(); ++i)
42 {
43 std::string jointName = leftRNS->getNode(i)->getName();
44 leftJointNames.push_back(jointName);
45 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
46 const SensorValueBase* sv = useSensorValue(jointName);
47 leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
48 const SensorValue1DoFActuatorVelocity* velocitySensor =
49 sv->asA<SensorValue1DoFActuatorVelocity>();
50 const SensorValue1DoFActuatorPosition* positionSensor =
51 sv->asA<SensorValue1DoFActuatorPosition>();
52
53 if (!velocitySensor)
54 {
55 ARMARX_WARNING << "No velocitySensor available for " << jointName;
56 }
57 if (!positionSensor)
58 {
59 ARMARX_WARNING << "No positionSensor available for " << jointName;
60 }
61
62
63 leftVelocitySensors.push_back(velocitySensor);
64 leftPositionSensors.push_back(positionSensor);
65 };
66
67 rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
68
69 for (size_t i = 0; i < rightRNS->getSize(); ++i)
70 {
71 std::string jointName = rightRNS->getNode(i)->getName();
72 rightJointNames.push_back(jointName);
73 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
74 const SensorValueBase* sv = useSensorValue(jointName);
75 rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
76
77 const SensorValue1DoFActuatorVelocity* velocitySensor =
78 sv->asA<SensorValue1DoFActuatorVelocity>();
79 const SensorValue1DoFActuatorPosition* positionSensor =
80 sv->asA<SensorValue1DoFActuatorPosition>();
81
82 if (!velocitySensor)
83 {
84 ARMARX_WARNING << "No velocitySensor available for " << jointName;
85 }
86 if (!positionSensor)
87 {
88 ARMARX_WARNING << "No positionSensor available for " << jointName;
89 }
90
91 rightVelocitySensors.push_back(velocitySensor);
92 rightPositionSensors.push_back(positionSensor);
93 };
94
95
96 // const SensorValueBase* svlf = prov->getSensorValue("FT L");
97 const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
98 leftForceTorque = svlf->asA<SensorValueForceTorque>();
99 // const SensorValueBase* svrf = prov->getSensorValue("FT R");
100 const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
101 rightForceTorque = svrf->asA<SensorValueForceTorque>();
102
103 leftIK.reset(new VirtualRobot::DifferentialIK(
104 leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
105 rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
106 rightRNS->getRobot()->getRootNode(),
107 VirtualRobot::JacobiProvider::eSVDDamped));
108
109
110 tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
111 taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
112 taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
113 taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
114 taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
115 taskSpaceDMPConfig.DMPAmplitude = 1.0;
116 taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
117 taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
118 taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
119 taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
120 taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
121 taskSpaceDMPConfig.phaseStopParas.Dori = 0;
122 taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
123 taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
124 taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
125
126
127 objectDMP.reset(new tsvmp::TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
128 ARMARX_IMPORTANT << "dmp finieshed";
129
130 tcpLeft = leftRNS->getTCP();
131 tcpRight = rightRNS->getTCP();
132
133 KpImpedance.resize(cfg->KpImpedance.size());
134 ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
135
136 for (int i = 0; i < KpImpedance.size(); ++i)
137 {
138 KpImpedance(i) = cfg->KpImpedance.at(i);
139 }
140
141 KdImpedance.resize(cfg->KdImpedance.size());
142 ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
143
144 for (int i = 0; i < KdImpedance.size(); ++i)
145 {
146 KdImpedance(i) = cfg->KdImpedance.at(i);
147 }
148
149 KpAdmittance.resize(cfg->KpAdmittance.size());
150 ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
151
152 for (int i = 0; i < KpAdmittance.size(); ++i)
153 {
154 KpAdmittance(i) = cfg->KpAdmittance.at(i);
155 }
156
157 KdAdmittance.resize(cfg->KdAdmittance.size());
158 ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
159
160 for (int i = 0; i < KdAdmittance.size(); ++i)
161 {
162 KdAdmittance(i) = cfg->KdAdmittance.at(i);
163 }
164
165 KmAdmittance.resize(cfg->KmAdmittance.size());
166 ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
167
168 for (int i = 0; i < KmAdmittance.size(); ++i)
169 {
170 KmAdmittance(i) = cfg->KmAdmittance.at(i);
171 }
172
173 leftDesiredJointValues.resize(leftTargets.size());
174 ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
175
176 for (size_t i = 0; i < leftTargets.size(); ++i)
177 {
178 leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
179 }
180
181 rightDesiredJointValues.resize(rightTargets.size());
182 ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
183
184 for (size_t i = 0; i < rightTargets.size(); ++i)
185 {
186 rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
187 }
188
189 KmPID.resize(cfg->KmPID.size());
190 ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
191
192 for (int i = 0; i < KmPID.size(); ++i)
193 {
194 KmPID(i) = cfg->KmPID.at(i);
195 }
196
197
198 modifiedAcc.setZero(12);
199 modifiedTwist.setZero(12);
200 ARMARX_INFO << "got controller params";
201
202
203 boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4],
204 cfg->boxInitialPose[5],
205 cfg->boxInitialPose[6],
206 cfg->boxInitialPose[3]);
207 for (int i = 0; i < 3; ++i)
208 {
209 boxInitialPose(i, 3) = cfg->boxInitialPose[i];
210 }
211
212 NJointBimanualForceControllerSensorData initSensorData;
213 initSensorData.deltaT = 0;
214 initSensorData.currentTime = 0;
215 initSensorData.currentPose = boxInitialPose;
216 initSensorData.currentTwist.setZero();
217 controllerSensorData.reinitAllBuffers(initSensorData);
218
219
220 NJointBimanualForceControllerInterfaceData initInterfaceData;
221 initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
222 initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
223 interfaceData.reinitAllBuffers(initInterfaceData);
224
225 leftInitialPose = boxInitialPose;
226 leftInitialPose(0, 3) -= cfg->boxWidth;
227 rightInitialPose = boxInitialPose;
228 rightInitialPose(0, 3) += cfg->boxWidth;
229
230 forcePIDControllers.resize(12);
231 for (size_t i = 0; i < 6; i++)
232 {
233 forcePIDControllers.at(i).reset(new PIDController(
234 cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
235 forcePIDControllers.at(i + 6).reset(new PIDController(
236 cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
237 forcePIDControllers.at(i)->reset();
238 forcePIDControllers.at(i + 6)->reset();
239 }
240
241 // filter
242 filterCoeff = cfg->filterCoeff;
243 ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
244 filteredOldValue.setZero(12);
245
246 // static compensation
247 massLeft = cfg->massLeft;
248 CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
249 forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1],
250 cfg->forceOffsetLeft[2];
251 torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1],
252 cfg->torqueOffsetLeft[2];
253
254 massRight = cfg->massRight;
255 CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
256 forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1],
257 cfg->forceOffsetRight[2];
258 torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1],
259 cfg->torqueOffsetRight[2];
260
261 sensorFrame2TcpFrameLeft.setZero();
262 sensorFrame2TcpFrameRight.setZero();
263
264 NJointBimanualForceControlData initData;
265 initData.boxPose = boxInitialPose;
266 initData.boxTwist.setZero(6);
267 reinitTripleBuffer(initData);
268
269 firstLoop = true;
270 ARMARX_INFO << "left initial pose: \n"
271 << leftInitialPose << "\n right initial pose: \n"
272 << rightInitialPose;
273
274 ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
275 ARMARX_IMPORTANT << "finished construction!";
276
277 dmpStarted = false;
278
279 targetWrench.setZero(cfg->targetWrench.size());
280 for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
281 {
282 targetWrench(i) = cfg->targetWrench[i];
283 }
284 }
285
286 std::string
287 NJointBimanualForceController::getClassName(const Ice::Current&) const
288 {
289 return "NJointBimanualForceController";
290 }
291
292 // void NJointBimanualForceController::rtPreActivateController()
293 // {
294 // // modifiedLeftPose = tcpLeft->getPoseInRootFrame();
295 // // modifiedRightPose = tcpRight->getPoseInRootFrame();
296 // // Eigen::Matrix4f leftSensorFrame = leftRNS->getRobot()->getRobotNode("ArmL_FT")->getPoseInRootFrame();
297 // // Eigen::Matrix4f rightSensorFrame = rightRNS->getRobot()->getRobotNode("ArmR_FT")->getPoseInRootFrame();
298 // // sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) = modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
299 // // sensorFrame2TcpFrameRight.block<3, 3>(0, 0) = modifiedRightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
300 // // CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
301 // // CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
302 // }
303
304
305 void
306 NJointBimanualForceController::controllerRun()
307 {
308 if (!controllerSensorData.updateReadBuffer() || !dmpStarted)
309 {
310 return;
311 }
312
313 double deltaT = controllerSensorData.getReadBuffer().deltaT;
314 Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
315 Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
316
317 if (objectDMP->canVal < 1e-8)
318 {
319 finished = true;
320 }
321
322 objectDMP->flow(deltaT, currentPose, currentTwist);
323
324 LockGuardType guard{controlDataMutex};
325 getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
326 getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
327 writeControlStruct();
328 }
329
330 void
331 NJointBimanualForceController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
332 const IceUtil::Time& timeSinceLastIteration)
333 {
334 if (firstLoop)
335 {
336 modifiedLeftPose = tcpLeft->getPoseInRootFrame();
337 modifiedRightPose = tcpRight->getPoseInRootFrame();
338 modifiedLeftPose.block<3, 1>(0, 3) = modifiedLeftPose.block<3, 1>(0, 3) * 0.001;
339 modifiedRightPose.block<3, 1>(0, 3) = modifiedRightPose.block<3, 1>(0, 3) * 0.001;
340
341 Eigen::Matrix4f leftSensorFrame =
342 rtGetRobot()->getSensor("FT L")->getRobotNode()->getPoseInRootFrame();
343 Eigen::Matrix4f rightSensorFrame =
344 rtGetRobot()->getSensor("FT R")->getRobotNode()->getPoseInRootFrame();
345 leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
346 rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
347
348 sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) =
349 modifiedLeftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
350 sensorFrame2TcpFrameRight.block<3, 3>(0, 0) =
351 modifiedRightPose.block<3, 3>(0, 0).transpose() *
352 rightSensorFrame.block<3, 3>(0, 0);
353 CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
354 CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
355 firstLoop = false;
356 ARMARX_INFO << "modified left pose:\n " << modifiedLeftPose;
357 ARMARX_INFO << "modified right pose:\n " << modifiedRightPose;
358 }
359 double deltaT = timeSinceLastIteration.toSecondsDouble();
360
361
362 // grasp matrix
363 Eigen::Vector3f rToBoxCoM;
364 rToBoxCoM << cfg->boxWidth, 0, 0;
365 Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
366 Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
367
368 interfaceData.getWriteBuffer().currentLeftPose = currentLeftPose;
369 interfaceData.getWriteBuffer().currentRightPose = currentRightPose;
370 interfaceData.commitWrite();
371
372 Eigen::VectorXf currentTargetWrench = targetWrench;
373 if (fabs(currentLeftPose(0, 3) - currentRightPose(0, 3)) < 0.8 * 2 * cfg->boxWidth)
374 {
375 currentTargetWrench.setZero();
376 }
377 currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
378 currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
379 Eigen::MatrixXf graspMatrix;
380 graspMatrix.setZero(6, 12);
381 graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
382 graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
383 Eigen::Vector3f r = -currentLeftPose.block<3, 3>(0, 0) * rToBoxCoM;
384 graspMatrix(4, 2) = -r(0);
385 graspMatrix(3, 2) = r(1);
386 graspMatrix(3, 1) = -r(2);
387 graspMatrix(4, 0) = r(2);
388 graspMatrix(5, 0) = -r(1);
389 graspMatrix(5, 1) = r(0);
390 r = currentRightPose.block<3, 3>(0, 0) * rToBoxCoM;
391 graspMatrix(4, 8) = -r(0);
392 graspMatrix(3, 8) = r(1);
393 graspMatrix(3, 7) = -r(2);
394 graspMatrix(4, 6) = r(2);
395 graspMatrix(5, 6) = -r(1);
396 graspMatrix(5, 7) = r(0);
397 // projection of grasp matrix
398 Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
399 Eigen::MatrixXf G_range = pinvG * graspMatrix;
400 Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
401
402 // box pose
403 Eigen::Matrix4f boxCurrentPose = currentLeftPose;
404 boxCurrentPose.block<3, 1>(0, 3) =
405 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
406 Eigen::VectorXf boxCurrentTwist;
407 boxCurrentTwist.setZero(6);
408
409 // cartesian vel controller
410 Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
411
412 Eigen::MatrixXf jacobiL =
413 leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
414
415 Eigen::VectorXf leftqpos;
416 Eigen::VectorXf leftqvel;
417 leftqpos.resize(leftPositionSensors.size());
418 leftqvel.resize(leftVelocitySensors.size());
419 for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
420 {
421 leftqpos(i) = leftPositionSensors[i]->position;
422 leftqvel(i) = leftVelocitySensors[i]->velocity;
423 }
424
425 Eigen::MatrixXf jacobiR =
426 rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
427
428 // jacobiL used in L304
429 jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
430 jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
431
432 Eigen::VectorXf rightqpos;
433 Eigen::VectorXf rightqvel;
434 rightqpos.resize(rightPositionSensors.size());
435 rightqvel.resize(rightVelocitySensors.size());
436
437 for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
438 {
439 rightqpos(i) = rightPositionSensors[i]->position;
440 rightqvel(i) = rightVelocitySensors[i]->velocity;
441 }
442
443 // what is the unit of jacobiL, 0.001?
444 Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
445 Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
446 Eigen::VectorXf currentTwist(12);
447 currentTwist << currentLeftTwist, currentRightTwist;
448 Eigen::MatrixXf pinvGraspMatrixT =
449 leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), 0);
450 boxCurrentTwist = pinvGraspMatrixT * currentTwist;
451
452
453 controllerSensorData.getWriteBuffer().currentPose = boxCurrentPose;
454 controllerSensorData.getWriteBuffer().currentTwist = boxCurrentTwist;
455 controllerSensorData.getWriteBuffer().deltaT = deltaT;
456 controllerSensorData.getWriteBuffer().currentTime += deltaT;
457 controllerSensorData.commitWrite();
458
459
460 Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
461 Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
462
463 Eigen::VectorXf leftJointControlWrench;
464 Eigen::VectorXf rightJointControlWrench;
465
466
467 //Todo: calculate desired wrench from required box pose
468 // Eigen::VectorXf boxPoseError(6);
469 // Eigen::Matrix3f diffMat = boxPose.block<3, 3>(0, 0) * boxCurrentPose.block<3, 3>(0, 0).transpose();
470 // boxPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
471 // boxPoseError.head<3>() = boxPose.block<3, 1>(0, 3) - boxCurrentPose.block<3, 1>(0, 3);
472 // Eigen::VectorXf computedBoxWrench(6);
473 // computedBoxWrench = KpAdmittance.cwiseProduct(boxPoseError);// + KdAdmittance.cwiseProduct(boxTwist - boxCurrentTwist);
474 // Eigen::VectorXf wrenchDMP = graspMatrix.transpose() * computedBoxWrench;
475 // wrenchDMP.setZero();
476 Eigen::VectorXf twistDMP = graspMatrix.transpose() * boxTwist;
477 Eigen::VectorXf deltaInitialPose = deltaT * twistDMP;
478 leftInitialPose.block<3, 1>(0, 3) =
479 leftInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(0, 0);
480 rightInitialPose.block<3, 1>(0, 3) =
481 rightInitialPose.block<3, 1>(0, 3) + deltaInitialPose.block<3, 1>(6, 0);
482 leftInitialPose.block<3, 3>(0, 0) =
483 VirtualRobot::MathTools::rpy2eigen3f(
484 deltaInitialPose(3), deltaInitialPose(4), deltaInitialPose(5)) *
485 leftInitialPose.block<3, 3>(0, 0);
486 rightInitialPose.block<3, 3>(0, 0) =
487 VirtualRobot::MathTools::rpy2eigen3f(
488 deltaInitialPose(9), deltaInitialPose(10), deltaInitialPose(11)) *
489 rightInitialPose.block<3, 3>(0, 0);
490
491
492 // static compensation
493 Eigen::Vector3f gravity;
494 gravity << 0.0, 0.0, -9.8;
495 Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
496 Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
497 Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
498
499 Eigen::Vector3f localGravityRight =
500 currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
501 Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
502 Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
503
504 // mapping of measured wrenches
505 Eigen::VectorXf wrenchesMeasured(12);
506 wrenchesMeasured << leftForceTorque->force - forceOffsetLeft,
507 leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight,
508 rightForceTorque->torque - torqueOffsetRight;
509 for (size_t i = 0; i < 12; i++)
510 {
511 wrenchesMeasured(i) =
512 (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
513 }
514 filteredOldValue = wrenchesMeasured;
515 wrenchesMeasured.block<3, 1>(0, 0) =
516 sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
517 localForceVecLeft;
518 wrenchesMeasured.block<3, 1>(3, 0) =
519 sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
520 localTorqueVecLeft;
521 wrenchesMeasured.block<3, 1>(6, 0) =
522 sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
523 localForceVecRight;
524 wrenchesMeasured.block<3, 1>(9, 0) =
525 sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
526 localTorqueVecRight;
527 if (wrenchesMeasured.norm() < cfg->forceThreshold)
528 {
529 wrenchesMeasured.setZero();
530 }
531
532 // PID force controller
533 // Eigen::VectorXf wrenchesConstrainedInLocal(12);
534 // wrenchesConstrainedInLocal.block<3, 1>(0, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(0, 0);
535 // wrenchesConstrainedInLocal.block<3, 1>(3, 0) = currentLeftPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(3, 0);
536 // wrenchesConstrainedInLocal.block<3, 1>(6, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(6, 0);
537 // wrenchesConstrainedInLocal.block<3, 1>(9, 0) = currentRightPose.block<3, 3>(0, 0).transpose() * wrenchesConstrained.block<3, 1>(9, 0);
538 Eigen::VectorXf forcePID(12);
539 // Eigen::VectorXf forcePIDControlValue(12);
540 // for (size_t i = 0; i < 12; i++)
541 // {
542 // forcePIDControllers[i]->update(deltaT, wrenchesConstrainedInLocal(i), cfg->targetWrench[i]);
543 // forcePIDControllers[i]->update(deltaT, wrenchesMeasured(i), cfg->targetWrench[i]);
544 // forcePIDControlValue(i) = forcePIDControllers[i]->getControlValue();
545 // forcePID(i) = - forcePIDControllers[i]->getControlValue();
546
547 // }
548 for (size_t i = 0; i < 6; i++)
549 {
550 forcePID(i) = cfg->forceP[i] * (currentTargetWrench(i) - wrenchesMeasured(i));
551 forcePID(i + 6) =
552 cfg->forceP[i] * (currentTargetWrench(i + 6) - wrenchesMeasured(i + 6));
553 }
554 Eigen::VectorXf forcePIDInRoot(12);
555 forcePIDInRoot.block<3, 1>(0, 0) =
556 currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(0, 0);
557 forcePIDInRoot.block<3, 1>(3, 0) =
558 currentLeftPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(3, 0);
559 forcePIDInRoot.block<3, 1>(6, 0) =
560 currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(6, 0);
561 forcePIDInRoot.block<3, 1>(9, 0) =
562 currentRightPose.block<3, 3>(0, 0) * forcePID.block<3, 1>(9, 0);
563
564 // forcePIDInRoot = PG * forcePIDInRoot;
565 Eigen::VectorXf forcePIDInRootForDebug = forcePIDInRoot;
566
567 Eigen::VectorXf wrenchesMeasuredInRoot(12);
568 wrenchesMeasuredInRoot.block<3, 1>(0, 0) =
569 currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
570 wrenchesMeasuredInRoot.block<3, 1>(3, 0) =
571 currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
572 wrenchesMeasuredInRoot.block<3, 1>(6, 0) =
573 currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
574 wrenchesMeasuredInRoot.block<3, 1>(9, 0) =
575 currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
576 Eigen::VectorXf wrenchesConstrained = PG * wrenchesMeasuredInRoot;
577 // wrenchesConstrained.setZero();
578
579
580 // admittance
581 Eigen::VectorXf poseError(12);
582 poseError.block<3, 1>(0, 0) =
583 leftInitialPose.block<3, 1>(0, 3) - modifiedLeftPose.block<3, 1>(0, 3);
584 Eigen::Matrix3f diffMat =
585 leftInitialPose.block<3, 3>(0, 0) * modifiedLeftPose.block<3, 3>(0, 0).transpose();
586 poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
587 poseError.block<3, 1>(6, 0) =
588 rightInitialPose.block<3, 1>(0, 3) - modifiedRightPose.block<3, 1>(0, 3);
589 diffMat =
590 rightInitialPose.block<3, 3>(0, 0) * modifiedRightPose.block<3, 3>(0, 0).transpose();
591 poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
592
593 Eigen::VectorXf acc;
594 Eigen::VectorXf twist;
595 twist.setZero(12);
596 acc.setZero(12);
597 for (size_t i = 0; i < 6; i++)
598 {
599 // acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) + wrenchDMP(i) - KmAdmittance(i) * wrenchesConstrained(i);
600 // acc(i + 6) = KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) + wrenchDMP(i + 6) - KmAdmittance(i) * wrenchesConstrained(i + 6);
601 acc(i) = KpAdmittance(i) * poseError(i) - KdAdmittance(i) * modifiedTwist(i) -
602 KmAdmittance(i) * wrenchesConstrained(i) - KmPID(i) * forcePIDInRoot(i);
603 acc(i + 6) =
604 KpAdmittance(i) * poseError(i + 6) - KdAdmittance(i) * modifiedTwist(i + 6) -
605 KmAdmittance(i) * wrenchesConstrained(i + 6) - KmPID(i) * forcePIDInRoot(i + 6);
606 }
607 twist = modifiedTwist + 0.5 * deltaT * (acc + modifiedAcc);
608 Eigen::VectorXf deltaPose = 0.5 * deltaT * (twist + modifiedTwist);
609 modifiedAcc = acc;
610 modifiedTwist = twist;
611
612 modifiedLeftPose.block<3, 1>(0, 3) =
613 modifiedLeftPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(0, 0);
614 modifiedRightPose.block<3, 1>(0, 3) =
615 modifiedRightPose.block<3, 1>(0, 3) + deltaPose.block<3, 1>(6, 0);
616 modifiedLeftPose.block<3, 3>(0, 0) =
617 VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5)) *
618 modifiedLeftPose.block<3, 3>(0, 0);
619 modifiedRightPose.block<3, 3>(0, 0) =
620 VirtualRobot::MathTools::rpy2eigen3f(deltaPose(9), deltaPose(10), deltaPose(11)) *
621 modifiedRightPose.block<3, 3>(0, 0);
622
623 // for (size_t i = 0; i < 6; i++)
624 // {
625 // poseError(i) = (wrenchDMP(i) + wrenchesConstrained(i)) / KpAdmittance(i);
626 // poseError(i + 6) = (wrenchDMP(i + 6) + wrenchesConstrained(i + 6)) / KpAdmittance(i);
627 // }
628 // modifiedLeftPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(0, 0);
629 // modifiedRightPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3) + poseError.block<3, 1>(6, 0);
630 // modifiedLeftPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(3), poseError(4), poseError(5)) * leftInitialPose.block<3, 3>(0, 0);
631 // modifiedRightPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(poseError(9), poseError(10), poseError(11)) * rightInitialPose.block<3, 3>(0, 0) * ;
632
633 // impedance control
634 diffMat =
635 modifiedLeftPose.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
636 poseError.block<3, 1>(0, 0) =
637 modifiedLeftPose.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
638 poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
639
640 diffMat =
641 modifiedRightPose.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
642 poseError.block<3, 1>(6, 0) =
643 modifiedRightPose.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
644 poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
645
646 Eigen::VectorXf forceImpedance(12);
647 for (size_t i = 0; i < 6; i++)
648 {
649 forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
650 forceImpedance(i + 6) =
651 KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
652 }
653
654
655 // nullspace
656 Eigen::VectorXf leftNullspaceTorque =
657 cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
658 Eigen::VectorXf rightNullspaceTorque =
659 cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
660
661 float lambda = 1;
662
663 // forcePIDInRoot.setZero();
664 forcePIDInRoot.setZero();
665 leftJointControlWrench = forceImpedance.head<6>() + forcePIDInRoot.head<6>();
666 rightJointControlWrench = forceImpedance.tail<6>() + forcePIDInRoot.tail<6>();
667 Eigen::MatrixXf jtpinvL =
668 leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
669 Eigen::MatrixXf jtpinvR =
670 rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
671 Eigen::VectorXf leftJointDesiredTorques =
672 jacobiL.transpose() * leftJointControlWrench +
673 (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
674 Eigen::VectorXf rightJointDesiredTorques =
675 jacobiR.transpose() * rightJointControlWrench +
676 (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
677
678
679 // torque limit
680 for (size_t i = 0; i < leftTargets.size(); ++i)
681 {
682 float desiredTorque = leftJointDesiredTorques(i);
683
684 if (isnan(desiredTorque))
685 {
686 desiredTorque = 0;
687 }
688
689 desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
690 desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
691
692 debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] =
693 leftJointDesiredTorques(i);
694
695 leftTargets.at(i)->torque = desiredTorque;
696 }
697
698
699 for (size_t i = 0; i < rightTargets.size(); ++i)
700 {
701 float desiredTorque = rightJointDesiredTorques(i);
702
703 if (isnan(desiredTorque))
704 {
705 desiredTorque = 0;
706 }
707
708 desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
709 desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
710
711 debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] =
712 rightJointDesiredTorques(i);
713
714 rightTargets.at(i)->torque = desiredTorque;
715 }
716 // debugOutputData.getWriteBuffer().leftControlSignal_x = leftJointControlWrench(0);
717
718
719 debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
720 debugOutputData.getWriteBuffer().poseError = poseError;
721 debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
722 debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
723 // debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
724 // debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
725
726 debugOutputData.getWriteBuffer().modifiedPoseRight_x = modifiedRightPose(0, 3);
727 debugOutputData.getWriteBuffer().modifiedPoseRight_y = modifiedRightPose(1, 3);
728 debugOutputData.getWriteBuffer().modifiedPoseRight_z = modifiedRightPose(2, 3);
729
730 debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
731 debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
732 debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
733
734
735 debugOutputData.getWriteBuffer().modifiedPoseLeft_x = modifiedLeftPose(0, 3);
736 debugOutputData.getWriteBuffer().modifiedPoseLeft_y = modifiedLeftPose(1, 3);
737 debugOutputData.getWriteBuffer().modifiedPoseLeft_z = modifiedLeftPose(2, 3);
738
739 debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
740 debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
741 debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
742
743
744 debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
745 debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
746 debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
747
748 debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
749 debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
750 debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
751 debugOutputData.getWriteBuffer().rx = r(0);
752 debugOutputData.getWriteBuffer().ry = r(1);
753 debugOutputData.getWriteBuffer().rz = r(2);
754
755 debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
756 debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
757 debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
758 debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
759 debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
760 debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
761
762 debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
763
764 debugOutputData.commitWrite();
765 }
766
767 void
768 NJointBimanualForceController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
769 const Ice::Current&)
770 {
771 objectDMP->learnDMPFromFiles(fileNames);
772 }
773
774 void
775 NJointBimanualForceController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
776 {
777 LockGuardType guard(controllerMutex);
778 objectDMP->setGoalPoseVec(goals);
779 }
780
781 void
782 NJointBimanualForceController::runDMP(const Ice::DoubleSeq& goals, const Ice::Current&)
783 {
784 while (!interfaceData.updateReadBuffer())
785 {
786 usleep(1000);
787 }
788
789 Eigen::Matrix4f leftPose = interfaceData.getUpToDateReadBuffer().currentLeftPose;
790 Eigen::Matrix4f rightPose = interfaceData.getUpToDateReadBuffer().currentRightPose;
791
792 VirtualRobot::MathTools::Quaternion boxOri =
793 VirtualRobot::MathTools::eigen4f2quat(leftPose);
794 Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
795
796 std::vector<double> boxInitialPose;
797 for (size_t i = 0; i < 3; ++i)
798 {
799 boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
800 }
801 boxInitialPose.push_back(boxOri.w);
802 boxInitialPose.push_back(boxOri.x);
803 boxInitialPose.push_back(boxOri.y);
804 boxInitialPose.push_back(boxOri.z);
805
806 virtualtimer = cfg->timeDuration;
807 objectDMP->prepareExecution(boxInitialPose, goals);
808
809 finished = false;
810 dmpStarted = true;
811 }
812
813 void
814 NJointBimanualForceController::setViaPoints(Ice::Double u,
815 const Ice::DoubleSeq& viapoint,
816 const Ice::Current&)
817 {
818 // LockGuardType guard(controllerMutex);
819 ARMARX_INFO << "setting via points ";
820 objectDMP->setViaPose(u, viapoint);
821 }
822
823 void
824 NJointBimanualForceController::onPublish(const SensorAndControl&,
825 const DebugDrawerInterfacePrx&,
826 const DebugObserverInterfacePrx& debugObs)
827 {
828
829 StringVariantBaseMap datafields;
830 auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
831 for (auto& pair : values)
832 {
833 datafields[pair.first] = new Variant(pair.second);
834 }
835
836 Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
837 for (int i = 0; i < forceImpedance.rows(); ++i)
838 {
839 std::stringstream ss;
840 ss << i;
841 std::string data_name = "forceImpedance_" + ss.str();
842 datafields[data_name] = new Variant(forceImpedance(i));
843 }
844
845 Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
846 for (int i = 0; i < forcePID.rows(); ++i)
847 {
848 std::stringstream ss;
849 ss << i;
850 std::string data_name = "forcePID_" + ss.str();
851 datafields[data_name] = new Variant(forcePID(i));
852 }
853
854
855 Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
856 for (int i = 0; i < poseError.rows(); ++i)
857 {
858 std::stringstream ss;
859 ss << i;
860 std::string data_name = "poseError_" + ss.str();
861 datafields[data_name] = new Variant(poseError(i));
862 }
863
864 Eigen::VectorXf wrenchesConstrained =
865 debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
866 for (int i = 0; i < wrenchesConstrained.rows(); ++i)
867 {
868 std::stringstream ss;
869 ss << i;
870 std::string data_name = "wrenchesConstrained_" + ss.str();
871 datafields[data_name] = new Variant(wrenchesConstrained(i));
872 }
873
874 Eigen::VectorXf wrenchesMeasuredInRoot =
875 debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
876 for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
877 {
878 std::stringstream ss;
879 ss << i;
880 std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
881 datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
882 }
883
884
885 // Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
886 // for (size_t i = 0; i < wrenchDMP.rows(); ++i)
887 // {
888 // std::stringstream ss;
889 // ss << i;
890 // std::string data_name = "wrenchDMP_" + ss.str();
891 // datafields[data_name] = new Variant(wrenchDMP(i));
892 // }
893
894 // Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
895 // for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
896 // {
897 // std::stringstream ss;
898 // ss << i;
899 // std::string data_name = "computedBoxWrench_" + ss.str();
900 // datafields[data_name] = new Variant(computedBoxWrench(i));
901 // }
902
903
904 datafields["modifiedPoseRight_x"] =
905 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
906 datafields["modifiedPoseRight_y"] =
907 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
908 datafields["modifiedPoseRight_z"] =
909 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
910 datafields["currentPoseLeft_x"] =
911 new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
912 datafields["currentPoseLeft_y"] =
913 new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
914 datafields["currentPoseLeft_z"] =
915 new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
916
917
918 datafields["modifiedPoseLeft_x"] =
919 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
920 datafields["modifiedPoseLeft_y"] =
921 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
922 datafields["modifiedPoseLeft_z"] =
923 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
924
925 datafields["currentPoseRight_x"] =
926 new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
927 datafields["currentPoseRight_y"] =
928 new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
929 datafields["currentPoseRight_z"] =
930 new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
931 datafields["dmpBoxPose_x"] =
932 new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
933 datafields["dmpBoxPose_y"] =
934 new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
935 datafields["dmpBoxPose_z"] =
936 new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
937 datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
938 datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
939 datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
940
941 datafields["modifiedTwist_lx"] =
942 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
943 datafields["modifiedTwist_ly"] =
944 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
945 datafields["modifiedTwist_lz"] =
946 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
947 datafields["modifiedTwist_rx"] =
948 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
949 datafields["modifiedTwist_ry"] =
950 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
951 datafields["modifiedTwist_rz"] =
952 new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
953
954 datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
955 datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
956 datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
957
958
959 debugObs->setDebugChannel("BimanualForceController", datafields);
960 }
961
962 void
963 NJointBimanualForceController::onInitNJointController()
964 {
965 ARMARX_INFO << "init ...";
966 runTask("NJointBimanualForceController",
967 [&]
968 {
969 CycleUtil c(1);
970 getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
971 while (getState() == eManagedIceObjectStarted)
972 {
973 if (isControllerActive())
974 {
975 controllerRun();
976 }
977 c.waitForCycleDuration();
978 }
979 });
980 }
981
982 void
987} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition CycleUtil.h:41
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition ManagedIceObject.cpp:776
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition ManagedIceObject.cpp:769
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition NJointControllerBase.h:778
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition NJointController.cpp:383
armarx::NJointControllerBase::runTask
void runTask(const std::string &taskName, Task &&task)
Executes a given task in a separate thread from the Application ThreadPool.
Definition NJointControllerBase.h:754
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition NJointController.cpp:293
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition NJointControllerBase.h:846
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition NJointController.cpp:410
armarx::SensorValueBase
The SensorValueBase class.
Definition SensorValueBase.h:41
armarx::SensorValueBase::asA
const T * asA() const
Definition SensorValueBase.h:82
armarx::Variant
The Variant class is described here: Variants.
Definition Variant.h:224
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceControlData
Definition NJointBimanualForceController.h:33
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceControlData::boxTwist
Eigen::VectorXf boxTwist
Definition NJointBimanualForceController.h:37
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceControlData::boxPose
Eigen::Matrix4f boxPose
Definition NJointBimanualForceController.h:36
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::onDisconnectNJointController
void onDisconnectNJointController()
Definition NJointBimanualForceController.cpp:983
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition NJointBimanualForceController.cpp:768
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition NJointBimanualForceController.cpp:775
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition NJointBimanualForceController.cpp:331
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::getClassName
std::string getClassName(const Ice::Current &) const
Definition NJointBimanualForceController.cpp:287
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::controllerRun
void controllerRun()
Definition NJointBimanualForceController.cpp:306
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::onInitNJointController
void onInitNJointController()
Definition NJointBimanualForceController.cpp:963
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::firstLoop
bool firstLoop
Definition NJointBimanualForceController.h:245
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointBimanualForceController.cpp:824
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::NJointBimanualForceController
NJointBimanualForceController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition NJointBimanualForceController.cpp:28
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition NJointBimanualForceController.cpp:814
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualForceController::runDMP
void runDMP(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition NJointBimanualForceController.cpp:782
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition TaskSpaceVMP.h:91
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
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::registrationControllerNJointBimanualForceController
NJointControllerRegistration< NJointBimanualForceController > registrationControllerNJointBimanualForceController("NJointBimanualForceController")
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::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T > > &src, Thrower thrower)
Definition SimoxCSpace.cpp:772
armarx::RobotUnitPtr
IceUtil::Handle< class RobotUnit > RobotUnitPtr
Definition FTSensor.h:34
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition JointController.h:42
armarx::SensorAndControl
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition NJointControllerBase.h:73
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition TaskSpaceVMP.h:52
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition TaskSpaceVMP.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition TaskSpaceVMP.h:51
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition TaskSpaceVMP.h:53
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition TaskSpaceVMP.h:48
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition TaskSpaceVMP.h:50
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition TaskSpaceVMP.h:55
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition TaskSpaceVMP.h:56
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition TaskSpaceVMP.h:60
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition TaskSpaceVMP.h:62
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition TaskSpaceVMP.h:64
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition TaskSpaceVMP.h:66
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition TaskSpaceVMP.h:61
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition TaskSpaceVMP.h:67
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition TaskSpaceVMP.h:63