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
35namespace 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
343
344 armarx::PropertyDefinitionsPtr
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
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
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::onInitComponent
void onInitComponent() override
Definition FrameTracking.cpp:38
armarx::FrameTracking::setFrame
void setFrame(const std::string &frameName, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:358
armarx::FrameTracking::lookAtFrame
void lookAtFrame(const std::string &frameName, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:375
armarx::FrameTracking::localRobot
VirtualRobot::RobotPtr localRobot
Definition FrameTracking.h:236
armarx::FrameTracking::onDisconnectComponent
void onDisconnectComponent() override
Definition FrameTracking.cpp:329
armarx::FrameTracking::robotStateComponent
RobotStateComponentInterfacePrx robotStateComponent
Definition FrameTracking.h:224
armarx::FrameTracking::_enableTracking
void _enableTracking(bool enable)
Definition FrameTracking.cpp:754
armarx::FrameTracking::enableTracking
void enableTracking(bool enable, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:352
armarx::FrameTracking::scanInWorkspace
void scanInWorkspace(const ::armarx::Vector3fSeq &points, float maxVelocity, float acceleration, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:539
armarx::FrameTracking::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition FrameTracking.cpp:345
armarx::FrameTracking::lookAtPointInGlobalFrame
void lookAtPointInGlobalFrame(const Vector3f &point, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:392
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::FrameTracking::_guiTab
RemoteGui::TabProxy _guiTab
Definition FrameTracking.h:243
armarx::FrameTracking::_doPositionControl
void _doPositionControl(const HeadState &headstate)
Definition FrameTracking.cpp:735
armarx::FrameTracking::getFrame
std::string getFrame(const Ice::Current &=Ice::emptyCurrent) const override
Definition FrameTracking.cpp:369
armarx::FrameTracking::enabled
std::atomic< bool > enabled
Definition FrameTracking.h:229
armarx::FrameTracking::_calculateJointAnglesContinously
HeadState _calculateJointAnglesContinously(const std::string &frame)
Definition FrameTracking.cpp:628
armarx::FrameTracking::isLookingAtPointInGlobalFrame
bool isLookingAtPointInGlobalFrame(const Vector3f &point, float max_diff, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:404
armarx::FrameTracking::_looksAtPoint
bool _looksAtPoint(const Eigen::Vector3f &point, float max_diff)
Definition FrameTracking.cpp:619
armarx::FrameTracking::lookAtPointInRobotFrame
void lookAtPointInRobotFrame(const Vector3f &point, const Ice::Current &=Ice::emptyCurrent) override
Definition FrameTracking.cpp:418
armarx::FrameTracking::onConnectComponent
void onConnectComponent() override
Definition FrameTracking.cpp:59
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:605
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:694
armarx::FrameTracking::onExitComponent
void onExitComponent() override
Definition FrameTracking.cpp:340
armarx::FrameTracking::processorTask
PeriodicTask< FrameTracking >::pointer_type processorTask
Definition FrameTracking.h:226
armarx::FrameTracking::_lookAtPoint
void _lookAtPoint(const Eigen::Vector3f &point)
Definition FrameTracking.cpp:613
armarx::FrameTracking::_calculateJointAngles
HeadState _calculateJointAngles(const Eigen::Vector3f &point)
Definition FrameTracking.cpp:643
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:430
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