NJointBimanualObjLevelMultiMPController.cpp
Go to the documentation of this file.
1#include "NJointBimanualObjLevelMultiMPController.h"
2
3#include <ArmarXCore/core/ArmarXObjectScheduler.h>
4#include <ArmarXCore/core/time/CycleUtil.h>
5
6
7// Simox
8#include <VirtualRobot/IK/DifferentialIK.h>
9#include <VirtualRobot/MathTools.h>
10#include <VirtualRobot/Nodes/RobotNode.h>
11#include <VirtualRobot/Robot.h>
12#include <VirtualRobot/RobotNodeSet.h>
13
14// armarx
15#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
16#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
17#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
18#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
19#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
20#include <RobotAPI/libraries/core/PIDController.h>
21#include <RobotAPI/libraries/core/math/MathUtils.h>
22
23namespace armarx::control::deprecated_njoint_mp_controller::bimanual
24{
25 NJointControllerRegistration<NJointBimanualObjLevelMultiMPController>
26 registrationControllerNJointBimanualObjLevelMultiMPController(
27 "NJointBimanualObjLevelMultiMPController");
28
29 NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController(
30 const RobotUnitPtr& robUnit,
31 const armarx::NJointControllerConfigPtr& config,
32 const VirtualRobot::RobotPtr&)
33 {
34 ARMARX_INFO << "Initializing Bimanual Object Level Controller";
35
36 // initialize robot kinematic chain and sensors
37 useSynchronizedRtRobot();
38 cfg = NJointBimanualObjLevelMultiMPControllerConfigPtr::dynamicCast(config);
39
40 leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
41
42 for (size_t i = 0; i < leftRNS->getSize(); ++i)
43 {
44 std::string jointName = leftRNS->getNode(i)->getName();
45 leftJointNames.push_back(jointName);
46 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
47 const SensorValueBase* sv = useSensorValue(jointName);
48 leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
49 const SensorValue1DoFActuatorVelocity* velocitySensor =
50 sv->asA<SensorValue1DoFActuatorVelocity>();
51 const SensorValue1DoFActuatorPosition* positionSensor =
52 sv->asA<SensorValue1DoFActuatorPosition>();
53
54 if (!velocitySensor)
55 {
56 ARMARX_WARNING << "No velocitySensor available for " << jointName;
57 }
58 if (!positionSensor)
59 {
60 ARMARX_WARNING << "No positionSensor available for " << jointName;
61 }
62
63
64 leftVelocitySensors.push_back(velocitySensor);
65 leftPositionSensors.push_back(positionSensor);
66 };
67
68 rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
69
70 for (size_t i = 0; i < rightRNS->getSize(); ++i)
71 {
72 std::string jointName = rightRNS->getNode(i)->getName();
73 rightJointNames.push_back(jointName);
74 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
75 const SensorValueBase* sv = useSensorValue(jointName);
76 rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
77
78 const SensorValue1DoFActuatorVelocity* velocitySensor =
79 sv->asA<SensorValue1DoFActuatorVelocity>();
80 const SensorValue1DoFActuatorPosition* positionSensor =
81 sv->asA<SensorValue1DoFActuatorPosition>();
82
83 if (!velocitySensor)
84 {
85 ARMARX_WARNING << "No velocitySensor available for " << jointName;
86 }
87 if (!positionSensor)
88 {
89 ARMARX_WARNING << "No positionSensor available for " << jointName;
90 }
91
92 rightVelocitySensors.push_back(velocitySensor);
93 rightPositionSensors.push_back(positionSensor);
94 };
95
96
97 const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
98 leftForceTorque = svlf->asA<SensorValueForceTorque>();
99 const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
100 rightForceTorque = svrf->asA<SensorValueForceTorque>();
101
102 leftIK.reset(new VirtualRobot::DifferentialIK(
103 leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
104 rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
105 rightRNS->getRobot()->getRootNode(),
106 VirtualRobot::JacobiProvider::eSVDDamped));
107
108 tcpLeft = leftRNS->getTCP();
109 tcpRight = rightRNS->getTCP();
110
111 /* ----------------- initialize DMP ----------------- */
112 tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
113 taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
114 taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
115 taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
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 taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
126
127 taskSpaceDMPConfig.DMPStyle = cfg->dmpObjType;
128 objectDMP.reset(new tsvmp::TaskSpaceDMPController("objDMP", taskSpaceDMPConfig, false));
129 taskSpaceDMPConfig.DMPStyle = cfg->dmpLeftType;
130 leftTCPInObjDMP.reset(
131 new tsvmp::TaskSpaceDMPController("leftDMP", taskSpaceDMPConfig, false));
132 taskSpaceDMPConfig.DMPStyle = cfg->dmpRightType;
133 rightTCPInObjDMP.reset(
134 new tsvmp::TaskSpaceDMPController("rightDMP", taskSpaceDMPConfig, false));
135 ARMARX_IMPORTANT << "dmps initialized";
136
137 /* ----------------- initialize control parameters ----------------- */
138 auto setParamVec = [&](Eigen::VectorXf& param, ::Ice::FloatSeq& cfgParam, const size_t n)
139 {
140 param.setZero(n);
141 ARMARX_CHECK_EQUAL(cfgParam.size(), n);
142
143 for (size_t i = 0; i < n; ++i)
144 {
145 param(i) = cfgParam.at(i);
146 }
147 };
148
149 setParamVec(KpImpedance, cfg->KpImpedance, 12);
150 setParamVec(KdImpedance, cfg->KdImpedance, 12);
151 setParamVec(KpAdmittance, cfg->KpAdmittance, 6);
152 setParamVec(KdAdmittance, cfg->KdAdmittance, 6);
153 setParamVec(KmAdmittance, cfg->KmAdmittance, 6);
154 setParamVec(KmPID, cfg->KmPID, 6);
155
156 ARMARX_INFO << "got controller params";
157
158 /* ------------- initialize default joint values ------------------- */
159 leftDesiredJointValues.resize(leftTargets.size());
160 ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
161
162 for (size_t i = 0; i < leftTargets.size(); ++i)
163 {
164 leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
165 }
166
167 rightDesiredJointValues.resize(rightTargets.size());
168 ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
169
170 for (size_t i = 0; i < rightTargets.size(); ++i)
171 {
172 rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
173 }
174
175 /* ------------- obtain object initial poses ------------------- */
176 // object pose (position in meter)
177 objInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->objInitialPose[4],
178 cfg->objInitialPose[5],
179 cfg->objInitialPose[6],
180 cfg->objInitialPose[3]);
181 for (int i = 0; i < 3; ++i)
182 {
183 objInitialPose(i, 3) = cfg->objInitialPose[i];
184 }
185
186 virtualAcc.setZero(6);
187 virtualVel.setZero(6);
188 virtualPose = objInitialPose;
189
190 objTransMatrixInAnchor = Eigen::Matrix4f::Identity();
191
192 // obtain left & right initial pose in the object frame (in meter)
193 leftInitialPose = Eigen::Matrix4f::Identity();
194 rightInitialPose = Eigen::Matrix4f::Identity();
195 Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
196 Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
197 leftInitialPose.block<3, 3>(0, 0) =
198 objInitialPose.block<3, 3>(0, 0).transpose() * leftPose.block<3, 3>(0, 0);
199 leftInitialPose.block<3, 1>(0, 3) =
200 objInitialPose.block<3, 3>(0, 0).transpose() *
201 (leftPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
202 rightInitialPose.block<3, 3>(0, 0) =
203 objInitialPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
204 rightInitialPose.block<3, 1>(0, 3) =
205 objInitialPose.block<3, 3>(0, 0).transpose() *
206 (rightPose.block<3, 1>(0, 3) * 0.001 - objInitialPose.block<3, 1>(0, 3));
207
208 integratedPoseObj = virtualPose;
209 integratedPoseLeft = leftInitialPose; // need to be represented in the object local frame.
210 integratedPoseRight = rightInitialPose;
211 /* ------------- initialize buffers ------------------- */
212 // interface to RT
213 Inferface2rtData interface2rtData;
214 interface2rtData.KpImpedance = KpImpedance;
215 interface2rtData.KdImpedance = KdImpedance;
216 interface2rtData.KmAdmittance = KmAdmittance;
217 interface2rtData.KpAdmittance = KpAdmittance;
218 interface2rtData.KdAdmittance = KdAdmittance;
219 interface2rtBuffer.reinitAllBuffers(interface2rtData);
220
221 // RT to DMP
222 RT2ControlData rt2ControlData;
223 rt2ControlData.deltaT = 0;
224 rt2ControlData.currentTime = 0;
225 rt2ControlData.currentPoseObj = objInitialPose;
226 rt2ControlData.currentTwistObj.setZero();
227 rt2ControlData.currentPoseObj = leftInitialPose;
228 rt2ControlData.currentTwistObj.setZero();
229 rt2ControlData.currentPoseObj = rightInitialPose;
230 rt2ControlData.currentTwistObj.setZero();
231 rt2ControlBuffer.reinitAllBuffers(rt2ControlData);
232
233 // DMP (or interface) to RT Target
234 NJointBimanualObjLevelMultiMPControlData initData;
235 initData.objTargetPose = objInitialPose;
236 initData.objTargetTwist.setZero(6);
237
238 initData.leftTargetPoseInObj = leftInitialPose;
239 initData.leftTargetTwistInObj.setZero(6);
240
241 initData.rightTargetPoseInObj = rightInitialPose;
242 initData.rightTargetTwistInObj.setZero(6);
243 reinitTripleBuffer(initData);
244
245 // Control interface: RT to interface
246 RT2InterfaceData rt2InterfaceData;
247 rt2InterfaceData.currentLeftPoseInObjFrame = leftInitialPose;
248 rt2InterfaceData.currentRightPoseInObjFrame = rightInitialPose;
249 rt2InterfaceData.currentObjPose = objInitialPose;
250 rt2InterfaceData.currentObjForce.setZero();
251 rt2InterfaceData.currentObjVel.setZero();
252 rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
253
254
255 /* ------------- initialize PID force controller ------------------- */
256 forcePIDControllers.resize(12);
257 for (size_t i = 0; i < 6; i++)
258 {
259 forcePIDControllers.at(i).reset(new PIDController(
260 cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
261 forcePIDControllers.at(i + 6).reset(new PIDController(
262 cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
263 forcePIDControllers.at(i)->reset();
264 forcePIDControllers.at(i + 6)->reset();
265 }
266
267 /* ------------- initialize force torque sensor related ------------------- */
268 // sensor frame to tcp frame transformation
269 sensorFrame2TcpFrameLeft.setZero();
270 sensorFrame2TcpFrameRight.setZero();
271
272 // filter
273 filterCoeff = cfg->filterCoeff;
274 filteredOldValue.setZero(12);
275
276 // first loop calibrate
277 ftcalibrationTimer = 0;
278 ftOffset.setZero(12);
279
280 // target wrench
281 targetWrench.setZero(cfg->targetWrench.size());
282 for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
283 {
284 targetWrench(i) = cfg->targetWrench[i];
285 }
286
287 /* ------------- static compensation ------------------- */
288 massLeft = cfg->massLeft;
289 CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
290 forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1],
291 cfg->forceOffsetLeft[2];
292 torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1],
293 cfg->torqueOffsetLeft[2];
294
295 massRight = cfg->massRight;
296 CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
297 forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1],
298 cfg->forceOffsetRight[2];
299 torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1],
300 cfg->torqueOffsetRight[2];
301
302 /* ------------- Pose Offset ------------------- */
303 fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
304 objCom2TCPLeftInObjFrame.setZero();
305 objCom2TCPRightInObjFrame.setZero();
306
307 /* ------------- setup flags ------------------- */
308 firstLoop = true;
309 dmpStarted = false;
310 ARMARX_IMPORTANT << "finished construction!";
311 }
312
313 void
317
318 std::string
319 NJointBimanualObjLevelMultiMPController::getClassName(const Ice::Current&) const
320 {
321 return "NJointBimanualObjLevelMultiMPController";
322 }
323
324 void
325 NJointBimanualObjLevelMultiMPController::controllerRun()
326 {
327 if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
328 {
329 return;
330 }
331
332 // get status from RT loop
333 double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
334 Eigen::Matrix4f currentPoseObj = rt2ControlBuffer.getReadBuffer().currentPoseObj;
335 Eigen::VectorXf currentTwistObj = rt2ControlBuffer.getReadBuffer().currentTwistObj;
336 Eigen::Matrix4f currentPoseLeftInObj =
337 rt2ControlBuffer.getReadBuffer().currentPoseLeftInObj;
338 Eigen::VectorXf currentTwistLeftInObj =
339 rt2ControlBuffer.getReadBuffer().currentTwistLeftInObj;
340 Eigen::Matrix4f currentPoseRightInObj =
341 rt2ControlBuffer.getReadBuffer().currentPoseRightInObj;
342 Eigen::VectorXf currentTwistRightInObj =
343 rt2ControlBuffer.getReadBuffer().currentTwistRightInObj;
344
345 // set can val from outside
346 rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
347
348 // DMP flow control
349 bool objectDMPFinished = false;
350 bool leftTCPInObjDMPFinished = false;
351 bool rightTCPInObjDMPFinished = false;
352
353 if (objectDMP->canVal > 1e-8 || objectDMP->config.DMPStyle == "Periodic")
354 {
355 objectDMP->flow(deltaT, currentPoseObj, currentTwistObj);
356 getWriterControlStruct().objTargetPose = objectDMP->getTargetPoseMat();
357 getWriterControlStruct().objTargetTwist = objectDMP->getTargetVelocity();
358 }
359 else
360 {
361 objectDMPFinished = true;
362 }
363
364
365 if (leftTCPInObjDMP->canVal > 1e-8 || leftTCPInObjDMP->config.DMPStyle == "Periodic")
366 {
367 leftTCPInObjDMP->flow(deltaT, currentPoseLeftInObj, currentTwistLeftInObj);
368 getWriterControlStruct().leftTargetPoseInObj = leftTCPInObjDMP->getTargetPoseMat();
369 getWriterControlStruct().leftTargetTwistInObj = leftTCPInObjDMP->getTargetVelocity();
370 }
371 else
372 {
373 leftTCPInObjDMPFinished = true;
374 }
375
376 if (rightTCPInObjDMP->canVal > 1e-8 || rightTCPInObjDMP->config.DMPStyle == "Periodic")
377 {
378 rightTCPInObjDMP->flow(deltaT, currentPoseRightInObj, currentTwistRightInObj);
379 getWriterControlStruct().rightTargetPoseInObj = rightTCPInObjDMP->getTargetPoseMat();
380 getWriterControlStruct().rightTargetTwistInObj = rightTCPInObjDMP->getTargetVelocity();
381 }
382 else
383 {
384 rightTCPInObjDMPFinished = true;
385 }
386
387
388 if (objectDMPFinished && leftTCPInObjDMPFinished && rightTCPInObjDMPFinished)
389 {
390 finished = true;
391 dmpStarted = false;
392 }
393
394 // set target to RT loop
395 LockGuardType guard{controlDataMutex};
396 writeControlStruct();
397 }
398
399 void
400 NJointBimanualObjLevelMultiMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
401 const IceUtil::Time& timeSinceLastIteration)
402 {
403 // ------------------------------------------- current tcp pose -------------------------------------------
404 Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
405 Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
406
407 currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
408 currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
409
410 double deltaT = timeSinceLastIteration.toSecondsDouble();
411 ftcalibrationTimer += deltaT;
412
413 if (firstLoop)
414 {
415 Eigen::Matrix4f leftPose = currentLeftPose;
416 Eigen::Matrix4f rightPose = currentRightPose;
417
418 Eigen::Vector3f objCom2TCPLeftInGlobal =
419 leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
420 Eigen::Vector3f objCom2TCPRightInGlobal =
421 rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
422
423 // Temporary (until we could detect and set object pose by perception)
424 // T_obj = T_{leftHand} * T_{objTransMatrixInAnchor}
425 objTransMatrixInAnchor.block<3, 3>(0, 0) =
426 leftPose.block<3, 3>(0, 0).transpose() * objInitialPose.block<3, 3>(0, 0);
427 objTransMatrixInAnchor.block<3, 1>(0, 3) =
428 leftPose.block<3, 3>(0, 0).transpose() *
429 (objInitialPose.block<3, 1>(0, 3) - leftPose.block<3, 1>(0, 3));
430
431 objCom2TCPLeftInObjFrame =
432 virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
433 objCom2TCPRightInObjFrame =
434 virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
435
436
437 Eigen::Matrix4f leftSensorFrame =
438 leftRNS->getRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
439 Eigen::Matrix4f rightSensorFrame =
440 rightRNS->getRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
441 leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
442 rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
443
444 // sensor frame 2 tcp frame transformation
445 sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) =
446 leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
447 sensorFrame2TcpFrameRight.block<3, 3>(0, 0) =
448 rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
449 CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
450 CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
451 firstLoop = false;
452 }
453
454 Eigen::Matrix4f currentLeftPoseInObjFrame = Eigen::Matrix4f::Identity();
455 Eigen::Matrix4f currentRightPoseInObjFrame = Eigen::Matrix4f::Identity();
456 currentLeftPoseInObjFrame.block<3, 3>(0, 0) =
457 virtualPose.block<3, 3>(0, 0).transpose() * currentLeftPose.block<3, 3>(0, 0);
458 currentLeftPoseInObjFrame.block<3, 1>(0, 3) =
459 virtualPose.block<3, 3>(0, 0).transpose() *
460 (currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
461 currentRightPoseInObjFrame.block<3, 3>(0, 0) =
462 virtualPose.block<3, 3>(0, 0).transpose() * currentRightPose.block<3, 3>(0, 0);
463 currentRightPoseInObjFrame.block<3, 1>(0, 3) =
464 virtualPose.block<3, 3>(0, 0).transpose() *
465 (currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3));
466 rt2InterfaceBuffer.getWriteBuffer().currentLeftPoseInObjFrame = currentLeftPoseInObjFrame;
467 rt2InterfaceBuffer.getWriteBuffer().currentRightPoseInObjFrame = currentRightPoseInObjFrame;
468
469
470 // --------------------------------------------- get control parameters ---------------------------------------
471 KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
472 KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
473 KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
474 KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
475 KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
476
477 if (ftcalibrationTimer < cfg->ftCalibrationTime)
478 {
479 ftOffset.block<3, 1>(0, 0) =
480 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
481 ftOffset.block<3, 1>(3, 0) =
482 0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
483 ftOffset.block<3, 1>(6, 0) =
484 0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
485 ftOffset.block<3, 1>(9, 0) =
486 0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
487
488 for (int i = 0; i < KmAdmittance.size(); ++i)
489 {
490 KmAdmittance(i) = 0;
491 }
492 }
493 else
494 {
495 for (int i = 0; i < KmAdmittance.size(); ++i)
496 {
497 KmAdmittance(i) = cfg->KmAdmittance.at(i);
498 }
499 }
500
501 // -------------------------------------------- target wrench ---------------------------------------------
502 Eigen::VectorXf deltaPoseForWrenchControl;
503 deltaPoseForWrenchControl.setZero(12);
504 for (size_t i = 0; i < 12; ++i)
505 {
506 if (KpImpedance(i) == 0)
507 {
508 deltaPoseForWrenchControl(i) = 0;
509 }
510 else
511 {
512 deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
513 if (deltaPoseForWrenchControl(i) > 0.1)
514 {
515 deltaPoseForWrenchControl(i) = 0.1;
516 }
517 else if (deltaPoseForWrenchControl(i) < -0.1)
518 {
519 deltaPoseForWrenchControl(i) = -0.1;
520 }
521 // deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
522 }
523 }
524
525
526 // --------------------------------------------- grasp matrix ---------------------------------------------
527
528 Eigen::Vector3f objCom2TCPLeftInGlobal =
529 currentLeftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
530 Eigen::Vector3f objCom2TCPRightInGlobal =
531 currentRightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
532
533 Eigen::MatrixXf graspMatrix;
534 graspMatrix.setZero(6, 12);
535 graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
536 graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
537 // graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
538 // graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
539
540 // Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
541 graspMatrix.block<3, 3>(3, 0) = skew(objCom2TCPLeftInGlobal);
542
543 // rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
544 graspMatrix.block<3, 3>(3, 6) = skew(objCom2TCPRightInGlobal);
545
546 // // projection of grasp matrix
547 // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
548 // Eigen::MatrixXf G_range = pinvG * graspMatrix;
549 // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
550 float lambda = 1;
551 Eigen::MatrixXf pinvGraspMatrixT =
552 leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
553
554 // ---------------------------------------------- object pose ----------------------------------------------
555 Eigen::Matrix4f objCurrentPose = currentLeftPose * objTransMatrixInAnchor;
556 Eigen::VectorXf objCurrentTwist;
557 // getObjStatus(objCurrentPose, objCurrentTwist);
558 objCurrentTwist.setZero(6);
559
560 // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
561 Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
562 // Jacobian matrices
563 Eigen::MatrixXf jacobiL =
564 leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
565 Eigen::MatrixXf jacobiR =
566 rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
567 jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
568 jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
569
570 // qpos, qvel
571 Eigen::VectorXf leftqpos;
572 Eigen::VectorXf leftqvel;
573 leftqpos.resize(leftPositionSensors.size());
574 leftqvel.resize(leftVelocitySensors.size());
575 for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
576 {
577 leftqpos(i) = leftPositionSensors[i]->position;
578 leftqvel(i) = leftVelocitySensors[i]->velocity;
579 }
580
581 Eigen::VectorXf rightqpos;
582 Eigen::VectorXf rightqvel;
583 rightqpos.resize(rightPositionSensors.size());
584 rightqvel.resize(rightVelocitySensors.size());
585
586 for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
587 {
588 rightqpos(i) = rightPositionSensors[i]->position;
589 rightqvel(i) = rightVelocitySensors[i]->velocity;
590 }
591
592 // -------------------------------------- compute TCP and object velocity -------------------------------------
593 Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
594 Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
595
596 Eigen::VectorXf currentTwist(12);
597 currentTwist << currentLeftTwist, currentRightTwist;
598 objCurrentTwist = pinvGraspMatrixT * currentTwist;
599
600 rt2ControlBuffer.getWriteBuffer().currentPoseObj = objCurrentPose;
601 rt2ControlBuffer.getWriteBuffer().currentTwistObj = objCurrentTwist;
602 rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
603 rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
604 rt2ControlBuffer.commitWrite();
605
606 // --------------------------------------------- get ft sensor ---------------------------------------------
607 // static compensation
608 Eigen::Vector3f gravity;
609 gravity << 0.0, 0.0, -9.8;
610 Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
611 Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
612 Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
613
614 Eigen::Vector3f localGravityRight =
615 currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
616 Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
617 Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
618
619 // mapping of measured wrenches
620 Eigen::VectorXf wrenchesMeasured(12);
621 wrenchesMeasured << leftForceTorque->force - forceOffsetLeft,
622 leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight,
623 rightForceTorque->torque - torqueOffsetRight;
624 for (size_t i = 0; i < 12; i++)
625 {
626 wrenchesMeasured(i) =
627 (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
628 }
629 filteredOldValue = wrenchesMeasured;
630 wrenchesMeasured.block<3, 1>(0, 0) =
631 sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
632 localForceVecLeft;
633 wrenchesMeasured.block<3, 1>(3, 0) =
634 sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
635 localTorqueVecLeft;
636 wrenchesMeasured.block<3, 1>(6, 0) =
637 sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
638 localForceVecRight;
639 wrenchesMeasured.block<3, 1>(9, 0) =
640 sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
641 localTorqueVecRight;
642 // if (wrenchesMeasured.norm() < cfg->forceThreshold)
643 // {
644 // wrenchesMeasured.setZero();
645 // }
646
647 Eigen::VectorXf forceTorqueMeasurementInRoot(12);
648 forceTorqueMeasurementInRoot.block<3, 1>(0, 0) =
649 currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
650 forceTorqueMeasurementInRoot.block<3, 1>(3, 0) =
651 currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
652 forceTorqueMeasurementInRoot.block<3, 1>(6, 0) =
653 currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
654 forceTorqueMeasurementInRoot.block<3, 1>(9, 0) =
655 currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
656
657
658 // map to object
659 Eigen::VectorXf objFTValue = graspMatrix * forceTorqueMeasurementInRoot;
660 for (size_t i = 0; i < 6; i++)
661 {
662 if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
663 {
664 objFTValue(i) = 0;
665 }
666 else
667 {
668 objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
669 }
670 }
671
672 // pass sensor value to statechart
673 rt2InterfaceBuffer.getWriteBuffer().currentObjPose = objCurrentPose;
674 rt2InterfaceBuffer.getWriteBuffer().currentObjVel = objCurrentTwist.head<3>();
675 rt2InterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head<3>();
676 rt2InterfaceBuffer.commitWrite();
677
678
679 // --------------------------------------------- get MP target ---------------------------------------------
680 Eigen::Matrix4f objTargetPose = rtGetControlStruct().objTargetPose;
681 Eigen::VectorXf objTargetTwist = rtGetControlStruct().objTargetTwist;
682
683 Eigen::Matrix4f leftPoseInObj = rtGetControlStruct().leftTargetPoseInObj;
684 Eigen::VectorXf leftTwistInObj = rtGetControlStruct().leftTargetTwistInObj;
685
686 Eigen::Matrix4f rightPoseInObj = rtGetControlStruct().rightTargetPoseInObj;
687 Eigen::VectorXf rightTwistInObj = rtGetControlStruct().rightTargetTwistInObj;
688
689 // integrate mp target
690 integrateVel2Pose(deltaT, objTargetTwist, integratedPoseObj);
691 integrateVel2Pose(deltaT, leftTwistInObj, integratedPoseLeft);
692 integrateVel2Pose(deltaT, rightTwistInObj, integratedPoseRight);
693
694 // --------------------------------------------- obj admittance control ---------------------------------------------
695 // do admittance control on the object, calculate the virtual pose as attractor for the impedance controller
696 Eigen::VectorXf objPoseError(6);
697 objPoseError.head<3>() = virtualPose.block<3, 1>(0, 3) - objTargetPose.block<3, 1>(0, 3);
698 Eigen::Matrix3f objDiffMat =
699 virtualPose.block<3, 3>(0, 0) * objTargetPose.block<3, 3>(0, 0).transpose();
700 objPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
701
702
703 Eigen::VectorXf objAcc;
704 Eigen::VectorXf objVel;
705 objAcc.setZero(6);
706 objVel.setZero(6);
707 for (size_t i = 0; i < 6; i++)
708 {
709 // objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
710 objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) -
711 KdAdmittance(i) * virtualVel(i);
712 }
713 objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
714 Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
715 virtualAcc = objAcc;
716 virtualVel = objVel;
717 virtualPose.block<3, 1>(0, 3) += deltaObjPose.head<3>();
718 virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
719 deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) *
720 virtualPose.block<3, 3>(0, 0);
721
722 // --------------------------------------------- convert to tcp pose ---------------------------------------------
723 // [R_v, P_v // 0, 1] * [R_l P_l// 0, 1] = [R_v*R_l R_v * P_l + P_v]
724 Eigen::Matrix4f tcpTargetPoseLeft = virtualPose * leftPoseInObj;
725 Eigen::Matrix4f tcpTargetPoseRight = virtualPose * rightPoseInObj;
726 // tcpTargetPoseLeft.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
727 // tcpTargetPoseRight.block<3, 1>(0, 3) += virtualPose.block<3, 3>(0, 0) * (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
728
729
730 // --------------------------------------------- Impedance control ---------------------------------------------
731 Eigen::VectorXf poseError(12);
732 Eigen::Matrix3f diffMat =
733 tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
734 poseError.block<3, 1>(0, 0) =
735 tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
736 poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
737
738 diffMat =
739 tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
740 poseError.block<3, 1>(6, 0) =
741 tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
742 poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
743
744 Eigen::VectorXf forceImpedance(12);
745 for (size_t i = 0; i < 12; i++)
746 {
747 forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
748 }
749
750 // --------------------------------------------- Nullspace control ---------------------------------------------
751 Eigen::VectorXf leftNullspaceTorque =
752 cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
753 Eigen::VectorXf rightNullspaceTorque =
754 cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
755
756 // --------------------------------------------- Set Torque Control Command ---------------------------------------------
757 // float lambda = 1;
758 Eigen::MatrixXf jtpinvL =
759 leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
760 Eigen::MatrixXf jtpinvR =
761 rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
762 Eigen::VectorXf leftJointDesiredTorques =
763 jacobiL.transpose() * forceImpedance.head<6>() +
764 (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
765 Eigen::VectorXf rightJointDesiredTorques =
766 jacobiR.transpose() * forceImpedance.tail<6>() +
767 (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
768
769 // torque limit
770 for (size_t i = 0; i < leftTargets.size(); ++i)
771 {
772 float desiredTorque = leftJointDesiredTorques(i);
773 if (isnan(desiredTorque))
774 {
775 desiredTorque = 0;
776 }
777 desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
778 desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
779 rt2DebugBuffer.getWriteBuffer().desired_torques[leftJointNames[i]] =
780 leftJointDesiredTorques(i);
781 leftTargets.at(i)->torque = desiredTorque;
782 }
783
784 for (size_t i = 0; i < rightTargets.size(); ++i)
785 {
786 float desiredTorque = rightJointDesiredTorques(i);
787 if (isnan(desiredTorque))
788 {
789 desiredTorque = 0;
790 }
791 desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
792 desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
793 rt2DebugBuffer.getWriteBuffer().desired_torques[rightJointNames[i]] =
794 rightJointDesiredTorques(i);
795 rightTargets.at(i)->torque = desiredTorque;
796 }
797
798 // --------------------------------------------- debug output ---------------------------------------------
799 // impedance control, target TS force
800 rt2DebugBuffer.getWriteBuffer().forceImpedance = forceImpedance;
801 rt2DebugBuffer.getWriteBuffer().poseError = poseError;
802 rt2DebugBuffer.getWriteBuffer().forceTorqueMeasurementInRoot = forceTorqueMeasurementInRoot;
803
804 // object current pose and current twist
805 rt2DebugBuffer.getWriteBuffer().objPoseVec = eigenPose2EigenVec(objCurrentPose);
806 rt2DebugBuffer.getWriteBuffer().objCurrentTwist = objCurrentTwist;
807
808 // object force and torque
809 rt2DebugBuffer.getWriteBuffer().objForceTorque = objFTValue;
810
811 // virtual pose, vel and acc
812 rt2DebugBuffer.getWriteBuffer().virtualPoseVec = eigenPose2EigenVec(virtualPose);
813 rt2DebugBuffer.getWriteBuffer().virtualVel = virtualVel;
814 rt2DebugBuffer.getWriteBuffer().virtualAcc = virtualAcc;
815
816 // integrated pose
817 rt2DebugBuffer.getWriteBuffer().integratedPoseObjVec =
818 eigenPose2EigenVec(integratedPoseObj);
819 rt2DebugBuffer.getWriteBuffer().integratedPoseLeftVec =
820 eigenPose2EigenVec(integratedPoseLeft);
821 rt2DebugBuffer.getWriteBuffer().integratedPoseRightVec =
822 eigenPose2EigenVec(integratedPoseRight);
823
824
825 // hand poses
826 rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootLeft =
827 eigenPose2EigenVec(tcpTargetPoseLeft);
828 rt2DebugBuffer.getWriteBuffer().targetHandPoseInRootRight =
829 eigenPose2EigenVec(tcpTargetPoseRight);
830 rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootLeft =
831 eigenPose2EigenVec(currentLeftPose);
832 rt2DebugBuffer.getWriteBuffer().currentHandPoseInRootRight =
833 eigenPose2EigenVec(currentRightPose);
834
835 // dmp targets
836 rt2DebugBuffer.getWriteBuffer().objTargetPoseVec = eigenPose2EigenVec(objTargetPose);
837 rt2DebugBuffer.getWriteBuffer().leftPoseVecInObj = eigenPose2EigenVec(leftPoseInObj);
838 rt2DebugBuffer.getWriteBuffer().rightPoseVecInObj = eigenPose2EigenVec(rightPoseInObj);
839 rt2DebugBuffer.getWriteBuffer().objTargetTwist = objTargetTwist;
840
841 // parameters
842 rt2DebugBuffer.getWriteBuffer().KpImpedance = KpImpedance;
843 rt2DebugBuffer.getWriteBuffer().KdImpedance = KdImpedance;
844 rt2DebugBuffer.getWriteBuffer().KpAdmittance = KpAdmittance;
845 rt2DebugBuffer.getWriteBuffer().KdAdmittance = KdAdmittance;
846 rt2DebugBuffer.getWriteBuffer().KmAdmittance = KmAdmittance;
847 rt2DebugBuffer.getWriteBuffer().KmPID = KmPID;
848
849 rt2DebugBuffer.commitWrite();
850 }
851
852 void
853 NJointBimanualObjLevelMultiMPController::getObjStatus(Eigen::Matrix4f& pose,
854 Eigen::VectorXf& twist)
855 {
856 // this is a temporary function to get status of the object,
857 // in fact, this should be set via the interface function.
858 Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
859 leftPose.block<3, 1>(0, 3) *= 0.001;
860 pose = leftPose * objTransMatrixInAnchor;
861 twist.setZero(6);
862 }
863
864 // TODO
865 // void NJointBimanualObjLevelMultiMPController::setObjStatus()
866 // {
867 // // ice interface function, create a buffer for this
868 // }
869
870
871 void
872 NJointBimanualObjLevelMultiMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
873 const Ice::Current&)
874 {
875 objectDMP->learnDMPFromFiles(fileNames);
876 }
877
878 void
879 NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles(
880 const Ice::StringSeq& objFileNames,
881 const Ice::StringSeq& leftFileNames,
882 const Ice::StringSeq& rightFileNames,
883 const Ice::Current&)
884 {
885 objectDMP->learnDMPFromFiles(objFileNames);
886 leftTCPInObjDMP->learnDMPFromFiles(leftFileNames);
887 rightTCPInObjDMP->learnDMPFromFiles(rightFileNames);
888 }
889
890 void
891 NJointBimanualObjLevelMultiMPController::setGoals(const Ice::DoubleSeq& goals,
892 const Ice::Current& ice)
893 {
894 LockGuardType guard(controllerMutex);
895 objectDMP->setGoalPoseVec(goals);
896 }
897
898 void
899 NJointBimanualObjLevelMultiMPController::setMultiMPGoals(const Ice::DoubleSeq& goalObj,
900 const Ice::DoubleSeq& goalLeft,
901 const Ice::DoubleSeq& goalRight,
902 const Ice::Current& ice)
903 {
904 LockGuardType guard(controllerMutex);
905 objectDMP->setGoalPoseVec(goalObj);
906 leftTCPInObjDMP->setGoalPoseVec(goalLeft);
907 rightTCPInObjDMP->setGoalPoseVec(goalRight);
908 }
909
910 void
911 NJointBimanualObjLevelMultiMPController::integrateVel2Pose(const double deltaT,
912 Eigen::VectorXf& vel,
913 Eigen::Matrix4f& pose)
914 {
915 Eigen::Matrix3f deltaRot =
916 VirtualRobot::MathTools::rpy2eigen3f(vel.tail<3>() * static_cast<float>(deltaT));
917 pose.block<3, 3>(0, 0) = deltaRot * pose.block<3, 3>(0, 0);
918 pose.block<3, 1>(0, 3) += vel.head<3>() * static_cast<float>(deltaT);
919 }
920
921 std::vector<double>
922 NJointBimanualObjLevelMultiMPController::eigenPose2Vec(const Eigen::Matrix4f& pose)
923 {
924 VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
925 std::vector<double> poseVec{pose(0, 3), pose(1, 3), pose(2, 3), ori.w, ori.x, ori.y, ori.z};
926 return poseVec;
927 }
928
929 Eigen::VectorXf
930 NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec(const Eigen::Matrix4f& pose)
931 {
932 VirtualRobot::MathTools::Quaternion ori = VirtualRobot::MathTools::eigen4f2quat(pose);
933 Eigen::VectorXf poseVec;
934 poseVec.setZero(7);
935 poseVec(0) = pose(0, 3);
936 poseVec(1) = pose(1, 3);
937 poseVec(2) = pose(2, 3);
938 poseVec(3) = ori.w;
939 poseVec(4) = ori.x;
940 poseVec(5) = ori.y;
941 poseVec(6) = ori.z;
942 return poseVec;
943 }
944
945 void
946 NJointBimanualObjLevelMultiMPController::runDMP(const Ice::DoubleSeq& goalObj,
947 const Ice::DoubleSeq& goalLeft,
948 const Ice::DoubleSeq& goalRight,
949 Ice::Double timeDuration,
950 const Ice::Current&)
951 {
952 while (!rt2InterfaceBuffer.updateReadBuffer())
953 {
954 usleep(1000);
955 }
956
957 Eigen::Matrix4f leftInitPose =
958 rt2InterfaceBuffer.getUpToDateReadBuffer().currentLeftPoseInObjFrame;
959 Eigen::Matrix4f rightInitPose =
960 rt2InterfaceBuffer.getUpToDateReadBuffer().currentRightPoseInObjFrame;
961 Eigen::Matrix4f objInitPose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjPose;
962 std::vector<double> objInitPoseVec = eigenPose2Vec(objInitPose);
963 std::vector<double> leftInitPoseVec = eigenPose2Vec(leftInitPose);
964 std::vector<double> rightInitPoseVec = eigenPose2Vec(rightInitPose);
965 ARMARX_INFO << "get init poses: \n"
966 << VAROUT(objInitPoseVec) << "\n"
967 << VAROUT(leftInitPoseVec) << "\n"
968 << VAROUT(rightInitPoseVec);
969
970 // set the integrated left/right pose when start to run dmp
971 integratedPoseObj = objInitPose;
972 integratedPoseLeft = leftInitPose;
973 integratedPoseRight = rightInitPose;
974
975 objectDMP->prepareExecution(objInitPoseVec, goalObj);
976 objectDMP->canVal = timeDuration;
977 objectDMP->config.motionTimeDuration = timeDuration;
978
979 leftTCPInObjDMP->prepareExecution(leftInitPose, getLocalPose(goalObj, goalLeft));
980 leftTCPInObjDMP->canVal = timeDuration;
981 leftTCPInObjDMP->config.motionTimeDuration = timeDuration;
982
983 rightTCPInObjDMP->prepareExecution(rightInitPose, getLocalPose(goalObj, goalRight));
984 rightTCPInObjDMP->canVal = timeDuration;
985 rightTCPInObjDMP->config.motionTimeDuration = timeDuration;
986 ARMARX_INFO << "DMP prepare execution is done";
987
988 finished = false;
989 dmpStarted = true;
990 }
991
992 Eigen::Matrix4f
993 NJointBimanualObjLevelMultiMPController::getLocalPose(const Eigen::Matrix4f& newCoordinate,
994 const Eigen::Matrix4f& globalTargetPose)
995 {
996 Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
997
998 localPose.block<3, 3>(0, 0) =
999 newCoordinate.block<3, 3>(0, 0).inverse() * globalTargetPose.block<3, 3>(0, 0);
1000 localPose.block<3, 1>(0, 3) =
1001 newCoordinate.block<3, 3>(0, 0).inverse() *
1002 (globalTargetPose.block<3, 1>(0, 3) - newCoordinate.block<3, 1>(0, 3));
1003
1004
1005 return localPose;
1006 }
1007
1008 Eigen::Matrix4f
1009 NJointBimanualObjLevelMultiMPController::getLocalPose(
1010 const std::vector<double>& newCoordinateVec,
1011 const std::vector<double>& globalTargetPoseVec)
1012 {
1013 Eigen::Matrix4f newCoordinate =
1014 VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4),
1015 newCoordinateVec.at(5),
1016 newCoordinateVec.at(6),
1017 newCoordinateVec.at(3));
1018 newCoordinate(0, 3) = newCoordinateVec.at(0);
1019 newCoordinate(1, 3) = newCoordinateVec.at(1);
1020 newCoordinate(2, 3) = newCoordinateVec.at(2);
1021
1022 Eigen::Matrix4f globalTargetPose =
1023 VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4),
1024 globalTargetPoseVec.at(5),
1025 globalTargetPoseVec.at(6),
1026 globalTargetPoseVec.at(3));
1027 globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
1028 globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
1029 globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
1030
1031 return getLocalPose(newCoordinate, globalTargetPose);
1032 }
1033
1034 void
1035 NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts,
1036 const Ice::DoubleSeq& goals,
1037 Ice::Double timeDuration,
1038 const Ice::Current&)
1039 {
1040 ARMARX_ERROR << "implementation not complete!!!";
1041 while (!rt2InterfaceBuffer.updateReadBuffer())
1042 {
1043 usleep(1000);
1044 }
1045 ARMARX_IMPORTANT << "obj level control: setup dmp ...";
1046 objectDMP->prepareExecution(starts, goals);
1047 objectDMP->canVal = timeDuration;
1048 objectDMP->config.motionTimeDuration = timeDuration;
1049
1050 finished = false;
1051 dmpStarted = true;
1052
1053 ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
1054 }
1055
1056 void
1057 NJointBimanualObjLevelMultiMPController::setViaPoints(Ice::Double u,
1058 const Ice::DoubleSeq& viapoint,
1059 const Ice::Current&)
1060 {
1061 // LockGuardType guard(controllerMutex);
1062 ARMARX_INFO << "setting via points ";
1063 objectDMP->setViaPose(u, viapoint);
1064 }
1065
1066 void
1067 NJointBimanualObjLevelMultiMPController::removeAllViaPoints(const Ice::Current&)
1068 {
1069 objectDMP->removeAllViaPoints();
1070 }
1071
1072 void
1073 NJointBimanualObjLevelMultiMPController::setKpImpedance(const Ice::FloatSeq& value,
1074 const Ice::Current&)
1075 {
1076
1077 Eigen::VectorXf setpoint;
1078 setpoint.setZero(value.size());
1079 ARMARX_CHECK_EQUAL(value.size(), 12);
1080
1081 for (size_t i = 0; i < value.size(); ++i)
1082 {
1083 setpoint(i) = value.at(i);
1084 }
1085
1086 LockGuardType guard{interfaceDataMutex};
1087 interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
1088 interface2rtBuffer.commitWrite();
1089 }
1090
1091 void
1092 NJointBimanualObjLevelMultiMPController::setAmplitude(Ice::Double amp, const Ice::Current&)
1093 {
1094 LockGuardType guard{controllerMutex};
1095 if (objectDMP->config.DMPStyle == "Periodic")
1096 {
1097 objectDMP->setAmplitude(amp);
1098 }
1099
1100 if (leftTCPInObjDMP->config.DMPStyle == "Periodic")
1101 {
1102 leftTCPInObjDMP->setAmplitude(amp);
1103 }
1104
1105 if (rightTCPInObjDMP->config.DMPStyle == "Periodic")
1106 {
1107 rightTCPInObjDMP->setAmplitude(amp);
1108 }
1109 }
1110
1111 void
1112 NJointBimanualObjLevelMultiMPController::setKdImpedance(const Ice::FloatSeq& value,
1113 const Ice::Current&)
1114 {
1115 Eigen::VectorXf setpoint;
1116 setpoint.setZero(value.size());
1117 ARMARX_CHECK_EQUAL(value.size(), 12);
1118
1119 for (size_t i = 0; i < value.size(); ++i)
1120 {
1121 setpoint(i) = value.at(i);
1122 }
1123
1124 LockGuardType guard{interfaceDataMutex};
1125 interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
1126 interface2rtBuffer.commitWrite();
1127 }
1128
1129 void
1130 NJointBimanualObjLevelMultiMPController::setKpAdmittance(const Ice::FloatSeq& value,
1131 const Ice::Current&)
1132 {
1133 Eigen::VectorXf setpoint;
1134 setpoint.setZero(value.size());
1135 ARMARX_CHECK_EQUAL(value.size(), 6);
1136
1137 for (size_t i = 0; i < value.size(); ++i)
1138 {
1139 setpoint(i) = value.at(i);
1140 }
1141
1142 LockGuardType guard{interfaceDataMutex};
1143 interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
1144 interface2rtBuffer.commitWrite();
1145 }
1146
1147 void
1148 NJointBimanualObjLevelMultiMPController::setKdAdmittance(const Ice::FloatSeq& value,
1149 const Ice::Current&)
1150 {
1151 Eigen::VectorXf setpoint;
1152 setpoint.setZero(value.size());
1153 ARMARX_CHECK_EQUAL(value.size(), 6);
1154
1155 for (size_t i = 0; i < value.size(); ++i)
1156 {
1157 setpoint(i) = value.at(i);
1158 }
1159
1160 LockGuardType guard{interfaceDataMutex};
1161 interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
1162 interface2rtBuffer.commitWrite();
1163 }
1164
1165 void
1166 NJointBimanualObjLevelMultiMPController::setKmAdmittance(const Ice::FloatSeq& value,
1167 const Ice::Current&)
1168 {
1169 Eigen::VectorXf setpoint;
1170 setpoint.setZero(value.size());
1171 ARMARX_CHECK_EQUAL(value.size(), 6);
1172
1173 for (size_t i = 0; i < value.size(); ++i)
1174 {
1175 setpoint(i) = value.at(i);
1176 }
1177
1178 LockGuardType guard{interfaceDataMutex};
1179 interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
1180 interface2rtBuffer.commitWrite();
1181 }
1182
1183 std::vector<float>
1184 NJointBimanualObjLevelMultiMPController::getCurrentObjVel(const Ice::Current&)
1185 {
1186 Eigen::Vector3f tvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
1187 std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
1188 return tvelvec;
1189 }
1190
1191 std::vector<float>
1192 NJointBimanualObjLevelMultiMPController::getCurrentObjForce(const Ice::Current&)
1193 {
1194 Eigen::Vector3f fvel = rt2InterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
1195 std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
1196 return fvelvec;
1197 }
1198
1199 void
1200 NJointBimanualObjLevelMultiMPController::publishVec(const Eigen::VectorXf& bufferVec,
1201 const std::string name,
1202 StringVariantBaseMap& datafields)
1203 {
1204 for (int i = 0; i < bufferVec.rows(); ++i)
1205 {
1206 std::string data_name = name + "_" + std::to_string(i);
1207 datafields[data_name] = new Variant(bufferVec(i));
1208 }
1209 }
1210
1211 void
1212 NJointBimanualObjLevelMultiMPController::onPublish(const SensorAndControl&,
1213 const DebugDrawerInterfacePrx&,
1214 const DebugObserverInterfacePrx& debugObs)
1215 {
1216
1217 StringVariantBaseMap datafields;
1218 auto values = rt2DebugBuffer.getUpToDateReadBuffer().desired_torques;
1219 for (auto& pair : values)
1220 {
1221 datafields[pair.first] = new Variant(pair.second);
1222 }
1223
1224 publishVec(
1225 rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance, "forceImpedance", datafields);
1226 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forcePID, "forcePID", datafields);
1227 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().poseError, "poseError", datafields);
1228
1229 // ------------------ force torque measurement of hands ------------------
1230 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot,
1231 "forceTorqueMeasurementInRoot",
1232 datafields);
1233
1234 // ------------------ object force torques ------------------
1235 publishVec(
1236 rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque, "objForceTorque", datafields);
1237
1238 // ------------------ virtual pose, vel and acc ------------------
1239 publishVec(
1240 rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec, "virtualPoseVec", datafields);
1241 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().virtualVel, "virtualVel", datafields);
1242
1243 // ------------------ object pose vec, vel ------------------
1244 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec, "objPoseVec", datafields);
1245 publishVec(
1246 rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist, "objCurrentTwist", datafields);
1247
1248 // ------------------ hand poses ------------------
1249 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft,
1250 "targetHandPoseInRootLeft",
1251 datafields);
1252 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight,
1253 "targetHandPoseInRootRight",
1254 datafields);
1255 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft,
1256 "currentHandPoseInRootLeft",
1257 datafields);
1258 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight,
1259 "currentHandPoseInRootRight",
1260 datafields);
1261
1262 // ------------------ dmp targets ------------------
1263 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec,
1264 "objTargetPoseVec",
1265 datafields);
1266 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj,
1267 "leftPoseVecInObj",
1268 datafields);
1269 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj,
1270 "rightPoseVecInObj",
1271 datafields);
1272 publishVec(
1273 rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist, "objTargetTwist", datafields);
1274
1275 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseObjVec,
1276 "integratedPoseObjVec",
1277 datafields);
1278 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseLeftVec,
1279 "integratedPoseLeftVec",
1280 datafields);
1281 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().integratedPoseRightVec,
1282 "integratedPoseRightVec",
1283 datafields);
1284
1285 // ------------------ controller parameters ------------------
1286 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance, "KpImpedance", datafields);
1287 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance, "KdImpedance", datafields);
1288 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance, "KpAdmittance", datafields);
1289 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance, "KdAdmittance", datafields);
1290 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance, "KmAdmittance", datafields);
1291 publishVec(rt2DebugBuffer.getUpToDateReadBuffer().KmPID, "KmPID", datafields);
1292
1293 // Eigen::VectorXf forceImpedance = rt2DebugBuffer.getUpToDateReadBuffer().forceImpedance;
1294 // for (int i = 0; i < forceImpedance.rows(); ++i)
1295 // {
1296 // std::stringstream ss;
1297 // ss << i;
1298 // std::string data_name = "forceImpedance_" + ss.str();
1299 // datafields[data_name] = new Variant(forceImpedance(i));
1300 // }
1301
1302 // Eigen::VectorXf forcePID = rt2DebugBuffer.getUpToDateReadBuffer().forcePID;
1303 // for (int i = 0; i < forcePID.rows(); ++i)
1304 // {
1305 // std::stringstream ss;
1306 // ss << i;
1307 // std::string data_name = "forcePID_" + ss.str();
1308 // datafields[data_name] = new Variant(forcePID(i));
1309 // }
1310
1311
1312 // Eigen::VectorXf poseError = rt2DebugBuffer.getUpToDateReadBuffer().poseError;
1313 // for (int i = 0; i < poseError.rows(); ++i)
1314 // {
1315 // std::stringstream ss;
1316 // ss << i;
1317 // std::string data_name = "poseError_" + ss.str();
1318 // datafields[data_name] = new Variant(poseError(i));
1319 // }
1320
1321 // // ------------------ force torque measurement of hands ------------------
1322 // Eigen::VectorXf forceTorqueMeasurementInRoot = rt2DebugBuffer.getUpToDateReadBuffer().forceTorqueMeasurementInRoot;
1323 // for (int i = 0; i < forceTorqueMeasurementInRoot.rows(); ++i)
1324 // {
1325 // std::stringstream ss;
1326 // ss << i;
1327 // std::string data_name = "forceTorqueMeasurementInRoot_" + ss.str();
1328 // datafields[data_name] = new Variant(forceTorqueMeasurementInRoot(i));
1329 // }
1330
1331 // ------------------ object force torques ------------------
1332 // Eigen::VectorXf objForceTorque = rt2DebugBuffer.getUpToDateReadBuffer().objForceTorque;
1333 // for (int i = 0; i < objForceTorque.rows(); ++i)
1334 // {
1335 // std::stringstream ss;
1336 // ss << i;
1337 // std::string data_name = "objForceTorque_" + ss.str();
1338 // datafields[data_name] = new Variant(objForceTorque(i));
1339 // }
1340
1341 // ------------------ virtual pose, vel and acc ------------------
1342 // Eigen::VectorXf virtualPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().virtualPoseVec;
1343 // for (int i = 0; i < virtualPoseVec.rows(); ++i)
1344 // {
1345 // std::stringstream ss;
1346 // ss << i;
1347 // std::string data_name = "virtualPoseVec_" + ss.str();
1348 // datafields[data_name] = new Variant(virtualPoseVec(i));
1349 // }
1350
1351 // Eigen::VectorXf virtualVel = rt2DebugBuffer.getUpToDateReadBuffer().virtualVel;
1352 // for (int i = 0; i < virtualVel.rows(); ++i)
1353 // {
1354 // std::stringstream ss;
1355 // ss << i;
1356 // std::string data_name = "virtualVel_" + ss.str();
1357 // datafields[data_name] = new Variant(virtualVel(i));
1358 // }
1359
1360 // ------------------ object pose vec, vel ------------------
1361 // Eigen::VectorXf objPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objPoseVec;
1362 // for (int i = 0; i < objPoseVec.rows(); ++i)
1363 // {
1364 // std::stringstream ss;
1365 // ss << i;
1366 // std::string data_name = "objPoseVec_" + ss.str();
1367 // datafields[data_name] = new Variant(objPoseVec(i));
1368 // }
1369
1370 // Eigen::VectorXf objCurrentTwist = rt2DebugBuffer.getUpToDateReadBuffer().objCurrentTwist;
1371 // for (int i = 0; i < objCurrentTwist.rows(); ++i)
1372 // {
1373 // std::stringstream ss;
1374 // ss << i;
1375 // std::string data_name = "objCurrentTwist_" + ss.str();
1376 // datafields[data_name] = new Variant(objCurrentTwist(i));
1377 // }
1378
1379 // ------------------ hand poses ------------------
1380 // Eigen::VectorXf targetHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootLeft;
1381 // for (int i = 0; i < targetHandPoseInRootLeft.rows(); ++i)
1382 // {
1383 // std::stringstream ss;
1384 // ss << i;
1385 // std::string data_name = "targetHandPoseInRootLeft_" + ss.str();
1386 // datafields[data_name] = new Variant(targetHandPoseInRootLeft(i));
1387 // }
1388 // Eigen::VectorXf targetHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().targetHandPoseInRootRight;
1389 // for (int i = 0; i < targetHandPoseInRootRight.rows(); ++i)
1390 // {
1391 // std::stringstream ss;
1392 // ss << i;
1393 // std::string data_name = "targetHandPoseInRootRight_" + ss.str();
1394 // datafields[data_name] = new Variant(targetHandPoseInRootRight(i));
1395 // }
1396 // Eigen::VectorXf currentHandPoseInRootLeft = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootLeft;
1397 // for (int i = 0; i < currentHandPoseInRootLeft.rows(); ++i)
1398 // {
1399 // std::stringstream ss;
1400 // ss << i;
1401 // std::string data_name = "currentHandPoseInRootLeft_" + ss.str();
1402 // datafields[data_name] = new Variant(currentHandPoseInRootLeft(i));
1403 // }
1404 // Eigen::VectorXf currentHandPoseInRootRight = rt2DebugBuffer.getUpToDateReadBuffer().currentHandPoseInRootRight;
1405 // for (int i = 0; i < currentHandPoseInRootRight.rows(); ++i)
1406 // {
1407 // std::stringstream ss;
1408 // ss << i;
1409 // std::string data_name = "currentHandPoseInRootRight_" + ss.str();
1410 // datafields[data_name] = new Variant(currentHandPoseInRootRight(i));
1411 // }
1412 // ------------------ dmp targets ------------------
1413 // Eigen::VectorXf objTargetPoseVec = rt2DebugBuffer.getUpToDateReadBuffer().objTargetPoseVec;
1414 // for (int i = 0; i < objTargetPoseVec.rows(); ++i)
1415 // {
1416 // std::stringstream ss;
1417 // ss << i;
1418 // std::string data_name = "objTargetPoseVec_" + ss.str();
1419 // datafields[data_name] = new Variant(objTargetPoseVec(i));
1420 // }
1421 // Eigen::VectorXf leftPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().leftPoseVecInObj;
1422 // for (int i = 0; i < leftPoseVecInObj.rows(); ++i)
1423 // {
1424 // std::stringstream ss;
1425 // ss << i;
1426 // std::string data_name = "leftPoseVecInObj_" + ss.str();
1427 // datafields[data_name] = new Variant(leftPoseVecInObj(i));
1428 // }
1429 // Eigen::VectorXf rightPoseVecInObj = rt2DebugBuffer.getUpToDateReadBuffer().rightPoseVecInObj;
1430 // for (int i = 0; i < rightPoseVecInObj.rows(); ++i)
1431 // {
1432 // std::stringstream ss;
1433 // ss << i;
1434 // std::string data_name = "rightPoseVecInObj_" + ss.str();
1435 // datafields[data_name] = new Variant(rightPoseVecInObj(i));
1436 // }
1437 // Eigen::VectorXf objTargetTwist = rt2DebugBuffer.getUpToDateReadBuffer().objTargetTwist;
1438 // for (int i = 0; i < objTargetTwist.rows(); ++i)
1439 // {
1440 // std::stringstream ss;
1441 // ss << i;
1442 // std::string data_name = "objTargetTwist_" + ss.str();
1443 // datafields[data_name] = new Variant(objTargetTwist(i));
1444 // }
1445
1446 // parameters
1447 // Eigen::VectorXf KpImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KpImpedance;
1448 // for (int i = 0; i < KpImpedance.rows(); ++i)
1449 // {
1450 // std::stringstream ss;
1451 // ss << i;
1452 // std::string data_name = "KpImpedance_" + ss.str();
1453 // datafields[data_name] = new Variant(KpImpedance(i));
1454 // }
1455 // Eigen::VectorXf KdImpedance = rt2DebugBuffer.getUpToDateReadBuffer().KdImpedance;
1456 // for (int i = 0; i < KdImpedance.rows(); ++i)
1457 // {
1458 // std::stringstream ss;
1459 // ss << i;
1460 // std::string data_name = "KdImpedance_" + ss.str();
1461 // datafields[data_name] = new Variant(KdImpedance(i));
1462 // }
1463 // Eigen::VectorXf KpAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KpAdmittance;
1464 // for (int i = 0; i < KpAdmittance.rows(); ++i)
1465 // {
1466 // std::stringstream ss;
1467 // ss << i;
1468 // std::string data_name = "KpAdmittance_" + ss.str();
1469 // datafields[data_name] = new Variant(KpAdmittance(i));
1470 // }
1471 // Eigen::VectorXf KdAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KdAdmittance;
1472 // for (int i = 0; i < KdAdmittance.rows(); ++i)
1473 // {
1474 // std::stringstream ss;
1475 // ss << i;
1476 // std::string data_name = "KdAdmittance_" + ss.str();
1477 // datafields[data_name] = new Variant(KdAdmittance(i));
1478 // }
1479 // Eigen::VectorXf KmAdmittance = rt2DebugBuffer.getUpToDateReadBuffer().KmAdmittance;
1480 // for (int i = 0; i < KmAdmittance.rows(); ++i)
1481 // {
1482 // std::stringstream ss;
1483 // ss << i;
1484 // std::string data_name = "KmAdmittance_" + ss.str();
1485 // datafields[data_name] = new Variant(KmAdmittance(i));
1486 // }
1487 // Eigen::VectorXf KmPID = rt2DebugBuffer.getUpToDateReadBuffer().KmPID;
1488 // for (int i = 0; i < KmPID.rows(); ++i)
1489 // {
1490 // std::stringstream ss;
1491 // ss << i;
1492 // std::string data_name = "KmPID_" + ss.str();
1493 // datafields[data_name] = new Variant(KmPID(i));
1494 // }
1495
1496 debugObs->setDebugChannel("BimanualForceController", datafields);
1497 }
1498
1499 void
1500 NJointBimanualObjLevelMultiMPController::onInitNJointController()
1501 {
1502 ARMARX_INFO << "====== init bimanual multi mp controller ======";
1503 runTask("NJointBimanualObjLevelMultiMPController",
1504 [&]
1505 {
1506 CycleUtil c(1);
1507 getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
1508 while (getState() == eManagedIceObjectStarted)
1509 {
1510 if (isControllerActive())
1511 {
1512 controllerRun();
1513 }
1514 c.waitForCycleDuration();
1515 }
1516 });
1517 }
1518
1519 void
1520 NJointBimanualObjLevelMultiMPController::onDisconnectNJointController()
1521 {
1522 ARMARX_INFO << "====== stop bimanual multi mp controller ======";
1523 }
1524} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
VAROUT
#define VAROUT(x)
Definition StringHelpers.h:198
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition CycleUtil.h:41
armarx::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::NJointBimanualObjLevelMultiMPControlData
Definition NJointBimanualObjLevelMultiMPController.h:29
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::rightTargetPoseInObj
Eigen::Matrix4f rightTargetPoseInObj
Definition NJointBimanualObjLevelMultiMPController.h:38
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::objTargetPose
Eigen::Matrix4f objTargetPose
Definition NJointBimanualObjLevelMultiMPController.h:32
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::rightTargetTwistInObj
Eigen::VectorXf rightTargetTwistInObj
Definition NJointBimanualObjLevelMultiMPController.h:39
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::objTargetTwist
Eigen::VectorXf objTargetTwist
Definition NJointBimanualObjLevelMultiMPController.h:33
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::leftTargetPoseInObj
Eigen::Matrix4f leftTargetPoseInObj
Definition NJointBimanualObjLevelMultiMPController.h:35
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPControlData::leftTargetTwistInObj
Eigen::VectorXf leftTargetTwistInObj
Definition NJointBimanualObjLevelMultiMPController.h:36
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::eigenPose2Vec
std::vector< double > eigenPose2Vec(const Eigen::Matrix4f &pose)
Definition NJointBimanualObjLevelMultiMPController.cpp:922
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::getLocalPose
Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f &newCoordinate, const Eigen::Matrix4f &globalTargetPose)
Definition NJointBimanualObjLevelMultiMPController.cpp:993
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::runDMP
void runDMP(const Ice::DoubleSeq &goalObj, const Ice::DoubleSeq &goalLeft, const Ice::DoubleSeq &goalRight, Ice::Double timeDuration, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:946
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::onDisconnectNJointController
void onDisconnectNJointController()
Definition NJointBimanualObjLevelMultiMPController.cpp:1520
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:872
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::eigenPose2EigenVec
Eigen::VectorXf eigenPose2EigenVec(const Eigen::Matrix4f &pose)
Definition NJointBimanualObjLevelMultiMPController.cpp:930
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition NJointBimanualObjLevelMultiMPController.cpp:314
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::NJointBimanualObjLevelMultiMPController
NJointBimanualObjLevelMultiMPController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition NJointBimanualObjLevelMultiMPController.cpp:29
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setAmplitude
void setAmplitude(Ice::Double amp, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1092
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:891
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKpAdmittance
void setKpAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1130
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKdAdmittance
void setKdAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1148
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::runDMPWithVirtualStart
void runDMPWithVirtualStart(const Ice::DoubleSeq &starts, const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1035
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKpImpedance
void setKpImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1073
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKmAdmittance
void setKmAdmittance(const Ice::FloatSeq &value, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1166
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition NJointBimanualObjLevelMultiMPController.cpp:400
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::integrateVel2Pose
void integrateVel2Pose(const double deltaT, Eigen::VectorXf &vel, Eigen::Matrix4f &pose)
Definition NJointBimanualObjLevelMultiMPController.cpp:911
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::getClassName
std::string getClassName(const Ice::Current &) const
Definition NJointBimanualObjLevelMultiMPController.cpp:319
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setKdImpedance
void setKdImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1112
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setMultiMPGoals
void setMultiMPGoals(const Ice::DoubleSeq &goalObj, const Ice::DoubleSeq &goalLeft, const Ice::DoubleSeq &goalRight, const Ice::Current &ice)
Definition NJointBimanualObjLevelMultiMPController.cpp:899
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::controllerRun
void controllerRun()
Definition NJointBimanualObjLevelMultiMPController.cpp:325
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::publishVec
void publishVec(const Eigen::VectorXf &bufferVec, const std::string name, StringVariantBaseMap &datafields)
Definition NJointBimanualObjLevelMultiMPController.cpp:1200
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::onInitNJointController
void onInitNJointController()
Definition NJointBimanualObjLevelMultiMPController.cpp:1500
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::firstLoop
bool firstLoop
Definition NJointBimanualObjLevelMultiMPController.h:326
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::learnMultiDMPFromFiles
void learnMultiDMPFromFiles(const Ice::StringSeq &objFileNames, const Ice::StringSeq &leftFileNames, const Ice::StringSeq &rightFileNames, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:879
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::getObjStatus
void getObjStatus(Eigen::Matrix4f &pose, Eigen::VectorXf &twist)
Definition NJointBimanualObjLevelMultiMPController.cpp:853
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1212
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition NJointBimanualObjLevelMultiMPController.cpp:1057
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelMultiMPController::skew
Eigen::Matrix3f skew(Eigen::Vector3f vec)
Definition NJointBimanualObjLevelMultiMPController.h:74
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_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition NJointBimanualCartesianAdmittanceController.cpp:31
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualObjLevelMultiMPController
NJointControllerRegistration< NJointBimanualObjLevelMultiMPController > registrationControllerNJointBimanualObjLevelMultiMPController("NJointBimanualObjLevelMultiMPController")
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