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#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
36
37namespace armarx
38{
39 void
40 FrameTracking::onInitComponent()
41 {
42 usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
43 usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
44 usingProxy(getProperty<std::string>("KinematicUnitObserverName").getValue());
45 if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
46 {
47 usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
48 }
49
50 enabled = false;
51 frameName = getProperty<std::string>("FrameOnStartup").getValue();
52
53 maxYawVelocity = getProperty<float>("MaxYawVelocity").getValue();
54 yawAcceleration = getProperty<float>("YawAcceleration").getValue();
55
56 maxPitchVelocity = getProperty<float>("MaxPitchVelocity").getValue();
57 pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
58 }
59
60 void
61 FrameTracking::onConnectComponent()
62 {
63 robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
64 getProperty<std::string>("RobotStateComponentName").getValue());
65 kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(
66 getProperty<std::string>("KinematicUnitName").getValue());
67 kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(
68 getProperty<std::string>("KinematicUnitObserverName").getValue());
69
70 localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
71 headYawJoint =
72 localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
73 if (!headYawJoint ||
74 !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
75 {
76 ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue()
77 << " is not a valid joint.";
78 getArmarXManager()->asyncShutdown();
79 }
80 headPitchJoint =
81 localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
82 if (!headPitchJoint ||
83 !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
84 {
85 ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue()
86 << " is not a valid joint.";
87 getArmarXManager()->asyncShutdown();
88 }
89 cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
90 if (!cameraNode)
91 {
92 ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue()
93 << " is not a valid node.";
94 getArmarXManager()->asyncShutdown();
95 }
96
97 processorTask = new PeriodicTask<FrameTracking>(this, &FrameTracking::process, 30);
98 _enableTracking(getProperty<bool>("EnableTrackingOnStartup").getValue());
99
100 if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
101 {
102 _remoteGui = getProxy<RemoteGuiInterfacePrx>(
103 getProperty<std::string>("RemoteGuiName").getValue());
104 RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
105
106 rootLayoutBuilder.addChild(
107 RemoteGui::makeHBoxLayout()
108 .addChild(RemoteGui::makeTextLabel("Tracking: "))
109 .addChild(RemoteGui::makeTextLabel("Enabled"))
110 .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
111 .addChild(RemoteGui::makeTextLabel("Frame"))
112 .addChild(RemoteGui::makeComboBox("tracking_frame")
113 .options(localRobot->getRobotNodeNames())
114 .addOptions({""})
115 .value(frameName)));
116
117 rootLayoutBuilder.addChild(
118 RemoteGui::makeHBoxLayout()
119 .addChild(RemoteGui::makeTextLabel("Look at frame: "))
120 .addChild(RemoteGui::makeComboBox("frame_look")
121 .options(localRobot->getRobotNodeNames())
122 .value(frameName))
123 .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at")));
124
125 rootLayoutBuilder.addChild(
126 RemoteGui::makeHBoxLayout()
127 .addChild(RemoteGui::makeTextLabel("Look at global point: "))
128 .addChild(RemoteGui::makeFloatSpinBox("global_point_x")
129 .min(-1000000000)
130 .max(1000000000)
131 .steps(2 * 1000000000 / 10)
132 .value(0.f))
133 .addChild(RemoteGui::makeFloatSpinBox("global_point_y")
134 .min(-1000000000)
135 .max(1000000000)
136 .steps(2 * 1000000000 / 10)
137 .value(0.f))
138 .addChild(RemoteGui::makeFloatSpinBox("global_point_z")
139 .min(-1000000000)
140 .max(1000000000)
141 .steps(2 * 1000000000 / 10)
142 .value(0.f))
143 .addChild(
144 RemoteGui::makeButton("button_look_at_global_point").label("look at")));
145
146 rootLayoutBuilder.addChild(
147 RemoteGui::makeHBoxLayout()
148 .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
149 .addChild(RemoteGui::makeFloatSpinBox("robot_point_x")
150 .min(-1000000000)
151 .max(1000000000)
152 .steps(2 * 1000000000 / 10)
153 .value(0.f))
154 .addChild(RemoteGui::makeFloatSpinBox("robot_point_y")
155 .min(-1000000000)
156 .max(1000000000)
157 .steps(2 * 1000000000 / 10)
158 .value(0.f))
159 .addChild(RemoteGui::makeFloatSpinBox("robot_point_z")
160 .min(-1000000000)
161 .max(1000000000)
162 .steps(2 * 1000000000 / 10)
163 .value(0.f))
164 .addChild(
165 RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
166
167 rootLayoutBuilder.addChild(
168 RemoteGui::makeHBoxLayout()
169 .addChild(RemoteGui::makeTextLabel("Scan: "))
170 .addChild(RemoteGui::makeTextLabel("yaw from "))
171 .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from")
172 .min(headYawJoint->getJointLimitLo())
173 .max(headYawJoint->getJointLimitHi())
174 .steps(static_cast<int>((headYawJoint->getJointLimitHi() -
175 headYawJoint->getJointLimitLo()) /
176 0.1)))
177 .addChild(RemoteGui::makeTextLabel("yaw to "))
178 .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to")
179 .min(headYawJoint->getJointLimitLo())
180 .max(headYawJoint->getJointLimitHi())
181 .steps(static_cast<int>((headYawJoint->getJointLimitHi() -
182 headYawJoint->getJointLimitLo()) /
183 0.1)))
184 .addChild(RemoteGui::makeTextLabel("pitch "))
185 .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch")
186 .min(headPitchJoint->getJointLimitLo())
187 .max(headPitchJoint->getJointLimitHi())
188 .steps(static_cast<int>((headPitchJoint->getJointLimitHi() -
189 headPitchJoint->getJointLimitLo()) /
190 0.1)))
191 .addChild(RemoteGui::makeTextLabel("velocity "))
192 .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity")
193 .min(0)
194 .max(6)
195 .steps(static_cast<int>(6 / 0.1))
196 .value(0.8f))
197 .addChild(
198 RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
199
200 rootLayoutBuilder.addChild(
201 RemoteGui::makeHBoxLayout()
202 .addChild(RemoteGui::makeTextLabel("Scan: "))
203 .addChild(RemoteGui::makeTextLabel("from "))
204 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x")
205 .min(-1000000000)
206 .max(1000000000)
207 .steps(2 * 1000000000 / 10)
208 .value(0.f))
209 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y")
210 .min(-1000000000)
211 .max(1000000000)
212 .steps(2 * 1000000000 / 10)
213 .value(0.f))
214 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z")
215 .min(-1000000000)
216 .max(1000000000)
217 .steps(2 * 1000000000 / 10)
218 .value(0.f))
219 .addChild(RemoteGui::makeTextLabel("to "))
220 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x")
221 .min(-1000000000)
222 .max(1000000000)
223 .steps(2 * 1000000000 / 10)
224 .value(0.f))
225 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y")
226 .min(-1000000000)
227 .max(1000000000)
228 .steps(2 * 1000000000 / 10)
229 .value(0.f))
230 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z")
231 .min(-1000000000)
232 .max(1000000000)
233 .steps(2 * 1000000000 / 10)
234 .value(0.f))
235 .addChild(RemoteGui::makeTextLabel("velocity "))
236 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity")
237 .min(0)
238 .max(6)
239 .steps(static_cast<int>(6 / 0.1))
240 .value(0.8f))
241 .addChild(RemoteGui::makeTextLabel("acceleration "))
242 .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration")
243 .min(0)
244 .max(8)
245 .steps(static_cast<int>(8 / 0.1))
246 .value(4.0f))
247 .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
248
249 rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
250
251 _guiTask = new SimplePeriodicTask<>(
252 [this]()
253 {
254 bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
255 std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
256
257 _guiTab.receiveUpdates();
258
259 if (oldEnabledGui == enabled)
260 {
261 // only apply changes of gui if not already changed by ice
262 _enableTracking(_guiTab.getValue<bool>("enabled").get());
263 }
264 _guiTab.getValue<bool>("enabled").set(enabled);
265
266 if (oldFrameGui == frameName &&
267 oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
268 {
269 // only apply changes of gui if not already changed by ice
270 setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
271 }
272 _guiTab.getValue<std::string>("tracking_frame").set(frameName);
273
274 _guiTab.sendUpdates();
275
276 if (_guiTab.getButton("button_look_at_frame").clicked())
277 {
278 lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
279 }
280
281 if (_guiTab.getButton("button_look_at_global_point").clicked())
282 {
283 lookAtPointInGlobalFrame(
284 Vector3f{_guiTab.getValue<float>("global_point_x").get(),
285 _guiTab.getValue<float>("global_point_y").get(),
286 _guiTab.getValue<float>("global_point_z").get()});
287 }
288
289 if (_guiTab.getButton("button_look_at_robot_point").clicked())
290 {
291 lookAtPointInRobotFrame(
292 Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
293 _guiTab.getValue<float>("robot_point_y").get(),
294 _guiTab.getValue<float>("robot_point_z").get()});
295 }
296
297 if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
298 {
299 scanInConfigurationSpace(
300 _guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
301 _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
302 {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
303 _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
304 }
305
306 if (_guiTab.getButton("button_scan_in_workspace").clicked())
307 {
308 scanInWorkspace(
309 {{_guiTab.getValue<float>("scan_in_workspace_from_x").get(),
310 _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
311 _guiTab.getValue<float>("scan_in_workspace_from_z").get()},
312 {_guiTab.getValue<float>("scan_in_workspace_to_x").get(),
313 _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
314 _guiTab.getValue<float>("scan_in_workspace_to_z").get()}},
315 _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
316 _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
317 }
318 },
319 10);
320
321 RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
322
323 _remoteGui->createTab(getName(), rootLayout);
324 _guiTab = RemoteGui::TabProxy(_remoteGui, getName());
325
326 _guiTask->start();
327 }
328 }
329
330 void
331 FrameTracking::onDisconnectComponent()
332 {
333 _enableTracking(false);
334 if (_guiTask)
335 {
336 _guiTask->stop();
337 _guiTask = nullptr;
338 }
339 }
340
341 void
345
346 armarx::PropertyDefinitionsPtr
352
353 void
354 FrameTracking::enableTracking(bool enable, const Ice::Current&)
355 {
356 _enableTracking(enable);
357 }
358
359 void
360 FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
361 {
362 if (enabled)
363 {
364 ARMARX_WARNING << "Disable tracking to set new frame.";
365 return;
366 }
367 this->frameName = frameName;
368 }
369
370 std::string
371 FrameTracking::getFrame(const Ice::Current&) const
372 {
373 return frameName;
374 }
375
376 void
377 FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
378 {
379 if (enabled)
380 {
381 ARMARX_WARNING << "Disable tracking to use lookAt functions.";
382 return;
383 }
384 if (!localRobot->hasRobotNode(frameName))
385 {
386 ARMARX_ERROR << frameName << " does not exist.";
387 return;
388 }
389 syncronizeLocalClone();
390 _lookAtFrame(frameName);
391 }
392
393 void
394 FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
395 {
396 if (enabled)
397 {
398 ARMARX_WARNING << "Disable tracking to use lookAt functions.";
399 return;
400 }
401 syncronizeLocalClone();
402 _lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
403 }
404
405 bool
406 FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point,
407 float max_diff,
408 const Ice::Current&)
409 {
410 if (enabled)
411 {
412 ARMARX_WARNING << "Disable tracking to use lookAt functions.";
413 return false;
414 }
415 syncronizeLocalClone();
416 return _looksAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)), max_diff);
417 }
418
419 void
420 FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
421 {
422 if (enabled)
423 {
424 ARMARX_WARNING << "Disable tracking to use lookAt functions.";
425 return;
426 }
427 syncronizeLocalClone();
428 _lookAtPoint(ToEigen(point));
429 }
430
431 void
432 FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
433 {
434 const float currentYaw = headYawJoint->getJointValue();
435 const float currentPitch = headPitchJoint->getJointValue();
436
437 const float currentYawVel =
438 DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
439 "jointvelocities", headYawJoint->getName()))
440 ->getFloat();
441 const float currentPitchVel =
442 DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
443 "jointvelocities", headPitchJoint->getName()))
444 ->getFloat();
445
446 FrameTracking::HeadState headState;
447 headState.currentYawPos = currentYaw;
448 headState.currentYawVel = currentYawVel;
449 headState.currentPitchPos = currentPitch;
450 headState.currentPitchVel = currentPitchVel;
451
452
453 headState.desiredYawPos = yaw;
454 headState.desiredPitchPos = pitch;
455 _doPositionControl(headState);
456 struct timespec req = {0, 30 * 1000000L};
457 while (std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
458 std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.))
459 {
460 ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw
461 << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
462 syncronizeLocalClone();
463 // sleep for 30 milliseconds
464 nanosleep(&req, nullptr);
465 }
466 auto currentModes = kinematicUnitInterfacePrx->getControlModes();
467 kinematicUnitInterfacePrx->switchControlMode(
468 {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
469 {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
470 }
471
472 void
473 FrameTracking::scanInConfigurationSpace(float yawFrom,
474 float yawTo,
475 const Ice::FloatSeq& pitchValues,
476 float velocity,
477 const Ice::Current&)
478 {
479 if (enabled)
480 {
481 ARMARX_WARNING << "Disable tracking to use scan functions.";
482 return;
483 }
484 velocity = std::abs(velocity);
485
486 syncronizeLocalClone();
487 auto currentModes = kinematicUnitInterfacePrx->getControlModes();
488 kinematicUnitInterfacePrx->switchControlMode(
489 {{headYawJoint->getName(), ControlMode::eVelocityControl},
490 {headPitchJoint->getName(), ControlMode::eVelocityControl}});
491
492 // to initial yaw
493 {
494 bool wasGreater = headYawJoint->getJointValue() > yawFrom;
495 float yawVelocityToInit = wasGreater ? -velocity : velocity;
496 kinematicUnitInterfacePrx->setJointVelocities(
497 {{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
498 // if the joint angle was greater before we want to run as long as it is greater
499 // otherwise we want to run as long as it is smaler
500 while ((wasGreater && headYawJoint->getJointValue() > yawFrom) ||
501 (!wasGreater && headYawJoint->getJointValue() < yawFrom))
502 {
503 syncronizeLocalClone();
504 }
505 }
506
507 for (const auto& p : pitchValues)
508 {
509 // to pitch value
510 bool wasGreaterP = headPitchJoint->getJointValue() > p;
511 float velocityPitch = wasGreaterP ? -velocity : velocity;
512 kinematicUnitInterfacePrx->setJointVelocities(
513 {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
514 while ((wasGreaterP && headPitchJoint->getJointValue() > p) ||
515 (!wasGreaterP && headPitchJoint->getJointValue() < p))
516 {
517 syncronizeLocalClone();
518 }
519
520 // to yaw value
521 bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
522 float velocityYaw = wasGreaterY ? -velocity : velocity;
523 kinematicUnitInterfacePrx->setJointVelocities(
524 {{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
525 while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) ||
526 (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
527 {
528 syncronizeLocalClone();
529 }
530
531 std::swap(yawFrom, yawTo);
532 }
533 kinematicUnitInterfacePrx->setJointVelocities(
534 {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
535 kinematicUnitInterfacePrx->switchControlMode(
536 {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
537 {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
538 }
539
540 void
541 FrameTracking::scanInWorkspace(const Vector3fSeq& points,
542 float maxVelocity,
543 float acceleration,
544 const Ice::Current&)
545 {
546 if (enabled)
547 {
548 ARMARX_WARNING << "Disable tracking to use scan functions.";
549 return;
550 }
551 syncronizeLocalClone();
552 auto currentModes = kinematicUnitInterfacePrx->getControlModes();
553 kinematicUnitInterfacePrx->switchControlMode(
554 {{headYawJoint->getName(), ControlMode::eVelocityControl},
555 {headPitchJoint->getName(), ControlMode::eVelocityControl}});
556 struct timespec req = {0, 30 * 1000000L};
557 for (const auto& p : points)
558 {
559 auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
560 auto target = _calculateJointAngles(pEigen);
561 while (std::abs(target.currentYawPos - target.desiredYawPos) >
562 static_cast<float>(M_PI / 180.) ||
563 std::abs(target.currentPitchPos - target.desiredPitchPos) >
564 static_cast<float>(M_PI / 180.))
565 {
566 ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos
567 << " pitch: " << target.currentPitchPos << " - "
568 << target.desiredPitchPos;
569 syncronizeLocalClone();
570 target = _calculateJointAngles(pEigen);
571 _doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
572 // sleep for 30 milliseconds
573 nanosleep(&req, nullptr);
574 }
575 }
576 kinematicUnitInterfacePrx->setJointVelocities(
577 {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
578 kinematicUnitInterfacePrx->switchControlMode(
579 {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
580 {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
581 }
582
583 void
584 FrameTracking::process()
585 {
586 if (!localRobot->hasRobotNode(frameName))
587 {
588 ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
589 std::thread([this]() { _enableTracking(false); }).detach();
590 return;
591 }
592 syncronizeLocalClone();
593 _doVelocityControl(_calculateJointAnglesContinously(frameName),
594 maxYawVelocity,
595 yawAcceleration,
596 maxPitchVelocity,
597 pitchAcceleration);
598 }
599
600 void
605
606 void
607 FrameTracking::_lookAtFrame(const std::string& frameName)
608 {
609 auto frame = localRobot->getRobotNode(frameName);
610 auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
611 _lookAtPoint(posInRobotFrame);
612 }
613
614 void
615 FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
616 {
617 _doPositionControl(_calculateJointAngles(point));
618 }
619
620 bool
621 FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
622 {
623 auto head_state = _calculateJointAngles(point);
624 float diff = std::abs(head_state.desiredPitchPos - head_state.currentPitchPos) +
625 std::abs(head_state.currentYawPos - head_state.desiredYawPos);
626 return max_diff > diff;
627 }
628
629 FrameTracking::HeadState
630 FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
631 {
632 auto frame = localRobot->getRobotNode(frameName);
633 auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
634 // do nothing if the robot works above his head
635 // he should already look upwards because if this component runs continously
636 if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() +
637 posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
638 {
639 return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
640 }
641 return _calculateJointAngles(posInRobotFrame);
642 }
643
644 FrameTracking::HeadState
645 FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
646 {
647 float yaw = -std::atan2(point.x(), point.y());
648 // make shure the joint value satisfies the joint limits
649 yaw = std::max(headYawJoint->getJointLimitLo(), yaw);
650 yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
651 // we dont want the robot to move from one limit to the other in one step
652 const float currentYaw = headYawJoint->getJointValue();
653 if (!headYawJoint->isLimitless() &&
654 std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() -
655 headYawJoint->getJointLimitLo() -
656 static_cast<float>(M_PI) / 8)
657 {
658 yaw = currentYaw;
659 }
660
661 const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(
662 localRobot->toGlobalCoordinateSystemVec(point));
663 const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
664 const float headHeightRealativeToPitchJoint =
665 headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
666 float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) +
667 std::asin(headHeightRealativeToPitchJoint / pj.norm());
668 // make shure the joint value satisfies the joint limits
669 pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
670 pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
671 const float currentPitch = headPitchJoint->getJointValue();
672
673 ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point
674 << " using yaw=" << yaw << " and pitch=" << pitch;
675
676 const float currentYawVel =
677 DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
678 "jointvelocities", headYawJoint->getName()))
679 ->getFloat();
680 const float currentPitchVel =
681 DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
682 "jointvelocities", headPitchJoint->getName()))
683 ->getFloat();
684
685 FrameTracking::HeadState headState;
686 headState.currentYawPos = currentYaw;
687 headState.desiredYawPos = yaw;
688 headState.currentYawVel = currentYawVel;
689 headState.currentPitchPos = currentPitch;
690 headState.desiredPitchPos = pitch;
691 headState.currentPitchVel = currentPitchVel;
692 return headState;
693 }
694
695 void
696 FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState,
697 float maxYawVelocity,
698 float yawAcceleration,
699 float maxPitchVelocity,
700 float pitchAcceleration)
701 {
702 if (headState.stop)
703 {
704 kinematicUnitInterfacePrx->setJointVelocities(
705 {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
706 return;
707 }
708
709 float desiredYawVelocity =
710 positionThroughVelocityControlWithAccelerationBounds(30.f / 1000,
711 35.f / 1000,
712 headState.currentYawVel,
713 maxYawVelocity,
714 yawAcceleration,
715 yawAcceleration,
716 headState.currentYawPos,
717 headState.desiredYawPos,
718 1.f);
719 float desiredPitchVelocity =
720 positionThroughVelocityControlWithAccelerationBounds(30.f / 1000,
721 35.f / 1000,
722 headState.currentPitchVel,
723 maxPitchVelocity,
724 pitchAcceleration,
725 pitchAcceleration,
726 headState.currentPitchPos,
727 headState.desiredPitchPos,
728 1.f);
729
730 // control mode is set when enable task
731 kinematicUnitInterfacePrx->setJointVelocities(
732 {{headYawJoint->getName(), desiredYawVelocity},
733 {headPitchJoint->getName(), desiredPitchVelocity}});
734 }
735
736 void
737 FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
738 {
739 auto currentModes = kinematicUnitInterfacePrx->getControlModes();
740 kinematicUnitInterfacePrx->switchControlMode(
741 {{headYawJoint->getName(), ControlMode::ePositionControl},
742 {headPitchJoint->getName(), ControlMode::ePositionControl}});
743 if (headState.stop)
744 {
745 return;
746 }
747 kinematicUnitInterfacePrx->setJointAngles(
748 {{headYawJoint->getName(), headState.desiredYawPos},
749 {headPitchJoint->getName(), headState.desiredPitchPos}});
750 kinematicUnitInterfacePrx->switchControlMode(
751 {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
752 {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
753 }
754
755 void
756 FrameTracking::_enableTracking(bool enable)
757 {
758 if (this->enabled == enable)
759 {
760 return;
761 }
762 this->enabled = enable;
763 if (enable)
764 {
765 kinematicUnitInterfacePrx->switchControlMode(
766 {{headYawJoint->getName(), ControlMode::eVelocityControl},
767 {headPitchJoint->getName(), ControlMode::eVelocityControl}});
768 processorTask->start();
769 }
770 else
771 {
772 kinematicUnitInterfacePrx->setJointVelocities(
773 {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
774 processorTask->stop();
775 }
776 }
777
778 ARMARX_REGISTER_COMPONENT_EXECUTABLE(FrameTracking, "FrameTracking");
779} // namespace armarx
ARMARX_REGISTER_COMPONENT_EXECUTABLE
#define ARMARX_REGISTER_COMPONENT_EXECUTABLE(ComponentT, applicationName)
Definition Decoupled.h:29
M_PI
#define M_PI
Definition MathTools.h:17
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition Component.cpp:90
armarx::Component::getProperty
Property< PropertyType > getProperty(const std::string &name)
Definition PropertyUser.h:180
armarx::FrameTracking
Brief description of class FrameTracking.
Definition FrameTracking.h:111
armarx::FrameTracking::onInitComponent
void onInitComponent() override
Definition FrameTracking.cpp:40
armarx::FrameTracking::setFrame
void setFrame(const std::string &frameName, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:360
armarx::FrameTracking::lookAtFrame
void lookAtFrame(const std::string &frameName, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:377
armarx::FrameTracking::localRobot
VirtualRobot::RobotPtr localRobot
Definition FrameTracking.h:236
armarx::FrameTracking::onDisconnectComponent
void onDisconnectComponent() override
Definition FrameTracking.cpp:331
armarx::FrameTracking::robotStateComponent
RobotStateComponentInterfacePrx robotStateComponent
Definition FrameTracking.h:224
armarx::FrameTracking::_enableTracking
void _enableTracking(bool enable)
Definition FrameTracking.cpp:756
armarx::FrameTracking::enableTracking
void enableTracking(bool enable, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:354
armarx::FrameTracking::scanInWorkspace
void scanInWorkspace(const ::armarx::Vector3fSeq &points, float maxVelocity, float acceleration, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:541
armarx::FrameTracking::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition FrameTracking.cpp:347
armarx::FrameTracking::lookAtPointInGlobalFrame
void lookAtPointInGlobalFrame(const Vector3f &point, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:394
armarx::FrameTracking::scanInConfigurationSpace
void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq &pitchValues, float velocity, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:473
armarx::FrameTracking::_guiTab
RemoteGui::TabProxy _guiTab
Definition FrameTracking.h:243
armarx::FrameTracking::_doPositionControl
void _doPositionControl(const HeadState &headstate)
Definition FrameTracking.cpp:737
armarx::FrameTracking::getFrame
std::string getFrame(const Ice::Current &=Ice::emptyCurrent) const override
Definition FrameTracking.cpp:371
armarx::FrameTracking::enabled
std::atomic< bool > enabled
Definition FrameTracking.h:229
armarx::FrameTracking::_calculateJointAnglesContinously
HeadState _calculateJointAnglesContinously(const std::string &frame)
Definition FrameTracking.cpp:630
armarx::FrameTracking::isLookingAtPointInGlobalFrame
bool isLookingAtPointInGlobalFrame(const Vector3f &point, float max_diff, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:406
armarx::FrameTracking::_looksAtPoint
bool _looksAtPoint(const Eigen::Vector3f &point, float max_diff)
Definition FrameTracking.cpp:621
armarx::FrameTracking::lookAtPointInRobotFrame
void lookAtPointInRobotFrame(const Vector3f &point, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:420
armarx::FrameTracking::onConnectComponent
void onConnectComponent() override
Definition FrameTracking.cpp:61
armarx::FrameTracking::headYawJoint
VirtualRobot::RobotNodePtr headYawJoint
Definition FrameTracking.h:237
armarx::FrameTracking::kinematicUnitObserverInterfacePrx
KinematicUnitObserverInterfacePrx kinematicUnitObserverInterfacePrx
Definition FrameTracking.h:227
armarx::FrameTracking::_lookAtFrame
void _lookAtFrame(const std::string &frame)
Definition FrameTracking.cpp:607
armarx::FrameTracking::cameraNode
VirtualRobot::RobotNodePtr cameraNode
Definition FrameTracking.h:239
armarx::FrameTracking::_remoteGui
RemoteGuiInterfacePrx _remoteGui
Definition FrameTracking.h:241
armarx::FrameTracking::_guiTask
SimplePeriodicTask ::pointer_type _guiTask
Definition FrameTracking.h:242
armarx::FrameTracking::_doVelocityControl
void _doVelocityControl(const HeadState &headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
Definition FrameTracking.cpp:696
armarx::FrameTracking::onExitComponent
void onExitComponent() override
Definition FrameTracking.cpp:342
armarx::FrameTracking::processorTask
PeriodicTask< FrameTracking >::pointer_type processorTask
Definition FrameTracking.h:226
armarx::FrameTracking::_lookAtPoint
void _lookAtPoint(const Eigen::Vector3f &point)
Definition FrameTracking.cpp:615
armarx::FrameTracking::_calculateJointAngles
HeadState _calculateJointAngles(const Eigen::Vector3f &point)
Definition FrameTracking.cpp:645
armarx::FrameTracking::headPitchJoint
VirtualRobot::RobotNodePtr headPitchJoint
Definition FrameTracking.h:238
armarx::FrameTracking::kinematicUnitInterfacePrx
KinematicUnitInterfacePrx kinematicUnitInterfacePrx
Definition FrameTracking.h:225
armarx::FrameTracking::moveJointsTo
void moveJointsTo(float yaw, float pitch, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:432
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::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::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition ManagedIceObject.cpp:108
armarx::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition ManagedIceObject.cpp:407
armarx::ManagedIceObject::getArmarXManager
ArmarXManagerPtr getArmarXManager() const
Returns the ArmarX manager used to add and remove components.
Definition ManagedIceObject.cpp:360
armarx::PeriodicTask
The periodic task executes one thread method repeatedly using the time period specified in the constr...
Definition PeriodicTask.h:56
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition RemoteRobot.cpp:381
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition RemoteRobot.cpp:522
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
ARMARX_WARNING
#define ARMARX_WARNING
The logging level for unexpected behaviour, but not a serious problem.
Definition Logging.h:193
armarx::RemoteGui::makeCheckBox
detail::CheckBoxBuilder makeCheckBox(std::string const &name)
Definition BoolWidgets.h:27
armarx::RemoteGui::makeComboBox
detail::ComboBoxBuilder makeComboBox(std::string const &name)
Definition StringWidgets.h:61
armarx::RemoteGui::makeVBoxLayout
detail::VBoxLayoutBuilder makeVBoxLayout(std::string const &name="")
Definition LayoutWidgets.h:286
armarx::RemoteGui::makeFloatSpinBox
detail::FloatSpinBoxBuilder makeFloatSpinBox(std::string const &name)
Definition FloatWidgets.h:58
armarx::RemoteGui::makeTextLabel
detail::LabelBuilder makeTextLabel(std::string const &text)
Definition StaticWidgets.h:24
armarx::RemoteGui::makeHBoxLayout
detail::HBoxLayoutBuilder makeHBoxLayout(std::string const &name="")
Definition LayoutWidgets.h:268
armarx::RemoteGui::makeButton
detail::ButtonBuilder makeButton(std::string const &name)
Definition IntegerWidgets.h:52
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition forward_declarations.h:35
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition VectorHelpers.h:297
armarx::SimplePeriodicTask
SimplePeriodicTask(Ts...) -> SimplePeriodicTask< std::function< void(void)> >
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition VectorHelpers.h:327
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::RemoteGui::detail::ChildrenMixin::addChild
Derived & addChild(WidgetPtr const &child)
Definition LayoutWidgets.h:27