NJointBimanualCartesianAdmittanceController.cpp
Go to the documentation of this file.
1#include "NJointBimanualCartesianAdmittanceController.h"
2
3#include <SimoxUtility/math/isfinite.h>
4#include <SimoxUtility/math/pose/is_homogeneous_transform.h>
5#include <SimoxUtility/math/pose/pose.h>
6
7#include <ArmarXCore/core/time/CycleUtil.h>
8
9// Simox
10#include <VirtualRobot/IK/DifferentialIK.h>
11#include <VirtualRobot/MathTools.h>
12#include <VirtualRobot/Nodes/RobotNode.h>
13#include <VirtualRobot/Nodes/Sensor.h>
14#include <VirtualRobot/Robot.h>
15#include <VirtualRobot/RobotNodeSet.h>
16
17// armarx
18#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
19#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
20#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
21#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
22#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
23#include <RobotAPI/libraries/core/PIDController.h>
24#include <RobotAPI/libraries/core/math/MathUtils.h>
25
26// control
27#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
28#include <armarx/control/deprecated_njoint_mp_controller/bimanual/CartesianAdmittanceControllerInterface.h>
29
30namespace armarx::control::deprecated_njoint_mp_controller::bimanual
31{
32 NJointControllerRegistration<NJointBimanualCartesianAdmittanceController>
33 registrationControllerNJointBimanualCartesianAdmittanceController(
34 "NJointBimanualCartesianAdmittanceController");
35
36 NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController(
37 const RobotUnitPtr& robUnit,
38 const armarx::NJointControllerConfigPtr& config,
39 const VirtualRobot::RobotPtr& robot)
40 {
41 ARMARX_INFO << "Preparing ... bimanual ";
42 useSynchronizedRtRobot();
43 auto cfgPtr = NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(config);
44 ARMARX_CHECK_NOT_NULL(cfgPtr);
45 //init rtData
46 {
47 auto initRtData =
48 [&](auto& data, const auto& rnsName, const auto& ftName, const auto& ftRefFrame)
49 {
50 data.rns = rtGetRobot()->getRobotNodeSet(rnsName);
51 ARMARX_CHECK_NOT_NULL(data.rns) << "No robot node set " << rnsName;
52 data.tcp = data.rns->getTCP();
53 ARMARX_CHECK_NOT_NULL(data.tcp) << "No TCP in robot node set " << rnsName;
54 data.frameFTSensor = rtGetRobot()->getSensor(ftName)->getRobotNode();
55 ARMARX_CHECK_NOT_NULL(data.frameFTSensor) << "No ref frame for ft sensor in robot ";
56
57 for (size_t i = 0; i < data.rns->getSize(); ++i)
58 {
59 std::string jointName = data.rns->getNode(i)->getName();
60 data.jointNames.push_back(jointName);
61 ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
62 const SensorValueBase* sv = useSensorValue(jointName);
63 ARMARX_CHECK_NOT_NULL(ct) << "No control target available for " << jointName;
64 ARMARX_CHECK_NOT_NULL(sv) << "No sensor value available for " << jointName;
65 data.targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
66 const SensorValue1DoFActuatorVelocity* velocitySensor =
67 sv->asA<SensorValue1DoFActuatorVelocity>();
68 const SensorValue1DoFActuatorPosition* positionSensor =
69 sv->asA<SensorValue1DoFActuatorPosition>();
70 ARMARX_CHECK_NOT_NULL(velocitySensor)
71 << "No velocitySensor available for " << jointName;
72 ARMARX_CHECK_NOT_NULL(positionSensor)
73 << "No positionSensor available for " << jointName;
74 data.velocitySensors.push_back(velocitySensor);
75 data.positionSensors.push_back(positionSensor);
76 }
77 const auto ftDev = robUnit->getSensorDevice(ftName);
78 ARMARX_CHECK_NOT_NULL(ftDev) << "No sensor device available for " << ftName;
79 const SensorValueBase* svlf = ftDev->getSensorValue();
80 ARMARX_CHECK_NOT_NULL(svlf) << "No sensor value available for " << ftName;
81 data.forceTorque = svlf->asA<SensorValueForceTorque>();
82 ARMARX_CHECK_NOT_NULL(data.forceTorque)
83 << "Sensor value for " << ftName << " is not of type SensorValueForceTorque";
84 data.IK.reset(
85 new VirtualRobot::DifferentialIK(data.rns,
86 data.rns->getRobot()->getRootNode(),
87 VirtualRobot::JacobiProvider::eSVDDamped));
88 };
89
90 initRtData(rt.left,
91 cfgPtr->kinematicChainLeft,
92 cfgPtr->ftSensorLeft,
93 cfgPtr->ftSensorLeftFrame);
94 initRtData(rt.right,
95 cfgPtr->kinematicChainRight,
96 cfgPtr->ftSensorRight,
97 cfgPtr->ftSensorRightFrame);
98 }
99
100 //init cfg + check it
101 {
102 setConfig(cfgPtr);
103 cfgBuf.reinitAllBuffers(cfgBuf.getWriteBuffer());
104 }
105
106
107 // {
108 // rt2ControlData initSensorData;
109 // initSensorData.deltaT = 0;
110 // initSensorData.currentTime = 0;
111 // initSensorData.currentPose = boxInitialPose;
112 // initSensorData.currentTwist.setZero();
113 // rt2ControlBuffer.reinitAllBuffers(initSensorData);
114 // }
115
116
117 // {
118 // ControlInterfaceData initInterfaceData;
119 // initInterfaceData.currentLeftPose = rt.left.tcp->getPoseInRootFrame();
120 // initInterfaceData.currentRightPose = rt.right.tcp->getPoseInRootFrame();
121 // controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
122 // }
123
124 //////////////////////////////TODO
125 // leftInitialPose = rt.left.tcp->getPoseInRootFrame();
126 // rightInitialPose = rt.right.rns->getPoseInRootFrame();
127 // leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
128 // rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
129
130 // // leftInitialPose = boxInitialPose;
131 // // leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
132 // // rightInitialPose = boxInitialPose;
133 // // rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
134 //////////////////////////////TODO
135 // forcePIDControllers.resize(12);
136 // for (size_t i = 0; i < 6; i++)
137 // {
138 // forcePIDControllers.at(i).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
139 // forcePIDControllers.at(i + 6).reset(new PIDController(cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
140 // forcePIDControllers.at(i)->reset();
141 // forcePIDControllers.at(i + 6)->reset();
142 // }
143 //////////////////////////////TODO
144 // filter
145 // filterCoeff = cfg->filterCoeff;
146 // ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
147 // filteredOldValue.setZero(12);
148
149 // static compensation
150 rt.left.sensorFrame2TcpFrame.setZero();
151 rt.right.sensorFrame2TcpFrame.setZero();
152 // NJointBimanualObjLevelControlData initData;
153 // initData.boxPose = boxInitialPose;
154 // initData.boxTwist.setZero(6);
155 // reinitTripleBuffer(initData);
156
157 // ARMARX_INFO << "left initial pose: \n" << leftInitialPose << "\n right initial pose: \n" << rightInitialPose;
158
159 // ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
160 ARMARX_IMPORTANT << "finished construction!";
161
162 // targetWrench.setZero(cfg->targetWrench.size());
163 // for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
164 // {
165 // targetWrench(i) = cfg->targetWrench[i];
166 // }
167 }
168
169 void
170 NJointBimanualCartesianAdmittanceController::rtPreActivateController()
171 {
172 // NJointBimanualObjLevelControlData initData;
173 // initData.boxPose = boxInitPose;
174 // initData.boxTwist.resize(6);
175 // reinitTripleBuffer(initData);
176
177 rt.virtualAcc.setZero();
178 rt.virtualVel.setZero();
179 rt.virtualPose.setZero();
180 rt.filteredOldValue.setZero();
181 // rt.ftOffset.setZero();
182 rt.firstLoop = true;
183 rt.ftcalibrationTimer = 0;
184
185
186 const Eigen::Matrix4f leftPose =
187 simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
188 const Eigen::Matrix4f rightPose =
189 simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
190
191 rt.virtualPose.block<3, 1>(0, 3) =
192 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
193 rt.virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
194
195 const Eigen::Matrix4f leftSensorFrame =
196 simox::math::scaled_position(rt.left.frameFTSensor->getPoseInRootFrame(), 0.001);
197 const Eigen::Matrix4f rightSensorFrame =
198 simox::math::scaled_position(rt.right.frameFTSensor->getPoseInRootFrame(), 0.001);
199
200 rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) =
201 leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
202 rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) =
203 rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
204
205 // ARMARX_INFO << "modified left pose:\n " << leftPose;
206 // ARMARX_INFO << "modified right pose:\n " << rightPose;
207 }
208
209 std::string
210 NJointBimanualCartesianAdmittanceController::getClassName(const Ice::Current&) const
211 {
212 return "NJointBimanualCartesianAdmittanceController";
213 }
214
215 // void NJointBimanualCartesianAdmittanceController::controllerRun()
216 // {
217 // if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
218 // {
219 // return;
220 // }
221
222 // double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
223 // Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
224 // Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
225 // //ARMARX_IMPORTANT << "canVal: " << objectDMP->canVal;
226
227 // if (objectDMP->canVal < 1e-8)
228 // {
229 // finished = true;
230 // dmpStarted = false;
231 // }
232
233 // objectDMP->flow(deltaT, currentPose, currentTwist);
234
235 // LockGuardType guard {controlDataMutex};
236 // getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
237 // getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
238 // writeControlStruct();
239 // }
240
241
242 void
243 NJointBimanualCartesianAdmittanceController::rtRun(const IceUtil::Time&,
244 const IceUtil::Time& timeSinceLastIteration)
245 {
246
247 const Eigen::Matrix4f currentLeftPose =
248 simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
249 const Eigen::Matrix4f currentRightPose =
250 simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
251 const Eigen::Matrix4f currentBoxPose = [&]
252 {
253 Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
254 pose.block<3, 1>(0, 3) =
255 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
256 pose.block<3, 3>(0, 0) = currentLeftPose.block<3, 3>(0, 0);
257 return pose;
258 }();
259 // {
260 // controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
261 // controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
262 // controlInterfaceBuffer.commitWrite();
263 // }
264 const double deltaT = timeSinceLastIteration.toSecondsDouble();
265
266 ARMARX_ON_SCOPE_EXIT
267 {
268 rt.firstLoop = false;
269 };
270
271 rt.ftcalibrationTimer += deltaT;
272
273 // -------------------------------------------- config ---------------------------------------------
274
275 if (cfgBuf.updateReadBuffer())
276 {
277 auto& cfg = cfgBuf._getNonConstReadBuffer();
278 const Eigen::Vector3f tmpL =
279 rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecLeft;
280 const Eigen::Vector3f tmpR =
281 rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecRight;
282 cfg.CoMVecLeft = tmpL;
283 cfg.CoMVecRight = tmpR;
284 }
285 if (rt.firstLoop)
286 {
287 auto& trg = targBuf._getNonConstReadBuffer();
288 trg.pose = currentBoxPose;
289 trg.vel.setZero();
290 }
291 auto& dbgOut = debugOutputData.getWriteBuffer();
292 const auto& targ = targBuf.getWriteBuffer();
293 const auto& cfg = cfgBuf.getReadBuffer();
294
295 if (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
296 {
297 // rt.ftOffset.block<3, 1>(0, 0) = 0.5 * rt.ftOffset.block<3, 1>(0, 0) + 0.5 * rt.right.forceTorque->force;
298 // rt.ftOffset.block<3, 1>(3, 0) = 0.5 * rt.ftOffset.block<3, 1>(3, 0) + 0.5 * rt.right.forceTorque->torque;
299 // rt.ftOffset.block<3, 1>(6, 0) = 0.5 * rt.ftOffset.block<3, 1>(6, 0) + 0.5 * rt.left.forceTorque->force;
300 // rt.ftOffset.block<3, 1>(9, 0) = 0.5 * rt.ftOffset.block<3, 1>(9, 0) + 0.5 * rt.left.forceTorque->torque;
301 // cfg.KmAdmittance.setZero();
302 }
303
304 const Eigen::Vector6f KmAdmittance = (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
305 ? Eigen::Vector6f::Zero()
306 : cfg.KmAdmittance;
307
308 // -------------------------------------------- target wrench ---------------------------------------------
309 const Eigen::Vector12f deltaPoseForWrenchControl =
310 cfg.targetWrench.array() / cfg.KpImpedance.array();
311
312 // ------------------------------------------- current tcp pose -------------------------------------------
313
314
315 // --------------------------------------------- grasp matrix ---------------------------------------------
316 const auto skew = [](auto& vec)
317 {
318 Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
319 mat(1, 2) = -vec(0);
320 mat(0, 2) = vec(1);
321 mat(0, 1) = -vec(2);
322 mat(2, 1) = vec(0);
323 mat(2, 0) = -vec(1);
324 mat(1, 0) = vec(2);
325 return mat;
326 };
327 const Eigen::Vector3f objCom2TCPLeft{-cfg.boxWidth * 0.5f, 0.f, 0.f};
328 const Eigen::Vector3f objCom2TCPRight{+cfg.boxWidth * 0.5f, 0.f, 0.f};
329
330 Eigen::Matrix_6_12_f graspMatrix;
331 graspMatrix.setZero();
332 graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
333 graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
334
335 const Eigen::Vector3f rLeft = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPLeft;
336 const Eigen::Vector3f rRight = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPRight;
337
338 graspMatrix.block<3, 3>(3, 0) = skew(rLeft);
339 graspMatrix.block<3, 3>(3, 6) = skew(rRight);
340
341 // // projection of grasp matrix
342 // Eigen::MatrixXf pinvG = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
343 // Eigen::MatrixXf G_range = pinvG * graspMatrix;
344 // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
345 float lambda = 1;
346 const Eigen::MatrixXf pinvGraspMatrixT =
347 rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
348
349 // ---------------------------------------------- object pose ----------------------------------------------
350 Eigen::Matrix4f boxCurrentPose = currentRightPose;
351 boxCurrentPose.block<3, 1>(0, 3) =
352 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
353 Eigen::Vector6f boxCurrentTwist = Eigen::Vector6f::Zero();
354
355 // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
356 const Eigen::MatrixXf I =
357 Eigen::MatrixXf::Identity(rt.left.targets.size(), rt.left.targets.size());
358 // Jacobian matrices
359 Eigen::MatrixXf jacobiL = rt.left.IK->getJacobianMatrix(
360 rt.left.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
361 Eigen::MatrixXf jacobiR = rt.right.IK->getJacobianMatrix(
362 rt.right.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
363 jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
364 jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
365
366 // qpos, qvel
367 Eigen::VectorXf leftqpos(rt.left.positionSensors.size());
368 Eigen::VectorXf leftqvel(rt.left.velocitySensors.size());
369 for (size_t i = 0; i < rt.left.velocitySensors.size(); ++i)
370 {
371 leftqpos(i) = rt.left.positionSensors[i]->position;
372 leftqvel(i) = rt.left.velocitySensors[i]->velocity;
373 }
374
375 Eigen::VectorXf rightqpos(rt.right.positionSensors.size());
376 Eigen::VectorXf rightqvel(rt.right.velocitySensors.size());
377 for (size_t i = 0; i < rt.right.velocitySensors.size(); ++i)
378 {
379 rightqpos(i) = rt.right.positionSensors[i]->position;
380 rightqvel(i) = rt.right.velocitySensors[i]->velocity;
381 }
382
383 // -------------------------------------- compute TCP and object velocity -------------------------------------
384 const Eigen::Vector6f currentLeftTwist = jacobiL * leftqvel;
385 const Eigen::Vector6f currentRightTwist = jacobiR * rightqvel;
386
387 Eigen::Vector12f currentTwist;
388 currentTwist << currentLeftTwist, currentRightTwist;
389 boxCurrentTwist = pinvGraspMatrixT * currentTwist;
390
391 // rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
392 // rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
393 // rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
394 // rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
395 // rt2ControlBuffer.commitWrite();
396
397 // --------------------------------------------- get ft sensor ---------------------------------------------
398 // static compensation
399 const Eigen::Vector3f gravity{0.0, 0.0, -9.8};
400 const Eigen::Vector3f localGravityLeft =
401 currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
402 const Eigen::Vector3f localForceVecLeft = cfg.massLeft * localGravityLeft;
403 const Eigen::Vector3f localTorqueVecLeft = cfg.CoMVecLeft.cross(localForceVecLeft);
404
405 const Eigen::Vector3f localGravityRight =
406 currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
407 const Eigen::Vector3f localForceVecRight = cfg.massRight * localGravityRight;
408 const Eigen::Vector3f localTorqueVecRight = cfg.CoMVecRight.cross(localForceVecRight);
409
410 // mapping of measured wrenches
411 Eigen::Vector12f wrenchesMeasured;
412 wrenchesMeasured << rt.right.forceTorque->force - cfg.forceOffsetLeft,
413 rt.right.forceTorque->torque - cfg.torqueOffsetLeft,
414 rt.left.forceTorque->force - cfg.forceOffsetRight,
415 rt.left.forceTorque->torque - cfg.torqueOffsetRight;
416 for (size_t i = 0; i < 12; i++)
417 {
418 wrenchesMeasured(i) = (1 - cfg.filterCoeff) * wrenchesMeasured(i) +
419 cfg.filterCoeff * rt.filteredOldValue(i);
420 }
421 rt.filteredOldValue = wrenchesMeasured;
422 wrenchesMeasured.block<3, 1>(0, 0) =
423 rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
424 localForceVecLeft;
425 wrenchesMeasured.block<3, 1>(3, 0) =
426 rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
427 localTorqueVecLeft;
428 wrenchesMeasured.block<3, 1>(6, 0) =
429 rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
430 localForceVecRight;
431 wrenchesMeasured.block<3, 1>(9, 0) =
432 rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
433 localTorqueVecRight;
434 // if (wrenchesMeasured.norm() < cfg->forceThreshold)
435 // {
436 // wrenchesMeasured.setZero();
437 // }
438
439 Eigen::Vector12f wrenchesMeasuredInRoot;
440 wrenchesMeasuredInRoot.block<3, 1>(0, 0) =
441 currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
442 wrenchesMeasuredInRoot.block<3, 1>(3, 0) =
443 currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
444 wrenchesMeasuredInRoot.block<3, 1>(6, 0) =
445 currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
446 wrenchesMeasuredInRoot.block<3, 1>(9, 0) =
447 currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
448
449 // map to object
450 Eigen::Vector6f objFTValue = graspMatrix * wrenchesMeasuredInRoot;
451 for (size_t i = 0; i < 6; i++)
452 {
453 if (fabs(objFTValue(i)) < cfg.forceThreshold(i))
454 {
455 objFTValue(i) = 0;
456 }
457 else
458 {
459 objFTValue(i) -= cfg.forceThreshold(i) * objFTValue(i) / fabs(objFTValue(i));
460 }
461 }
462
463 // --------------------------------------------- get MP target ---------------------------------------------
464 const Eigen::Matrix4f boxPose = targ.pose;
465 const Eigen::Vector6f boxTwist = targ.vel;
466 // --------------------------------------------- obj admittance control ---------------------------------------------
467 // admittance
468 Eigen::Vector6f objPoseError;
469 objPoseError.head<3>() = rt.virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
470 const Eigen::Matrix3f objDiffMat =
471 rt.virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
472 objPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
473
474
475 Eigen::Vector6f objAcc = Eigen::Vector6f::Zero();
476 Eigen::Vector6f objVel = Eigen::Vector6f::Zero();
477 for (size_t i = 0; i < 6; i++)
478 {
479 objAcc(i) = KmAdmittance(i) * objFTValue(i) - cfg.KpAdmittance(i) * objPoseError(i) -
480 cfg.KdAdmittance(i) * rt.virtualVel(i);
481 }
482 objVel = rt.virtualVel + 0.5 * deltaT * (objAcc + rt.virtualAcc);
483 const Eigen::Vector6f deltaObjPose = 0.5 * deltaT * (objVel + rt.virtualVel);
484 rt.virtualAcc = objAcc;
485 rt.virtualVel = objVel;
486 rt.virtualPose.block<3, 1>(0, 3) += deltaObjPose.head<3>();
487 rt.virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
488 deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) *
489 rt.virtualPose.block<3, 3>(0, 0);
490
491 // --------------------------------------------- convert to tcp pose ---------------------------------------------
492 Eigen::Matrix4f tcpTargetPoseLeft = rt.virtualPose;
493 Eigen::Matrix4f tcpTargetPoseRight = rt.virtualPose;
494 tcpTargetPoseLeft.block<3, 1>(0, 3) +=
495 rt.virtualPose.block<3, 3>(0, 0) *
496 (objCom2TCPLeft - deltaPoseForWrenchControl.block<3, 1>(0, 0));
497 tcpTargetPoseRight.block<3, 1>(0, 3) +=
498 rt.virtualPose.block<3, 3>(0, 0) *
499 (objCom2TCPRight - deltaPoseForWrenchControl.block<3, 1>(6, 0));
500
501 // --------------------------------------------- Impedance control ---------------------------------------------
502 Eigen::Vector12f poseError;
503 Eigen::Matrix3f diffMat =
504 tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
505 poseError.block<3, 1>(0, 0) =
506 tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
507 poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
508
509 diffMat =
510 tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
511 poseError.block<3, 1>(6, 0) =
512 tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
513 poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
514
515 Eigen::Vector12f forceImpedance;
516 for (size_t i = 0; i < 12; i++)
517 {
518 forceImpedance(i) =
519 cfg.KpImpedance(i) * poseError(i) - cfg.KdImpedance(i) * currentTwist(i);
520 // forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
521 }
522
523 // --------------------------------------------- Nullspace control ---------------------------------------------
524 const Eigen::VectorXf leftNullspaceTorque =
525 cfg.knull * (cfg.desiredJointValuesLeft - leftqpos) - cfg.dnull * leftqvel;
526 const Eigen::VectorXf rightNullspaceTorque =
527 cfg.knull * (cfg.desiredJointValuesRight - rightqpos) - cfg.dnull * rightqvel;
528
529 // --------------------------------------------- Set Torque Control Command ---------------------------------------------
530 // float lambda = 1;
531
532 // torque limit
533 const auto setTargets =
534 [&](auto& rtarm, const auto& jacobi, const auto& nullspaceTorque, int forceImpOffset)
535 {
536 const Eigen::MatrixXf jtpinv =
537 rtarm.IK->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
538 const Eigen::VectorXf desiredJointTorques =
539 jacobi.transpose() * forceImpedance.block<6, 1>(forceImpOffset, 0) +
540 (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
541 for (size_t i = 0; i < rtarm.targets.size(); ++i)
542 {
543 float desiredTorque = desiredJointTorques(i);
544 if (isnan(desiredTorque))
545 {
546 desiredTorque = 0;
547 }
548 desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
549 desiredTorque =
550 (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
551 dbgOut.desired_torques[rtarm.jointNames[i]] = desiredJointTorques(i);
552 rtarm.targets.at(i)->torque = desiredTorque;
553 }
554 };
555 setTargets(rt.left, jacobiL, leftNullspaceTorque, 0);
556 setTargets(rt.right, jacobiR, rightNullspaceTorque, 6);
557 {
558 const Eigen::MatrixXf jtpinvL =
559 rt.left.IK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
560 const Eigen::VectorXf leftJointDesiredTorques =
561 jacobiL.transpose() * forceImpedance.head<6>() +
562 (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
563 for (size_t i = 0; i < rt.left.targets.size(); ++i)
564 {
565 float desiredTorque = leftJointDesiredTorques(i);
566 if (isnan(desiredTorque))
567 {
568 desiredTorque = 0;
569 }
570 desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
571 desiredTorque =
572 (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
573 dbgOut.desired_torques[rt.left.jointNames[i]] = leftJointDesiredTorques(i);
574 rt.left.targets.at(i)->torque = desiredTorque;
575 }
576 }
577
578 {
579 const Eigen::MatrixXf jtpinvR =
580 rt.right.IK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
581 const Eigen::VectorXf rightJointDesiredTorques =
582 jacobiR.transpose() * forceImpedance.tail<6>() +
583 (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
584 for (size_t i = 0; i < rt.right.targets.size(); ++i)
585 {
586 float desiredTorque = rightJointDesiredTorques(i);
587 if (isnan(desiredTorque))
588 {
589 desiredTorque = 0;
590 }
591 desiredTorque = (desiredTorque > cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
592 desiredTorque =
593 (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
594 dbgOut.desired_torques[rt.right.jointNames[i]] = rightJointDesiredTorques(i);
595 rt.right.targets.at(i)->torque = desiredTorque;
596 }
597 }
598
599 // --------------------------------------------- debug output ---------------------------------------------
600 dbgOut.currentBoxPose = currentBoxPose;
601 dbgOut.forceImpedance = forceImpedance;
602 dbgOut.poseError = poseError;
603 // dbgOut.wrenchesConstrained = wrenchesConstrained;
604 dbgOut.wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
605 // dbgOut.wrenchDMP = wrenchDMP;
606 // dbgOut.computedBoxWrench = computedBoxWrench;
607
608 dbgOut.virtualPose_x = rt.virtualPose(0, 3);
609 dbgOut.virtualPose_y = rt.virtualPose(1, 3);
610 dbgOut.virtualPose_z = rt.virtualPose(2, 3);
611
612 dbgOut.objPose_x = boxCurrentPose(0, 3);
613 dbgOut.objPose_y = boxCurrentPose(1, 3);
614 dbgOut.objPose_z = boxCurrentPose(2, 3);
615
616 dbgOut.objForce_x = objFTValue(0);
617 dbgOut.objForce_y = objFTValue(1);
618 dbgOut.objForce_z = objFTValue(2);
619 dbgOut.objTorque_x = objFTValue(3);
620 dbgOut.objTorque_y = objFTValue(4);
621 dbgOut.objTorque_z = objFTValue(5);
622
623 dbgOut.objVel_x = objVel(0);
624 dbgOut.objVel_y = objVel(1);
625 dbgOut.objVel_z = objVel(2);
626 dbgOut.objVel_rx = objVel(3);
627 dbgOut.objVel_ry = objVel(4);
628 dbgOut.objVel_rz = objVel(5);
629
630 dbgOut.deltaPose_x = deltaObjPose(0);
631 dbgOut.deltaPose_y = deltaObjPose(1);
632 dbgOut.deltaPose_z = deltaObjPose(2);
633 dbgOut.deltaPose_rx = deltaObjPose(3);
634 dbgOut.deltaPose_ry = deltaObjPose(4);
635 dbgOut.deltaPose_rz = deltaObjPose(5);
636
637 dbgOut.modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
638 dbgOut.modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
639 dbgOut.modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
640
641 dbgOut.currentPoseLeft_x = currentLeftPose(0, 3);
642 dbgOut.currentPoseLeft_y = currentLeftPose(1, 3);
643 dbgOut.currentPoseLeft_z = currentLeftPose(2, 3);
644
645
646 dbgOut.modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
647 dbgOut.modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
648 dbgOut.modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
649
650 dbgOut.currentPoseRight_x = currentRightPose(0, 3);
651 dbgOut.currentPoseRight_y = currentRightPose(1, 3);
652 dbgOut.currentPoseRight_z = currentRightPose(2, 3);
653
654
655 dbgOut.dmpBoxPose_x = boxPose(0, 3);
656 dbgOut.dmpBoxPose_y = boxPose(1, 3);
657 dbgOut.dmpBoxPose_z = boxPose(2, 3);
658
659 dbgOut.dmpTwist_x = boxTwist(0);
660 dbgOut.dmpTwist_y = boxTwist(1);
661 dbgOut.dmpTwist_z = boxTwist(2);
662 dbgOut.rx = rRight(0);
663 dbgOut.ry = rRight(1);
664 dbgOut.rz = rRight(2);
665
666 // dbgOut.modifiedTwist_lx = twistDMP(0);
667 // dbgOut.modifiedTwist_ly = twistDMP(1);
668 // dbgOut.modifiedTwist_lz = twistDMP(2);
669 // dbgOut.modifiedTwist_rx = twistDMP(6);
670 // dbgOut.modifiedTwist_ry = twistDMP(7);
671 // dbgOut.modifiedTwist_rz = twistDMP(8);
672
673 // dbgOut.forcePID = forcePIDInRootForDebug;
674
675 debugOutputData.commitWrite();
676 }
677
678 void
679 NJointBimanualCartesianAdmittanceController::onPublish(
680 const SensorAndControl&,
681 const DebugDrawerInterfacePrx&,
682 const DebugObserverInterfacePrx& debugObs)
683 {
684 std::lock_guard guard{debugOutputDataReadMutex};
685 const auto& buf = debugOutputData.getUpToDateReadBuffer();
686 StringVariantBaseMap datafields;
687
688 for (const auto& [name, val] : buf.desired_torques)
689 {
690 datafields[name] = new Variant(val);
691 }
692
693 const auto& reportElements = [&](const auto& vec, const std::string& pre)
694 {
695 for (int i = 0; i < vec.rows(); ++i)
696 {
697 datafields[pre + std::to_string(i)] = new Variant(vec(i));
698 }
699 };
700 reportElements(buf.forceImpedance, "forceImpedance_");
701 reportElements(buf.forcePID, "forcePID_");
702 reportElements(buf.poseError, "poseError_");
703 reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
704 reportElements(buf.wrenchesMeasuredInRoot, "wrenchesMeasuredInRoot_");
705 reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
706 reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
707
708 datafields["virtualPose_x"] = new Variant(buf.virtualPose_x);
709 datafields["virtualPose_y"] = new Variant(buf.virtualPose_y);
710 datafields["virtualPose_z"] = new Variant(buf.virtualPose_z);
711
712 datafields["objPose_x"] = new Variant(buf.objPose_x);
713 datafields["objPose_y"] = new Variant(buf.objPose_y);
714 datafields["objPose_z"] = new Variant(buf.objPose_z);
715
716 datafields["objForce_x"] = new Variant(buf.objForce_x);
717 datafields["objForce_y"] = new Variant(buf.objForce_y);
718 datafields["objForce_z"] = new Variant(buf.objForce_z);
719 datafields["objTorque_x"] = new Variant(buf.objTorque_x);
720 datafields["objTorque_y"] = new Variant(buf.objTorque_y);
721 datafields["objTorque_z"] = new Variant(buf.objTorque_z);
722
723 datafields["objVel_x"] = new Variant(buf.objVel_x);
724 datafields["objVel_y"] = new Variant(buf.objVel_y);
725 datafields["objVel_z"] = new Variant(buf.objVel_z);
726 datafields["objVel_rx"] = new Variant(buf.objVel_rx);
727 datafields["objVel_ry"] = new Variant(buf.objVel_ry);
728 datafields["objVel_rz"] = new Variant(buf.objVel_rz);
729
730 datafields["deltaPose_x"] = new Variant(buf.deltaPose_x);
731 datafields["deltaPose_y"] = new Variant(buf.deltaPose_y);
732 datafields["deltaPose_z"] = new Variant(buf.deltaPose_z);
733 datafields["deltaPose_rx"] = new Variant(buf.deltaPose_rx);
734 datafields["deltaPose_ry"] = new Variant(buf.deltaPose_ry);
735 datafields["deltaPose_rz"] = new Variant(buf.deltaPose_rz);
736
737 datafields["modifiedPoseRight_x"] = new Variant(buf.modifiedPoseRight_x);
738 datafields["modifiedPoseRight_y"] = new Variant(buf.modifiedPoseRight_y);
739 datafields["modifiedPoseRight_z"] = new Variant(buf.modifiedPoseRight_z);
740 datafields["currentPoseLeft_x"] = new Variant(buf.currentPoseLeft_x);
741 datafields["currentPoseLeft_y"] = new Variant(buf.currentPoseLeft_y);
742 datafields["currentPoseLeft_z"] = new Variant(buf.currentPoseLeft_z);
743
744
745 datafields["modifiedPoseLeft_x"] = new Variant(buf.modifiedPoseLeft_x);
746 datafields["modifiedPoseLeft_y"] = new Variant(buf.modifiedPoseLeft_y);
747 datafields["modifiedPoseLeft_z"] = new Variant(buf.modifiedPoseLeft_z);
748
749 datafields["currentPoseRight_x"] = new Variant(buf.currentPoseRight_x);
750 datafields["currentPoseRight_y"] = new Variant(buf.currentPoseRight_y);
751 datafields["currentPoseRight_z"] = new Variant(buf.currentPoseRight_z);
752 datafields["dmpBoxPose_x"] = new Variant(buf.dmpBoxPose_x);
753 datafields["dmpBoxPose_y"] = new Variant(buf.dmpBoxPose_y);
754 datafields["dmpBoxPose_z"] = new Variant(buf.dmpBoxPose_z);
755 datafields["dmpTwist_x"] = new Variant(buf.dmpTwist_x);
756 datafields["dmpTwist_y"] = new Variant(buf.dmpTwist_y);
757 datafields["dmpTwist_z"] = new Variant(buf.dmpTwist_z);
758
759 datafields["modifiedTwist_lx"] = new Variant(buf.modifiedTwist_lx);
760 datafields["modifiedTwist_ly"] = new Variant(buf.modifiedTwist_ly);
761 datafields["modifiedTwist_lz"] = new Variant(buf.modifiedTwist_lz);
762 datafields["modifiedTwist_rx"] = new Variant(buf.modifiedTwist_rx);
763 datafields["modifiedTwist_ry"] = new Variant(buf.modifiedTwist_ry);
764 datafields["modifiedTwist_rz"] = new Variant(buf.modifiedTwist_rz);
765
766 datafields["rx"] = new Variant(buf.rx);
767 datafields["ry"] = new Variant(buf.ry);
768 datafields["rz"] = new Variant(buf.rz);
769
770
771 debugObs->setDebugChannel("BimanualForceController", datafields);
772 }
773
774 Eigen::Matrix4f
775 NJointBimanualCartesianAdmittanceController::getBoxPose(const Ice::Current&) const
776 {
777 std::lock_guard guard{debugOutputDataReadMutex};
778 return debugOutputData.getUpToDateReadBuffer().currentBoxPose;
779 }
780
781 void
782 NJointBimanualCartesianAdmittanceController::setBoxPose(const Eigen::Matrix4f& pose,
783 const Ice::Current&)
784 {
785 ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
786 std::lock_guard guard{targBufWriteMutex};
787 targBuf.getWriteBuffer().pose = pose;
788 targBuf.commitWrite();
789 }
790
791 void
792 NJointBimanualCartesianAdmittanceController::setBoxWidth(float w, const Ice::Current&)
793 {
794 ARMARX_CHECK_GREATER_EQUAL(w, 0);
795 std::lock_guard g{cfgBufWriteMutex};
796 cfgBuf.getWriteBuffer().boxWidth = w;
797 cfgBuf.commitWrite();
798 }
799
800 void
801 NJointBimanualCartesianAdmittanceController::setBoxVelocity(const Eigen::Vector3f& velXYZ,
802 const Eigen::Vector3f& velRPY,
803 const Ice::Current&)
804 {
805 ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
806 ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
807 std::lock_guard guard{targBufWriteMutex};
808 targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
809 targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
810 targBuf.commitWrite();
811 }
812
813 void
814 NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity(
815 const Eigen::Matrix4f& pose,
816 const Eigen::Vector3f& velXYZ,
817 const Eigen::Vector3f& velRPY,
818 const Ice::Current&)
819 {
820 ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
821 ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
822 ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
823 std::lock_guard guard{targBufWriteMutex};
824 targBuf.getWriteBuffer().pose = pose;
825 targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
826 targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
827 targBuf.commitWrite();
828 }
829
830 void
831 NJointBimanualCartesianAdmittanceController::moveBoxPose(const Eigen::Matrix4f& pose,
832 const Ice::Current&)
833 {
834 ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
835 std::lock_guard guard{targBufWriteMutex};
836 const Eigen::Matrix4f tmp = pose * targBuf.getWriteBuffer().pose;
837 targBuf.getWriteBuffer().pose = tmp;
838 targBuf.commitWrite();
839 }
840
841 void
842 NJointBimanualCartesianAdmittanceController::moveBoxPosition(const Eigen::Vector3f& pos,
843 const Ice::Current&)
844 {
845 ARMARX_CHECK_EXPRESSION(simox::math::isfinite(pos));
846 std::lock_guard guard{targBufWriteMutex};
847 targBuf.getWriteBuffer().pose.topRightCorner<3, 1>() += pos;
848 targBuf.commitWrite();
849 }
850
851 void
852 NJointBimanualCartesianAdmittanceController::setConfig(
853 const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr,
854 const Ice::Current&)
855 {
856 ARMARX_CHECK_NOT_NULL(ptr);
857 ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesLeft.size(), rt.left.targets.size());
858 ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesRight.size(), rt.right.targets.size());
859 std::lock_guard g{cfgBufWriteMutex};
860 updateConfig(*ptr);
861 cfgBuf.commitWrite();
862 }
863
864 void
865 NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft(
866 const Ice::FloatSeq& vals,
867 const Ice::Current&)
868 {
869 std::lock_guard g{cfgBufWriteMutex};
870 updateDesiredJointValuesLeft(vals);
871 cfgBuf.commitWrite();
872 }
873
874 void
875 NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight(
876 const Ice::FloatSeq& vals,
877 const Ice::Current&)
878 {
879 std::lock_guard g{cfgBufWriteMutex};
880 updateDesiredJointValuesRight(vals);
881 cfgBuf.commitWrite();
882 }
883
884 void
885 NJointBimanualCartesianAdmittanceController::setNullspaceConfig(
886 const detail::NJBmanCartAdmCtrl::Nullspace& nullspace,
887 const Ice::Current&)
888 {
889 std::lock_guard g{cfgBufWriteMutex};
890 updateNullspaceConfig(nullspace);
891 cfgBuf.commitWrite();
892 }
893
894 void
895 NJointBimanualCartesianAdmittanceController::setAdmittanceConfig(
896 const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject,
897 const Ice::Current&)
898 {
899 std::lock_guard g{cfgBufWriteMutex};
900 updateAdmittanceConfig(admittanceObject);
901 cfgBuf.commitWrite();
902 }
903
904 void
905 NJointBimanualCartesianAdmittanceController::setForceConfig(
906 const detail::NJBmanCartAdmCtrl::Force& left,
907 const detail::NJBmanCartAdmCtrl::Force& right,
908 const Ice::Current&)
909 {
910 std::lock_guard g{cfgBufWriteMutex};
911 updateForceConfig(left, right);
912 cfgBuf.commitWrite();
913 }
914
915 void
916 NJointBimanualCartesianAdmittanceController::setImpedanceConfig(
917 const detail::NJBmanCartAdmCtrl::Impedance& left,
918 const detail::NJBmanCartAdmCtrl::Impedance& right,
919 const Ice::Current&)
920 {
921 std::lock_guard g{cfgBufWriteMutex};
922 updateImpedanceConfig(left, right);
923 cfgBuf.commitWrite();
924 }
925
926 void
927 NJointBimanualCartesianAdmittanceController::updateConfig(
928 const NJointBimanualCartesianAdmittanceControllerConfig& cfg)
929 {
930 std::lock_guard g{cfgBufWriteMutex};
931 auto& buf = cfgBuf.getWriteBuffer();
932 updateNullspaceConfig(cfg.nullspace);
933 updateAdmittanceConfig(cfg.admittanceObject);
934 updateForceConfig(cfg.forceLeft, cfg.forceRight);
935 updateImpedanceConfig(cfg.impedanceLeft, cfg.impedanceRight);
936 buf.torqueLimit = cfg.torqueLimit;
937 buf.filterCoeff = cfg.filterCoeff;
938 buf.ftCalibrationTime = cfg.ftCalibrationTime;
939 buf.boxWidth = cfg.box.width;
940 }
941
942 void
943 NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft(
944 const Ice::FloatSeq& vals)
945 {
946 std::lock_guard g{cfgBufWriteMutex};
947 auto& buf = cfgBuf.getWriteBuffer();
948 ARMARX_CHECK_EQUAL(vals.size(), rt.left.targets.size());
949 buf.desiredJointValuesLeft = Eigen::Map<const Eigen::VectorXf>(vals.data(), vals.size());
950 }
951
952 void
953 NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight(
954 const Ice::FloatSeq& vals)
955 {
956 std::lock_guard g{cfgBufWriteMutex};
957 auto& buf = cfgBuf.getWriteBuffer();
958 ARMARX_CHECK_EQUAL(vals.size(), rt.right.targets.size());
959 buf.desiredJointValuesRight = Eigen::Map<const Eigen::VectorXf>(vals.data(), vals.size());
960 }
961
962 void
963 NJointBimanualCartesianAdmittanceController::updateNullspaceConfig(
964 const detail::NJBmanCartAdmCtrl::Nullspace& nullspace)
965 {
966 std::lock_guard g{cfgBufWriteMutex};
967 auto& cfg = cfgBuf.getWriteBuffer();
968 updateDesiredJointValuesLeft(nullspace.desiredJointValuesLeft);
969 updateDesiredJointValuesRight(nullspace.desiredJointValuesRight);
970 cfg.knull = nullspace.k;
971 cfg.dnull = nullspace.d;
972 }
973
974 void
975 NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig(
976 const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
977 {
978 std::lock_guard g{cfgBufWriteMutex};
979 auto& cfg = cfgBuf.getWriteBuffer();
980 cfg.KmAdmittance.block<3, 1>(0, 0) = admittanceObject.KmXYZ;
981 cfg.KmAdmittance.block<3, 1>(3, 0) = admittanceObject.KmRPY;
982
983 cfg.KpAdmittance.block<3, 1>(0, 0) = admittanceObject.KpXYZ;
984 cfg.KpAdmittance.block<3, 1>(3, 0) = admittanceObject.KpRPY;
985
986 cfg.KdAdmittance.block<3, 1>(0, 0) = admittanceObject.KdXYZ;
987 cfg.KdAdmittance.block<3, 1>(3, 0) = admittanceObject.KdRPY;
988 }
989
990 void
991 NJointBimanualCartesianAdmittanceController::updateForceConfig(
992 const detail::NJBmanCartAdmCtrl::Force& forceLeft,
993 const detail::NJBmanCartAdmCtrl::Force& forceRight)
994 {
995 std::lock_guard g{cfgBufWriteMutex};
996 auto& cfg = cfgBuf.getWriteBuffer();
997 //left
998 cfg.massLeft = forceLeft.mass;
999 cfg.CoMVecLeft = forceLeft.com;
1000 cfg.forceOffsetLeft = forceLeft.offsetForce;
1001 cfg.torqueOffsetLeft = forceLeft.offsetTorque;
1002 cfg.targetWrench.block<3, 1>(0, 0) = forceLeft.wrenchXYZ;
1003 cfg.targetWrench.block<3, 1>(3, 0) = forceLeft.wrenchRPY;
1004 cfg.forceThreshold.block<3, 1>(0, 0) = forceLeft.forceThreshold;
1005 //right
1006 cfg.massRight = forceRight.mass;
1007 cfg.CoMVecRight = forceRight.com;
1008 cfg.forceOffsetRight = forceRight.offsetForce;
1009 cfg.torqueOffsetRight = forceRight.offsetTorque;
1010 cfg.targetWrench.block<3, 1>(6, 0) = forceRight.wrenchXYZ;
1011 cfg.targetWrench.block<3, 1>(9, 0) = forceRight.wrenchRPY;
1012 cfg.forceThreshold.block<3, 1>(3, 0) = forceRight.forceThreshold;
1013 }
1014
1015 void
1016 NJointBimanualCartesianAdmittanceController::updateImpedanceConfig(
1017 const detail::NJBmanCartAdmCtrl::Impedance& impedanceLeft,
1018 const detail::NJBmanCartAdmCtrl::Impedance& impedanceRight)
1019 {
1020 std::lock_guard g{cfgBufWriteMutex};
1021 auto& cfg = cfgBuf.getWriteBuffer();
1022 cfg.KpImpedance.block<3, 1>(0, 0) = impedanceLeft.KpXYZ;
1023 cfg.KpImpedance.block<3, 1>(3, 0) = impedanceLeft.KpRPY;
1024 cfg.KpImpedance.block<3, 1>(6, 0) = impedanceRight.KpXYZ;
1025 cfg.KpImpedance.block<3, 1>(9, 0) = impedanceRight.KpRPY;
1026
1027 cfg.KdImpedance.block<3, 1>(0, 0) = impedanceLeft.KdXYZ;
1028 cfg.KdImpedance.block<3, 1>(3, 0) = impedanceLeft.KdRPY;
1029 cfg.KdImpedance.block<3, 1>(6, 0) = impedanceRight.KdXYZ;
1030 cfg.KdImpedance.block<3, 1>(9, 0) = impedanceRight.KdRPY;
1031 }
1032} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
armarx::NJointControllerBase::useSensorValue
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition NJointController.cpp:383
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition NJointController.cpp:293
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition NJointControllerBase.h:846
armarx::NJointControllerBase::useControlTarget
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition NJointController.cpp:410
armarx::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::NJointBimanualCartesianAdmittanceController::setBoxWidth
void setBoxWidth(float w, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:792
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight
void updateDesiredJointValuesRight(const Ice::FloatSeq &cfg)
Definition NJointBimanualCartesianAdmittanceController.cpp:953
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition NJointBimanualCartesianAdmittanceController.cpp:170
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setConfig
void setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr &ptr, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:852
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft
void setDesiredJointValuesLeft(const Ice::FloatSeq &vals, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:865
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig
void updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
Definition NJointBimanualCartesianAdmittanceController.cpp:975
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft
void updateDesiredJointValuesLeft(const Ice::FloatSeq &cfg)
Definition NJointBimanualCartesianAdmittanceController.cpp:943
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateNullspaceConfig
void updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace &nullspace)
Definition NJointBimanualCartesianAdmittanceController.cpp:963
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxPose
void setBoxPose(const Eigen::Matrix4f &pose, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:782
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxVelocity
void setBoxVelocity(const Eigen::Vector3f &velXYZ, const Eigen::Vector3f &velRPY, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:801
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateConfig
void updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig &cfg)
Definition NJointBimanualCartesianAdmittanceController.cpp:927
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::moveBoxPose
void moveBoxPose(const Eigen::Matrix4f &pose, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:831
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController
NJointBimanualCartesianAdmittanceController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition NJointBimanualCartesianAdmittanceController.cpp:36
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition NJointBimanualCartesianAdmittanceController.cpp:243
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateForceConfig
void updateForceConfig(const detail::NJBmanCartAdmCtrl::Force &left, const detail::NJBmanCartAdmCtrl::Force &right)
Definition NJointBimanualCartesianAdmittanceController.cpp:991
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::updateImpedanceConfig
void updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance &left, const detail::NJBmanCartAdmCtrl::Impedance &right)
Definition NJointBimanualCartesianAdmittanceController.cpp:1016
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setNullspaceConfig
void setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace &nullspace, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:885
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setAdmittanceConfig
void setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance &admittanceObject, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:895
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity
void setBoxPoseAndVelocity(const Eigen::Matrix4f &pose, const Eigen::Vector3f &velXYZ, const Eigen::Vector3f &velRPY, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:814
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::moveBoxPosition
void moveBoxPosition(const Eigen::Vector3f &pos, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:842
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setImpedanceConfig
void setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance &left, const detail::NJBmanCartAdmCtrl::Impedance &right, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:916
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setForceConfig
void setForceConfig(const detail::NJBmanCartAdmCtrl::Force &left, const detail::NJBmanCartAdmCtrl::Force &right, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:905
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight
void setDesiredJointValuesRight(const Ice::FloatSeq &vals, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointBimanualCartesianAdmittanceController.cpp:875
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointBimanualCartesianAdmittanceController.cpp:679
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualCartesianAdmittanceController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition NJointBimanualCartesianAdmittanceController.cpp:210
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition ExpressionException.h:73
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition ExpressionException.h:123
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition ExpressionException.h:206
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_ON_SCOPE_EXIT
#define ARMARX_ON_SCOPE_EXIT
Executes given code when the enclosing scope is left.
Definition OnScopeExit.h:120
Eigen::Vector6f
Matrix< float, 6, 1 > Vector6f
Definition CartesianNaturalPositionController.h:40
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition NJointBimanualCartesianAdmittanceController.cpp:31
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualCartesianAdmittanceController
NJointControllerRegistration< NJointBimanualCartesianAdmittanceController > registrationControllerNJointBimanualCartesianAdmittanceController("NJointBimanualCartesianAdmittanceController")
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