NJointBimanualObjLevelVelController.cpp
Go to the documentation of this file.
1 #include "NJointBimanualObjLevelVelController.h"
2 
3 #include <VirtualRobot/MathTools.h>
4 #include <VirtualRobot/VirtualRobot.h>
5 
6 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
7 #include <ArmarXCore/core/time/CycleUtil.h>
8 
9 
10 namespace armarx::control::deprecated_njoint_mp_controller::bimanual
11 {
12  NJointControllerRegistration<NJointBimanualObjLevelVelController>
13  registrationControllerNJointBimanualObjLevelVelController(
14  "NJointBimanualObjLevelVelController");
15 
16  NJointBimanualObjLevelVelController::NJointBimanualObjLevelVelController(
17  const RobotUnitPtr& robUnit,
18  const armarx::NJointControllerConfigPtr& config,
19  const VirtualRobot::RobotPtr&)
20  {
21  ARMARX_INFO << "Preparing ... bimanual ";
22  ARMARX_INFO << "I am here";
23 
24  useSynchronizedRtRobot();
25  cfg = NJointBimanualObjLevelVelControllerConfigPtr::dynamicCast(config);
26  // ARMARX_CHECK_EXPRESSION(prov);
27  // RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
28  // ARMARX_CHECK_EXPRESSION(robotUnit);
29  leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
30 
31  for (size_t i = 0; i < leftRNS->getSize(); ++i)
32  {
33  std::string jointName = leftRNS->getNode(i)->getName();
34  leftJointNames.push_back(jointName);
35  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
36  const SensorValueBase* sv = useSensorValue(jointName);
37  leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
38  const SensorValue1DoFActuatorVelocity* velocitySensor =
39  sv->asA<SensorValue1DoFActuatorVelocity>();
40  const SensorValue1DoFActuatorPosition* positionSensor =
41  sv->asA<SensorValue1DoFActuatorPosition>();
42 
43  if (!velocitySensor)
44  {
45  ARMARX_WARNING << "No velocitySensor available for " << jointName;
46  }
47  if (!positionSensor)
48  {
49  ARMARX_WARNING << "No positionSensor available for " << jointName;
50  }
51 
52 
53  leftVelocitySensors.push_back(velocitySensor);
54  leftPositionSensors.push_back(positionSensor);
55  };
56 
57  rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
58 
59  for (size_t i = 0; i < rightRNS->getSize(); ++i)
60  {
61  std::string jointName = rightRNS->getNode(i)->getName();
62  rightJointNames.push_back(jointName);
63  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
64  const SensorValueBase* sv = useSensorValue(jointName);
65  rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
66 
67  const SensorValue1DoFActuatorVelocity* velocitySensor =
68  sv->asA<SensorValue1DoFActuatorVelocity>();
69  const SensorValue1DoFActuatorPosition* positionSensor =
70  sv->asA<SensorValue1DoFActuatorPosition>();
71 
72  if (!velocitySensor)
73  {
74  ARMARX_WARNING << "No velocitySensor available for " << jointName;
75  }
76  if (!positionSensor)
77  {
78  ARMARX_WARNING << "No positionSensor available for " << jointName;
79  }
80 
81  rightVelocitySensors.push_back(velocitySensor);
82  rightPositionSensors.push_back(positionSensor);
83  };
84 
85  leftIK.reset(new VirtualRobot::DifferentialIK(
86  leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
87  rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
88  rightRNS->getRobot()->getRootNode(),
89  VirtualRobot::JacobiProvider::eSVDDamped));
90 
91 
92  leftTCPController.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
93  rightTCPController.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
94 
95  double phaseL = 10;
96  double phaseK = 10;
97  double phaseDist0 = 50;
98  double phaseDist1 = 10;
99  double phaseKpPos = 1;
100  double phaseKpOri = 0.1;
101  double posToOriRatio = 10;
102  tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
103  taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
104  taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
105  taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
106  taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
107  taskSpaceDMPConfig.DMPAmplitude = 1.0;
108  taskSpaceDMPConfig.phaseStopParas.Dori = 0;
109  taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
110  taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
111  taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
112  taskSpaceDMPConfig.phaseStopParas.Kpos = phaseKpPos;
113  taskSpaceDMPConfig.phaseStopParas.Kori = phaseKpOri;
114  taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
115  taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
116  taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
117 
118 
119  objectDMP.reset(new tsvmp::TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
120  ARMARX_IMPORTANT << "dmp finished";
121 
122  tcpLeft = leftRNS->getTCP();
123  tcpRight = rightRNS->getTCP();
124 
125  //initialize control parameters
126  KpImpedance.setZero(cfg->KpImpedance.size());
127  ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
128 
129  for (int i = 0; i < KpImpedance.size(); ++i)
130  {
131  KpImpedance(i) = cfg->KpImpedance.at(i);
132  }
133 
134  KdImpedance.setZero(cfg->KdImpedance.size());
135  ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 6);
136 
137  for (int i = 0; i < KdImpedance.size(); ++i)
138  {
139  KdImpedance(i) = cfg->KdImpedance.at(i);
140  }
141 
142  Inferface2rtData initInt2rtData;
143  initInt2rtData.KpImpedance = KpImpedance;
144  initInt2rtData.KdImpedance = KdImpedance;
145  interface2rtBuffer.reinitAllBuffers(initInt2rtData);
146 
147  leftDesiredJointValues.resize(leftTargets.size());
148  ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
149 
150  for (size_t i = 0; i < leftTargets.size(); ++i)
151  {
152  leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
153  }
154 
155  rightDesiredJointValues.resize(rightTargets.size());
156  ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
157 
158  for (size_t i = 0; i < rightTargets.size(); ++i)
159  {
160  rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
161  }
162 
163  virtualPose = Eigen::Matrix4f::Identity();
164  ARMARX_INFO << "got controller params";
165 
166  rt2ControlData initSensorData;
167  initSensorData.deltaT = 0;
168  initSensorData.currentTime = 0;
169  initSensorData.currentPose = leftRNS->getTCP()->getPoseInRootFrame();
170  initSensorData.currentTwist.setZero();
171  rt2ControlBuffer.reinitAllBuffers(initSensorData);
172 
173 
174  ControlInterfaceData initInterfaceData;
175  initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
176  initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
177  initInterfaceData.currentObjPose = leftRNS->getTCP()->getPoseInRootFrame();
178  initInterfaceData.currentObjVel.setZero();
179  controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
180 
181 
182  leftInitialPose = tcpLeft->getPoseInRootFrame();
183  rightInitialPose = tcpRight->getPoseInRootFrame();
184  leftInitialPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3);
185  rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3);
186 
187  // TODO the following is only predefined for balance ball
188  fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
189 
190  Eigen::Matrix4f rightLeveledRotation =
191  VirtualRobot::MathTools::quat2eigen4f(0.5, -0.5, -0.5, -0.5);
192  Eigen::Matrix4f leftLeveledRotation =
193  VirtualRobot::MathTools::quat2eigen4f(0.5, 0.5, 0.5, -0.5);
194  fixedLeftRightRotOffset = leftLeveledRotation.block<3, 3>(0, 0).transpose() *
195  rightLeveledRotation.block<3, 3>(0, 0);
196 
197 
198  boxInitialPose = leftInitialPose;
199  boxInitialPose(0, 3) = (leftInitialPose(0, 3) + rightInitialPose(0, 3)) / 2;
200  boxInitialPose(1, 3) = (leftInitialPose(1, 3) + rightInitialPose(1, 3)) / 2;
201  boxInitialPose(2, 3) = (leftInitialPose(2, 3) + rightInitialPose(2, 3)) / 2;
202 
203  NJointBimanualObjLevelVelControlData initData;
204  initData.boxPose = boxInitialPose;
205  reinitTripleBuffer(initData);
206 
207  dmpGoal = boxInitialPose;
208 
209  firstLoop = true;
210  ARMARX_INFO << "left initial pose: \n"
211  << leftInitialPose << "\n right initial pose: \n"
212  << rightInitialPose;
213  dmpStarted = false;
214  objCom2TCPLeftInObjFrame.setZero();
215  objCom2TCPRightInObjFrame.setZero();
216  }
217 
218  void
219  NJointBimanualObjLevelVelController::setMPWeights(const DoubleSeqSeq& weights,
220  const Ice::Current&)
221  {
222  objectDMP->setWeights(weights);
223  }
224 
225  DoubleSeqSeq
226  NJointBimanualObjLevelVelController::getMPWeights(const Ice::Current&)
227  {
228  DMP::DVec2d res = objectDMP->getWeights();
229  DoubleSeqSeq resvec;
230  for (size_t i = 0; i < res.size(); ++i)
231  {
232  std::vector<double> cvec;
233  for (size_t j = 0; j < res[i].size(); ++j)
234  {
235  cvec.push_back(res[i][j]);
236  }
237  resvec.push_back(cvec);
238  }
239 
240  return resvec;
241  }
242 
243  void
244  NJointBimanualObjLevelVelController::setMPRotWeights(const DoubleSeqSeq& weights,
245  const Ice::Current&)
246  {
247  objectDMP->setRotWeights(weights);
248  }
249 
250  DoubleSeqSeq
251  NJointBimanualObjLevelVelController::getMPRotWeights(const Ice::Current&)
252  {
253  DMP::DVec2d res = objectDMP->getRotWeights();
254  DoubleSeqSeq resvec;
255  for (size_t i = 0; i < res.size(); ++i)
256  {
257  std::vector<double> cvec;
258  for (size_t j = 0; j < res[i].size(); ++j)
259  {
260  cvec.push_back(res[i][j]);
261  }
262  resvec.push_back(cvec);
263  }
264 
265  return resvec;
266  }
267 
268  void
269  NJointBimanualObjLevelVelController::rtPreActivateController()
270  {
271  Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
272  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
273  Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
274  leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3);
275  rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
276  boxInitPose.block<3, 1>(0, 3) =
277  0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
278  boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
279 
280  NJointBimanualObjLevelVelControlData initData;
281  initData.boxPose = boxInitPose;
282  reinitTripleBuffer(initData);
283  }
284 
285  std::string
286  NJointBimanualObjLevelVelController::getClassName(const Ice::Current&) const
287  {
288  return "NJointBimanualObjLevelVelController";
289  }
290 
291  void
292  NJointBimanualObjLevelVelController::controllerRun()
293  {
294  if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
295  {
296  return;
297  }
298 
299  double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
300  Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
301  Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
302 
303  if (objectDMP->canVal < 0)
304  {
305  finished = true;
306  dmpStarted = false;
307  LockGuardType guard{controlDataMutex};
308  getWriterControlStruct().boxPose = dmpGoal;
309  writeControlStruct();
310  }
311  else
312  {
313  objectDMP->flow(deltaT, currentPose, currentTwist);
314  // VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(objectDMP->getTargetPoseMat());
315  LockGuardType guard{controlDataMutex};
316  getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
317  writeControlStruct();
318  }
319  }
320 
321 
322  Eigen::VectorXf
323  NJointBimanualObjLevelVelController::calcIK(VirtualRobot::DifferentialIKPtr ik,
324  const Eigen::MatrixXf& jacobi,
325  const Eigen::VectorXf& cartesianVel,
326  const Eigen::VectorXf& nullspaceVel)
327  {
328  Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
329 
330  Eigen::MatrixXf nullspace = lu_decomp.kernel();
331  Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
332  for (int i = 0; i < nullspace.cols(); i++)
333  {
334  float squaredNorm = nullspace.col(i).squaredNorm();
335  // Prevent division by zero
336  if (squaredNorm > 1.0e-32f)
337  {
338  nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) /
339  nullspace.col(i).squaredNorm();
340  }
341  }
342 
343  Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(
344  jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
345  Eigen::VectorXf jointVel = inv * cartesianVel;
346  return jointVel;
347  }
348 
349  void
350  NJointBimanualObjLevelVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
351  const IceUtil::Time& timeSinceLastIteration)
352  {
353  Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
354  Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
355 
356  double deltaT = timeSinceLastIteration.toSecondsDouble();
357 
358  if (firstLoop)
359  {
360  Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
361  Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
362 
363  leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3);
364  rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
365 
366  virtualPose.block<3, 1>(0, 3) =
367  0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
368  virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
369  // fixedLeftRightRotOffset = virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
370 
371  Eigen::Vector3f objCom2TCPLeftInGlobal =
372  leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
373  Eigen::Vector3f objCom2TCPRightInGlobal =
374  rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
375 
376  objCom2TCPLeftInObjFrame =
377  virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
378  objCom2TCPRightInObjFrame =
379  virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
380  firstLoop = false;
381  }
382 
383  // --------------------------------------------- get control parameters ---------------------------------------
384  KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
385  KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
386 
387  // --------------------------------------------- grasp matrix ---------------------------------------------
388 
389  Eigen::MatrixXf graspMatrix;
390  graspMatrix.setZero(6, 12);
391  graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
392  graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
393  Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
394  graspMatrix.block<3, 3>(3, 0) = skew(rvec);
395 
396  rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
397  graspMatrix.block<3, 3>(3, 6) = skew(rvec);
398 
399  float lambda = 1;
400  Eigen::MatrixXf pinvGraspMatrixT =
401  leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
402 
403  // ---------------------------------------------- object pose ----------------------------------------------
404  Eigen::Matrix4f boxCurrentPose = currentLeftPose;
405  boxCurrentPose.block<3, 1>(0, 3) =
406  0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
407  Eigen::VectorXf boxCurrentTwist;
408  boxCurrentTwist.setZero(6);
409 
410  // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
411  Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
412  // Jacobian matrices
413  Eigen::MatrixXf jacobiL =
414  leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
415  Eigen::MatrixXf jacobiR =
416  rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
417 
418  // qpos, qvel
419  Eigen::VectorXf leftqpos;
420  Eigen::VectorXf leftqvel;
421  leftqpos.resize(leftPositionSensors.size());
422  leftqvel.resize(leftVelocitySensors.size());
423  for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
424  {
425  leftqpos(i) = leftPositionSensors[i]->position;
426  leftqvel(i) = leftVelocitySensors[i]->velocity;
427  }
428 
429  Eigen::VectorXf rightqpos;
430  Eigen::VectorXf rightqvel;
431  rightqpos.resize(rightPositionSensors.size());
432  rightqvel.resize(rightVelocitySensors.size());
433 
434  for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
435  {
436  rightqpos(i) = rightPositionSensors[i]->position;
437  rightqvel(i) = rightVelocitySensors[i]->velocity;
438  }
439 
440  // -------------------------------------- compute TCP and object velocity -------------------------------------
441  Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
442  Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
443 
444  Eigen::VectorXf currentTwist(12);
445  currentTwist << currentLeftTwist, currentRightTwist;
446  boxCurrentTwist = pinvGraspMatrixT * currentTwist;
447 
448  rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
449  rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
450  rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
451  rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
452  rt2ControlBuffer.commitWrite();
453 
454  // pass sensor value to statechart
455  controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
456  controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
457  controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
458  controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
459  controlInterfaceBuffer.commitWrite();
460 
461 
462  // --------------------------------------------- get MP target ---------------------------------------------
463  virtualPose = rtGetControlStruct().boxPose;
464  // Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
465 
466  // --------------------------------------------- convert to tcp pose ---------------------------------------------
467  Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
468  Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
469 
470  tcpTargetPoseRight.block<3, 3>(0, 0) =
471  virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
472  tcpTargetPoseLeft.block<3, 1>(0, 3) +=
473  virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
474  tcpTargetPoseRight.block<3, 1>(0, 3) +=
475  virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
476 
477  // --------------------------------------------- Velocity control ---------------------------------------------
478 
479  Eigen::Matrix3f diffMatLeft =
480  tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).inverse();
481  Eigen::Vector3f errorRPYLeft = VirtualRobot::MathTools::eigen3f2rpy(diffMatLeft);
482  Eigen::Matrix3f diffMatRight =
483  tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).inverse();
484  Eigen::Vector3f errorRPYRight = VirtualRobot::MathTools::eigen3f2rpy(diffMatRight);
485 
486  Eigen::Vector6f leftTargetVel, rightTargetVel;
487  for (size_t i = 0; i < 3; ++i)
488  {
489  leftTargetVel(i) = KpImpedance(i) * (tcpTargetPoseLeft(i, 3) - currentLeftPose(i, 3)) +
490  KdImpedance(i) * (-currentLeftTwist(i));
491  leftTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYLeft(i) +
492  KdImpedance(i + 3) * (-currentLeftTwist(i + 3));
493  rightTargetVel(i) =
494  KpImpedance(i) * (tcpTargetPoseRight(i, 3) - currentRightPose(i, 3)) +
495  KdImpedance(i) * (-currentRightTwist(i));
496  rightTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYRight(i) +
497  KdImpedance(i + 3) * (-currentRightTwist(i + 3));
498  }
499 
500 
501  Eigen::VectorXf leftJointNullSpaceVel =
502  cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel +
503  cfg->jointLimitAvoidanceKp * leftTCPController->calculateJointLimitAvoidance();
504  Eigen::VectorXf leftJointTargetVel =
505  calcIK(leftIK, jacobiL, leftTargetVel, leftJointNullSpaceVel);
506 
507 
508  Eigen::VectorXf rightJointNullSpaceVel =
509  cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel +
510  cfg->jointLimitAvoidanceKp * rightTCPController->calculateJointLimitAvoidance();
511  Eigen::VectorXf rightJointTargetVel =
512  calcIK(rightIK, jacobiR, rightTargetVel, rightJointNullSpaceVel);
513 
514  for (size_t i = 0; i < leftTargets.size(); ++i)
515  {
516  float desiredVel = leftJointTargetVel(i);
517  debugOutputData.getWriteBuffer().desired_vels[leftJointNames[i]] = desiredVel;
518 
519  if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
520  {
521  desiredVel = 0.0;
522  }
523 
524  leftTargets.at(i)->velocity = desiredVel;
525  }
526 
527  for (size_t i = 0; i < rightTargets.size(); ++i)
528  {
529  float desiredVel = rightJointTargetVel(i);
530  debugOutputData.getWriteBuffer().desired_vels[rightJointNames[i]] = desiredVel;
531 
532  if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
533  {
534  desiredVel = 0.0;
535  }
536 
537  rightTargets.at(i)->velocity = desiredVel;
538  }
539 
540 
541  // --------------------------------------------- debug output ---------------------------------------------
542  debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
543  debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
544  debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
545 
546  debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
547  debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
548  debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
549 
550 
551  VirtualRobot::MathTools::Quaternion leftQuat =
552  VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
553  debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
554  debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
555  debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
556  debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
557 
558  debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
559  debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
560  debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
561 
562  VirtualRobot::MathTools::Quaternion rightQuat =
563  VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
564  debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
565  debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
566  debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
567  debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
568 
569 
570  debugOutputData.getWriteBuffer().dmpBoxPose_x = virtualPose(0, 3);
571  debugOutputData.getWriteBuffer().dmpBoxPose_y = virtualPose(1, 3);
572  debugOutputData.getWriteBuffer().dmpBoxPose_z = virtualPose(2, 3);
573 
574  VirtualRobot::MathTools::Quaternion dmpQuat =
575  VirtualRobot::MathTools::eigen4f2quat(virtualPose);
576  debugOutputData.getWriteBuffer().dmpBoxPose_qx = dmpQuat.x;
577  debugOutputData.getWriteBuffer().dmpBoxPose_qy = dmpQuat.y;
578  debugOutputData.getWriteBuffer().dmpBoxPose_qz = dmpQuat.z;
579  debugOutputData.getWriteBuffer().dmpBoxPose_qw = dmpQuat.w;
580  debugOutputData.commitWrite();
581  }
582 
583  void
584  NJointBimanualObjLevelVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
585  const Ice::Current&)
586  {
587  objectDMP->learnDMPFromFiles(fileNames);
588  }
589 
590 
591  void
592  NJointBimanualObjLevelVelController::setGoals(const Ice::DoubleSeq& goals,
593  const Ice::Current& ice)
594  {
595  LockGuardType guard(controllerMutex);
596  objectDMP->setGoalPoseVec(goals);
597  dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
598  dmpGoal(0, 3) = goals[0];
599  dmpGoal(1, 3) = goals[1];
600  dmpGoal(2, 3) = goals[2];
601  }
602 
603 
604  void
605  NJointBimanualObjLevelVelController::runDMP(const Ice::DoubleSeq& goals,
606  Ice::Double timeDuration,
607  const Ice::Current&)
608  {
609  while (!controlInterfaceBuffer.updateReadBuffer())
610  {
611  usleep(1000);
612  }
613 
614  Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
615  Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
616 
617  VirtualRobot::MathTools::Quaternion boxOri =
618  VirtualRobot::MathTools::eigen4f2quat(leftPose);
619  Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
620 
621 
622  std::vector<double> boxInitialPose;
623  for (size_t i = 0; i < 3; ++i)
624  {
625  boxInitialPose.push_back(boxPosi(i)); //Important: mm -> m
626  }
627  boxInitialPose.push_back(boxOri.w);
628  boxInitialPose.push_back(boxOri.x);
629  boxInitialPose.push_back(boxOri.y);
630  boxInitialPose.push_back(boxOri.z);
631 
632  dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
633  dmpGoal(0, 3) = goals[0];
634  dmpGoal(1, 3) = goals[1];
635  dmpGoal(2, 3) = goals[2];
636 
637  objectDMP->prepareExecution(boxInitialPose, goals);
638  objectDMP->canVal = timeDuration;
639  objectDMP->config.motionTimeDuration = timeDuration;
640 
641 
642  finished = false;
643  dmpStarted = true;
644  }
645 
646  void
647  NJointBimanualObjLevelVelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts,
648  const Ice::DoubleSeq& goals,
649  Ice::Double timeDuration,
650  const Ice::Current&)
651  {
652  while (!controlInterfaceBuffer.updateReadBuffer())
653  {
654  usleep(1000);
655  }
656  ARMARX_IMPORTANT << "obj level control: setup dmp ...";
657 
658  dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
659  dmpGoal(0, 3) = goals[0];
660  dmpGoal(1, 3) = goals[1];
661  dmpGoal(2, 3) = goals[2];
662 
663  objectDMP->prepareExecution(starts, goals);
664  objectDMP->canVal = timeDuration;
665  objectDMP->config.motionTimeDuration = timeDuration;
666 
667  finished = false;
668  dmpStarted = true;
669 
670  ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
671  }
672 
673  void
674  NJointBimanualObjLevelVelController::setViaPoints(Ice::Double u,
675  const Ice::DoubleSeq& viapoint,
676  const Ice::Current&)
677  {
678  // LockGuardType guard(controllerMutex);
679  ARMARX_INFO << "setting via points ";
680  objectDMP->setViaPose(u, viapoint);
681  }
682 
683  void
684  NJointBimanualObjLevelVelController::removeAllViaPoints(const Ice::Current&)
685  {
686  objectDMP->removeAllViaPoints();
687  }
688 
689  void
690  NJointBimanualObjLevelVelController::setKpImpedance(const Ice::FloatSeq& value,
691  const Ice::Current&)
692  {
693 
694  Eigen::VectorXf setpoint;
695  setpoint.setZero(value.size());
696  ARMARX_CHECK_EQUAL(value.size(), 6);
697 
698  for (size_t i = 0; i < value.size(); ++i)
699  {
700  setpoint(i) = value.at(i);
701  }
702 
703  LockGuardType guard{interfaceDataMutex};
704  interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
705  interface2rtBuffer.commitWrite();
706  }
707 
708  void
709  NJointBimanualObjLevelVelController::setKdImpedance(const Ice::FloatSeq& value,
710  const Ice::Current&)
711  {
712  Eigen::VectorXf setpoint;
713  setpoint.setZero(value.size());
714  ARMARX_CHECK_EQUAL(value.size(), 6);
715 
716  for (size_t i = 0; i < value.size(); ++i)
717  {
718  setpoint(i) = value.at(i);
719  }
720 
721  LockGuardType guard{interfaceDataMutex};
722  interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
723  interface2rtBuffer.commitWrite();
724  }
725 
726 
727  std::vector<float>
728  NJointBimanualObjLevelVelController::getCurrentObjVel(const Ice::Current&)
729  {
730  Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
731  std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
732  return tvelvec;
733  }
734 
735 
736  void
737  NJointBimanualObjLevelVelController::onPublish(const SensorAndControl&,
738  const DebugDrawerInterfacePrx&,
739  const DebugObserverInterfacePrx& debugObs)
740  {
741 
742  StringVariantBaseMap datafields;
743  auto values = debugOutputData.getUpToDateReadBuffer().desired_vels;
744  for (auto& pair : values)
745  {
746  datafields[pair.first] = new Variant(pair.second);
747  }
748 
749 
750  datafields["virtualPose_x"] =
751  new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
752  datafields["virtualPose_y"] =
753  new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
754  datafields["virtualPose_z"] =
755  new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
756 
757  datafields["currentPoseLeft_x"] =
758  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
759  datafields["currentPoseLeft_y"] =
760  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
761  datafields["currentPoseLeft_z"] =
762  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
763 
764  datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
765  datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
766  datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
767  datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
768 
769 
770  datafields["currentPoseRight_x"] =
771  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
772  datafields["currentPoseRight_y"] =
773  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
774  datafields["currentPoseRight_z"] =
775  new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
776  datafields["rightQuat_w"] =
777  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
778  datafields["rightQuat_x"] =
779  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
780  datafields["rightQuat_y"] =
781  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
782  datafields["rightQuat_z"] =
783  new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
784 
785  datafields["dmpBoxPose_x"] =
786  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
787  datafields["dmpBoxPose_y"] =
788  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
789  datafields["dmpBoxPose_z"] =
790  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
791 
792  datafields["dmpBoxPose_qx"] =
793  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qx);
794  datafields["dmpBoxPose_qy"] =
795  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qy);
796  datafields["dmpBoxPose_qz"] =
797  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qz);
798  datafields["dmpBoxPose_qw"] =
799  new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qw);
800 
801  debugObs->setDebugChannel("BimanualForceController", datafields);
802  }
803 
804  void
805  NJointBimanualObjLevelVelController::onInitNJointController()
806  {
807 
808 
809  ARMARX_INFO << "init ...";
810  runTask("NJointBimanualObjLevelVelController",
811  [&]
812  {
813  CycleUtil c(1);
814  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
815  while (getState() == eManagedIceObjectStarted)
816  {
817  if (isControllerActive())
818  {
819  controllerRun();
820  }
821  c.waitForCycleDuration();
822  }
823  });
824  }
825 
826  void
827  NJointBimanualObjLevelVelController::onDisconnectNJointController()
828  {
829  ARMARX_INFO << "stopped ...";
830  }
831 } // namespace armarx::ctrl::njoint_ctrl::bimanual_force
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::setMPRotWeights
void setMPRotWeights(const DoubleSeqSeq &weights, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:244
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelVelControlData >::reinitTripleBuffer
void reinitTripleBuffer(const NJointBimanualObjLevelVelControlData &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::ControlTargetBase::asA
const T * asA() const
Definition: ControlTargetBase.h:76
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kori
float Kori
Definition: TaskSpaceVMP.h:47
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::learnDMPFromFiles
void learnDMPFromFiles(const Ice::StringSeq &fileNames, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:584
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dpos
float Dpos
Definition: TaskSpaceVMP.h:46
armarx::TripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:90
armarx::NJointControllerBase::useSynchronizedRtRobot
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
Definition: NJointController.cpp:293
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelVelControlData >::rtGetControlStruct
const NJointBimanualObjLevelVelControlData & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPKernelSize
int DMPKernelSize
Definition: TaskSpaceVMP.h:54
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::skew
Eigen::Matrix3f skew(Eigen::Vector3f vec)
Definition: NJointBimanualObjLevelVelController.h:59
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::setViaPoints
void setViaPoints(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:674
armarx::SensorValueBase::asA
const T * asA() const
Definition: SensorValueBase.h:82
armarx::SensorValueBase
The SensorValueBase class.
Definition: SensorValueBase.h:40
armarx::ControlTargetBase
Brief description of class JointControlTargetBase.
Definition: ControlTargetBase.h:47
armarx::TripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:146
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::NJointBimanualObjLevelVelControllerInterface::getMPRotWeights
DoubleSeqSeq getMPRotWeights()
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::setGoals
void setGoals(const Ice::DoubleSeq &goals, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:592
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::ManagedIceObject::getState
int getState() const
Retrieve current state of the ManagedIceObject.
Definition: ManagedIceObject.cpp:725
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig
Definition: TaskSpaceVMP.h:52
armarx::CartesianVelocityController
Definition: CartesianVelocityController.h:36
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::phaseStopParas
PhaseStopParams phaseStopParas
Definition: TaskSpaceVMP.h:60
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelVelControlData >::getWriterControlStruct
NJointBimanualObjLevelVelControlData & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::goDist
float goDist
Definition: TaskSpaceVMP.h:41
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelVelControlData >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::motionTimeDuration
float motionTimeDuration
Definition: TaskSpaceVMP.h:59
NJointBimanualObjLevelVelController.h
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:845
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::backDist
float backDist
Definition: TaskSpaceVMP.h:42
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::maxValue
float maxValue
Definition: TaskSpaceVMP.h:43
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:919
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::runDMPWithVirtualStart
void runDMPWithVirtualStart(const Ice::DoubleSeq &starts, const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:647
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Dori
float Dori
Definition: TaskSpaceVMP.h:48
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::controllerRun
void controllerRun()
Definition: NJointBimanualObjLevelVelController.cpp:292
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::onDisconnectNJointController
void onDisconnectNJointController()
Definition: NJointBimanualObjLevelVelController.cpp:827
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:777
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::runDMP
void runDMP(const Ice::DoubleSeq &goals, Ice::Double timeDuration, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:605
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:753
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::firstLoop
bool firstLoop
Definition: NJointBimanualObjLevelVelController.h:247
armarx::NJointBimanualObjLevelVelControllerInterface::removeAllViaPoints
void removeAllViaPoints()
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelVelControlData >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:102
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
TODO make protected and use attorneys.
Definition: NJointBimanualObjLevelVelController.cpp:350
ArmarXObjectScheduler.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::calcIK
Eigen::VectorXf calcIK(VirtualRobot::DifferentialIKPtr ik, const Eigen::MatrixXf &jacobi, const Eigen::VectorXf &cartesianVel, const Eigen::VectorXf &nullspaceVel)
Definition: NJointBimanualObjLevelVelController.cpp:323
armarx::NJointControllerWithTripleBuffer< NJointBimanualObjLevelVelControlData >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::NJointBimanualObjLevelVelControllerInterface::getCurrentObjVel
Ice::FloatSeq getCurrentObjVel()
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::setKpImpedance
void setKpImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:690
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPAmplitude
float DMPAmplitude
Definition: TaskSpaceVMP.h:57
armarx::NJointBimanualObjLevelVelControllerInterface::getMPWeights
DoubleSeqSeq getMPWeights()
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::slop
float slop
Definition: TaskSpaceVMP.h:44
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::Kpos
float Kpos
Definition: TaskSpaceVMP.h:45
CycleUtil.h
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::rtPreActivateController
void rtPreActivateController()
This function is called before the controller is activated.
Definition: NJointBimanualObjLevelVelController.cpp:269
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::setKdImpedance
void setKdImpedance(const Ice::FloatSeq &value, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:709
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::control::deprecated_njoint_mp_controller::bimanual::registrationControllerNJointBimanualObjLevelVelController
NJointControllerRegistration< NJointBimanualObjLevelVelController > registrationControllerNJointBimanualObjLevelVelController("NJointBimanualObjLevelVelController")
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelControlData
Definition: NJointBimanualObjLevelVelController.h:30
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::onPublish
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition: NJointBimanualObjLevelVelController.cpp:737
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::setMPWeights
void setMPWeights(const DoubleSeqSeq &weights, const Ice::Current &)
Definition: NJointBimanualObjLevelVelController.cpp:219
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelControlData::boxPose
Eigen::Matrix4f boxPose
Definition: NJointBimanualObjLevelVelController.h:34
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPStyle
std::string DMPStyle
Definition: TaskSpaceVMP.h:56
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPControllerConfig::DMPMode
std::string DMPMode
Definition: TaskSpaceVMP.h:55
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::NJointBimanualObjLevelVelController
NJointBimanualObjLevelVelController(const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: NJointBimanualObjLevelVelController.cpp:16
Eigen::Matrix< float, 6, 1 >
armarx::control::deprecated_njoint_mp_controller::tsvmp::PhaseStopParams::mm2radi
float mm2radi
Definition: TaskSpaceVMP.h:49
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::getClassName
std::string getClassName(const Ice::Current &) const
Definition: NJointBimanualObjLevelVelController.cpp:286
armarx::control::deprecated_njoint_mp_controller::bimanual::NJointBimanualObjLevelVelController::onInitNJointController
void onInitNJointController()
Definition: NJointBimanualObjLevelVelController.cpp:805
armarx::TripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:108
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_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:153
armarx::control::deprecated_njoint_mp_controller::tsvmp::TaskSpaceDMPController
Brief description of class TaskSpaceDMPController.
Definition: TaskSpaceVMP.h:83
armarx::ManagedIceObject::getObjectScheduler
ArmarXObjectSchedulerPtr getObjectScheduler() const
Definition: ManagedIceObject.cpp:731
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
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::control::deprecated_njoint_mp_controller::bimanual
Definition: NJointBimanualCartesianAdmittanceController.cpp:9
armarx::TripleBuffer::updateReadBuffer
bool updateReadBuffer() const
Swaps in the hidden buffer if it contains new data.
Definition: TripleBuffer.h:131