Component.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 control::ArmarXObjects::controller_creator
17 * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
18 * @date 2023
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22
23
24#include "Component.h"
25
26#include <Eigen/Core>
27
28#include <IceUtil/Time.h>
29
30#include <VirtualRobot/MathTools.h>
31#include <VirtualRobot/Nodes/RobotNode.h>
32#include <VirtualRobot/RobotNodeSet.h>
33
34// #include <SimoxUtility/color/Color.h>
35#include "ArmarXCore/core/time/Metronome.h"
36#include <ArmarXCore/core/logging/Logging.h>
37#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
38#include <ArmarXCore/core/time/Clock.h>
39#include <ArmarXCore/core/time/TimeUtil.h>
40#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
41// for kinesthetic teaching
42#include <ArmarXCore/core/csv/CsvWriter.h>
43
44#include <ArmarXGui/interface/StaticPlotterInterface.h>
45
46#include <RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.h>
47#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
48#include <RobotAPI/libraries/armem_robot_state/types.h>
49#include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
50#include <RobotAPI/libraries/core/math/TimeSeriesUtils.h>
51
52#include <armarx/control/client/ControllerBuilder.h>
53#include <armarx/control/common/aron_conversions.h>
54#include <armarx/control/common/ft/FTSensorTrigger.h>
55#include <armarx/control/common/utils.h>
56#include <armarx/control/njoint_mp_controller/controller_descriptions.h>
57
58//#include <armarx/control/njoint_mp_controller/task_space/aron/aron_conversions.h>
59#include <armarx/control/common/control_law/aron/CollisionAvoidanceControllerConfig.aron.generated.h>
60
61namespace armarx::control::components::controller_creator
62{
63
64 const std::string Component::defaultName = "controller_creator";
65
66 Component::Component()
67 {
68 addPlugin(virtualRobotReader_);
69 }
70
71 Component::~Component() = default;
72
73 armarx::PropertyDefinitionsPtr
74 Component::createPropertyDefinitions()
75 {
76 armarx::PropertyDefinitionsPtr def =
77 new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
78
79 def->optional(properties.robotName, "robotName");
80
81 def->optional(properties.defaultRobotNodeSetLeft,
82 "default_robotNodeSet_left",
83 "default kinematic chain name of left arm.");
84 def->optional(properties.defaultRobotNodeSetRight,
85 "default_robotNodeSet_right",
86 "default kinematic chain name of right arm.");
87
88 const std::string defaultName = "";
89 def->component(m_robot_unit, defaultName);
90 //def->component(m_force_torque_observer, properties.ftObserverName);
91 // Publish to a topic (passing the TopicListenerPrx).
92 // def->topic(myTopicListener);
93
94 // Subscribe to a topic (passing the topic name).
95 // def->topic<PlatformUnitListener>("MyTopic");
96
97 // Use (and depend on) another component (passing the ComponentInterfacePrx).
98 // def->component(myComponentProxy)
99
100
101 // Add a required property. (The component won't start without a value being set.)
102 // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
103
104 // Add an optional property.
105 // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
106 // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
107
108 return def;
109 }
110
111 void
112 Component::onInitComponent()
113 {
114 // Topics and properties defined above are automagically registered.
115
116 // Keep debug observer data until calling `sendDebugObserverBatch()`.
117 // (Requires the armarx::DebugObserverComponentPluginUser.)
118 // setDebugObserverBatchModeEnabled(true);
119 }
120
121 void
122 Component::onConnectComponent()
123 {
124 // Do things after connecting to topics and components.
125 ARMARX_CHECK_NOT_NULL(m_robot_unit);
126
127 /* (Requires the armarx::DebugObserverComponentPluginUser.)
128 // Use the debug observer to log data over time.
129 // The data can be viewed in the ObserverView and the LivePlotter.
130 // (Before starting any threads, we don't need to lock mutexes.)
131 {
132 setDebugObserverDatafield("numBoxes", properties.numBoxes);
133 setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
134 sendDebugObserverBatch();
135 }
136 */
137
138 /* (Requires the armarx::ArVizComponentPluginUser.)
139 // Draw boxes in ArViz.
140 // (Before starting any threads, we don't need to lock mutexes.)
141 drawBoxes(properties, arviz);
142 */
143
144 /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
145 // Setup the remote GUI.
146 {
147 createRemoteGuiTab();
148 RemoteGui_startRunningTask();
149 }
150 */
151 ARMARX_INFO << properties.robotName;
152 // m_robot = m_virtualRobotReaderPlugin->get().getRobot(properties.robotName);
153 if (!m_robot)
154 {
155 // m_robot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);
156 m_robot = addRobot("controller_creator_robot", VirtualRobot::RobotIO::eStructure);
157 }
158 ARMARX_CHECK_NOT_NULL(m_robot);
159 ARMARX_CHECK(synchronizeLocalClone(m_robot));
160
161 m_rns_l = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetLeft);
162 m_rns_r = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetRight);
163
164 //m_ft_l.reset(new armarx::control::common::ft::ForceTorqueUtility(m_force_torque_observer, properties.ftSensorNameLeft));
165 //m_ft_r.reset(new armarx::control::common::ft::ForceTorqueUtility(m_force_torque_observer, properties.ftSensorNameRight));
166
167
168 /* m_trigger =trigger::FTWristSensorTrigger leftTrigger(leftWristSensor);
169 // FT sensor handling
170 */
171 //trigger::FTWristSensorTrigger rightTrigger(rightWristSensor);
172
173 // RobotUnitInterfacePrx robotUnit = getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit();
174 // robotUnit->loadLibFromPackage("armarx_control", "libarmarx_control_njoint_controller");
175
176 ARMARX_IMPORTANT << "controller creator is ready";
177
178 // TODO add load all default config to memory
179 // armarx::control::common::ControllerTypeNames.to_name()
180 // getControlComponentPlugin().configMemoryWriter().storeDefaultConfig(controlTypeStr, AronDict, name);
181 }
182
183 void
187
188 void
192
193 /*
194 // std::string
195 // Component::createBiKAC(const std::string& namePrefix,
196 // const ::armarx::aron::data::dto::DictPtr& configDict,
197 // bool activate,
198 // bool allowReuse)
199 // {
200 // /// create controllers with controller builder
201 // auto builder = controllerBuilder<control::common::ControllerType::BiKAC>();
202 // builder.withConfig(configDict).allowReuse(allowReuse).daemonize();
203
204 // std::ostringstream oss;
205 // for (const auto& pair : builder.config().cfg)
206 // {
207 // oss << pair.first;
208 // if (&pair != &(*builder.config().cfg.rbegin()))
209 // {
210 // oss << ",";
211 // }
212 // }
213 // std::string robotNodeSets = oss.str();
214
215 // if (namePrefix.empty())
216 // {
217 // builder.withNamePrefix(robotNodeSets);
218 // }
219 // else
220 // {
221 // builder.allowReuse(true).withNamePrefix(namePrefix + "_" + robotNodeSets);
222 // }
223
224 // ARMARX_TRACE;
225 // ARMARX_INFO << "creating controller";
226
227 // auto ctrlWrapper = builder.create();
228
229 // ARMARX_TRACE;
230 // ARMARX_CHECK(ctrlWrapper);
231
232 // const std::string controllerName = builder.getControllerName();
233 // ARMARX_INFO << "using controller name " << controllerName;
234 // ARMARX_TRACE;
235
236 // Clock::WaitFor(Duration::MilliSeconds(300));
237 // if (allowReuse)
238 // {
239 // ARMARX_INFO << "setting nullspace target: if the default is null use the cfg from the "
240 // "existing controller";
241 // auto ctrlConfig = ctrlWrapper->getConfig().cfg;
242 // for (const auto& pair : builder.config().cfg)
243 // {
244 // if (not pair.second.desiredNullspaceJointAngles.has_value())
245 // {
246 // ctrlWrapper->config.cfg.at(pair.first).desiredNullspaceJointAngles =
247 // ctrlConfig.at(pair.first).desiredNullspaceJointAngles;
248 // }
249 // ARMARX_INFO << "Activating controller ..."
250 // << ctrlWrapper->config.cfg.at(pair.first)
251 // .desiredNullspaceJointAngles.has_value();
252 // }
253 // }
254
255 // ARMARX_TRACE;
256 // ctrlWrapper->updateConfig();
257 // if (activate)
258 // {
259 // ctrlWrapper->activate();
260 // ARMARX_TRACE;
261 // while (not ctrlWrapper->isActive())
262 // {
263 // Clock::WaitFor(Duration::MilliSeconds(10));
264 // }
265 // ARMARX_INFO << "Controller " << controllerName << " is active";
266 // }
267 // else
268 // {
269 // ARMARX_INFO << "User have to activate " << controllerName
270 // << " controller in your application";
271 // }
272 // return controllerName;
273 // }
274
275 // void
276 // Component::createKVILImpedanceMPController(
277 // const std::string& controllerNamePrefix,
278 // const std::string& robotNodeSet,
279 // const std::string& configFilename,
280 // bool allowReuse
281 //
282 // // nlohmann::json& cfg,
283 // // const std::string& skill,
284 // // const std::string& kinematic_chain,
285 // // const std::string& control_config_file)
286 // {
287 // /// create controllers with controller builder
288 // auto builder = controllerBuilder<control::common::ControllerType::KVILImpedanceMP>();
289 // builder.withNodeSet(robotNodeSet).withNamePrefix(controllerNamePrefix);
290 //
291 // /// load predefined config from file
292 // {
293 // if (configFilename.empty())
294 // {
295 // const std::string controllerClassName =
296 // control::common::ControllerTypeNames.to_name(control::common::ControllerType::TSMPImp);
297 // const armarx::PackagePath configPath("armarx_control", "controller_config/" + controllerClassName + "/default.json");
298 // ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`.";
299 // ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath()));
300 //
301 // builder.withConfig(configPath.toSystemPath());
302 // }
303 // else
304 // {
305 // builder.withConfig(configFilename);
306 // }
307 //
308 //
309 // /// to be removed
310 // ARMARX_INFO << VAROUT(builder.config().kpImpedance);
311 // ARMARX_INFO << VAROUT(builder.config().kdImpedance);
312 // ARMARX_INFO << VAROUT(builder.config().kpNullspaceTorque);
313 // ARMARX_INFO << VAROUT(builder.config().kdNullspaceTorque);
314 // ARMARX_INFO << VAROUT(builder.config().desiredPose);
315 // ARMARX_INFO << VAROUT(builder.config().desiredTwist);
316 // if (builder.config().desiredNullspaceJointAngles.has_value())
317 // ARMARX_INFO << VAROUT(builder.config().desiredNullspaceJointAngles.value());
318 // ARMARX_INFO << VAROUT(builder.config().torqueLimit);
319 // ARMARX_INFO << VAROUT(builder.config().qvelFilter);
320 // }
321 //
322 // NJointControllerInterfacePrx controller = m_robot_unit->getNJointController(ctrl_name);
323 // bool createNewController = true;
324 // if (controller)
325 // {
326 // ARMARX_INFO << "controller exists, check config ...";
327 // m_kvil_imp_mp_ctrl = NJointKVILImpedanceMPControllerInterfacePrx::checkedCast(controller);
328 //
329 // if (m_kvil_imp_mp_ctrl->getKinematicChainName() == kinematic_chain)
330 // {
331 // ARMARX_INFO << "requested controller already exists, reuse the current one";
332 // createNewController = false;
333 // }
334 // else
335 // {
336 // if (controller->isControllerActive())
337 // {
338 // controller->deactivateController();
339 // while (controller->isControllerActive())
340 // {
341 // TimeUtil::SleepMS(10);
342 // }
343 // }
344 // controller->deleteController();
345 // TimeUtil::SleepMS(10);
346 // }
347 // }
348 // else
349 // {
350 // ARMARX_INFO << "requested controller doesn't exist, create a new one";
351 // }
352 //
353 //
354 // /// -------------------- acquire controller configurations from json file --------------------
355 // m_rns = m_robot->getRobotNodeSet(kinematic_chain);
356 // int n_joints = (int)m_rns->getJointValues().size();
357 // ARMARX_INFO << VAROUT(n_joints);
358 //
359 // /// -------------------- create new controller or get the existing controller and update the setting --------------------
360 // if (createNewController)
361 // {
362 // ARMARX_IMPORTANT << "Initialize ts admittance controllers for " << skill << " with kinematic chain " << kinematic_chain << " (dim = " << n_joints << ").";
363 // NJointTaskspaceControllerConfigPtr ctrl_cfg = new NJointTaskspaceControllerConfig;
364 // ARMARX_INFO << "start to init config";
365 // ctrl_cfg->nodeSetName = kinematic_chain;
366 // ctrl_cfg->controlParamJsonFile = control_config_file;
367 //
368 // m_kvil_imp_mp_ctrl = NJointKVILImpedanceMPControllerInterfacePrx::checkedCast(
369 // m_robot_unit->createNJointController(
370 // ctrl_class_name,
371 // ctrl_name,
372 // ctrl_cfg));
373 // }
374 // else
375 // {
376 // ARMARX_IMPORTANT << "Use existing controller " << ctrl_name << " for skill " << skill;
377 // m_kvil_imp_mp_ctrl->reconfigureController(control_config_file);
378 //
379 // }
380 // ARMARX_CHECK_NOT_NULL(m_kvil_imp_mp_ctrl);
381 //
382 // m_kvil_imp_mp_ctrl->learnFromCSV(std::vector<std::string>());
383 //
384 // /// -------------------- Activate RT Controller --------------------
385 // p2CStruct p2c_struct;
386 // p2c_struct.curve_kpt_position.setZero();
387 // curve_kpt_position_buffer.reinitAllBuffers(p2c_struct);
388 //
389 // curveInfoStruct curve_info_struct;
390 // curve_info_struct.curve_proj_point.setZero();
391 // curve_info_struct.curve_proj_value = 0.0f;
392 // curve_info_struct.curve_vec.setZero();
393 // curve_proj_info_buffer.reinitAllBuffers(curve_info_struct);
394 //
395 // m_kvil_imp_mp_ctrl->activateController();
396 // usleep(100000);
397 // m_kvil_imp_mp_ctrl->start("all", std::vector<double>(), std::vector<double>(), -1);
398 // while (true)
399 // {
400 // float current_canonical_value = m_kvil_imp_mp_ctrl->getCanVal("null");
401 // if (current_canonical_value < 0.999)
402 // {
403 // break;
404 // }
405 // }
406 // task_running.store(true);
407 //
408 //
409 // /// Wait after the controller has finished until either the goal has been reached or until a timeout is reached
410 // CycleUtil c(1);
411 // auto after_controller_startTime = TimeUtil::GetTime();
412 // // std::vector<float> kpt_position {0, 0, 0};
413 // // Eigen::Vector3f proj_point;
414 // // proj_point.setZero();
415 // // Eigen::Vector3f curve_vec;
416 // // curve_vec.setZero();
417 // ARMARX_INFO << "Starting loop";
418 // kpt_controller_running.store(true);
419 // while ((TimeUtil::GetTime() - after_controller_startTime).toSecondsDouble() < 20.0)
420 // {
421 // float current_canonical_value = static_cast<float>(m_kvil_imp_mp_ctrl->getCanVal("null"));
422 // ARMARX_INFO << VAROUT(current_canonical_value);
423 // curve_kpt_position_buffer.getWriteBuffer().curve_kpt_position = m_kvil_imp_mp_ctrl->getCurveKeypointPosition();
424 // curve_kpt_position_buffer.commitWrite();
425 //
426 // auto curve_info = curve_proj_info_buffer.getUpToDateReadBuffer();
427 // m_kvil_imp_mp_ctrl->setCurveProjection(curve_info.curve_proj_point, curve_info.curve_proj_value, curve_info.curve_vec);
428 //
429 // c.waitForCycleDuration();
430 // }
431 // kpt_controller_running.store(false);
432 // if (stop_with_mp)
433 // {
434 // enableVelocityMode();
435 // m_ts_adm_mp_ctrl->stop("default");
436 // ARMARX_INFO << "deativate controller";
437 // m_ts_adm_mp_ctrl->deactivateController();
438 // task_running.store(false);
439 // }
440 //
441 // // CycleUtil c(1);
442 // // while (!CALL_INTERUPTED)
443 // // {
444 // // c.waitForCycleDuration();
445 // // }
446 //
447 // // ARMARX_INFO << "interupted ... (deativate controller)";
448 // // enableVelocityMode();
449 // // m_kvil_imp_mp_ctrl->deactivateController();
450 // // task_running.store(false);
451 // ARMARX_IMPORTANT << "Finished Execution in MotionControl component";
452 // }
453 */
454
455 template <control::common::ControllerType T>
456 std::string
457 Component::createComplianceController(const std::string& namePrefix,
458 const aron::data::dto::DictPtr& configDict,
459 const std::string& configFilename,
460 bool flagActivate,
461 bool flagAllowReuse,
462 bool flagFromMemory)
463 {
464 auto builder = controllerBuilder<T>();
465 auto ctrlWrapper = builder.createTSComplianceCtrl(namePrefix, configDict);
466 const std::string controllerName = builder.getControllerName();
467 return controllerName;
468 }
469
470 std::string
471 Component::createController(const std::string& namePrefix,
472 const std::string& controllerType,
473 const ::armarx::aron::data::dto::DictPtr& configDict,
474 const std::string& configFilename,
475 bool activate,
476 bool allowReuse,
477 bool cfgFromMemory,
478 const Ice::Current&)
479 {
480 ARMARX_INFO << "creating controller of type " << controllerType;
481
482 std::string controllerName;
483
484 using T = control::common::ControllerType;
485
486 const auto type = control::common::ControllerTypeShort.from_name(controllerType);
487
488 switch (type)
489 {
490 /// ------------------------------------ controllers -----------------------------------
491 case T::TSAdm:
492 controllerName = createComplianceController<T::TSAdm>(
493 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
494 break;
495 case T::TSImp:
496 controllerName = createComplianceController<T::TSImp>(
497 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
498 break;
499 case T::TSImpCol:
500 controllerName = createComplianceController<T::TSImpCol>(
501 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
502 break;
503 // case T::TSImpObjCol:
504 // controllerName = createComplianceController<T::TSImpObjCol>(
505 // namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
506 // break;
507 case T::TSMixImpVelCol:
508 controllerName = createComplianceController<T::TSMixImpVelCol>(
509 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
510 break;
511 case T::TSMixImpVel:
512 controllerName = createComplianceController<T::TSMixImpVel>(
513 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
514 break;
515 case T::TSVel:
516 controllerName = createComplianceController<T::TSVel>(
517 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
518 break;
519 case T::TSVelCol:
520 controllerName = createComplianceController<T::TSVelCol>(
521 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
522 break;
523 case T::TSVeloCol:
524 controllerName = createComplianceController<T::TSVeloCol>(
525 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
526 break;
527 /// ----------------------------------- MP controllers ---------------------------------
528 case T::TSMPAdm:
529 controllerName = createComplianceController<T::TSMPAdm>(
530 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
531 break;
532 case T::TSMPImp:
533 controllerName = createComplianceController<T::TSMPImp>(
534 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
535 break;
536 case T::TSMPImpCol:
537 controllerName = createComplianceController<T::TSMPImpCol>(
538 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
539 break;
540 // case T::TSMPImpObjCol:
541 // controllerName = createComplianceController<T::TSMPImpObjCol>(
542 // namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
543 // break;
544 case T::TSMPMixImpVel:
545 controllerName = createComplianceController<T::TSMPMixImpVel>(
546 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
547 break;
548 case T::TSMPVel:
549 controllerName = createComplianceController<T::TSMPVel>(
550 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
551 break;
552 case T::TSMPVelCol:
553 controllerName = createComplianceController<T::TSMPVelCol>(
554 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
555 break;
556 case T::TSMPVeloCol:
557 controllerName = createComplianceController<T::TSMPVeloCol>(
558 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
559 break;
560 case T::TSMPMixImpVelCol:
561 controllerName = createComplianceController<T::TSMPMixImpVelCol>(
562 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
563 break;
564 case T::TSMPMixImpVelColWiping:
565 controllerName = createComplianceController<T::TSMPMixImpVelColWiping>(
566 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
567 break;
568 case T::TSMPVelWiping:
569 controllerName = createComplianceController<T::TSMPVelWiping>(
570 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
571 break;
572 // case T::BiKAC:
573 // controllerName = createBiKAC(namePrefix, configDict, activate, allowReuse);
574 // break;
575
576 case T::TSMPImpWiping:
577 controllerName = createComplianceController<T::TSMPImpWiping>(
578 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
579 break;
580 // case T::BiKAC:
581 // controllerName = createBiKAC(namePrefix, configDict, activate, allowReuse);
582 // break;
583 case T::ZMQTSImp:
584 controllerName = createComplianceController<T::ZMQTSImp>(
585 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
586 break;
587 case T::ZMQTSImpCol:
588 controllerName = createComplianceController<T::ZMQTSImpCol>(
589 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
590 break;
591 case T::ZMQTSMixImpVelCol:
592 controllerName = createComplianceController<T::ZMQTSMixImpVelCol>(
593 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
594 break;
595 case T::ZMQTSMixImpVel:
596 controllerName = createComplianceController<T::ZMQTSMixImpVel>(
597 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
598 break;
599 case T::ZMQTSVel:
600 controllerName = createComplianceController<T::ZMQTSVel>(
601 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
602 break;
603
604 /// zenoh controller
605 case T::ZenohTSImp:
606 controllerName = createComplianceController<T::ZenohTSImp>(
607 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
608 break;
609 case T::ZenohTSImpCol:
610 controllerName = createComplianceController<T::ZenohTSImpCol>(
611 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
612 break;
613 case T::ZenohTSMixImpVelCol:
614 controllerName = createComplianceController<T::ZenohTSMixImpVelCol>(
615 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
616 break;
617 case T::ZenohTSMixImpVel:
618 controllerName = createComplianceController<T::ZenohTSMixImpVel>(
619 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
620 break;
621 case T::ZenohTSVel:
622 controllerName = createComplianceController<T::ZenohTSVel>(
623 namePrefix, configDict, configFilename, activate, allowReuse, cfgFromMemory);
624 break;
625 default:
626 ARMARX_ERROR << "controller type: " << controllerType << " is not supported yet";
627 }
628 return controllerName;
629 }
630
631 void
632 Component::createZeroTorqueController(const std::string& kinematicChainName)
633 {
634 const float motionThresholdRadian = 0.1f;
635 const float noMotionTimeoutMs = 2000.0f;
636
637 /// ---------------------------------- creating zero torque controller ----------------------------------
638 ARMARX_INFO << "Kinesthetic teaching: Initialize zero torque controllers for "
639 << VAROUT(kinematicChainName);
640 const std::string ctrlClassName = "NJointZeroTorqueController";
641 const std::string ctrl_name = ctrlClassName + "_" + kinematicChainName;
642
643 armem::robot_state::RobotReader::Hand hand = armem::robot_state::RobotReader::Hand::Left;
644 ARMARX_IMPORTANT << "THIS kinematicChainName is " << kinematicChainName;
645 if (kinematicChainName == "RightArm" || kinematicChainName == "RightArmWithoutWrist")
646 {
647 hand = armem::robot_state::RobotReader::Hand::Right;
648 }
649
650 auto& robotReader = virtualRobotReader_->get();
651 ARMARX_INFO << "robot reader is ready";
652 // ARMARX_CHECK_NOT_NULL(robotReader);
653 armarx::control::common::ft::FTSensorTrigger wristSensorTrigger(
654 robotReader, properties.robotName, hand);
655 /*armarx::kinesthetic_learning::kinesthetic_teaching::trigger::FTWristSensorTrigger m_trigger(
656 wristSensor);*/
657
658 ARMARX_INFO << "wrist trigger has been created";
659
660 NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
661 config->maxTorque = 0;
662
663 auto nodeSet = m_robot->getRobotNodeSet(kinematicChainName);
664 for (auto& name : nodeSet->getNodeNames())
665 {
666 config->jointNames.push_back(name);
667 }
668 NJointZeroTorqueControllerInterfacePrx zeroTorqueController =
669 NJointZeroTorqueControllerInterfacePrx::checkedCast(
670 m_robot_unit->createNJointController(ctrlClassName, ctrl_name, config));
671 ARMARX_CHECK_NOT_NULL(zeroTorqueController);
672 ARMARX_INFO << "zero torque controller created";
673
674 wristSensorTrigger.waitForForceThreshold(20.);
675 ARMARX_INFO << "Start the demonstration ...";
676 ARMARX_IMPORTANT << "after trigger";
677
678 zeroTorqueController->activateController();
679
680 IceUtil::Time lastMotionTime = TimeUtil::GetTime();
681 bool initialMotionDetected = false;
682
683 VirtualRobot::RobotNodeSetPtr rns = m_robot->getRobotNodeSet(kinematicChainName);
684 std::vector<std::string> jointNames = rns->getNodeNames();
685 Eigen::VectorXf jve = rns->getJointValuesEigen();
686
687 while (true) // stop run function if returning true
688 {
689 const IceUtil::Time now = TimeUtil::GetTime();
690
691 ARMARX_CHECK(robotReader.synchronizeRobotJoints(*m_robot, armarx::DateTime::Now()));
692 const Eigen::VectorXf jve2 = rns->getJointValuesEigen();
693 if ((jve2 - jve).norm() > motionThresholdRadian)
694 {
695 jve = jve2;
696 initialMotionDetected = true;
697 lastMotionTime = now;
698 }
699
700 if (initialMotionDetected &&
701 (now - lastMotionTime).toMilliSeconds() > noMotionTimeoutMs)
702 {
703 ARMARX_IMPORTANT << "Stop recording";
704 break;
705 }
706
707 TimeUtil::USleep(2000);
708 }
709 ARMARX_IMPORTANT << "Demonstration done! Processing data ...";
710
711 zeroTorqueController->deactivateController();
712 while (zeroTorqueController->isControllerActive())
713 {
714 usleep(10000);
715 }
716 zeroTorqueController->deleteController();
717 ARMARX_INFO << "zero torque controller is deleted";
718 enableVelocityMode();
719 }
720
721 void
722 Component::toggleZeroTorque(const std::string& leftNodesetName,
723 const std::string& rightNodesetName,
724 const Ice::Current&)
725 {
726 }
727
728 std::string
729 Component::kinestheticTeaching(const std::string& kinematicChainName,
730 const std::string& side,
731 const std::string& fileName,
732 const std::string& nameSuffix,
733 const Ice::Current&)
734 {
735 /// ---------------------------------- creating zero torque controller ----------------------------------
736 ARMARX_INFO << "Kinesthetic teaching: Initialize zero torque controllers for "
737 << VAROUT(kinematicChainName);
738 const std::string ctrl_class_name = "NJointZeroTorqueController";
739 const std::string ctrl_name = "kinesthetic_teaching_" + ctrl_class_name + "_" + nameSuffix;
740
741 armem::robot_state::RobotReader::Hand hand = armem::robot_state::RobotReader::Hand::Left;
742 ARMARX_IMPORTANT << "THIS kinematicChainName is " << kinematicChainName;
743 if (kinematicChainName == "RightArm" || kinematicChainName == "RightArmWithoutWrist")
744 {
745 hand = armem::robot_state::RobotReader::Hand::Right;
746 }
747
748 auto& robotReader = virtualRobotReader_->get();
749 ARMARX_INFO << "robot reader is ready";
750 // ARMARX_CHECK_NOT_NULL(robotReader);
751 armarx::control::common::ft::FTSensorTrigger wristSensorTrigger(
752 robotReader, properties.robotName, hand);
753 /*armarx::kinesthetic_learning::kinesthetic_teaching::trigger::FTWristSensorTrigger m_trigger(
754 wristSensor);*/
755
756 ARMARX_INFO << "wrist trigger has been created";
757
758 NJointZeroTorqueControllerConfigPtr config(new NJointZeroTorqueControllerConfig());
759 config->maxTorque = 0;
760
761 auto nodeSet = m_robot->getRobotNodeSet(kinematicChainName);
762 for (auto& name : nodeSet->getNodeNames())
763 {
764 config->jointNames.push_back(name);
765 }
766 NJointZeroTorqueControllerInterfacePrx zeroTorqueController =
767 NJointZeroTorqueControllerInterfacePrx::checkedCast(
768 m_robot_unit->createNJointController(ctrl_class_name, ctrl_name, config));
769 ARMARX_CHECK_NOT_NULL(zeroTorqueController);
770 ARMARX_INFO << "zero torque controller created";
771
772 float motionThresholdRadian = 0.1f;
773 float noMotionTimeoutMs = 2000.0f;
774
775 VirtualRobot::RobotNodeSetPtr rns = m_robot->getRobotNodeSet(kinematicChainName);
776 std::vector<std::string> jointNames = rns->getNodeNames();
777 IceUtil::Time start = TimeUtil::GetTime();
778 IceUtil::Time lastMotionTime = start;
779 bool initialMotionDetected = false;
780
781 /// ---------------------------------- prepare data containers ----------------------------------
782 /// recording data
783 std::vector<std::vector<float>> jointData;
784 std::vector<std::vector<float>> cartData;
785 std::vector<float> timestamps;
786
787 /// init data
788 Eigen::VectorXf jve = rns->getJointValuesEigen();
789 Eigen::Matrix4f initPose = rns->getTCP()->getPoseInRootFrame();
790 VirtualRobot::MathTools::Quaternion initQuat =
791 VirtualRobot::MathTools::eigen4f2quat(initPose);
792 VirtualRobot::MathTools::Quaternion oldquatE = initQuat;
793
794 /// joint space
795 for (size_t i = 0; i < jointNames.size(); i++)
796 {
797 std::vector<float> cjoint{};
798 // cjoint.push_back(jve(i));
799 jointData.push_back(cjoint);
800 }
801
802 /// task space
803 // float quatvec[] = {initPose(0, 3), initPose(1, 3), initPose(2, 3), initQuat.w, initQuat.x, initQuat.y, initQuat.z};
804 for (size_t i = 0; i < 7; ++i)
805 {
806 std::vector<float> cpos{};
807 // cpos.push_back(quatvec[i]);
808 cartData.push_back(cpos);
809 }
810
811
812 int motionStartIndex = 0;
813 int motionEndIndex = 0;
814
815 int sigmaMs = 1;
816 int sampleMs = 10;
817
818 /*float forceSpike = 0.0f;
819 Eigen::Vector3f initForce;
820 if (side == "right")
821 {
822 initForce = m_ft_r->getForce();
823 }
824 else
825 {
826 initForce = m_ft_l->getForce();
827 }
828 ARMARX_IMPORTANT << "=============== Waiting for force spike ===============";
829 while (forceSpike < 10.0f)
830 {
831 Eigen::Vector3f force;
832 if (side == "right")
833 {
834 force = m_ft_r->getForce();
835 }
836 else
837 {
838 force = m_ft_l->getForce();
839 }
840 forceSpike = (force - initForce).norm();
841 }*/
842 // ARMARX_IMPORTANT << "before trigger";
843 //wristSensorTrigger.waitForForceSpike(armarx::control::common::ft::FTSensorTrigger::Edge::RISING);
844 wristSensorTrigger.waitForForceThreshold(20.);
845 ARMARX_INFO << "Start the demonstration ...";
846 // ARMARX_IMPORTANT << "after trigger";
847
848 zeroTorqueController->activateController();
849 while (true) // stop run function if returning true
850 {
851 synchronizeLocalClone(m_robot);
852
853 IceUtil::Time now = TimeUtil::GetTime();
854
855 Eigen::VectorXf jve2 = rns->getJointValuesEigen();
856 if ((jve2 - jve).norm() > motionThresholdRadian)
857 {
858 if (!initialMotionDetected)
859 {
860 ARMARX_IMPORTANT << "Start recording";
861 motionStartIndex = 0;
862 //start = now;
863 }
864 jve = jve2;
865 initialMotionDetected = true;
866 lastMotionTime = now;
867 }
868
869 double t = (now - start).toSecondsDouble();
870
871 std::vector<float> jvs = rns->getJointValues();
872 for (size_t i = 0; i < jvs.size(); i++)
873 {
874 jointData.at(i).push_back(jvs.at(i));
875 }
876
877 Eigen::Matrix4f tcpPose = rns->getTCP()->getPoseInRootFrame();
878 cartData.at(0).push_back(tcpPose(0, 3));
879 cartData.at(1).push_back(tcpPose(1, 3));
880 cartData.at(2).push_back(tcpPose(2, 3));
881 VirtualRobot::MathTools::Quaternion quat =
882 VirtualRobot::MathTools::eigen4f2quat(tcpPose);
883
884 // check flipped orientation quaternion
885 double dotProduct = quat.w * oldquatE.w + quat.x * oldquatE.x + quat.y * oldquatE.y +
886 quat.z * oldquatE.z;
887 if (dotProduct < 0)
888 {
889 cartData.at(3).push_back(-quat.w);
890 cartData.at(4).push_back(-quat.x);
891 cartData.at(5).push_back(-quat.y);
892 cartData.at(6).push_back(-quat.z);
893 oldquatE.w = -quat.w;
894 oldquatE.x = -quat.x;
895 oldquatE.y = -quat.y;
896 oldquatE.z = -quat.z;
897 }
898 else
899 {
900 cartData.at(3).push_back(quat.w);
901 cartData.at(4).push_back(quat.x);
902 cartData.at(5).push_back(quat.y);
903 cartData.at(6).push_back(quat.z);
904
905 oldquatE.w = quat.w;
906 oldquatE.x = quat.x;
907 oldquatE.y = quat.y;
908 oldquatE.z = quat.z;
909 }
910
911 timestamps.push_back(t);
912
913 if (initialMotionDetected &&
914 (now - lastMotionTime).toMilliSeconds() > noMotionTimeoutMs)
915 {
916 ARMARX_IMPORTANT << "Stop recording";
917 motionEndIndex = timestamps.size(); // - noMotionTimeoutMs / sampleMs;
918 break;
919 }
920
921 TimeUtil::USleep(sampleMs * 1000);
922 }
923 ARMARX_IMPORTANT << "Demonstration done! Processing data ...";
924
925 zeroTorqueController->deactivateController();
926 while (zeroTorqueController->isControllerActive())
927 {
928 usleep(10000);
929 }
930 zeroTorqueController->deleteController();
931 ARMARX_INFO << "zerotorque controller for kinesthetic teaching is deleteded";
932 enableVelocityMode();
933
934 motionStartIndex -= sigmaMs / sampleMs * 2;
935 if (motionStartIndex < 0)
936 {
937 motionStartIndex = 0;
938 }
939 //motionEndIndex += sigmaMs / sampleMs * 2;
940 //if (motionEndIndex >= data.size())
941 //{
942 // motionEndIndex = data.size() - 1;
943 //}
944
945 ARMARX_IMPORTANT << VAROUT(motionStartIndex) << VAROUT(motionEndIndex);
946
947 ARMARX_IMPORTANT << "copy timestamps";
948 timestamps = std::vector<float>(timestamps.begin() + motionStartIndex,
949 timestamps.begin() + motionEndIndex);
950 for (size_t n = 1; n < timestamps.size(); n++)
951 {
952 timestamps.at(n) = timestamps.at(n) - timestamps.at(0);
953 }
954 timestamps.at(0) = 0;
955
956 ARMARX_IMPORTANT << VAROUT(timestamps);
957 std::vector<float> newT = math::TimeSeriesUtils::MakeTimestamps(
958 timestamps.at(0), timestamps.at(timestamps.size() - 1), timestamps.size());
959 ARMARX_IMPORTANT << VAROUT(newT);
960
961 {
962 StringFloatSeqDict plotFiltered;
963 StringFloatSeqDict plotOrig;
964 StringFloatSeqDict plotResampled;
965 for (size_t i = 0; i < jointData.size(); i++)
966 {
967 jointData.at(i) = std::vector<float>(jointData.at(i).begin() + motionStartIndex,
968 jointData.at(i).begin() + motionEndIndex);
969 if (rns->getNode(i)->isLimitless())
970 {
971 math::LinearizeAngularTrajectory::LinearizeRef(jointData.at(i));
972 }
973 plotOrig[jointNames.at(i)] = jointData.at(i);
974 jointData.at(i) =
975 math::TimeSeriesUtils::Resample(timestamps, jointData.at(i), newT);
976 plotResampled[jointNames.at(i)] = jointData.at(i);
977 jointData.at(i) = math::TimeSeriesUtils::ApplyGaussianFilter(
978 jointData.at(i),
979 sigmaMs * 0.001f,
980 sampleMs * 0.001f,
981 math::TimeSeriesUtils::BorderMode::Nearest);
982 plotFiltered[jointNames.at(i)] = jointData.at(i);
983 }
984 // getStaticPlotter()->addPlotWithTimestampVector("original joint motion", timestamps, plotOrig);
985 // getStaticPlotter()->addPlotWithTimestampVector("resampled joint motion", newT, plotResampled);
986 // getStaticPlotter()->addPlotWithTimestampVector("filtered joint motion", newT, plotFiltered);
987 }
988
989 std::vector<std::string> cartName = {"x", "y", "z", "qw", "qx", "qy", "qz"};
990
991 timestamps = newT;
992
993 std::string trajName;
994 {
995 IceUtil::Time now = IceUtil::Time::now();
996 time_t timer = now.toSeconds();
997 struct tm* ts;
998 char buffer[80];
999 ts = localtime(&timer);
1000 strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
1001 std::string str(buffer);
1002 trajName = fileName + "-" + str;
1003 }
1004
1005
1006 double cutoutTime = (noMotionTimeoutMs / 1000.0f) * 0.8;
1007
1008 std::string packageName = "armarx_control";
1009
1010 armarx::CMakePackageFinder finder(packageName);
1011 std::string dataDir = finder.getDataDir() + "/" + packageName + "/kinesthetic_teaching/";
1012 std::string returnFileNamePattern = dataDir + trajName;
1013
1014 /// write task space data to RobotControllers Data dir
1015 ARMARX_IMPORTANT << "writing task space data!";
1016 std::string dmpForwardFile;
1017 std::string dmpBackwardFile;
1018 {
1019 std::vector<std::string> header;
1020 header.push_back("timestamp");
1021 for (size_t i = 0; i < cartName.size(); ++i)
1022 {
1023 header.push_back(cartName.at(i));
1024 }
1025
1026 std::string ffname = trajName + "-ts-forward.csv";
1027 std::string bfname = trajName + "-ts-backward.csv";
1028 dmpForwardFile = dataDir + ffname;
1029 dmpBackwardFile = dataDir + bfname;
1030 std::vector<std::string> fflist;
1031 fflist.push_back(ffname);
1032
1033 std::vector<std::string> bflist;
1034 bflist.push_back(bfname);
1035
1036 CsvWriter fwriter(dmpForwardFile, header, false);
1037 CsvWriter bwriter(dmpBackwardFile, header, false);
1038
1039 ARMARX_IMPORTANT << "writing taskspace trajectory as " << ffname << " and " << bfname;
1040
1041 double endTime = timestamps.at(timestamps.size() - 1);
1042 for (size_t n = 0; n < timestamps.size(); n++)
1043 {
1044
1045 std::vector<float> row;
1046 row.push_back(timestamps.at(n));
1047 for (size_t i = 0; i < cartData.size(); i++)
1048 {
1049 row.push_back(cartData.at(i).at(n));
1050 }
1051 fwriter.write(row);
1052
1053 if (endTime - timestamps.at(n) < cutoutTime)
1054 {
1055 Eigen::Matrix4f endPose = VirtualRobot::MathTools::quat2eigen4f(
1056 row.at(5), row.at(6), row.at(7), row.at(4));
1057 endPose(0, 3) = row.at(1);
1058 endPose(1, 3) = row.at(2);
1059 endPose(2, 3) = row.at(3);
1060 break;
1061 }
1062 }
1063 fwriter.close();
1064 int nt = 0;
1065 for (int n = timestamps.size() - 1; n >= 0 && nt < static_cast<int>(timestamps.size());
1066 n--, nt++)
1067 {
1068 std::vector<float> row;
1069 row.push_back(timestamps.at(nt));
1070 for (size_t i = 0; i < cartData.size(); i++)
1071 {
1072 row.push_back(cartData.at(i).at(n));
1073 }
1074 bwriter.write(row);
1075
1076 if (n == 0)
1077 {
1078 Eigen::Matrix4f endPose = VirtualRobot::MathTools::quat2eigen4f(
1079 row.at(5), row.at(6), row.at(7), row.at(4));
1080 endPose(0, 3) = row.at(1);
1081 endPose(1, 3) = row.at(2);
1082 endPose(2, 3) = row.at(3);
1083 break;
1084 }
1085 }
1086 bwriter.close();
1087 }
1088
1089 /// write joint space data to RobotControllers Data dir
1090 ARMARX_IMPORTANT << "writing joint space data!";
1091 std::string dmpJSForwardFile;
1092 std::string dmpJSBackwardFile;
1093 {
1094 std::vector<std::string> header;
1095 header.push_back("timestamp");
1096 for (size_t i = 0; i < jointNames.size(); ++i)
1097 {
1098 header.push_back(jointNames.at(i));
1099 }
1100
1101 std::string ffname = trajName + "-js-forward.csv";
1102 std::string bfname = trajName + "-js-backward.csv";
1103 dmpJSForwardFile = dataDir + ffname;
1104 dmpJSBackwardFile = dataDir + bfname;
1105 std::vector<std::string> fflist;
1106 fflist.push_back(ffname);
1107
1108 std::vector<std::string> bflist;
1109 bflist.push_back(bfname);
1110
1111 CsvWriter fwriter(dmpJSForwardFile, header, false);
1112 CsvWriter bwriter(dmpJSBackwardFile, header, false);
1113
1114 ARMARX_IMPORTANT << "writing jointspace trajectory as " << ffname << " and " << bfname;
1115
1116 double endTime = timestamps.at(timestamps.size() - 1);
1117 for (size_t n = 0; n < timestamps.size(); n++)
1118 {
1119
1120 std::vector<float> row;
1121 row.push_back(timestamps.at(n));
1122 for (size_t i = 0; i < jointNames.size(); i++)
1123 {
1124 row.push_back(jointData.at(i).at(n));
1125 }
1126 fwriter.write(row);
1127
1128 if (endTime - timestamps.at(n) < cutoutTime)
1129 {
1130 std::vector<float> endJointValue;
1131 for (size_t i = 0; i < jointNames.size(); i++)
1132 {
1133 endJointValue.push_back(jointData.at(i).at(n));
1134 }
1135 break;
1136 }
1137 }
1138 fwriter.close();
1139 ARMARX_INFO << "forward joint space data written to fine!";
1140 int nt = 0;
1141 for (int n = timestamps.size() - 1; n >= 0 && nt < static_cast<int>(timestamps.size());
1142 n--, nt++)
1143 {
1144 std::vector<float> row;
1145 row.push_back(timestamps.at(nt));
1146 for (size_t i = 0; i < jointNames.size(); i++)
1147 {
1148 row.push_back(jointData.at(i).at(n));
1149 }
1150 bwriter.write(row);
1151
1152 if (n == 0)
1153 {
1154 std::vector<float> endJointValue;
1155 for (size_t i = 0; i < jointNames.size(); i++)
1156 {
1157 endJointValue.push_back(jointData.at(i).at(n));
1158 }
1159 break;
1160 }
1161 }
1162 bwriter.close();
1163 }
1164 return returnFileNamePattern;
1165 // return "";
1166 }
1167
1168 void
1169 Component::enableVelocityMode(const Ice::Current&)
1170 {
1171 ARMARX_INFO << "enable velocity mode";
1172 // left arm
1173 {
1174 std::vector<std::string> joint_names = m_rns_l->getNodeNames();
1175 NameValueMap target_vels;
1176 NameControlModeMap control_modes;
1177 for (const std::string& joint_name : joint_names)
1178 {
1179 control_modes[joint_name] = eVelocityControl;
1180 target_vels[joint_name] = 0;
1181 }
1182 }
1183 // right arm
1184 {
1185 std::vector<std::string> joint_names = m_rns_r->getNodeNames();
1186 NameValueMap target_vels;
1187 NameControlModeMap control_modes;
1188 for (const std::string& joint_name : joint_names)
1189 {
1190 control_modes[joint_name] = eVelocityControl;
1191 target_vels[joint_name] = 0;
1192 }
1193 }
1194 // // platform
1195 // {
1196 // m_platform->move(0.0f, 0.0f, 0.0f);
1197 // }
1198 }
1199
1200 Ice::FloatSeq
1201 Component::getPoseInRootFrame(const std::string& nodeName, const Ice::Current&)
1202 {
1203 ARMARX_CHECK(synchronizeLocalClone(m_robot));
1204 Eigen::Matrix4f pose = m_robot->getRobotNode(nodeName)->getPoseInRootFrame();
1205 // ARMARX_INFO << VAROUT(pose);
1206 std::vector<Ice::Float> pose_seq;
1207 for (int i = 0; i < pose.rows(); i++)
1208 {
1209 for (int j = 0; j < pose.cols(); j++)
1210 {
1211 pose_seq.push_back(pose(i, j));
1212 }
1213 }
1214 return pose_seq;
1215 }
1216
1217 Ice::FloatSeq
1218 Component::getPoseInGlobalFrame(const std::string& nodeName, const Ice::Current&)
1219 {
1220 ARMARX_CHECK(synchronizeLocalClone(m_robot));
1221 Eigen::Matrix4f pose = m_robot->getRobotNode(nodeName)->getGlobalPose();
1222 // ARMARX_INFO << VAROUT(pose);
1223 std::vector<Ice::Float> pose_seq;
1224 for (int i = 0; i < pose.rows(); i++)
1225 {
1226 for (int j = 0; j < pose.cols(); j++)
1227 {
1228 pose_seq.push_back(pose(i, j));
1229 }
1230 }
1231 return pose_seq;
1232 }
1233
1234 std::string
1235 Component::getDefaultName() const
1236 {
1237 return Component::defaultName;
1238 }
1239
1240 std::string
1241 Component::GetDefaultName()
1242 {
1243 return Component::defaultName;
1244 }
1245
1246 /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
1247 void
1248 Component::createRemoteGuiTab()
1249 {
1250 using namespace armarx::RemoteGui::Client;
1251
1252 // Setup the widgets.
1253
1254 tab.boxLayerName.setValue(properties.boxLayerName);
1255
1256 tab.numBoxes.setValue(properties.numBoxes);
1257 tab.numBoxes.setRange(0, 100);
1258
1259 tab.drawBoxes.setLabel("Draw Boxes");
1260
1261 // Setup the layout.
1262
1263 GridLayout grid;
1264 int row = 0;
1265 {
1266 grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
1267 ++row;
1268
1269 grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
1270 ++row;
1271
1272 grid.add(tab.drawBoxes, {row, 0}, {2, 1});
1273 ++row;
1274 }
1275
1276 VBoxLayout root = {grid, VSpacer()};
1277 RemoteGui_createTab(getName(), root, &tab);
1278 }
1279
1280
1281 void
1282 Component::RemoteGui_update()
1283 {
1284 if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
1285 {
1286 std::scoped_lock lock(propertiesMutex);
1287 properties.boxLayerName = tab.boxLayerName.getValue();
1288 properties.numBoxes = tab.numBoxes.getValue();
1289
1290 {
1291 setDebugObserverDatafield("numBoxes", properties.numBoxes);
1292 setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
1293 sendDebugObserverBatch();
1294 }
1295 }
1296 if (tab.drawBoxes.wasClicked())
1297 {
1298 // Lock shared variables in methods running in separate threads
1299 // and pass them to functions. This way, the called functions do
1300 // not need to think about locking.
1301 std::scoped_lock lock(propertiesMutex);
1302 drawBoxes(properties, arviz);
1303 }
1304 }
1305 */
1306
1307
1308 /* (Requires the armarx::ArVizComponentPluginUser.)
1309 void
1310 Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
1311 {
1312 // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
1313 // See the ArVizExample in RobotAPI for more examples.
1314
1315 viz::Layer layer = arviz.layer(p.boxLayerName);
1316 for (int i = 0; i < p.numBoxes; ++i)
1317 {
1318 layer.add(viz::Box("box_" + std::to_string(i))
1319 .position(Eigen::Vector3f(i * 100, 0, 0))
1320 .size(20).color(simox::Color::blue()));
1321 }
1322 arviz.commit(layer);
1323 }
1324 */
1325
1326
1327 ARMARX_DECOUPLED_REGISTER_COMPONENT(Component);
1328
1329} // namespace armarx::control::components::controller_creator
ARMARX_DECOUPLED_REGISTER_COMPONENT
#define ARMARX_DECOUPLED_REGISTER_COMPONENT(ComponentT)
Definition Decoupled.h:34
aron_conversions.h
types.h
VAROUT
#define VAROUT(x)
Definition StringHelpers.h:198
str
std::string str(const T &t)
Definition UserAssistedSegmenterGuiWidgetController.cpp:43
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition CMakePackageFinder.h:53
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition Component.h:70
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition Component.cpp:90
armarx::CsvWriter::write
void write(const std::vector< std::string > &row)
Definition CsvWriter.cpp:43
armarx::DateTime::Now
static DateTime Now()
Definition DateTime.cpp:51
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&... params)
Definition ManagedIceObject.h:186
armarx::RobotStateComponentPluginUser::synchronizeLocalClone
bool synchronizeLocalClone(const VirtualRobot::RobotPtr &robot) const
Definition RobotStateComponentPlugin.cpp:470
armarx::RobotStateComponentPluginUser::addRobot
VirtualRobot::RobotPtr addRobot(const std::string &id, const VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &rns={}, const VirtualRobot::RobotNodePtr &node={})
Definition RobotStateComponentPlugin.cpp:405
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition TimeUtil.cpp:42
armarx::TimeUtil::USleep
static int USleep(long usec)
like timed_wait on boost condition_variables, but with timeserver support
Definition TimeUtil.cpp:159
armarx::armem::robot_state::RobotReader::Hand
Hand
Definition RobotReader.h:92
armarx::armem::robot_state::RobotReader::Hand::Right
@ Right
Definition RobotReader.h:94
armarx::armem::robot_state::RobotReader::Hand::Left
@ Left
Definition RobotReader.h:93
armarx::control::common::ft::FTSensorTrigger::waitForForceThreshold
void waitForForceThreshold(float threshold) const
Definition FTSensorTrigger.cpp:181
armarx::control::components::controller_creator::Component
Definition Component.h:67
armarx::control::components::controller_creator::Component::Component
Component()
Definition Component.cpp:66
armarx::control::components::controller_creator::Component::onInitComponent
void onInitComponent() override
Definition Component.cpp:112
armarx::control::components::controller_creator::Component::createZeroTorqueController
void createZeroTorqueController(const std::string &kinematicChainName)
Definition Component.cpp:632
armarx::control::components::controller_creator::Component::onDisconnectComponent
void onDisconnectComponent() override
Definition Component.cpp:184
armarx::control::components::controller_creator::Component::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition Component.cpp:74
armarx::control::components::controller_creator::Component::createComplianceController
std::string createComplianceController(const std::string &namePrefix, const ::armarx::aron::data::dto::DictPtr &configDict=nullptr, const std::string &configFilename="", bool flagActivate=true, bool flagAllowReuse=true, bool flagFromMemory=false)
armarx::control::components::controller_creator::Component::~Component
~Component() override
armarx::control::components::controller_creator::Component::kinestheticTeaching
std::string kinestheticTeaching(const std::string &kinematicChainName, const std::string &side, const std::string &fileName, const std::string &nameSuffix, const Ice::Current &=Ice::emptyCurrent) override
Definition Component.cpp:729
armarx::control::components::controller_creator::Component::createController
std::string createController(const std::string &namePrefix, const std::string &controllerType, const aron::data::dto::DictPtr &configDict=nullptr, const std::string &configFilename="", bool activate=false, bool allowReuse=true, bool cfgFromMemory=false, const Ice::Current &=Ice::emptyCurrent) override
Definition Component.cpp:471
armarx::control::components::controller_creator::Component::onConnectComponent
void onConnectComponent() override
Definition Component.cpp:122
armarx::control::components::controller_creator::Component::toggleZeroTorque
void toggleZeroTorque(const std::string &leftNodesetName, const std::string &rightNodesetName, const Ice::Current &=Ice::emptyCurrent) override
Definition Component.cpp:722
armarx::control::components::controller_creator::Component::GetDefaultName
static std::string GetDefaultName()
Get the component's default name.
Definition Component.cpp:1241
armarx::control::components::controller_creator::Component::getPoseInGlobalFrame
Ice::FloatSeq getPoseInGlobalFrame(const std::string &nodeName, const Ice::Current &=Ice::emptyCurrent) override
Definition Component.cpp:1218
armarx::control::components::controller_creator::Component::getPoseInRootFrame
Ice::FloatSeq getPoseInRootFrame(const std::string &nodeName, const Ice::Current &=Ice::emptyCurrent) override
Definition Component.cpp:1201
armarx::control::components::controller_creator::Component::onExitComponent
void onExitComponent() override
Definition Component.cpp:189
armarx::control::components::controller_creator::Component::getDefaultName
std::string getDefaultName() const override
Definition Component.cpp:1235
armarx::math::LinearizeAngularTrajectory::LinearizeRef
static void LinearizeRef(std::vector< float > &data)
Definition LinearizeAngularTrajectory.cpp:66
armarx::math::TimeSeriesUtils::Resample
static std::vector< float > Resample(const std::vector< float > &timestamps, const std::vector< float > &data, const std::vector< float > &newTimestamps)
Definition TimeSeriesUtils.cpp:37
armarx::math::TimeSeriesUtils::MakeTimestamps
static std::vector< float > MakeTimestamps(float start, float end, size_t count)
Definition TimeSeriesUtils.cpp:128
armarx::math::TimeSeriesUtils::ApplyGaussianFilter
static std::vector< float > ApplyGaussianFilter(const std::vector< float > &data, float sigma, float sampleTime, BorderMode mode)
Definition TimeSeriesUtils.cpp:105
controller_descriptions.h
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition ExpressionException.h:82
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition ExpressionException.h:206
ARMARX_INFO
#define ARMARX_INFO
The normal logging level.
Definition Logging.h:181
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
The logging level for always important information, but expected behaviour (in contrast to ARMARX_WAR...
Definition Logging.h:190
ARMARX_ERROR
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:196
armarx::control::components::controller_creator::ComponentInterface::enableVelocityMode
void enableVelocityMode()
armarx::aron::data::dto::DictPtr
::IceInternal::Handle< Dict > DictPtr
Definition aron_conversions.h:36
armarx::control::common::ControllerTypeShort
const simox::meta::EnumNames< ControllerType > ControllerTypeShort
Definition type.h:116
armarx::control::components::controller_creator
Definition Component.cpp:62
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition forward_declarations.h:35
norm
double norm(const Point &a)
Definition point.hpp:102
aron_conversions.h
Component.h