FrameTracking.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 RobotAPI::ArmarXObjects::FrameTracking
17  * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
18  * @date 2019
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "FrameTracking.h"
24 
25 #include <time.h>
26 
27 #include <ArmarXCore/core/ArmarXManager.h>
28 #include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
29 #include <ArmarXCore/observers/variant/DatafieldRef.h>
30 
31 #include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
32 
33 #include <RobotAPI/components/units/KinematicUnitObserver.h>
34 
35 namespace armarx
36 {
37  void
38  FrameTracking::onInitComponent()
39  {
40  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
41  usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
42  usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
43  if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
44  {
45  usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
46  }
47 
48  enabled = false;
49  frameName = getProperty<std::string>("FrameOnStartup").getValue();
50 
51  maxYawVelocity = getProperty<float>("MaxYawVelocity").getValue();
52  yawAcceleration = getProperty<float>("YawAcceleration").getValue();
53 
54  maxPitchVelocity = getProperty<float>("MaxPitchVelocity").getValue();
55  pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
56  }
57 
58  void
59  FrameTracking::onConnectComponent()
60  {
61  robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
62  getProperty<std::string>("RobotStateComponentName").getValue());
63  kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(
64  getProperty<std::string>("KinematicUnitName").getValue());
65  kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(
66  getProperty<std::string>("KinematicUnitObserverName").getValue());
67 
68  localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
69  headYawJoint =
70  localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
71  if (!headYawJoint ||
72  !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
73  {
74  ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue()
75  << " is not a valid joint.";
76  getArmarXManager()->asyncShutdown();
77  }
78  headPitchJoint =
79  localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
80  if (!headPitchJoint ||
81  !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
82  {
83  ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue()
84  << " is not a valid joint.";
85  getArmarXManager()->asyncShutdown();
86  }
87  cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
88  if (!cameraNode)
89  {
90  ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue()
91  << " is not a valid node.";
92  getArmarXManager()->asyncShutdown();
93  }
94 
95  processorTask = new PeriodicTask<FrameTracking>(this, &FrameTracking::process, 30);
96  _enableTracking(getProperty<bool>("EnableTrackingOnStartup").getValue());
97 
98  if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
99  {
100  _remoteGui = getProxy<RemoteGuiInterfacePrx>(
101  getProperty<std::string>("RemoteGuiName").getValue());
102  RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
103 
104  rootLayoutBuilder.addChild(
105  RemoteGui::makeHBoxLayout()
106  .addChild(RemoteGui::makeTextLabel("Tracking: "))
107  .addChild(RemoteGui::makeTextLabel("Enabled"))
108  .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
109  .addChild(RemoteGui::makeTextLabel("Frame"))
110  .addChild(RemoteGui::makeComboBox("tracking_frame")
111  .options(localRobot->getRobotNodeNames())
112  .addOptions({""})
113  .value(frameName)));
114 
115  rootLayoutBuilder.addChild(
116  RemoteGui::makeHBoxLayout()
117  .addChild(RemoteGui::makeTextLabel("Look at frame: "))
118  .addChild(RemoteGui::makeComboBox("frame_look")
119  .options(localRobot->getRobotNodeNames())
120  .value(frameName))
121  .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at")));
122 
123  rootLayoutBuilder.addChild(
124  RemoteGui::makeHBoxLayout()
125  .addChild(RemoteGui::makeTextLabel("Look at global point: "))
126  .addChild(RemoteGui::makeFloatSpinBox("global_point_x")
127  .min(-1000000000)
128  .max(1000000000)
129  .steps(2 * 1000000000 / 10)
130  .value(0.f))
131  .addChild(RemoteGui::makeFloatSpinBox("global_point_y")
132  .min(-1000000000)
133  .max(1000000000)
134  .steps(2 * 1000000000 / 10)
135  .value(0.f))
136  .addChild(RemoteGui::makeFloatSpinBox("global_point_z")
137  .min(-1000000000)
138  .max(1000000000)
139  .steps(2 * 1000000000 / 10)
140  .value(0.f))
141  .addChild(
142  RemoteGui::makeButton("button_look_at_global_point").label("look at")));
143 
144  rootLayoutBuilder.addChild(
145  RemoteGui::makeHBoxLayout()
146  .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
147  .addChild(RemoteGui::makeFloatSpinBox("robot_point_x")
148  .min(-1000000000)
149  .max(1000000000)
150  .steps(2 * 1000000000 / 10)
151  .value(0.f))
152  .addChild(RemoteGui::makeFloatSpinBox("robot_point_y")
153  .min(-1000000000)
154  .max(1000000000)
155  .steps(2 * 1000000000 / 10)
156  .value(0.f))
157  .addChild(RemoteGui::makeFloatSpinBox("robot_point_z")
158  .min(-1000000000)
159  .max(1000000000)
160  .steps(2 * 1000000000 / 10)
161  .value(0.f))
162  .addChild(
163  RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
164 
165  rootLayoutBuilder.addChild(
166  RemoteGui::makeHBoxLayout()
167  .addChild(RemoteGui::makeTextLabel("Scan: "))
168  .addChild(RemoteGui::makeTextLabel("yaw from "))
169  .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from")
170  .min(headYawJoint->getJointLimitLo())
171  .max(headYawJoint->getJointLimitHi())
172  .steps(static_cast<int>((headYawJoint->getJointLimitHi() -
173  headYawJoint->getJointLimitLo()) /
174  0.1)))
175  .addChild(RemoteGui::makeTextLabel("yaw to "))
176  .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to")
177  .min(headYawJoint->getJointLimitLo())
178  .max(headYawJoint->getJointLimitHi())
179  .steps(static_cast<int>((headYawJoint->getJointLimitHi() -
180  headYawJoint->getJointLimitLo()) /
181  0.1)))
182  .addChild(RemoteGui::makeTextLabel("pitch "))
183  .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch")
184  .min(headPitchJoint->getJointLimitLo())
185  .max(headPitchJoint->getJointLimitHi())
186  .steps(static_cast<int>((headPitchJoint->getJointLimitHi() -
187  headPitchJoint->getJointLimitLo()) /
188  0.1)))
189  .addChild(RemoteGui::makeTextLabel("velocity "))
190  .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity")
191  .min(0)
192  .max(6)
193  .steps(static_cast<int>(6 / 0.1))
194  .value(0.8f))
195  .addChild(
196  RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
197 
198  rootLayoutBuilder.addChild(
199  RemoteGui::makeHBoxLayout()
200  .addChild(RemoteGui::makeTextLabel("Scan: "))
201  .addChild(RemoteGui::makeTextLabel("from "))
202  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x")
203  .min(-1000000000)
204  .max(1000000000)
205  .steps(2 * 1000000000 / 10)
206  .value(0.f))
207  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y")
208  .min(-1000000000)
209  .max(1000000000)
210  .steps(2 * 1000000000 / 10)
211  .value(0.f))
212  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z")
213  .min(-1000000000)
214  .max(1000000000)
215  .steps(2 * 1000000000 / 10)
216  .value(0.f))
217  .addChild(RemoteGui::makeTextLabel("to "))
218  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x")
219  .min(-1000000000)
220  .max(1000000000)
221  .steps(2 * 1000000000 / 10)
222  .value(0.f))
223  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y")
224  .min(-1000000000)
225  .max(1000000000)
226  .steps(2 * 1000000000 / 10)
227  .value(0.f))
228  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z")
229  .min(-1000000000)
230  .max(1000000000)
231  .steps(2 * 1000000000 / 10)
232  .value(0.f))
233  .addChild(RemoteGui::makeTextLabel("velocity "))
234  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity")
235  .min(0)
236  .max(6)
237  .steps(static_cast<int>(6 / 0.1))
238  .value(0.8f))
239  .addChild(RemoteGui::makeTextLabel("acceleration "))
240  .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration")
241  .min(0)
242  .max(8)
243  .steps(static_cast<int>(8 / 0.1))
244  .value(4.0f))
245  .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
246 
247  rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
248 
249  _guiTask = new SimplePeriodicTask<>(
250  [this]()
251  {
252  bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
253  std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
254 
255  _guiTab.receiveUpdates();
256 
257  if (oldEnabledGui == enabled)
258  {
259  // only apply changes of gui if not already changed by ice
260  _enableTracking(_guiTab.getValue<bool>("enabled").get());
261  }
262  _guiTab.getValue<bool>("enabled").set(enabled);
263 
264  if (oldFrameGui == frameName &&
265  oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
266  {
267  // only apply changes of gui if not already changed by ice
268  setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
269  }
270  _guiTab.getValue<std::string>("tracking_frame").set(frameName);
271 
272  _guiTab.sendUpdates();
273 
274  if (_guiTab.getButton("button_look_at_frame").clicked())
275  {
276  lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
277  }
278 
279  if (_guiTab.getButton("button_look_at_global_point").clicked())
280  {
281  lookAtPointInGlobalFrame(
282  Vector3f{_guiTab.getValue<float>("global_point_x").get(),
283  _guiTab.getValue<float>("global_point_y").get(),
284  _guiTab.getValue<float>("global_point_z").get()});
285  }
286 
287  if (_guiTab.getButton("button_look_at_robot_point").clicked())
288  {
289  lookAtPointInRobotFrame(
290  Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
291  _guiTab.getValue<float>("robot_point_y").get(),
292  _guiTab.getValue<float>("robot_point_z").get()});
293  }
294 
295  if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
296  {
297  scanInConfigurationSpace(
298  _guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
299  _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
300  {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
301  _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
302  }
303 
304  if (_guiTab.getButton("button_scan_in_workspace").clicked())
305  {
306  scanInWorkspace(
307  {{_guiTab.getValue<float>("scan_in_workspace_from_x").get(),
308  _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
309  _guiTab.getValue<float>("scan_in_workspace_from_z").get()},
310  {_guiTab.getValue<float>("scan_in_workspace_to_x").get(),
311  _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
312  _guiTab.getValue<float>("scan_in_workspace_to_z").get()}},
313  _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
314  _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
315  }
316  },
317  10);
318 
319  RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
320 
321  _remoteGui->createTab(getName(), rootLayout);
322  _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
323 
324  _guiTask->start();
325  }
326  }
327 
328  void
329  FrameTracking::onDisconnectComponent()
330  {
331  _enableTracking(false);
332  if (_guiTask)
333  {
334  _guiTask->stop();
335  _guiTask = nullptr;
336  }
337  }
338 
339  void
340  FrameTracking::onExitComponent()
341  {
342  }
343 
344  armarx::PropertyDefinitionsPtr
345  FrameTracking::createPropertyDefinitions()
346  {
347  return armarx::PropertyDefinitionsPtr(
348  new FrameTrackingPropertyDefinitions(getConfigIdentifier()));
349  }
350 
351  void
352  FrameTracking::enableTracking(bool enable, const Ice::Current&)
353  {
354  _enableTracking(enable);
355  }
356 
357  void
358  FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
359  {
360  if (enabled)
361  {
362  ARMARX_WARNING << "Disable tracking to set new frame.";
363  return;
364  }
365  this->frameName = frameName;
366  }
367 
368  std::string
369  FrameTracking::getFrame(const Ice::Current&) const
370  {
371  return frameName;
372  }
373 
374  void
375  FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
376  {
377  if (enabled)
378  {
379  ARMARX_WARNING << "Disable tracking to use lookAt functions.";
380  return;
381  }
382  if (!localRobot->hasRobotNode(frameName))
383  {
384  ARMARX_ERROR << frameName << " does not exist.";
385  return;
386  }
387  syncronizeLocalClone();
388  _lookAtFrame(frameName);
389  }
390 
391  void
392  FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
393  {
394  if (enabled)
395  {
396  ARMARX_WARNING << "Disable tracking to use lookAt functions.";
397  return;
398  }
399  syncronizeLocalClone();
400  _lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
401  }
402 
403  bool
404  FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point,
405  float max_diff,
406  const Ice::Current&)
407  {
408  if (enabled)
409  {
410  ARMARX_WARNING << "Disable tracking to use lookAt functions.";
411  return false;
412  }
413  syncronizeLocalClone();
414  return _looksAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)), max_diff);
415  }
416 
417  void
418  FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
419  {
420  if (enabled)
421  {
422  ARMARX_WARNING << "Disable tracking to use lookAt functions.";
423  return;
424  }
425  syncronizeLocalClone();
426  _lookAtPoint(ToEigen(point));
427  }
428 
429  void
430  FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
431  {
432  const float currentYaw = headYawJoint->getJointValue();
433  const float currentPitch = headPitchJoint->getJointValue();
434 
435  const float currentYawVel =
436  DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
437  "jointvelocities", headYawJoint->getName()))
438  ->getFloat();
439  const float currentPitchVel =
440  DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
441  "jointvelocities", headPitchJoint->getName()))
442  ->getFloat();
443 
444  FrameTracking::HeadState headState;
445  headState.currentYawPos = currentYaw;
446  headState.currentYawVel = currentYawVel;
447  headState.currentPitchPos = currentPitch;
448  headState.currentPitchVel = currentPitchVel;
449 
450 
451  headState.desiredYawPos = yaw;
452  headState.desiredPitchPos = pitch;
453  _doPositionControl(headState);
454  struct timespec req = {0, 30 * 1000000L};
455  while (std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
456  std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.))
457  {
458  ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw
459  << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
460  syncronizeLocalClone();
461  // sleep for 30 milliseconds
462  nanosleep(&req, nullptr);
463  }
464  auto currentModes = kinematicUnitInterfacePrx->getControlModes();
465  kinematicUnitInterfacePrx->switchControlMode(
466  {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
467  {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
468  }
469 
470  void
471  FrameTracking::scanInConfigurationSpace(float yawFrom,
472  float yawTo,
473  const Ice::FloatSeq& pitchValues,
474  float velocity,
475  const Ice::Current&)
476  {
477  if (enabled)
478  {
479  ARMARX_WARNING << "Disable tracking to use scan functions.";
480  return;
481  }
482  velocity = std::abs(velocity);
483 
484  syncronizeLocalClone();
485  auto currentModes = kinematicUnitInterfacePrx->getControlModes();
486  kinematicUnitInterfacePrx->switchControlMode(
487  {{headYawJoint->getName(), ControlMode::eVelocityControl},
488  {headPitchJoint->getName(), ControlMode::eVelocityControl}});
489 
490  // to initial yaw
491  {
492  bool wasGreater = headYawJoint->getJointValue() > yawFrom;
493  float yawVelocityToInit = wasGreater ? -velocity : velocity;
494  kinematicUnitInterfacePrx->setJointVelocities(
495  {{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
496  // if the joint angle was greater before we want to run as long as it is greater
497  // otherwise we want to run as long as it is smaler
498  while ((wasGreater && headYawJoint->getJointValue() > yawFrom) ||
499  (!wasGreater && headYawJoint->getJointValue() < yawFrom))
500  {
501  syncronizeLocalClone();
502  }
503  }
504 
505  for (const auto& p : pitchValues)
506  {
507  // to pitch value
508  bool wasGreaterP = headPitchJoint->getJointValue() > p;
509  float velocityPitch = wasGreaterP ? -velocity : velocity;
510  kinematicUnitInterfacePrx->setJointVelocities(
511  {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
512  while ((wasGreaterP && headPitchJoint->getJointValue() > p) ||
513  (!wasGreaterP && headPitchJoint->getJointValue() < p))
514  {
515  syncronizeLocalClone();
516  }
517 
518  // to yaw value
519  bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
520  float velocityYaw = wasGreaterY ? -velocity : velocity;
521  kinematicUnitInterfacePrx->setJointVelocities(
522  {{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
523  while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) ||
524  (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
525  {
526  syncronizeLocalClone();
527  }
528 
529  std::swap(yawFrom, yawTo);
530  }
531  kinematicUnitInterfacePrx->setJointVelocities(
532  {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
533  kinematicUnitInterfacePrx->switchControlMode(
534  {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
535  {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
536  }
537 
538  void
539  FrameTracking::scanInWorkspace(const Vector3fSeq& points,
540  float maxVelocity,
541  float acceleration,
542  const Ice::Current&)
543  {
544  if (enabled)
545  {
546  ARMARX_WARNING << "Disable tracking to use scan functions.";
547  return;
548  }
549  syncronizeLocalClone();
550  auto currentModes = kinematicUnitInterfacePrx->getControlModes();
551  kinematicUnitInterfacePrx->switchControlMode(
552  {{headYawJoint->getName(), ControlMode::eVelocityControl},
553  {headPitchJoint->getName(), ControlMode::eVelocityControl}});
554  struct timespec req = {0, 30 * 1000000L};
555  for (const auto& p : points)
556  {
557  auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
558  auto target = _calculateJointAngles(pEigen);
559  while (std::abs(target.currentYawPos - target.desiredYawPos) >
560  static_cast<float>(M_PI / 180.) ||
561  std::abs(target.currentPitchPos - target.desiredPitchPos) >
562  static_cast<float>(M_PI / 180.))
563  {
564  ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos
565  << " pitch: " << target.currentPitchPos << " - "
566  << target.desiredPitchPos;
567  syncronizeLocalClone();
568  target = _calculateJointAngles(pEigen);
569  _doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
570  // sleep for 30 milliseconds
571  nanosleep(&req, nullptr);
572  }
573  }
574  kinematicUnitInterfacePrx->setJointVelocities(
575  {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
576  kinematicUnitInterfacePrx->switchControlMode(
577  {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
578  {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
579  }
580 
581  void
582  FrameTracking::process()
583  {
584  if (!localRobot->hasRobotNode(frameName))
585  {
586  ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
587  std::thread([this]() { _enableTracking(false); }).detach();
588  return;
589  }
590  syncronizeLocalClone();
591  _doVelocityControl(_calculateJointAnglesContinously(frameName),
592  maxYawVelocity,
593  yawAcceleration,
594  maxPitchVelocity,
595  pitchAcceleration);
596  }
597 
598  void
599  FrameTracking::syncronizeLocalClone()
600  {
601  armarx::RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
602  }
603 
604  void
605  FrameTracking::_lookAtFrame(const std::string& frameName)
606  {
607  auto frame = localRobot->getRobotNode(frameName);
608  auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
609  _lookAtPoint(posInRobotFrame);
610  }
611 
612  void
613  FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
614  {
615  _doPositionControl(_calculateJointAngles(point));
616  }
617 
618  bool
619  FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
620  {
621  auto head_state = _calculateJointAngles(point);
622  float diff = std::abs(head_state.desiredPitchPos - head_state.currentPitchPos) +
623  std::abs(head_state.currentYawPos - head_state.desiredYawPos);
624  return max_diff > diff;
625  }
626 
627  FrameTracking::HeadState
628  FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
629  {
630  auto frame = localRobot->getRobotNode(frameName);
631  auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
632  // do nothing if the robot works above his head
633  // he should already look upwards because if this component runs continously
634  if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() +
635  posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
636  {
637  return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
638  }
639  return _calculateJointAngles(posInRobotFrame);
640  }
641 
642  FrameTracking::HeadState
643  FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
644  {
645  float yaw = -std::atan2(point.x(), point.y());
646  // make shure the joint value satisfies the joint limits
647  yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
648  yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
649  // we dont want the robot to move from one limit to the other in one step
650  const float currentYaw = headYawJoint->getJointValue();
651  if (!headYawJoint->isLimitless() &&
652  std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() -
653  headYawJoint->getJointLimitLo() -
654  static_cast<float>(M_PI) / 8)
655  {
656  yaw = currentYaw;
657  }
658 
659  const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(
660  localRobot->toGlobalCoordinateSystemVec(point));
661  const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
662  const float headHeightRealativeToPitchJoint =
663  headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
664  float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) +
665  std::asin(headHeightRealativeToPitchJoint / pj.norm());
666  // make shure the joint value satisfies the joint limits
667  pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
668  pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
669  const float currentPitch = headPitchJoint->getJointValue();
670 
671  ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point
672  << " using yaw=" << yaw << " and pitch=" << pitch;
673 
674  const float currentYawVel =
675  DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
676  "jointvelocities", headYawJoint->getName()))
677  ->getFloat();
678  const float currentPitchVel =
679  DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
680  "jointvelocities", headPitchJoint->getName()))
681  ->getFloat();
682 
683  FrameTracking::HeadState headState;
684  headState.currentYawPos = currentYaw;
685  headState.desiredYawPos = yaw;
686  headState.currentYawVel = currentYawVel;
687  headState.currentPitchPos = currentPitch;
688  headState.desiredPitchPos = pitch;
689  headState.currentPitchVel = currentPitchVel;
690  return headState;
691  }
692 
693  void
694  FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState,
695  float maxYawVelocity,
696  float yawAcceleration,
697  float maxPitchVelocity,
698  float pitchAcceleration)
699  {
700  if (headState.stop)
701  {
702  kinematicUnitInterfacePrx->setJointVelocities(
703  {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
704  return;
705  }
706 
707  float desiredYawVelocity =
708  positionThroughVelocityControlWithAccelerationBounds(30.f / 1000,
709  35.f / 1000,
710  headState.currentYawVel,
711  maxYawVelocity,
712  yawAcceleration,
713  yawAcceleration,
714  headState.currentYawPos,
715  headState.desiredYawPos,
716  1.f);
717  float desiredPitchVelocity =
718  positionThroughVelocityControlWithAccelerationBounds(30.f / 1000,
719  35.f / 1000,
720  headState.currentPitchVel,
721  maxPitchVelocity,
722  pitchAcceleration,
723  pitchAcceleration,
724  headState.currentPitchPos,
725  headState.desiredPitchPos,
726  1.f);
727 
728  // control mode is set when enable task
729  kinematicUnitInterfacePrx->setJointVelocities(
730  {{headYawJoint->getName(), desiredYawVelocity},
731  {headPitchJoint->getName(), desiredPitchVelocity}});
732  }
733 
734  void
735  FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
736  {
737  auto currentModes = kinematicUnitInterfacePrx->getControlModes();
738  kinematicUnitInterfacePrx->switchControlMode(
739  {{headYawJoint->getName(), ControlMode::ePositionControl},
740  {headPitchJoint->getName(), ControlMode::ePositionControl}});
741  if (headState.stop)
742  {
743  return;
744  }
745  kinematicUnitInterfacePrx->setJointAngles(
746  {{headYawJoint->getName(), headState.desiredYawPos},
747  {headPitchJoint->getName(), headState.desiredPitchPos}});
748  kinematicUnitInterfacePrx->switchControlMode(
749  {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
750  {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
751  }
752 
753  void
754  FrameTracking::_enableTracking(bool enable)
755  {
756  if (this->enabled == enable)
757  {
758  return;
759  }
760  this->enabled = enable;
761  if (enable)
762  {
763  kinematicUnitInterfacePrx->switchControlMode(
764  {{headYawJoint->getName(), ControlMode::eVelocityControl},
765  {headPitchJoint->getName(), ControlMode::eVelocityControl}});
766  processorTask->start();
767  }
768  else
769  {
770  kinematicUnitInterfacePrx->setJointVelocities(
771  {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
772  processorTask->stop();
773  }
774  }
775 } // namespace armarx
armarx::FrameTracking::enabled
std::atomic< bool > enabled
Definition: FrameTracking.h:229
armarx::RemoteGui::detail::MinMaxMixin::min
Derived & min(Type min)
Definition: Basic.h:96
armarx::RemoteGui::makeVBoxLayout
detail::VBoxLayoutBuilder makeVBoxLayout(std::string const &name="")
Definition: LayoutWidgets.h:286
armarx::FrameTracking::_guiTask
SimplePeriodicTask ::pointer_type _guiTask
Definition: FrameTracking.h:242
armarx::FrameTracking::_looksAtPoint
bool _looksAtPoint(const Eigen::Vector3f &point, float max_diff)
Definition: FrameTracking.cpp:619
armarx::FrameTracking::scanInWorkspace
void scanInWorkspace(const ::armarx::Vector3fSeq &points, float maxVelocity, float acceleration, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:539
armarx::RemoteGui::makeHBoxLayout
detail::HBoxLayoutBuilder makeHBoxLayout(std::string const &name="")
Definition: LayoutWidgets.h:268
armarx::FrameTracking::processorTask
PeriodicTask< FrameTracking >::pointer_type processorTask
Definition: FrameTracking.h:226
armarx::FrameTracking::lookAtPointInRobotFrame
void lookAtPointInRobotFrame(const Vector3f &point, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:418
armarx::FrameTracking::onInitComponent
void onInitComponent() override
Definition: FrameTracking.cpp:38
armarx::RemoteGui::detail::VBoxLayoutBuilder
Definition: LayoutWidgets.h:108
armarx::RemoteGui::makeButton
detail::ButtonBuilder makeButton(std::string const &name)
Definition: IntegerWidgets.h:52
armarx::FrameTracking::enableTracking
void enableTracking(bool enable, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:352
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:522
ArmarXManager.h
armarx::FrameTracking::onConnectComponent
void onConnectComponent() override
Definition: FrameTracking.cpp:59
armarx::FrameTracking::_remoteGui
RemoteGuiInterfacePrx _remoteGui
Definition: FrameTracking.h:241
armarx::FrameTracking::frameName
std::string frameName
Definition: FrameTracking.h:230
WidgetBuilder.h
armarx::ManagedIceObject::getArmarXManager
ArmarXManagerPtr getArmarXManager() const
Returns the ArmarX manager used to add and remove components.
Definition: ManagedIceObject.cpp:360
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:297
armarx::FrameTracking::_doPositionControl
void _doPositionControl(const HeadState &headstate)
Definition: FrameTracking.cpp:735
armarx::FrameTrackingPropertyDefinitions
Definition: FrameTracking.h:48
armarx::RemoteGui::makeTextLabel
detail::LabelBuilder makeTextLabel(std::string const &text)
Definition: StaticWidgets.h:24
armarx::FrameTracking::kinematicUnitObserverInterfacePrx
KinematicUnitObserverInterfacePrx kinematicUnitObserverInterfacePrx
Definition: FrameTracking.h:227
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:234
armarx::FrameTracking::scanInConfigurationSpace
void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq &pitchValues, float velocity, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:471
armarx::armem::client::util::swap
void swap(SubscriptionHandle &first, SubscriptionHandle &second)
Definition: SubscriptionHandle.cpp:66
DatafieldRef.h
armarx::RemoteGui::TabProxy
Definition: WidgetProxy.h:17
armarx::RemoteGui::detail::ComboBoxBuilder::addOptions
ComboBoxBuilder & addOptions(std::vector< std::string > const &options)
Definition: StringWidgets.h:28
armarx::RemoteGui::detail::MinMaxMixin::max
Derived & max(Type max)
Definition: Basic.h:128
armarx::FrameTracking::yawAcceleration
float yawAcceleration
Definition: FrameTracking.h:232
armarx::RemoteGui::TabProxy::getButton
ButtonProxy getButton(std::string const &name)
Definition: WidgetProxy.cpp:6
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
armarx::RemoteGui::TabProxy::receiveUpdates
void receiveUpdates()
Definition: WidgetProxy.h:135
armarx::FrameTracking::getFrame
std::string getFrame(const Ice::Current &=Ice::emptyCurrent) const override
Definition: FrameTracking.cpp:369
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
armarx::RemoteGui::detail::ChildrenMixin::addChild
Derived & addChild(WidgetPtr const &child)
Definition: LayoutWidgets.h:27
armarx::FrameTracking::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: FrameTracking.cpp:345
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::RemoteGui::makeCheckBox
detail::CheckBoxBuilder makeCheckBox(std::string const &name)
Definition: BoolWidgets.h:27
armarx::FrameTracking::lookAtFrame
void lookAtFrame(const std::string &frameName, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:375
armarx::FrameTracking::_lookAtFrame
void _lookAtFrame(const std::string &frame)
Definition: FrameTracking.cpp:605
armarx::RemoteGui::ButtonProxy::clicked
bool clicked() const
Definition: WidgetProxy.h:362
armarx::RemoteGui::detail::ValueMixin::value
Derived & value(ValueT const &value)
Definition: Basic.h:84
armarx::FrameTracking::robotStateComponent
RobotStateComponentInterfacePrx robotStateComponent
Definition: FrameTracking.h:224
armarx::RemoteGui::detail::LabelMixin::label
Derived & label(std::string const &label)
Definition: Basic.h:216
armarx::FrameTracking::_lookAtPoint
void _lookAtPoint(const Eigen::Vector3f &point)
Definition: FrameTracking.cpp:613
max
T max(T t1, T t2)
Definition: gdiam.h:51
armarx::FrameTracking::headPitchJoint
VirtualRobot::RobotNodePtr headPitchJoint
Definition: FrameTracking.h:238
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::FrameTracking::maxYawVelocity
float maxYawVelocity
Definition: FrameTracking.h:231
armarx::FrameTracking::process
void process()
Definition: FrameTracking.cpp:582
armarx::FrameTracking::cameraNode
VirtualRobot::RobotNodePtr cameraNode
Definition: FrameTracking.h:239
armarx::FrameTracking::_doVelocityControl
void _doVelocityControl(const HeadState &headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
Definition: FrameTracking.cpp:694
armarx::FrameTracking::syncronizeLocalClone
void syncronizeLocalClone()
Definition: FrameTracking.cpp:599
armarx::FrameTracking::pitchAcceleration
float pitchAcceleration
Definition: FrameTracking.h:234
armarx::WidgetDescription::WidgetPtr
::IceInternal::Handle<::armarx::WidgetDescription::Widget > WidgetPtr
Definition: NJointControllerBase.h:67
armarx::FrameTracking::onExitComponent
void onExitComponent() override
Definition: FrameTracking.cpp:340
armarx::RemoteGui::TabProxy::getValue
ValueProxy< T > getValue(std::string const &name)
Definition: WidgetProxy.h:167
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition: RemoteRobot.cpp:381
armarx::FrameTracking::_calculateJointAnglesContinously
HeadState _calculateJointAnglesContinously(const std::string &frame)
Definition: FrameTracking.cpp:628
set
set(LIBS ArmarXCoreInterfaces ${CMAKE_THREAD_LIBS_INIT} ${dl_LIBRARIES} ${rt_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} BoostAssertionHandler ArmarXCPPUtility SimoxUtility) set(LIB_FILES ArmarXManager.cpp ArmarXMultipleObjectsScheduler.cpp ArmarXObjectScheduler.cpp ManagedIceObject.cpp ManagedIceObjectPlugin.cpp Component.cpp ComponentPlugin.cpp IceGridAdmin.cpp ArmarXObjectObserver.cpp IceManager.cpp PackagePath.cpp RemoteReferenceCount.cpp logging/LoggingUtil.cpp logging/Logging.cpp logging/LogSender.cpp logging/ArmarXLogBuf.cpp system/ArmarXDataPath.cpp system/DynamicLibrary.cpp system/ProcessWatcher.cpp system/FactoryCollectionBase.cpp system/cmake/CMakePackageFinder.cpp system/cmake/CMakePackageFinderCache.cpp system/cmake/ArmarXPackageToolInterface.cpp system/RemoteObjectNode.cpp services/sharedmemory/HardwareId.cpp services/tasks/RunningTask.cpp services/tasks/ThreadList.cpp services/tasks/ThreadPool.cpp services/profiler/Profiler.cpp services/profiler/FileLoggingStrategy.cpp services/profiler/IceLoggingStrategy.cpp application/Application.cpp application/ApplicationOptions.cpp application/ApplicationProcessFacet.cpp application/ApplicationNetworkStats.cpp application/properties/PropertyUser.cpp application/properties/Property.cpp application/properties/PropertyDefinition.cpp application/properties/PropertyDefinitionContainer.cpp application/properties/PropertyDefinitionHelpFormatter.cpp application/properties/PropertyDefinitionConfigFormatter.cpp application/properties/PropertyDefinitionBriefHelpFormatter.cpp application/properties/PropertyDefinitionXmlFormatter.cpp application/properties/PropertyDefinitionDoxygenFormatter.cpp application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.cpp application/properties/PropertyDefinitionContainerBriefHelpFormatter.cpp application/properties/IceProperties.cpp exceptions/Exception.cpp exceptions/local/UnexpectedEnumValueException.cpp util/FileSystemPathBuilder.cpp util/StringHelpers.cpp util/IceReportSkipper.cpp util/Throttler.cpp util/distributed/AMDCallbackCollection.cpp util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.cpp util/distributed/RemoteHandle/RemoteHandle.cpp util/distributed/RemoteHandle/RemoteHandleControlBlock.cpp time/ice_conversions.cpp time/json_conversions.cpp time/CallbackWaitLock.cpp time/Clock.cpp time/ClockType.cpp time/ClockTypeNames.cpp time/CycleUtil.cpp time/DateTime.cpp time/Duration.cpp time/Frequency.cpp time/LocalTimeServer.cpp time/Metronome.cpp time/ScopedStopWatch.cpp time/StopWatch.cpp time/Timer.cpp time/TimeKeeper.cpp time/TimeUtil.cpp csv/CsvWriter.cpp csv/CsvReader.cpp eigen/conversions.cpp eigen/ice_conversions.cpp) set(LIB_HEADERS ArmarXManager.h ArmarXDummyManager.h ArmarXMultipleObjectsScheduler.h ArmarXObjectObserver.h ArmarXObjectScheduler.h ArmarXFwd.h Component.h ComponentPlugin.h ComponentFactories.h CoreObjectFactories.h IceGridAdmin.h IceManager.h IceManagerImpl.h json_conversions.h ManagedIceObject.h ManagedIceObjectPlugin.h ManagedIceObjectImpl.h ManagedIceObjectDependency.h ManagedIceObjectRegistryInterface.h PackagePath.h RemoteReferenceCount.h system/ImportExport.h system/ImportExportComponent.h system/AbstractFactoryMethod.h system/FactoryCollectionBase.h system/Synchronization.h system/ArmarXDataPath.h system/DynamicLibrary.h system/ProcessWatcher.h system/ConditionSynchronization.h system/cmake/CMakePackageFinder.h system/cmake/CMakePackageFinderCache.h system/cmake/FindPackageX.cmake system/cmake/ArmarXPackageToolInterface.h system/RemoteObjectNode.h logging/LoggingUtil.h logging/LogSender.h logging/Logging.h logging/ArmarXLogBuf.h logging/SpamFilterData.h services/tasks/RunningTask.h services/tasks/PeriodicTask.h services/tasks/ThreadList.h services/tasks/TaskUtil.h services/tasks/ThreadPool.h services/sharedmemory/SharedMemoryProvider.h services/sharedmemory/SharedMemoryConsumer.h services/sharedmemory/IceSharedMemoryProvider.h services/sharedmemory/IceSharedMemoryConsumer.h services/sharedmemory/HardwareIdentifierProvider.h services/sharedmemory/HardwareId.h services/sharedmemory/exceptions/SharedMemoryExceptions.h services/profiler/Profiler.h services/profiler/LoggingStrategy.h services/profiler/FileLoggingStrategy.h services/profiler/IceLoggingStrategy.h application/Application.h application/ApplicationOptions.h application/ApplicationProcessFacet.h application/ApplicationNetworkStats.h application/properties/forward_declarations.h application/properties/Properties.h application/properties/Property.h application/properties/PluginEigen.h application/properties/PluginEnumNames.h application/properties/PluginCfgStruct.h application/properties/PluginAll.h application/properties/PropertyUser.h application/properties/PropertyDefinition.h application/properties/PropertyDefinition.hpp application/properties/PropertyDefinitionInterface.h application/properties/PropertyDefinitionContainer.h application/properties/PropertyDefinitionFormatter.h application/properties/PropertyDefinitionContainerFormatter.h application/properties/PropertyDefinitionConfigFormatter.h application/properties/PropertyDefinitionHelpFormatter.h application/properties/PropertyDefinitionBriefHelpFormatter.h application/properties/PropertyDefinitionXmlFormatter.h application/properties/PropertyDefinitionDoxygenFormatter.h application/properties/PropertyDefinitionDoxygenComponentPagesFormatter.h application/properties/PropertyDefinitionContainerBriefHelpFormatter.h application/properties/ProxyPropertyDefinition.h application/properties/IceProperties.h exceptions/Exception.h exceptions/LocalException.h exceptions/local/DynamicLibraryException.h exceptions/local/ExpressionException.h exceptions/local/FileIOException.h exceptions/local/InvalidPropertyValueException.h exceptions/local/MissingRequiredPropertyException.h exceptions/local/PropertyInheritanceCycleException.h exceptions/local/ProxyNotInitializedException.h exceptions/local/UnexpectedEnumValueException.h exceptions/local/UnmappedValueException.h exceptions/local/ValueRangeExceededException.h exceptions/user/NotImplementedYetException.h rapidxml/rapidxml.hpp rapidxml/rapidxml_print.hpp rapidxml/rapidxml_iterators.hpp rapidxml/rapidxml_utils.hpp rapidxml/wrapper/RapidXmlReader.h rapidxml/wrapper/RapidXmlWriter.h rapidxml/wrapper/DefaultRapidXmlReader.h rapidxml/wrapper/MultiNodeRapidXMLReader.h util/IceBlobToObject.h util/ObjectToIceBlob.h util/FileSystemPathBuilder.h util/FiniteStateMachine.h util/StringHelpers.h util/StringHelperTemplates.h util/algorithm.h util/OnScopeExit.h util/Predicates.h util/Preprocessor.h util/PropagateConst.h util/Registrar.h util/TemplateMetaProgramming.h util/TripleBuffer.h util/IceReportSkipper.h util/Throttler.h util/distributed/AMDCallbackCollection.h util/distributed/RemoteHandle/ClientSideRemoteHandleControlBlock.h util/distributed/RemoteHandle/RemoteHandle.h util/distributed/RemoteHandle/RemoteHandleControlBlock.h util/SimpleStatemachine.h time.h time_minimal.h time/forward_declarations.h time/ice_conversions.h time/json_conversions.h time/CallbackWaitLock.h time/Clock.h time/ClockType.h time/ClockTypeNames.h time/CycleUtil.h time/DateTime.h time/Duration.h time/Frequency.h time/LocalTimeServer.h time/Metronome.h time/ScopedStopWatch.h time/StopWatch.h time/Timer.h time/TimeUtil.h time/TimeKeeper.h csv/CsvWriter.h csv/CsvReader.h eigen/conversions.h eigen/ice_conversions.h ice_conversions.h ice_conversions/ice_conversions_boost_templates.h ice_conversions/ice_conversions_templates.h ice_conversions/ice_conversions_templates.tpp $
Definition: CMakeLists.txt:12
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:327
FrameTracking.h
armarx::FrameTracking::kinematicUnitInterfacePrx
KinematicUnitInterfacePrx kinematicUnitInterfacePrx
Definition: FrameTracking.h:225
armarx::FrameTracking::setFrame
void setFrame(const std::string &frameName, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:358
armarx::FrameTracking::maxPitchVelocity
float maxPitchVelocity
Definition: FrameTracking.h:233
armarx::RemoteGui::TabProxy::sendUpdates
void sendUpdates()
Definition: WidgetProxy.h:151
armarx::RemoteGui::makeComboBox
detail::ComboBoxBuilder makeComboBox(std::string const &name)
Definition: StringWidgets.h:61
armarx::FrameTracking::headYawJoint
VirtualRobot::RobotNodePtr headYawJoint
Definition: FrameTracking.h:237
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:108
armarx::FrameTracking::_calculateJointAngles
HeadState _calculateJointAngles(const Eigen::Vector3f &point)
Definition: FrameTracking.cpp:643
armarx::FrameTracking::lookAtPointInGlobalFrame
void lookAtPointInGlobalFrame(const Vector3f &point, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:392
armarx::PeriodicTask
Definition: ArmarXManager.h:70
min
T min(T t1, T t2)
Definition: gdiam.h:44
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::positionThroughVelocityControlWithAccelerationBounds
float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt, float currentV, float maxV, float acceleration, float deceleration, float currentPosition, float targetPosition, float p)
Definition: BasicControllers.cpp:166
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:35
armarx::FrameTracking::localRobot
VirtualRobot::RobotPtr localRobot
Definition: FrameTracking.h:236
armarx::FrameTracking::_enableTracking
void _enableTracking(bool enable)
Definition: FrameTracking.cpp:754
armarx::RemoteGui::makeFloatSpinBox
detail::FloatSpinBoxBuilder makeFloatSpinBox(std::string const &name)
Definition: FloatWidgets.h:58
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
armarx::FrameTracking::moveJointsTo
void moveJointsTo(float yaw, float pitch, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:430
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::RemoteGui::detail::FloatMinMaxMixin::steps
Derived & steps(int steps)
Definition: FloatWidgets.h:11
KinematicUnitObserver.h
armarx::SimplePeriodicTask
Usage:
Definition: ApplicationNetworkStats.h:32
armarx::FrameTracking::isLookingAtPointInGlobalFrame
bool isLookingAtPointInGlobalFrame(const Vector3f &point, float max_diff, const Ice::Current &=Ice::emptyCurrent) override
Definition: FrameTracking.cpp:404
armarx::FrameTracking::_guiTab
RemoteGui::TabProxy _guiTab
Definition: FrameTracking.h:243
armarx::FrameTracking::onDisconnectComponent
void onDisconnectComponent() override
Definition: FrameTracking.cpp:329