KeypointsMPController.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package TaskSpaceActiveImpedanceControl::ArmarXObjects::NJointTaskSpaceImpedanceController
17  * @author zhou ( you dot zhou at kit dot edu )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 #include <boost/algorithm/clamp.hpp>
23 
24 #include <SimoxUtility/math/convert/mat4f_to_pos.h>
25 #include <SimoxUtility/math/convert/mat4f_to_quat.h>
26 #include <SimoxUtility/math/compare/is_equal.h>
27 
28 #include <VirtualRobot/MathTools.h>
29 
30 #include <ArmarXCore/core/time/CycleUtil.h>
31 #include <ArmarXCore/core/ArmarXObjectScheduler.h>
32 #include <boost/archive/text_iarchive.hpp>
33 #include <boost/archive/text_oarchive.hpp>
34 
35 #include "KeypointsMPController.h"
36 
37 
38 namespace armarx::control::njoint_mp_controller::task_space
39 {
40 NJointControllerRegistration<KeypointMPController> registrationControllerKeypointMPController("KeypointMPController");
41 
42 KeypointMPController::KeypointMPController(
43  const RobotUnitPtr& robotUnit,
44  const NJointControllerConfigPtr& config,
45  const VirtualRobot::RobotPtr&)
46 {
47  ARMARX_INFO << "creating task-space admittance controller";
48  KeypointMPControllerConfigPtr cfg = KeypointMPControllerConfigPtr::dynamicCast(config);
49 
50  ARMARX_CHECK_EXPRESSION(cfg);
51  ARMARX_CHECK_EXPRESSION(robotUnit);
52  ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
53  useSynchronizedRtRobot();
54  kinematicChainName = cfg->nodeSetName;
55 
56  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
57  ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
58 
59  jointNames.clear();
60  for (size_t i = 0; i < rns->getSize(); ++i)
61  {
62  std::string jointName = rns->getNode(i)->getName();
63  jointNames.push_back(jointName);
64  ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
65  ARMARX_CHECK_EXPRESSION(ct);
66  const SensorValueBase* sv = useSensorValue(jointName);
67  ARMARX_CHECK_EXPRESSION(sv);
68  auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
69  ARMARX_CHECK_EXPRESSION(casted_ct);
70  targets.push_back(casted_ct);
71 
72  const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
73  const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
74  const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
75  if (!torqueSensor)
76  {
77  ARMARX_WARNING << "No Torque sensor available for " << jointName;
78  }
79  if (!velocitySensor)
80  {
81  ARMARX_WARNING << "No velocity sensor available for " << jointName;
82  }
83  if (!positionSensor)
84  {
85  ARMARX_WARNING << "No position sensor available for " << jointName;
86  }
87 
88  torqueSensors.push_back(torqueSensor);
89  velocitySensors.push_back(velocitySensor);
90  positionSensors.push_back(positionSensor);
91  };
92  controller.initialize(rns);
93 
94  ftsensor.initialize(rns, robotUnit, cfg->controlParamJsonFile);
95 
96  /// mp related
97  // pointVMP.reset(new mplib::representation::vmp::PrincipalComponentVMP());
98 
99  mplib::factories::VMPFactory mpfactory;
100  mpfactory.addConfig("kernelSize", 100);
101  mplib::representation::VMPType vmp_type = mplib::representation::VMPType::PrincipalComponent;
102  std::shared_ptr<mplib::representation::AbstractMovementPrimitive> vmp = mpfactory.createMP(vmp_type);
103  pointVMP = std::dynamic_pointer_cast<mplib::representation::vmp::PrincipalComponentVMP>(vmp);
104 
105  /// buffer related
106  common::FTSensor::FTBufferData ftData;
107  ftData.forceBaseline.setZero();
108  ftData.torqueBaseline.setZero();
109  ftData.enableTCPGravityCompensation = cfg->enableTCPGravityCompensation;
110  ftData.tcpMass = cfg->tcpMass;
111  ftData.tcpCoMInFTSensorFrame = cfg->tcpCoMInForceSensorFrame;
112  ftSensorBuffer.reinitAllBuffers(ftData);
113 
114  law::TaskspaceKeypointsAdmittanceController::Config configData;
115  configData.kpImpedance = cfg->kpImpedance;
116  configData.kdImpedance = cfg->kdImpedance;
117  configData.kpAdmittance = cfg->kpAdmittance;
118  configData.kdAdmittance = cfg->kdAdmittance;
119  configData.kmAdmittance = cfg->kmAdmittance;
120 
121  configData.kpNullspace = cfg->kpNullspace;
122  configData.kdNullspace = cfg->kdNullspace;
123 
124  configData.currentForceTorque.setZero();
125  configData.desiredTCPPose = Eigen::Matrix4f::Identity();
126  configData.desiredTCPTwist.setZero();
127  configData.desiredNullspaceJointAngles = cfg->desiredNullspaceJointAngles;
128 
129  configData.torqueLimit = cfg->torqueLimit;
130  configData.qvelFilter = cfg->qvelFilter;
131 
132  ARMARX_IMPORTANT << VAROUT(cfg->numKeypoints);
133  ARMARX_CHECK_EQUAL(cfg->initialKeypointPosition.size(), cfg->numKeypoints * 3);
134  ARMARX_CHECK_EQUAL(cfg->pointCtrlMask.size(), cfg->numKeypoints * 3);
135  configData.numPoints = cfg->numKeypoints;
136  // configData.keypointStiffness = cfg->keypointStiffness;
137  configData.keypointKp = cfg->keypointKp;
138  configData.keypointKi = cfg->keypointKi;
139  configData.keypointKd = cfg->keypointKd;
140  configData.maxControlValue = cfg->maxControlValue;
141  configData.maxDerivation = cfg->maxDerivation;
142 
143  configData.pointCtrlMask = cfg->pointCtrlMask;
144  configData.currentKeypointPosition = cfg->initialKeypointPosition;
145  configData.filteredKeypointPosition = cfg->initialKeypointPosition;
146  configData.desiredKeypointPosition = cfg->initialKeypointPosition;
147  configData.isRigid = cfg->isRigid;
148  configData.fixedTranslation.setZero(cfg->numKeypoints * 3);
149  reinitTripleBuffer(configData);
150 
151  numPoints = cfg->numKeypoints;
152  initialStateEigen = cfg->initialKeypointPosition;
153  controller.filterCoeff = cfg->filterCoeff;
154  controller.s.filteredKeypointPosition = cfg->initialKeypointPosition;
155  controller.s.isRigid = cfg->isRigid;
156  controller.s.fixedTranslation.setZero(cfg->numKeypoints * 3);
157  controller.s.currentKeypointPosition = cfg->initialKeypointPosition;
158 
159  controller.initPID(cfg->keypointKp, cfg->keypointKi, cfg->keypointKd, cfg->maxControlValue, cfg->maxDerivation);
160 
161  // controller.s.numPoints = cfg->numKeypoints;
162  // controller.s.desiredKeypointPosition = cfg->initialKeypointPosition;
163 
164 
165  rt2mpInfo rt2mp;
166  for (int i = 0; i < cfg->numKeypoints * 3; i++)
167  {
168  mplib::core::DVec state;
169  state.push_back(cfg->initialKeypointPosition[i]);
170  state.push_back(0.0);
171  rt2mp.currentState.push_back(state);
172  }
173  rt2mp.deltaT = 0.0;
174  rt2mpBuffer.reinitAllBuffers(rt2mp);
175 }
176 
177 std::string KeypointMPController::getClassName(const Ice::Current &) const
178 {
179  return "KeypointMPController";
180 }
181 
182 std::string KeypointMPController::getKinematicChainName(const Ice::Current &)
183 {
184  return kinematicChainName;
185 }
186 
187 void KeypointMPController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& timeSinceLastIteration)
188 {
189  rtUpdateControlStruct();
190 
191  bool valid = controller.updateControlStatus(
192  rtGetControlStruct(),
193  timeSinceLastIteration,
194  torqueSensors,
195  velocitySensors,
196  positionSensors);
197 
198  /// for mp controllers you can commit the status buffer now
199  {
200  for (size_t i = 0; i < (unsigned)controller.s.numPoints * 3; i++)
201  {
202  mplib::core::DVec state;
203  state.push_back(controller.s.currentKeypointPosition[i]);
204  state.push_back(0.0);
205  rt2mpBuffer.getWriteBuffer().currentState[i] = state;
206  }
207  rt2mpBuffer.getWriteBuffer().deltaT = controller.s.deltaT;
208  rt2mpBuffer.commitWrite();
209 
210  controlStatusBuffer.getWriteBuffer().currentPose = controller.s.currentPose;
211  controlStatusBuffer.getWriteBuffer().currentTwist = controller.s.currentTwist * 1000.0f;
212  controlStatusBuffer.getWriteBuffer().deltaT = controller.s.deltaT;
213  controlStatusBuffer.getWriteBuffer().currentKeypointPosition = controller.s.currentKeypointPosition;
214  controlStatusBuffer.getWriteBuffer().desiredKeypointPosition = controller.s.desiredKeypointPosition;
215  controlStatusBuffer.commitWrite();
216  }
217 
218  if (rtFirstRun.load())
219  {
220  rtFirstRun.store(false);
221  rtReady.store(false);
222 
223  ftsensor.reset();
224  controller.firstRun();
225  // ARMARX_IMPORTANT << "admittance control first run with\n" << VAROUT(controller.s.desiredTCPPose);
226  }
227  else
228  {
229  ftsensor.compensateTCPGravity(ftSensorBuffer.getReadBuffer());
230  if (!rtReady.load())
231  {
232  if(ftsensor.calibrate(controller.s.deltaT))
233  {
234  rtReady.store(true);
235  }
236  }
237  else
238  {
239  if (valid)
240  {
241  controller.s.currentForceTorque = ftsensor.getFilteredForceTorque(ftSensorBuffer.getReadBuffer());
242  }
243  }
244  }
245 
246  const auto& desiredJointTorques = controller.run(rtReady.load());
247  ARMARX_CHECK_EQUAL(int(targets.size()), int(desiredJointTorques.size()));
248 
249  /// write torque target to joint device
250  for (size_t i = 0; i < targets.size(); ++i)
251  {
252  targets.at(i)->torque = desiredJointTorques(i);
253  if (!targets.at(i)->isValid())
254  {
255  targets.at(i)->torque = 0;
256  }
257  }
258 
259  /// for debug output
260  {
261  controlStatusBuffer.getWriteBuffer().kpImpedance = controller.s.kpImpedance;
262  controlStatusBuffer.getWriteBuffer().kdImpedance = controller.s.kdImpedance;
263  controlStatusBuffer.getWriteBuffer().forceImpedance = controller.s.forceImpedance;
264  controlStatusBuffer.getWriteBuffer().currentForceTorque = controller.s.currentForceTorque;
265 
266  controlStatusBuffer.getWriteBuffer().virtualPose = controller.s.virtualPose;
267  controlStatusBuffer.getWriteBuffer().desiredTCPPose = controller.s.desiredTCPPose;
268  controlStatusBuffer.getWriteBuffer().virtualVel = controller.s.virtualVel;
269  controlStatusBuffer.getWriteBuffer().virtualAcc = controller.s.virtualAcc;
270  controlStatusBuffer.getWriteBuffer().currentKeypointPosition = controller.s.currentKeypointPosition;
271  controlStatusBuffer.getWriteBuffer().desiredKeypointPosition = controller.s.desiredKeypointPosition;
272  controlStatusBuffer.getWriteBuffer().filteredKeypointPosition = controller.s.filteredKeypointPosition;
273  controlStatusBuffer.getWriteBuffer().pointTrackingForce = controller.s.pointTrackingForce;
274  controlStatusBuffer.commitWrite();
275  }
276 }
277 
278 void KeypointMPController::toggleMP(const bool enableMP, const Ice::Current &)
279 {
280  mpEnabled.store(enableMP);
281  ARMARX_IMPORTANT << "Toggle MP: " << enableMP;
282 }
283 
284 bool KeypointMPController::getMPEnabled(const Ice::Current &)
285 {
286  return mpEnabled.load();
287 }
288 
289 void KeypointMPController::runMP()
290 {
291  if (!rtReady.load() && !mpEnabled.load())
292  {
293  // do something
294  usleep(1000);
295  }
296  else
297  {
298  mplib::core::DVec2d currentState = rt2mpBuffer.getUpToDateReadBuffer().currentState;
299  double deltaT = rt2mpBuffer.getUpToDateReadBuffer().deltaT;
300 
301  if (canVal > 1e-8 && mpRunning.load())
302  {
303  double phaseStop = 0;
304  double tau = 1.0;
305 
306  canVal -= tau * deltaT * 1 / ((1 + phaseStop) * timeDuration);
307  currentState = pointVMP->calculateDesiredState(canVal, currentState);
308 
309  Eigen::VectorXf desiredPosition;
310  desiredPosition.setZero(currentState.size());
311  for (size_t i = 0; i < currentState.size(); i++)
312  {
313  desiredPosition[i] = currentState[i][0];
314  }
315  getWriterControlStruct().desiredKeypointPosition = desiredPosition;
316  writeControlStruct();
317  }
318  else
319  {
320  // normally set velocity to zero. for keypoint velocity?
321  usleep(1000);
322  }
323  }
324 }
325 
326 void KeypointMPController::reconfigureController(const std::string &, const Ice::Current &)
327 {
328 
329 }
330 
331 void KeypointMPController::publishPose(StringVariantBaseMap& datafields, const std::string& name, const Eigen::Matrix4f& pose)
332 {
333  Eigen::Vector6f vec;
334  vec.head(3) = pose.block<3, 1>(0, 3);
335  vec.tail(3) = VirtualRobot::MathTools::eigen4f2rpy(pose);
336  publishVec6f(datafields, name, vec);
337 }
338 
339 void KeypointMPController::publishVec6f(StringVariantBaseMap& datafields, const std::string& name, const Eigen::Vector6f& vec)
340 {
341  datafields[name + "_x"] = new Variant(vec(0));
342  datafields[name + "_y"] = new Variant(vec(1));
343  datafields[name + "_z"] = new Variant(vec(2));
344  datafields[name + "_rx"] = new Variant(vec(3));
345  datafields[name + "_ry"] = new Variant(vec(4));
346  datafields[name + "_rz"] = new Variant(vec(5));
347 }
348 
349 void KeypointMPController::publishVecXf(StringVariantBaseMap& datafields, const std::string& name, const Eigen::Vector6f& vec)
350 {
351  for (int i = 0; i < vec.size(); i++)
352  {
353  datafields[name + "_" + std::to_string(i)] = new Variant(vec(i));
354  }
355 }
356 
357 void KeypointMPController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
358 {
359  StringVariantBaseMap datafields;
360  auto values = debugRTBuffer.getUpToDateReadBuffer().desired_torques;
361  for (auto& pair : values)
362  {
363  datafields[pair.first] = new Variant(pair.second);
364  }
365 
366  // publishPose(datafields, "virtual_pose", controlStatusBuffer.getUpToDateReadBuffer().virtualPose);
367  // publishPose(datafields, "desired_pose", controlStatusBuffer.getUpToDateReadBuffer().desiredTCPPose);
368  // publishVec6f(datafields, "virtual_vel", controlStatusBuffer.getUpToDateReadBuffer().virtualVel);
369  // publishVec6f(datafields, "virtual_acc", controlStatusBuffer.getUpToDateReadBuffer().virtualAcc);
370  // publishVec6f(datafields, "kp_Impedance", controlStatusBuffer.getUpToDateReadBuffer().kpImpedance);
371  // publishVec6f(datafields, "kd_Impedance", controlStatusBuffer.getUpToDateReadBuffer().kdImpedance);
372  // publishVec6f(datafields, "force_Impedance", controlStatusBuffer.getUpToDateReadBuffer().forceImpedance);
373  // publishVec6f(datafields, "current_ForceTorque", controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque);
374  // publishVecXf(datafields, "current_KeypointPosition", controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition);
375  // publishVecXf(datafields, "desired_KeypointPosition", controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition);
376 
377  // Eigen::Matrix4f virtualPose = controlStatusBuffer.getUpToDateReadBuffer().virtualPose;
378  // Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(virtualPose);
379  // datafields["virtualPose_x"] = new Variant(virtualPose(0, 3));
380  // datafields["virtualPose_y"] = new Variant(virtualPose(1, 3));
381  // datafields["virtualPose_z"] = new Variant(virtualPose(2, 3));
382  // datafields["virtualPose_rx"] = new Variant(rpy(0));
383  // datafields["virtualPose_ry"] = new Variant(rpy(1));
384  // datafields["virtualPose_rz"] = new Variant(rpy(2));
385 
386  // Eigen::Matrix4f desiredPose = controlStatusBuffer.getUpToDateReadBuffer().desiredTCPPose;
387  // Eigen::Vector3f rpyDesired = VirtualRobot::MathTools::eigen4f2rpy(desiredPose);
388  // datafields["desiredPose_x"] = new Variant(desiredPose(0, 3));
389  // datafields["desiredPose_y"] = new Variant(desiredPose(1, 3));
390  // datafields["desiredPose_z"] = new Variant(desiredPose(2, 3));
391  // datafields["desiredPose_rx"] = new Variant(rpyDesired(0));
392  // datafields["desiredPose_ry"] = new Variant(rpyDesired(1));
393  // datafields["desiredPose_rz"] = new Variant(rpyDesired(2));
394 
395  // datafields["virtualVel_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(0));
396  // datafields["virtualVel_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(1));
397  // datafields["virtualVel_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(2));
398  // datafields["virtualVel_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(3));
399  // datafields["virtualVel_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(4));
400  // datafields["virtualVel_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(5));
401 
402  // datafields["virtualAcc_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(0));
403  // datafields["virtualAcc_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(1));
404  // datafields["virtualAcc_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(2));
405  // datafields["virtualAcc_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(3));
406  // datafields["virtualAcc_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(4));
407  // datafields["virtualAcc_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(5));
408 
409  // datafields["kpImpedance_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(0));
410  // datafields["kpImpedance_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(1));
411  // datafields["kpImpedance_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(2));
412  // datafields["kpImpedance_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(3));
413  // datafields["kpImpedance_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(4));
414  // datafields["kpImpedance_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(5));
415 
416  // datafields["kdImpedance_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(0));
417  // datafields["kdImpedance_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(1));
418  // datafields["kdImpedance_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(2));
419  // datafields["kdImpedance_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(3));
420  // datafields["kdImpedance_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(4));
421  // datafields["kdImpedance_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(5));
422 
423  // datafields["forceImpedance_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(0));
424  // datafields["forceImpedance_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(1));
425  // datafields["forceImpedance_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(2));
426  // datafields["forceImpedance_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(3));
427  // datafields["forceImpedance_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(4));
428  // datafields["forceImpedance_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(5));
429 
430  // datafields["currentForceTorque_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(0));
431  // datafields["currentForceTorque_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(1));
432  // datafields["currentForceTorque_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(2));
433  // datafields["currentForceTorque_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(3));
434  // datafields["currentForceTorque_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(4));
435  // datafields["currentForceTorque_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(5));
436 
437 
438  datafields["currentKeypointPosition_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(0));
439  datafields["currentKeypointPosition_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(1));
440  datafields["currentKeypointPosition_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(2));
441  // datafields["currentKeypointPosition_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(3));
442  // datafields["currentKeypointPosition_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(4));
443  // datafields["currentKeypointPosition_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(5));
444 
445  datafields["desiredKeypointPosition_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(0));
446  datafields["desiredKeypointPosition_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(1));
447  datafields["desiredKeypointPosition_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(2));
448  // datafields["desiredKeypointPosition_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(3));
449  // datafields["desiredKeypointPosition_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(4));
450  // datafields["desiredKeypointPosition_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(5));
451 
452  datafields["filteredKeypointPosition_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(0));
453  datafields["filteredKeypointPosition_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(1));
454  datafields["filteredKeypointPosition_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(2));
455 
456  datafields["track_force_x"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(0));
457  datafields["track_force_y"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(1));
458  datafields["track_force_z"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(2));
459  datafields["track_force_rx"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(3));
460  datafields["track_force_ry"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(4));
461  datafields["track_force_rz"] = new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(5));
462 
463  debugObs->setDebugChannel("KeypointMPController", datafields);
464 }
465 
466 void KeypointMPController::setTCPPose(const Eigen::Matrix4f& pose, const Ice::Current&)
467 {
468  // LockGuardType guard {controlDataMutex};
469  // getWriterControlStruct().desiredTCPPose = pose;
470  // writeControlStruct();
471 }
472 
473 //void KeypointMPController::setNullspaceJointAngles(const Eigen::VectorXf& jointAngles, const Ice::Current &)
474 //{
475 // LockGuardType guard {controlDataMutex};
476 // getWriterControlStruct().desiredNullspaceJointAngles = jointAngles;
477 // writeControlStruct();
478 //}
479 
480 void KeypointMPController::setControlParameters(const std::string& name, const Eigen::VectorXf& value, const Ice::Current &)
481 {
482  LockGuardType guard {controlDataMutex};
483  if (name == "kpImpedance")
484  {
485  ARMARX_CHECK_EQUAL(value.size(), 6);
486  getWriterControlStruct().kpImpedance = value;
487  }
488  else if (name == "kdImpedance")
489  {
490  ARMARX_CHECK_EQUAL(value.size(), 6);
491  getWriterControlStruct().kdImpedance = value;
492  }
493  else if (name == "kpAdmittance")
494  {
495  ARMARX_CHECK_EQUAL(value.size(), 6);
496  getWriterControlStruct().kpAdmittance = value;
497  }
498  else if (name == "kdAdmittance")
499  {
500  ARMARX_CHECK_EQUAL(value.size(), 6);
501  getWriterControlStruct().kdAdmittance = value;
502  }
503  else if (name == "kmAdmittance")
504  {
505  ARMARX_CHECK_EQUAL(value.size(), 6);
506  getWriterControlStruct().kmAdmittance = value;
507  }
508  else if (name == "kpNullspace")
509  {
510  ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
511  getWriterControlStruct().kpNullspace = value;
512  }
513  else if (name == "kdNullspace")
514  {
515  ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
516  getWriterControlStruct().kdNullspace = value;
517  }
518  else
519  {
520  ARMARX_ERROR << name << " is not supported by TaskSpaceAdmittanceController";
521  }
522  writeControlStruct();
523 }
524 
525 void KeypointMPController::setForceTorqueBaseline(const Eigen::Vector3f& forceBaseline, const Eigen::Vector3f& torqueBaseline, const Ice::Current &)
526 {
527  ftSensorBuffer.getWriteBuffer().forceBaseline = forceBaseline;
528  ftSensorBuffer.getWriteBuffer().torqueBaseline = torqueBaseline;
529  ftSensorBuffer.commitWrite();
530 }
531 
532 void KeypointMPController::setTCPMass(Ice::Float mass, const Ice::Current &)
533 {
534  ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = true;
535  ftSensorBuffer.getWriteBuffer().tcpMass = mass;
536  ftSensorBuffer.commitWrite();
537 }
538 
539 void KeypointMPController::setTCPCoMInFTFrame(const Eigen::Vector3f& tcpCoMInFTSensorFrame, const Ice::Current &)
540 {
541  ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = true;
542  ftSensorBuffer.getWriteBuffer().tcpCoMInFTSensorFrame = tcpCoMInFTSensorFrame;
543  ftSensorBuffer.commitWrite();
544 }
545 
546 void KeypointMPController::calibrateFTSensor(const Ice::Current &)
547 {
548  rtReady.store(false);
549 }
550 
551 std::string KeypointMPController::getNames(const Ice::Current &)
552 {
553  return "KeypointMP";
554 }
555 
556 void KeypointMPController::start(const Ice::DoubleSeq& goal, const Ice::Current &)
557 {
558  pointVMP->prepareExecution(goal, initialState, 1.0);
559  mplib::representation::MPStateVec initState = mplib::core::SystemState::convert2DArrayToStates<mplib::representation::MPState>(initialState);
560 }
561 
562 void KeypointMPController::startWithTime(const Ice::DoubleSeq& goal, Ice::Double duration, const Ice::Current &)
563 {
564  timeDuration = duration;
565  pointVMP->prepareExecution(goal, initialState, 1.0);
566  mplib::representation::MPStateVec initState = mplib::core::SystemState::convert2DArrayToStates<mplib::representation::MPState>(initialState);
567  mpRunning.store(true);
568 }
569 
570 void KeypointMPController::startAsTraj(const Ice::Current &)
571 {
572  pointVMP->prepareExecution(goal, initialState, 1.0);
573  mplib::representation::MPStateVec initState = mplib::core::SystemState::convert2DArrayToStates<mplib::representation::MPState>(initialState);
574 }
575 
576 void KeypointMPController::startAsTrajWithTime(Ice::Double duration, const Ice::Current &)
577 {
578 
579 }
580 
581 void KeypointMPController::stop(const Ice::Current &)
582 {
583  mpRunning.store(false);
584 }
585 
586 void KeypointMPController::pause(const Ice::Current &)
587 {
588  mpRunning.store(false);
589 }
590 
591 void KeypointMPController::resume(const Ice::Current &)
592 {
593  mpRunning.store(true);
594 }
595 
596 void KeypointMPController::reset(const Ice::Current &)
597 {
598  if (mpRunning.load())
599  {
600  ARMARX_INFO << "Cannot reset running DMP";
601  }
602  rtFirstRun = true;
603 }
604 
605 bool KeypointMPController::isFinished(const Ice::Current &)
606 {
607  return !mpRunning.load();
608 }
609 
610 void KeypointMPController::learnFromCSV(const Ice::StringSeq& fileList, const Ice::Current &)
611 {
612  std::vector<mplib::core::SampledTrajectory> trajs;
613  for (auto& file : fileList)
614  {
615  mplib::core::SampledTrajectory traj;
616  traj.readFromCSVFile(file);
617  trajs.push_back(traj);
618  }
619  pointVMP->learnFromTrajectories(trajs);
620 
621  ARMARX_CHECK_EQUAL(trajs[0].dim(), (unsigned)numPoints * 3);
622  initialState.clear();
623  goal.clear();
624 
625  for (size_t i = 0; i < trajs[0].dim(); i++)
626  {
627  mplib::core::DVec state;
628  // state.push_back(trajs[0].begin()->getPosition(i));
629  // state.push_back(trajs[0].begin()->getDeriv(i, 1));
630  state.push_back(initialStateEigen[i]);
631  state.push_back(0.0);
632  initialState.push_back(state);
633  goal.push_back(trajs[0].rbegin()->getPosition(i));
634  }
635 }
636 
637 void KeypointMPController::setGoal(const Ice::DoubleSeq &, const Ice::Current &)
638 {
639 
640 }
641 
642 void KeypointMPController::setStartAndGoal(const Ice::DoubleSeq &, const Ice::DoubleSeq &, const Ice::Current &)
643 {
644 
645 }
646 
647 void KeypointMPController::setViaPoint(Ice::Double, const Ice::DoubleSeq &, const Ice::Current &)
648 {
649 
650 }
651 
652 void KeypointMPController::removeAllViaPoint(const Ice::Current &)
653 {
654 
655 }
656 
657 std::string KeypointMPController::serialize(const Ice::Current &)
658 {
659  // std::stringstream ss;
660  // boost::archive::text_oarchive oa{ss};
661  // oa << pointVMP.get();
662  // return ss.str();
663  return "notImplemented";
664 }
665 
666 Ice::DoubleSeq KeypointMPController::deserialize(const std::string &mpAsString, const Ice::Current &)
667 {
668  // std::stringstream ss;
669  // ss.str(mpAsString);
670  // boost::archive::text_iarchive ia{ss};
671 
672  // VMPPtr mpPointer;
673  // ia >> mpPointer;
674  // pointVMP.reset(mpPointer);
675  // return pointVMP->defaultGoal;
676  std::vector<double> dummy;
677  dummy.push_back(0.0);
678  return dummy;
679 }
680 
681 Ice::Double KeypointMPController::getCanVal(const Ice::Current &)
682 {
683  return canVal;
684 }
685 
686 void KeypointMPController::toggleGravityCompensation(const bool toggle, const Ice::Current &)
687 {
688  ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = toggle;
689  ftSensorBuffer.commitWrite();
690  rtReady.store(false); /// you also need to re-calibrate ft sensor
691 }
692 
693 void KeypointMPController::rtPreActivateController()
694 {
695  VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(kinematicChainName);
696  Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
697  ARMARX_IMPORTANT << "rt preactivate controller with target pose\n\n" << currentPose;
698  controller.s.currentPose = currentPose;
699  controller.s.desiredTCPPose = currentPose;
700  controller.s.previousTargetPose = currentPose;
701  controller.s.virtualPose = currentPose;
702  controller.desiredPose = currentPose;
703  controller.s.virtualVel.setZero();
704  controller.s.virtualAcc.setZero();
705  Eigen::VectorXf fixedTranslation;
706  fixedTranslation.setZero(controller.s.currentKeypointPosition.size());
707  ARMARX_IMPORTANT << VAROUT(controller.s.isRigid) << VAROUT(controller.s.currentKeypointPosition);
708  if (controller.s.isRigid)
709  {
710  for (int i = 0; i < numPoints; i++)
711  {
712  fixedTranslation.segment(3*i, 3) = controller.s.currentKeypointPosition.segment(3*i, 3) - currentPose.block<3, 1>(0, 3);
713  }
714  ARMARX_IMPORTANT << VAROUT(fixedTranslation);
715  }
716  controller.s.fixedTranslation = fixedTranslation;
717  controller.pid->reset();
718  getWriterControlStruct().fixedTranslation = controller.s.fixedTranslation;
719  getWriterControlStruct().desiredTCPPose = currentPose;
720  writeControlStruct();
721 }
722 
723 void KeypointMPController::onInitNJointController()
724 {
725  runTask("KeypointMPController", [&]
726  {
727  CycleUtil c(1);
728  getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
729  ARMARX_IMPORTANT << "Try to run mp";
730  while (getState() == eManagedIceObjectStarted)
731  {
732  if (isControllerActive())
733  {
734  runMP();
735  }
736  c.waitForCycleDuration();
737  }
738  });
739 }
740 
741 void KeypointMPController::setKeypoints(const Eigen::VectorXf& keypointPosition, const Ice::Current &)
742 {
743  LockGuardType guard {controlDataMutex};
744  ARMARX_CHECK_EQUAL(getWriterControlStruct().currentKeypointPosition.size(), keypointPosition.size());
745  getWriterControlStruct().currentKeypointPosition = keypointPosition;
746  writeControlStruct();
747 }
748 
749 void KeypointMPController::resetKeypoints(const int n_points, const float keypoint_stiffness, const bool is_rigid, const Eigen::VectorXf &ctrl_mask, const Eigen::VectorXf& init_keypoints_position, const Ice::Current &)
750 {
751  numPoints = n_points;
752  getWriterControlStruct().numPoints = n_points;
753  getWriterControlStruct().keypointKp = keypoint_stiffness;
754  getWriterControlStruct().isRigid = is_rigid;
755  getWriterControlStruct().pointCtrlMask = ctrl_mask;
756  getWriterControlStruct().currentKeypointPosition = init_keypoints_position;
757  writeControlStruct();
758 }
759 }
armarx::NJointControllerWithTripleBuffer< law::TaskspaceKeypointsAdmittanceController::Config >::reinitTripleBuffer
void reinitTripleBuffer(const law::TaskspaceKeypointsAdmittanceController::Config &initial)
Definition: NJointControllerWithTripleBuffer.h:68
armarx::control::njoint_mp_controller::task_space::KeypointMPController::deserialize
Ice::DoubleSeq deserialize(const std::string &mpAsString, const Ice::Current &) override
Definition: KeypointsMPController.cpp:666
armarx::control::njoint_mp_controller::task_space::KeypointMPController::KeypointMPController
KeypointMPController(const RobotUnitPtr &robotUnit, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &)
Definition: KeypointsMPController.cpp:42
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::njoint_mp_controller::task_space::KeypointMPController::publishPose
void publishPose(StringVariantBaseMap &datafields, const std::string &name, const Eigen::Matrix4f &pose)
Definition: KeypointsMPController.cpp:331
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
armarx::control::njoint_mp_controller::task_space::KeypointMPController::getKinematicChainName
std::string getKinematicChainName(const Ice::Current &) override
Definition: KeypointsMPController.cpp:182
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< law::TaskspaceKeypointsAdmittanceController::Config >::rtGetControlStruct
const law::TaskspaceKeypointsAdmittanceController::Config & rtGetControlStruct() const
Definition: NJointControllerWithTripleBuffer.h:32
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::control::njoint_mp_controller::task_space::KeypointMPController::publishVec6f
void publishVec6f(StringVariantBaseMap &datafields, const std::string &name, const Eigen::Vector6f &vec)
Definition: KeypointsMPController.cpp:339
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setTCPPose
void setTCPPose(const Eigen::Matrix4f &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:466
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::control::njoint_mp_controller::task_space::KeypointMPController::reconfigureController
void reconfigureController(const std::string &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:326
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setStartAndGoal
void setStartAndGoal(const Ice::DoubleSeq &, const Ice::DoubleSeq &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:642
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::njoint_mp_controller::task_space::KeypointMPController::start
void start(const Ice::DoubleSeq &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:556
armarx::control::njoint_mp_controller::task_space::KeypointMPController::getCanVal
Ice::Double getCanVal(const Ice::Current &) override
Definition: KeypointsMPController.cpp:681
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setKeypoints
void setKeypoints(const Eigen::VectorXf &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:741
armarx::NJointControllerWithTripleBuffer< law::TaskspaceKeypointsAdmittanceController::Config >::getWriterControlStruct
law::TaskspaceKeypointsAdmittanceController::Config & getWriterControlStruct()
Definition: NJointControllerWithTripleBuffer.h:54
armarx::control::njoint_mp_controller::task_space::KeypointMPController::serialize
std::string serialize(const Ice::Current &) override
Definition: KeypointsMPController.cpp:657
armarx::NJointControllerWithTripleBuffer< law::TaskspaceKeypointsAdmittanceController::Config >::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: NJointControllerWithTripleBuffer.h:14
armarx::NJointControllerBase::rtGetRobot
const VirtualRobot::RobotPtr & rtGetRobot()
TODO make protected and use attorneys.
Definition: NJointControllerBase.h:845
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setForceTorqueBaseline
void setForceTorqueBaseline(const Eigen::Vector3f &, const Eigen::Vector3f &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:525
armarx::control::njoint_mp_controller::task_space::KeypointMPController::calibrateFTSensor
void calibrateFTSensor(const Ice::Current &) override
Definition: KeypointsMPController.cpp:546
armarx::detail::ControlThreadOutputBufferEntry
Definition: ControlThreadOutputBuffer.h:177
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:919
armarx::control::njoint_mp_controller::task_space::KeypointMPController::pause
void pause(const Ice::Current &) override
Definition: KeypointsMPController.cpp:586
armarx::control::njoint_mp_controller::task_space::KeypointMPController::resetKeypoints
void resetKeypoints(const int n_points, const float keypoint_stiffness, const bool is_rigid, const Eigen::VectorXf &ctrl_mask, const Eigen::VectorXf &init_keypoints_position, const Ice::Current &) override
Definition: KeypointsMPController.cpp:749
armarx::control::njoint_mp_controller::task_space::KeypointMPController::startAsTrajWithTime
void startAsTrajWithTime(Ice::Double, const Ice::Current &) override
Definition: KeypointsMPController.cpp:576
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
controller
Definition: AddOperation.h:39
armarx::control::njoint_mp_controller::task_space::KeypointMPController::onPublish
void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) override
Definition: KeypointsMPController.cpp:357
armarx::control::njoint_mp_controller::task_space::KeypointMPController::getNames
std::string getNames(const Ice::Current &) override
Definition: KeypointsMPController.cpp:551
armarx::control::njoint_mp_controller::task_space::registrationControllerKeypointMPController
NJointControllerRegistration< KeypointMPController > registrationControllerKeypointMPController("KeypointMPController")
armarx::NJointControllerBase::isControllerActive
bool isControllerActive(const Ice::Current &=Ice::emptyCurrent) const final override
Definition: NJointControllerBase.h:777
armarx::control::njoint_mp_controller::task_space::KeypointMPController::startWithTime
void startWithTime(const Ice::DoubleSeq &, Ice::Double, const Ice::Current &) override
Definition: KeypointsMPController.cpp:562
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
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::NJointControllerWithTripleBuffer< law::TaskspaceKeypointsAdmittanceController::Config >::rtUpdateControlStruct
bool rtUpdateControlStruct()
Definition: NJointControllerWithTripleBuffer.h:38
armarx::control::njoint_mp_controller::task_space::KeypointMPController::learnFromCSV
void learnFromCSV(const Ice::StringSeq &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:610
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setTCPCoMInFTFrame
void setTCPCoMInFTFrame(const Eigen::Vector3f &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:539
armarx::control::njoint_mp_controller::task_space::KeypointMPController::getMPEnabled
bool getMPEnabled(const Ice::Current &) override
Definition: KeypointsMPController.cpp:284
armarx::NJointControllerWithTripleBuffer< law::TaskspaceKeypointsAdmittanceController::Config >::writeControlStruct
void writeControlStruct()
Definition: NJointControllerWithTripleBuffer.h:44
armarx::TripleBuffer::getReadBuffer
const T & getReadBuffer() const
Definition: TripleBuffer.h:102
armarx::control::njoint_mp_controller::task_space::KeypointMPController::toggleGravityCompensation
void toggleGravityCompensation(const bool toggle, const Ice::Current &) override
ft sensor
Definition: KeypointsMPController.cpp:686
ArmarXObjectScheduler.h
armarx::NJointControllerWithTripleBuffer< law::TaskspaceKeypointsAdmittanceController::Config >::controlDataMutex
MutexType controlDataMutex
Definition: NJointControllerWithTripleBuffer.h:73
armarx::control::njoint_mp_controller::task_space::KeypointMPController::resume
void resume(const Ice::Current &) override
Definition: KeypointsMPController.cpp:591
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setGoal
void setGoal(const Ice::DoubleSeq &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:637
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::control::njoint_mp_controller::task_space
This file is part of ArmarX.
Definition: AdmittanceController.cpp:26
CycleUtil.h
armarx::control::njoint_mp_controller::task_space::KeypointMPController::isFinished
bool isFinished(const Ice::Current &) override
Definition: KeypointsMPController.cpp:605
armarx::control::njoint_mp_controller::task_space::KeypointMPController::toggleMP
void toggleMP(const bool enableMP, const Ice::Current &) override
Definition: KeypointsMPController.cpp:278
armarx::control::njoint_mp_controller::task_space::KeypointMPController::rtPreActivateController
void rtPreActivateController() override
This function is called before the controller is activated.
Definition: KeypointsMPController.cpp:693
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::control::njoint_mp_controller::task_space::KeypointMPController::publishVecXf
void publishVecXf(StringVariantBaseMap &datafields, const std::string &name, const Eigen::Vector6f &vec)
Definition: KeypointsMPController.cpp:349
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_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::control::njoint_mp_controller::task_space::KeypointMPController::removeAllViaPoint
void removeAllViaPoint(const Ice::Current &) override
Definition: KeypointsMPController.cpp:652
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setViaPoint
void setViaPoint(Ice::Double, const Ice::DoubleSeq &, const Ice::Current &) override
Definition: KeypointsMPController.cpp:647
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
armarx::control::njoint_mp_controller::task_space::KeypointMPController::getClassName
std::string getClassName(const Ice::Current &) const override
Definition: KeypointsMPController.cpp:177
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setControlParameters
void setControlParameters(const std::string &, const Eigen::VectorXf &, const Ice::Current &) override
set control parameter
Definition: KeypointsMPController.cpp:480
armarx::control::njoint_mp_controller::task_space::KeypointMPController::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override
TODO make protected and use attorneys.
Definition: KeypointsMPController.cpp:187
armarx::control::njoint_mp_controller::task_space::KeypointMPController::setTCPMass
void setTCPMass(Ice::Float, const Ice::Current &) override
Definition: KeypointsMPController.cpp:532
Eigen::Matrix< float, 6, 1 >
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::control::njoint_mp_controller::task_space::KeypointMPController::reset
void reset(const Ice::Current &) override
Definition: KeypointsMPController.cpp:596
KeypointsMPController.h
armarx::TripleBuffer::reinitAllBuffers
std::enable_if< std::is_copy_constructible< U >::value >::type reinitAllBuffers(const T &init)
Definition: TripleBuffer.h:153
armarx::control::njoint_mp_controller::task_space::KeypointMPController::onInitNJointController
void onInitNJointController() override
Definition: KeypointsMPController.cpp:723
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::njoint_mp_controller::task_space::KeypointMPController::stop
void stop(const Ice::Current &) override
Definition: KeypointsMPController.cpp:581
armarx::control::njoint_mp_controller::task_space::KeypointMPController::startAsTraj
void startAsTraj(const Ice::Current &) override
Definition: KeypointsMPController.cpp:570