NaturalIKTest.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::NaturalIKTest
17 * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
18 * @date 2020
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22
23#include "NaturalIKTest.h"
24
25#include <random>
26
27#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
28#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
29#include <VirtualRobot/IK/constraints/PoseConstraint.h>
30#include <VirtualRobot/IK/constraints/PositionConstraint.h>
31#include <VirtualRobot/Robot.h>
32#include <VirtualRobot/RobotNodeSet.h>
33#include <VirtualRobot/XML/RobotIO.h>
34#include <VirtualRobot/math/Helpers.h>
35
36#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
37#include <ArmarXCore/core/time/CycleUtil.h>
38#include <ArmarXCore/core/time/TimeUtil.h>
39
40#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
41
42#include <RobotAPI/components/ArViz/Client/Client.h>
43#include <RobotAPI/libraries/core/CartesianPositionController.h>
44#include <RobotAPI/libraries/diffik/CompositeDiffIK.h>
45#include <RobotAPI/libraries/natik/NaturalIK.h>
46
47#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
48
49namespace armarx
50{
51 NaturalIKTestPropertyDefinitions::NaturalIKTestPropertyDefinitions(std::string prefix) :
52 armarx::ComponentPropertyDefinitions(prefix)
53 {
54 //defineRequiredProperty<std::string>("PropertyName", "Description");
55 //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
56
57 defineOptionalProperty<std::string>(
58 "DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
59 defineOptionalProperty<std::string>(
60 "ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
61
62 //defineOptionalProperty<std::string>("MyProxyName", "MyProxy", "Name of the proxy to be used");
63 }
64
65 std::string
66 NaturalIKTest::getDefaultName() const
67 {
68 return "NaturalIKTest";
69 }
70
71 void
72 NaturalIKTest::onInitComponent()
73 {
74 // Register offered topices and used proxies here.
75 offeringTopicFromProperty("DebugObserverName");
76 // debugDrawer.offeringTopic(*this); // Calls this->offeringTopic().
77
78 // Using a proxy will cause the component to wait until the proxy is available.
79 // usingProxyFromProperty("MyProxyName");
80
81 offeringTopicFromProperty("ArVizTopicName");
82 }
83
84 void
85 NaturalIKTest::onConnectComponent()
86 {
87 // Get topics and proxies here. Pass the *InterfacePrx type as template argument.
88 debugObserver = getTopicFromProperty<DebugObserverInterfacePrx>("DebugObserverName");
89 // debugDrawer.getTopic(*this); // Calls this->getTopic().
90
91 // getProxyFromProperty<MyProxyInterfacePrx>("MyProxyName");
92
93 createOrUpdateRemoteGuiTab(buildGui(),
94 [this](RemoteGui::TabProxy& prx) { processGui(prx); });
95
96 vizTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::vizTaskRun);
97 vizTask->start();
98 }
99
100 void
101 NaturalIKTest::onDisconnectComponent()
102 {
103 vizTask->stop();
104 vizTask = nullptr;
105 }
106
107 RemoteGui::WidgetPtr
108 NaturalIKTest::buildGui()
109 {
110 RemoteGui::detail::GroupBoxBuilder leftBox =
111 RemoteGui::makeGroupBox().label("Left").addChild(
112 RemoteGui::makeSimpleGridLayout()
113 .cols(2)
114 .addTextLabel("L.X")
115 .addChild(RemoteGui::makeFloatSlider("L.X").min(-600).max(600).value(0))
116
117 .addTextLabel("L.Y")
118 .addChild(RemoteGui::makeFloatSlider("L.Y").min(200).max(1200).value(600))
119
120 .addTextLabel("L.Z")
121 .addChild(RemoteGui::makeFloatSlider("L.Z").min(-600).max(600).value(-200))
122
123 .addTextLabel("L.scale")
124 .addChild(RemoteGui::makeFloatSlider("L.scale").min(0.5).max(2).value(1.3f))
125
126 .addTextLabel("L.minElbZ")
127 .addChild(RemoteGui::makeFloatSlider("L.minElbZ").min(500).max(2000).value(500))
128
129 .addChild(RemoteGui::makeCheckBox("L.setOri").value(true))
130 .addChild(new RemoteGui::HSpacer)
131
132 .addTextLabel("L.rX")
133 .addChild(RemoteGui::makeFloatSlider("L.rX").min(-180).max(180).value(0))
134
135 .addTextLabel("L.rY")
136 .addChild(RemoteGui::makeFloatSlider("L.rY").min(-180).max(180).value(0))
137
138 .addTextLabel("L.rZ")
139 .addChild(RemoteGui::makeFloatSlider("L.rZ").min(-180).max(180).value(0)));
140 RemoteGui::detail::GroupBoxBuilder rightBox =
141 RemoteGui::makeGroupBox().label("Right").addChild(
142 RemoteGui::makeSimpleGridLayout()
143 .cols(2)
144 .addTextLabel("R.X")
145 .addChild(RemoteGui::makeFloatSlider("R.X").min(-600).max(600).value(0))
146
147 .addTextLabel("R.Y")
148 .addChild(RemoteGui::makeFloatSlider("R.Y").min(200).max(1200).value(600))
149
150 .addTextLabel("R.Z")
151 .addChild(RemoteGui::makeFloatSlider("R.Z").min(-600).max(600).value(-200))
152
153 .addTextLabel("R.scale")
154 .addChild(RemoteGui::makeFloatSlider("R.scale").min(0.5).max(2).value(1.3f))
155
156 .addTextLabel("R.minElbZ")
157 .addChild(RemoteGui::makeFloatSlider("R.minElbZ").min(500).max(2000).value(500))
158
159 .addChild(RemoteGui::makeCheckBox("R.setOri").value(true))
160 .addChild(new RemoteGui::HSpacer)
161
162 .addTextLabel("R.rX")
163 .addChild(RemoteGui::makeFloatSlider("R.rX").min(-180).max(180).value(0))
164
165 .addTextLabel("R.rY")
166 .addChild(RemoteGui::makeFloatSlider("R.rY").min(-180).max(180).value(0))
167
168 .addTextLabel("R.rZ")
169 .addChild(RemoteGui::makeFloatSlider("R.rZ").min(-180).max(180).value(0)));
170
171 return RemoteGui::makeVBoxLayout()
172 .addChild(RemoteGui::makeCheckBox("useCompositeIK").value(false))
173 .addChild(
174 RemoteGui::makeSimpleGridLayout().cols(2).addChild(leftBox).addChild(rightBox))
175 .addChild(RemoteGui::makeSimpleGridLayout()
176 .cols(2)
177 .addTextLabel("elbowKp")
178 .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
179
180 .addTextLabel("jlaKp")
181 .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0)))
182 .addChild(RemoteGui::makeLabel("errors").value("<errors>"))
183 .addChild(RemoteGui::makeButton("runTest").label("run test"));
184 }
185
186 void
187 NaturalIKTest::processGui(RemoteGui::TabProxy& prx)
188 {
189 prx.receiveUpdates();
190
191 p.useCompositeIK = prx.getValue<bool>("useCompositeIK").get();
192
193 float x, y, z, rx, ry, rz;
194
195 x = prx.getValue<float>("R.X").get();
196 y = prx.getValue<float>("R.Y").get();
197 z = prx.getValue<float>("R.Z").get();
198 p.p_R.target = Eigen::Vector3f(x, y, z);
199 p.p_R.scale = prx.getValue<float>("R.scale").get();
200 p.p_R.ikparams.minimumElbowHeight = prx.getValue<float>("R.minElbZ").get();
201 p.p_R.setOri = prx.getValue<bool>("R.setOri").get();
202 rx = prx.getValue<float>("R.rX").get();
203 ry = prx.getValue<float>("R.rY").get();
204 rz = prx.getValue<float>("R.rZ").get();
205 p.p_R.targetRotation = Eigen::Vector3f(rx, ry, rz);
206
207 x = prx.getValue<float>("L.X").get();
208 y = prx.getValue<float>("L.Y").get();
209 z = prx.getValue<float>("L.Z").get();
210 p.p_L.target = Eigen::Vector3f(x, y, z);
211 p.p_L.scale = prx.getValue<float>("L.scale").get();
212 p.p_L.ikparams.minimumElbowHeight = prx.getValue<float>("L.minElbZ").get();
213 p.p_L.setOri = prx.getValue<bool>("L.setOri").get();
214 rx = prx.getValue<float>("L.rX").get();
215 ry = prx.getValue<float>("L.rY").get();
216 rz = prx.getValue<float>("L.rZ").get();
217 p.p_L.targetRotation = Eigen::Vector3f(rx, ry, rz);
218
219
220 p.p_R.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
221 p.p_R.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
222 p.p_R.ikparams.diffIKparams.returnIKSteps = true;
223
224 p.p_L.ikparams.diffIKparams.elbowKp = prx.getValue<float>("elbowKp").get();
225 p.p_L.ikparams.diffIKparams.jointLimitAvoidanceKp = prx.getValue<float>("jlaKp").get();
226 p.p_L.ikparams.diffIKparams.returnIKSteps = true;
227
228
229 p.targetValid = true;
230 std::stringstream ss;
231 ss << "err_R: " << p.err_R(0) << " " << p.err_R(1) << " " << p.err_R(2) << "\n";
232 ss << "err_L: " << p.err_L(0) << " " << p.err_L(1) << " " << p.err_L(2) << "";
233 prx.getValue<std::string>("errors").set(ss.str());
234
235 bool runTest = prx.getButtonClicked("runTest");
236 if (runTest)
237 {
238 runTestTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::testTaskRun);
239 runTestTask->start();
240 }
241
242
243 prx.sendUpdates();
244 }
245
246 struct IKStats
247 {
248 IKStats(const std::string& name) : name(name)
249 {
250 }
251
252 std::string name;
253 int solved = 0;
254 float duration = 0;
255 std::vector<Eigen::Vector3f> elbow;
256
257 Eigen::Vector3f
258 averageElb()
259 {
260 Eigen::Vector3f elb = Eigen::Vector3f ::Zero();
261 for (auto e : elbow)
262 {
263 elb += e;
264 }
265 return elb / elbow.size();
266 }
267 };
268
269 struct SoftPositionConstraint : public VirtualRobot::PositionConstraint
270 {
271 public:
272 SoftPositionConstraint(const VirtualRobot::RobotPtr& robot,
273 const VirtualRobot::RobotNodeSetPtr& nodeSet,
274 const VirtualRobot::SceneObjectPtr& node,
275 const VirtualRobot::SceneObjectPtr& tcp,
276 const Eigen::Vector3f& target,
277 VirtualRobot::IKSolver::CartesianSelection cartesianSelection =
278 VirtualRobot::IKSolver::All,
279 float tolerance = 3.0f) :
280 PositionConstraint(robot, nodeSet, node, target, cartesianSelection, tolerance),
281 tcp(tcp)
282 {
283 optimizationFunctions.clear();
284
285 addOptimizationFunction(0, true);
286 }
287
288 VirtualRobot::SceneObjectPtr tcp;
289
290 Eigen::VectorXf
291 optimizationGradient(unsigned int id) override
292 {
293 int size = nodeSet->getSize();
294 (void)size;
295
296 Eigen::MatrixXf J = ik->getJacobianMatrix(tcp); //.block(0, 0, 3, size);
297 //Eigen::Vector3f d = eef->getGlobalPose().block<3,1>(0,3) - target;
298
299 Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(J);
300 Eigen::MatrixXf nullspace = lu_decomp.kernel();
301
302 Eigen::VectorXf grad = PositionConstraint::optimizationGradient(id);
303
304 Eigen::VectorXf mapped = Eigen::VectorXf::Zero(grad.size());
305 for (int i = 0; i < nullspace.cols(); i++)
306 {
307 float squaredNorm = nullspace.col(i).squaredNorm();
308 // Prevent division by zero
309 if (squaredNorm > 1.0e-32f)
310 {
311 mapped += nullspace.col(i) * nullspace.col(i).dot(grad) /
312 nullspace.col(i).squaredNorm();
313 }
314 }
315 return mapped;
316 }
317
318 bool
319 checkTolerances() override
320 {
321 return true;
322 }
323 };
324
325 void
326 NaturalIKTest::testTaskRun()
327 {
328 CMakePackageFinder finder("armar6_rt");
329 std::string robotFile =
330 finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
331 ARMARX_IMPORTANT << "loading robot from " << robotFile;
332 VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
333
334 VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
335 VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
336
337 VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
338 VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
339
340 VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
341 VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
342 VirtualRobot::RobotNodePtr tcp_R = rns_R->getTCP();
343
344
345 float upper_arm_length =
346 (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
347 //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
348 float lower_arm_length =
349 (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
350
351
352 Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
353 Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
354
355 Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
356 (void)offset;
357
358 NaturalIK ik_R("Right", shoulder_R, 1);
359 NaturalIK ik_L("Left", shoulder_L, 1);
360
361 ik_R.setUpperArmLength(upper_arm_length);
362 ik_R.setLowerArmLength(lower_arm_length);
363 ik_L.setUpperArmLength(upper_arm_length);
364 ik_L.setLowerArmLength(lower_arm_length);
365
366
367 NaturalIK::ArmJoints arm_R;
368 arm_R.rns = rns_R;
369 arm_R.elbow = elb_R;
370 arm_R.tcp = rns_R->getTCP();
371
372 std::vector<NaturalIK::Parameters> ikParamList;
373 std::vector<IKStats> ikStats = {IKStats("NaturalIK"),
374 IKStats("SimpleDiffIK"),
375 IKStats("OptIK"),
376 IKStats("DiffIK"),
377 IKStats("CompositeIK")};
378
379 NaturalIK::Parameters p0;
380 p0.diffIKparams.resetRnsValues = false;
381 p0.diffIKparams.ikStepLengthInitial = 1;
382 p0.diffIKparams.ikStepLengthFineTune = 1;
383 p0.diffIKparams.maxJointAngleStep = 0.2f;
384
385 {
386 NaturalIK::Parameters p = p0;
387 p.diffIKparams.resetRnsValues = false;
388 p.diffIKparams.elbowKp = 1;
389 p.diffIKparams.jointLimitAvoidanceKp = 0.1;
390 p.diffIKparams.ikStepLengthInitial = 1;
391 ikParamList.emplace_back(p);
392 }
393 {
394 NaturalIK::Parameters p = p0;
395 p.diffIKparams.resetRnsValues = false;
396 p.diffIKparams.elbowKp = 0;
397 p.diffIKparams.jointLimitAvoidanceKp = 1;
398 ikParamList.emplace_back(p);
399 }
400
401 CompositeDiffIK::Parameters pc;
402 pc.resetRnsValues = false;
403
404
405 std::random_device rd; //Will be used to obtain a seed for the random number engine
406 std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
407 std::uniform_real_distribution<> dis(0.0, 1.0);
408
409 std::vector<Eigen::VectorXf> initialConfigurations;
410 for (VirtualRobot::RobotNodePtr rn : rns_R->getAllRobotNodes())
411 {
412 if (rn->isLimitless())
413 {
414 rn->setJointValue(0);
415 }
416 else
417 {
418 rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
419 }
420 }
421
422 rns_R->getNode(1)->setJointValue(0);
423 rns_R->getNode(3)->setJointValue(0);
424 rns_R->getNode(5)->setJointValue(0);
425 initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
426 rns_R->getNode(1)->setJointValue(0);
427 rns_R->getNode(3)->setJointValue(0);
428 rns_R->getNode(5)->setJointValue(M_PI);
429 initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
430 rns_R->getNode(1)->setJointValue(0);
431 rns_R->getNode(3)->setJointValue(M_PI);
432 rns_R->getNode(5)->setJointValue(0);
433 initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
434 rns_R->getNode(1)->setJointValue(0);
435 rns_R->getNode(3)->setJointValue(M_PI);
436 rns_R->getNode(5)->setJointValue(M_PI);
437 initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
438
439
440 std::vector<Eigen::Matrix4f> targets;
441 while (targets.size() < 100)
442 {
443 for (size_t i = 0; i < rns_R->getSize(); i++)
444 {
445 if (rns_R->getNode(i)->isLimitless())
446 {
447 rns_R->getNode(i)->setJointValue(dis(gen) * 2 * M_PI);
448 }
449 else
450 {
451 float lo = rns_R->getNode(i)->getJointLimitLo();
452 float hi = rns_R->getNode(i)->getJointLimitHi();
453 rns_R->getNode(i)->setJointValue(dis(gen) * (hi - lo) + lo);
454 }
455 }
456 Eigen::Vector3f tcpPos = tcp_R->getPositionInRootFrame();
457 if (tcpPos.y() > 0)
458 {
459 targets.emplace_back(tcp_R->getPoseInRootFrame());
460 }
461 }
462
463
464 IceUtil::Time start;
465 for (size_t i = 0; i < ikParamList.size(); i++)
466 {
467 start = TimeUtil::GetTime();
468 for (size_t n = 0; n < targets.size(); n++)
469 {
470 for (const Eigen::VectorXf& initjv : initialConfigurations)
471 {
472 rns_R->setJointValues(initjv);
473 NaturalDiffIK::Result ikResult =
474 ik_R.calculateIK(targets.at(n), arm_R, ikParamList.at(i));
475 if (ikResult.reached)
476 {
477 ikStats.at(i).solved = ikStats.at(i).solved + 1;
478 ikStats.at(i).elbow.emplace_back(elb_R->getPositionInRootFrame());
479 break;
480 }
481 }
482 }
483 ikStats.at(i).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
484 }
485 start = TimeUtil::GetTime();
486
487 std::vector<VirtualRobot::RobotNodePtr> elbJoints_R;
488 for (int i = 0; i < 4; i++)
489 {
490 elbJoints_R.push_back(rns_R->getNode(i));
491 }
492 VirtualRobot::RobotNodeSetPtr rnsElb_R = VirtualRobot::RobotNodeSet::createRobotNodeSet(
493 robot, "ELB_R", elbJoints_R, rns_R->getKinematicRoot(), elb_R);
494 for (size_t n = 0; n < targets.size(); n++)
495 {
496 rns_R->setJointValues(initialConfigurations.at(0));
497 VirtualRobot::ConstrainedOptimizationIK optIK(robot, rns_R);
498 //VirtualRobot::ConstraintPtr conPose(new VirtualRobot::PoseConstraint(robot, rns_R, tcp_R, targets.at(n)));
499 //optIK.addConstraint(conPose);
500 VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(
501 robot,
502 rns_R,
503 tcp_R,
504 math::Helpers::GetPosition(targets.at(n)),
505 VirtualRobot::IKSolver::CartesianSelection::All));
506 posConstraint->setOptimizationFunctionFactor(1);
507
508 VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(
509 robot,
510 rns_R,
511 tcp_R,
512 math::Helpers::GetOrientation(targets.at(n)),
513 VirtualRobot::IKSolver::CartesianSelection::All,
514 2.f / 180 * M_PI)); // was VirtualRobot::MathTools::deg2rad(...)
515 oriConstraint->setOptimizationFunctionFactor(1000);
516
517
518 Eigen::Vector3f elbowPos =
519 ik_R.solveSoechtingIK(math::Helpers::GetPosition(targets.at(n))).elbow;
520
521 VirtualRobot::ConstraintPtr elbConstraint(
522 new SoftPositionConstraint(robot,
523 rns_R,
524 elb_R,
525 tcp_R,
526 elbowPos,
527 VirtualRobot::IKSolver::CartesianSelection::All));
528 elbConstraint->setOptimizationFunctionFactor(0.01);
529 //elbConstraint->
530
531 //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationFunction(0));
532 //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationGradient(0));
533
534 optIK.addConstraint(posConstraint);
535 optIK.addConstraint(oriConstraint);
536 //optIK.addConstraint(elbConstraint);
537
538 optIK.initialize();
539 bool reached = optIK.solve();
540 if (reached)
541 {
542 ikStats.at(2).solved = ikStats.at(2).solved + 1;
543 ikStats.at(2).elbow.emplace_back(elb_R->getPositionInRootFrame());
544 //float perr = CartesianPositionController::GetPositionError(targets.at(n), tcp_R);
545 //float oerr = 180 / M_PI * CartesianPositionController::GetOrientationError(targets.at(n), tcp_R);
546 //ARMARX_IMPORTANT << VAROUT(perr) << VAROUT(oerr);
547 //ARMARX_IMPORTANT << VAROUT(targets.at(n)) << VAROUT(tcp_R->getPoseInRootFrame());
548 }
549 }
550 ikStats.at(2).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
551
552 start = TimeUtil::GetTime();
553 for (size_t n = 0; n < targets.size(); n++)
554 {
555 rns_R->setJointValues(initialConfigurations.at(0));
556 VirtualRobot::DifferentialIK diffIK(rns_R);
557 diffIK.setGoal(targets.at(n));
558 bool reached = diffIK.solveIK();
559 if (reached)
560 {
561 ikStats.at(3).solved = ikStats.at(3).solved + 1;
562 ikStats.at(3).elbow.emplace_back(elb_R->getPositionInRootFrame());
563 }
564 }
565 ikStats.at(3).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
566
567 start = TimeUtil::GetTime();
568 for (size_t n = 0; n < targets.size(); n++)
569 {
570 for (const Eigen::VectorXf& initjv : initialConfigurations)
571 {
572 rns_R->setJointValues(initjv);
573 CompositeDiffIK ik(rns_R);
574 CompositeDiffIK::TargetPtr t(new CompositeDiffIK::Target(
575 rns_R, tcp_R, targets.at(n), VirtualRobot::IKSolver::CartesianSelection::All));
576 ik.addTarget(t);
577 CompositeDiffIK::NullspaceJointTargetPtr nsjt(
578 new CompositeDiffIK::NullspaceJointTarget(rns_R));
579 nsjt->set(2, 0.2, 1);
580 ik.addNullspaceGradient(nsjt);
581 CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
582 new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
583 nsjla->setWeight(2, 0);
584 ik.addNullspaceGradient(nsjla);
585 CompositeDiffIK::Result result = ik.solve(pc);
586 if (result.reached)
587 {
588 ikStats.at(4).solved = ikStats.at(4).solved + 1;
589 ikStats.at(4).elbow.emplace_back(elb_R->getPositionInRootFrame());
590 break;
591 }
592 }
593 }
594 ikStats.at(4).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
595
596 for (size_t i = 0; i < ikStats.size(); i++)
597 {
598 ARMARX_IMPORTANT << ikStats.at(i).name << " solved: " << ikStats.at(i).solved
599 << ", T: " << ikStats.at(i).duration;
600 }
601
602 //ARMARX_IMPORTANT << "NaturalIK solved: " << ikStats.at(0).solved << ", T: " << durations.at(0);
603 //ARMARX_IMPORTANT << "SimpleDiffIK solved: " << ikStats.at(1).solved;
604 //ARMARX_IMPORTANT << "OptIK solved: " << ikStats.at(2).solved;
605 //ARMARX_IMPORTANT << "DiffIK solved: " << ikStats.at(3).solved;
606
607 //ARMARX_IMPORTANT << VAROUT(ikStats.at(0).averageElb());
608 //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).solved);
609 //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).averageElb());
610 }
611
612 struct Side
613 {
614 viz::Layer layer;
615 Eigen::Vector3f shoulder;
616 };
617
623
624 void
625 NaturalIKTest::vizTaskRun()
626 {
627 //ARMARX_IMPORTANT << "vizTask starts";
628 viz::Client arviz(*this);
629
630 viz::Layer layer_iksteps = arviz.layer("ikSteps");
631
632 viz::Layer layer_robot = arviz.layer("Robot");
633 viz::Robot vizrobot =
634 viz::Robot("robot")
635 .position(Eigen::Vector3f::Zero())
636 .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
637 vizrobot.useFullModel();
638 layer_robot.add(vizrobot);
639
640 CMakePackageFinder finder("armar6_rt");
641 std::string robotFile =
642 finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
643 ARMARX_IMPORTANT << "loading robot from " << robotFile;
644 VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
645
646 VirtualRobot::RobotNodeSetPtr rns_R = robot->getRobotNodeSet("RightArm");
647 VirtualRobot::RobotNodeSetPtr rns_L = robot->getRobotNodeSet("LeftArm");
648
649 VirtualRobot::RobotNodePtr sho1_R = rns_R->getNode(1);
650 VirtualRobot::RobotNodePtr sho1_L = rns_L->getNode(1);
651
652 VirtualRobot::RobotNodePtr elb_R = rns_R->getNode(4);
653 VirtualRobot::RobotNodePtr wri1_R = rns_R->getNode(6);
654
655 VirtualRobot::RobotNodePtr elb_L = rns_L->getNode(4);
656
657
658 //float value = 0.5f * (1.0f + std::sin(timeInSeconds));
659 //robot.joint("ArmR2_Sho1", value);
660 //robot.joint("ArmR3_Sho2", value);
661
662 //layer.add(robot);
663
664 float upper_arm_length =
665 (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
666 //float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
667 float lower_arm_length =
668 (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
669
670
671 viz::Layer layer_R = arviz.layer("Right");
672 viz::Layer layer_L = arviz.layer("Left");
673
674 //Eigen::Vector3f shoulder_R(NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
675 //Eigen::Vector3f shoulder_L(-NaturalIK::SoechtingAngles::MMM_SHOULDER_POS, 0, 0);
676 Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
677 Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
678
679 Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
680
681 NaturalIK ik_R("Right", shoulder_R, 1);
682 NaturalIK ik_L("Left", shoulder_L, 1);
683
684 ik_R.setUpperArmLength(upper_arm_length);
685 ik_R.setLowerArmLength(lower_arm_length);
686 ik_L.setUpperArmLength(upper_arm_length);
687 ik_L.setLowerArmLength(lower_arm_length);
688
689
690 NaturalIK::ArmJoints arm_R;
691 arm_R.rns = rns_R;
692 arm_R.elbow = elb_R;
693 arm_R.tcp = rns_R->getTCP();
694
695 NaturalIK::ArmJoints arm_L;
696 arm_L.rns = rns_L;
697 arm_L.elbow = elb_L;
698 arm_L.tcp = rns_L->getTCP();
699
700
701 auto makeVizSide =
702 [&](viz::Layer& layer, NaturalIK& ik, Eigen::Vector3f target, Eigen::Vector3f& err)
703 {
704 ik.setScale(p.p_R.scale);
705
706 Eigen::Vector3f vtgt = target;
707 NaturalIK::SoechtingAngles soechtingAngles = ik.CalculateSoechtingAngles(vtgt);
708 NaturalIK::SoechtingForwardPositions fwd = ik.forwardKinematics(soechtingAngles);
709 err = target - fwd.wrist;
710 vtgt = vtgt + 1.0 * err;
711
712 layer.add(viz::Box("Wrist_orig")
713 .position(fwd.wrist)
714 .size(20)
715 .color(viz::Color::fromRGBA(0, 0, 120)));
716
717 fwd = ik.forwardKinematics(ik.CalculateSoechtingAngles(vtgt));
718 err = target - fwd.wrist;
719
720
721 layer.clear();
722
723 layer.add(viz::Box("Shoulder")
724 .position(ik.getShoulderPos())
725 .size(20)
726 .color(viz::Color::fromRGBA(255, 0, 0)));
727 layer.add(viz::Box("Elbow").position(fwd.elbow).size(20).color(
728 viz::Color::fromRGBA(255, 0, 255)));
729 layer.add(viz::Box("Wrist").position(fwd.wrist).size(20).color(
730 viz::Color::fromRGBA(0, 0, 255)));
731
732 layer.add(viz::Box("Target").position(target).size(20).color(
733 viz::Color::fromRGBA(255, 165, 0)));
734
735 viz::Cylinder arrUpperArm = viz::Cylinder("UpperArm");
736 arrUpperArm.fromTo(ik.getShoulderPos(), fwd.elbow);
737 arrUpperArm.color(viz::Color::blue());
738
739 viz::Arrow arrLowerArm = viz::Arrow("LowerArm");
740 arrLowerArm.fromTo(fwd.elbow, fwd.wrist);
741 arrLowerArm.color(viz::Color::red());
742
743 layer.add(arrUpperArm);
744 layer.add(arrLowerArm);
745 };
746
747 CycleUtil c(20);
748 while (!vizTask->isStopped())
749 {
750 if (!p.targetValid)
751 {
752 c.waitForCycleDuration();
753 continue;
754 }
755
756 makeVizSide(layer_R, ik_R, p.p_R.target + offset, p.err_R);
757 makeVizSide(layer_L, ik_L, p.p_L.target + offset, p.err_L);
758
759
760 //Eigen::Quaternionf targetOri(0.70029222965240479,
761 // -0.6757887601852417,
762 // 0.036805182695388794,
763 // 0.22703713178634644);
764 //Eigen::Matrix4f target_R = math::Helpers::Pose(target + offset, targetOri.toRotationMatrix());
765
766 //Eigen::Quaternionf referenceOri(0,
767 // 0.70710688,
768 // -0.70710682,
769 // 0);
770 Eigen::Quaternionf referenceOri_R(0.70029222965240479,
771 -0.6757887601852417,
772 0.036805182695388794,
773 0.22703713178634644);
774 Eigen::Quaternionf referenceOri_L = mirrorOri(referenceOri_R);
775
776 Eigen::AngleAxisf aa_R(p.p_R.targetRotation.norm() / 180 * M_PI,
777 p.p_R.targetRotation.normalized());
778 Eigen::Matrix3f targetOri_R =
779 referenceOri_R.toRotationMatrix() * aa_R.toRotationMatrix();
780 Eigen::Matrix4f target_R = math::Helpers::Pose(p.p_R.target + offset, targetOri_R);
781
782 Eigen::AngleAxisf aa_L(p.p_L.targetRotation.norm() / 180 * M_PI,
783 p.p_L.targetRotation.normalized());
784 Eigen::Matrix3f targetOri_L =
785 referenceOri_L.toRotationMatrix() * aa_L.toRotationMatrix();
786 Eigen::Matrix4f target_L = math::Helpers::Pose(p.p_L.target + offset, targetOri_L);
787
788 // X_Platform
789 // Y_Platform
790 // Yaw_Platform
791 // ArmL1_Cla1
792 // ArmL2_Sho1
793 // ArmL3_Sho2
794 // ArmL4_Sho3
795 // ArmL5_Elb1
796 // ArmL6_Elb2
797 // ArmL7_Wri1
798 // ArmL8_Wri2
799 // ArmR1_Cla1
800 // ArmR2_Sho1
801 // ArmR3_Sho2
802 // ArmR4_Sho3
803 // ArmR5_Elb1
804 // ArmR6_Elb2
805 // ArmR7_Wri1
806 // ArmR8_Wri2
807
808 auto calcShoulderAngles = [&](NaturalIK& natik, Eigen::Matrix4f target)
809 {
810 NaturalIK::SoechtingForwardPositions fwd =
811 natik.solveSoechtingIK(math::Helpers::Position(target));
812 Eigen::Vector3f elb = fwd.elbow - fwd.shoulder;
813 float SE = fwd.soechtingAngles.SE;
814 elb = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
815 float SR = std::atan2(elb(0), -elb(2));
816 SR = std::max(-0.1f, SR);
817 ShoulderAngles sa;
818 sa.SE = SE;
819 sa.SR = SR;
820 sa.fwd = fwd;
821 return sa;
822 };
823
824 //VirtualRobot::RobotNodeSetPtr rnsP_R = robot->getRobotNodeSet("PlatformPlanningRightArm");
825 VirtualRobot::RobotNodeSetPtr rnsP = robot->getRobotNodeSet("PlatformPlanningBothArms");
826 if (p.useCompositeIK)
827 {
828 CompositeDiffIK cik(rnsP);
829
830
831 VirtualRobot::IKSolver::CartesianSelection mode_R =
832 p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All
833 : VirtualRobot::IKSolver::CartesianSelection::Position;
834 VirtualRobot::IKSolver::CartesianSelection mode_L =
835 p.p_L.setOri ? VirtualRobot::IKSolver::CartesianSelection::All
836 : VirtualRobot::IKSolver::CartesianSelection::Position;
837 CompositeDiffIK::TargetPtr tgt_R = cik.addTarget(arm_R.tcp, target_R, mode_R);
838 CompositeDiffIK::TargetPtr tgt_L = cik.addTarget(arm_L.tcp, target_L, mode_L);
839
840 CompositeDiffIK::NullspaceJointTargetPtr nsjt(
841 new CompositeDiffIK::NullspaceJointTarget(rnsP));
842 //NaturalIK::SoechtingForwardPositions fwd_R = ik_R.solveSoechtingIK(math::Helpers::Position(target_R));
843 //Eigen::Vector3f elb_R = fwd_R.elbow - fwd_R.shoulder;
844 //float SE = fwd_R.soechtingAngles.SE;
845 //elb_R = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
846 //float SR = std::atan2(elb_R(0), -elb_R(2));
847 //SR = std::max(-0.1f, SR);
848
849 ShoulderAngles sa_R = calcShoulderAngles(ik_R, target_R);
850 ShoulderAngles sa_L = calcShoulderAngles(ik_L, target_L);
851
852 nsjt->set("ArmR2_Sho1", +sa_R.SE, 1);
853 nsjt->set("ArmR3_Sho2", +sa_R.SR, 2);
854 nsjt->set("ArmR4_Sho3", -M_PI / 4, 0.5f);
855
856 nsjt->set("ArmL2_Sho1", -sa_L.SE, 1);
857 nsjt->set("ArmL3_Sho2", +sa_L.SR, 2);
858 nsjt->set("ArmL4_Sho3", +M_PI / 4, 0.5f);
859
860 nsjt->set("ArmR1_Cla1", 0, 0.5f);
861 nsjt->set("ArmL1_Cla1", 0, 0.5f);
862
863
864 //ARMARX_IMPORTANT << VAROUT(SE) << VAROUT(SR);
865 nsjt->kP = 1;
866 cik.addNullspaceGradient(nsjt);
867
868 //CompositeDiffIK::NullspaceTargetPtr nst_R(new CompositeDiffIK::NullspaceTarget(rnsP, arm_R.elbow,
869 // math::Helpers::CreatePose(fwd_R.elbow, Eigen::Matrix3f::Identity()),
870 // VirtualRobot::IKSolver::CartesianSelection::Position));
871 //nst_R->kP = 0;
872 //cik.addNullspaceGradient(nst_R);
873 CompositeDiffIK::NullspaceTargetPtr nst_R =
874 cik.addNullspacePositionTarget(arm_R.elbow, sa_R.fwd.elbow);
875 nst_R->kP = 0;
876 CompositeDiffIK::NullspaceTargetPtr nst_L =
877 cik.addNullspacePositionTarget(arm_L.elbow, sa_L.fwd.elbow);
878 nst_L->kP = 0;
879
880 CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
881 new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP));
882 nsjla->setWeight(0, 0);
883 nsjla->setWeight(1, 0);
884 nsjla->setWeight(2, 0);
885 nsjla->setWeight("ArmR4_Sho3", 0);
886 nsjla->setWeight("ArmL4_Sho3", 0);
887 nsjla->kP = 0.1;
888 cik.addNullspaceGradient(nsjla);
889 CompositeDiffIK::Parameters cp;
890 cp.resetRnsValues = true;
891 cp.returnIKSteps = true;
892 CompositeDiffIK::Result cikResult = cik.solve(cp);
893
894 layer_iksteps.clear();
895 int i = 0;
896 for (const CompositeDiffIK::TargetStep& s : tgt_R->ikSteps)
897 {
898 layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
899 .size(20)
900 .pose(s.tcpPose)
901 .color(viz::Color::blue()));
902 i++;
903 }
904 for (const CompositeDiffIK::TargetStep& s : tgt_L->ikSteps)
905 {
906 layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
907 .size(20)
908 .pose(s.tcpPose)
909 .color(viz::Color::blue()));
910 i++;
911 }
912 for (const CompositeDiffIK::NullspaceTargetStep& s : nst_R->ikSteps)
913 {
914 layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
915 .size(20)
916 .pose(s.tcpPose)
917 .color(viz::Color::blue()));
918 i++;
919 }
920 for (const CompositeDiffIK::NullspaceTargetStep& s : nst_L->ikSteps)
921 {
922 layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
923 .size(20)
924 .pose(s.tcpPose)
925 .color(viz::Color::blue()));
926 i++;
927 }
928 }
929 else
930 {
931 rnsP->getNode(0)->setJointValue(0);
932 rnsP->getNode(1)->setJointValue(0);
933 rnsP->getNode(2)->setJointValue(0);
934 NaturalDiffIK::Result ikResult;
935 if (p.p_R.setOri)
936 {
937 ikResult = ik_R.calculateIK(target_R, arm_R, p.p_R.ikparams);
938 }
939 else
940 {
941 ikResult = ik_R.calculateIKpos(
942 math::Helpers::Position(target_R), arm_R, p.p_R.ikparams);
943 }
944
945 layer_iksteps.clear();
946 std::stringstream ss;
947 int i = 0;
948 for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
949 {
950 ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm()
951 << "\n";
952 layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
953 .size(20)
954 .pose(s.tcpPose)
955 .color(viz::Color::blue()));
956 layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
957 .size(20)
958 .pose(s.elbPose)
959 .color(viz::Color::blue()));
960 i++;
961 }
962 //ARMARX_IMPORTANT << ss.str();
963 }
964
965
966 vizrobot.joints(rnsP->getJointValueMap());
967
968
969 arviz.commit({layer_R, layer_L, layer_robot, layer_iksteps});
970 //ARMARX_IMPORTANT << "arviz.commit";
971
972
973 c.waitForCycleDuration();
974 }
975 }
976
977 Eigen::Matrix4f
978 NaturalIKTest::mirrorPose(Eigen::Matrix4f oldPose)
979 {
980 Eigen::Quaternionf oriOld(oldPose.block<3, 3>(0, 0));
981 Eigen::Quaternionf ori;
982
983 ori.w() = oriOld.z();
984 ori.x() = oriOld.y() * -1;
985 ori.y() = oriOld.x() * -1;
986 ori.z() = oriOld.w();
987
988 Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
989 pose.block<3, 3>(0, 0) =
990 (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())).toRotationMatrix();
991 pose(0, 3) = -oldPose(0, 3);
992 pose(1, 3) = oldPose(1, 3);
993 pose(2, 3) = oldPose(2, 3);
994
995 return pose;
996 }
997
998 Eigen::Quaternionf
999 NaturalIKTest::mirrorOri(Eigen::Quaternionf oriOld)
1000 {
1001 Eigen::Quaternionf ori;
1002
1003 ori.w() = oriOld.z();
1004 ori.x() = oriOld.y() * -1;
1005 ori.y() = oriOld.x() * -1;
1006 ori.z() = oriOld.w();
1007
1008 return (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
1009 }
1010
1011 void
1015
1016 armarx::PropertyDefinitionsPtr
1022
1023 ARMARX_REGISTER_COMPONENT_EXECUTABLE(NaturalIKTest, "NaturalIKTest");
1024} // namespace armarx
lo
#define lo(x)
Definition AbstractInterface.h:47
hi
#define hi(x)
Definition AbstractInterface.h:46
ARMARX_REGISTER_COMPONENT_EXECUTABLE
#define ARMARX_REGISTER_COMPONENT_EXECUTABLE(ComponentT, applicationName)
Definition Decoupled.h:29
M_PI
#define M_PI
Definition MathTools.h:17
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition CMakePackageFinder.h:53
armarx::ComponentPropertyDefinitions::ComponentPropertyDefinitions
ComponentPropertyDefinitions(std::string prefix, bool hasObjectNameParameter=true)
Definition Component.cpp:46
armarx::Component::getTopicFromProperty
TopicProxyType getTopicFromProperty(const std::string &propertyName)
Get a topic proxy whose name is specified by the given property.
Definition Component.h:221
armarx::Component::offeringTopicFromProperty
void offeringTopicFromProperty(const std::string &propertyName)
Offer a topic whose name is specified by the given property.
Definition Component.cpp:182
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition Component.cpp:90
armarx::CompositeDiffIK::solve
Result solve(Parameters params)
Definition CompositeDiffIK.cpp:80
armarx::CompositeDiffIK::addTarget
void addTarget(const TargetPtr &target)
Definition CompositeDiffIK.cpp:45
armarx::CompositeDiffIK::NullspaceJointTargetPtr
std::shared_ptr< class NullspaceJointTarget > NullspaceJointTargetPtr
Definition CompositeDiffIK.h:118
armarx::CompositeDiffIK::NullspaceJointLimitAvoidancePtr
std::shared_ptr< class NullspaceJointLimitAvoidance > NullspaceJointLimitAvoidancePtr
Definition CompositeDiffIK.h:138
armarx::CompositeDiffIK::addNullspacePositionTarget
NullspaceTargetPtr addNullspacePositionTarget(const VirtualRobot::RobotNodePtr &tcp, const Eigen::Vector3f &target)
Definition CompositeDiffIK.cpp:67
armarx::CompositeDiffIK::NullspaceTargetPtr
std::shared_ptr< class NullspaceTarget > NullspaceTargetPtr
Definition CompositeDiffIK.h:98
armarx::CompositeDiffIK::addNullspaceGradient
void addNullspaceGradient(const NullspaceGradientPtr &gradient)
Definition CompositeDiffIK.cpp:61
armarx::CompositeDiffIK::TargetPtr
std::shared_ptr< Target > TargetPtr
Definition CompositeDiffIK.h:170
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition CycleUtil.h:41
armarx::NaturalIKTestPropertyDefinitions::NaturalIKTestPropertyDefinitions
NaturalIKTestPropertyDefinitions(std::string prefix)
Definition NaturalIKTest.cpp:51
armarx::NaturalIKTest
Brief description of class NaturalIKTest.
Definition NaturalIKTest.h:62
armarx::NaturalIKTest::onInitComponent
virtual void onInitComponent() override
Definition NaturalIKTest.cpp:72
armarx::NaturalIKTest::mirrorPose
Eigen::Matrix4f mirrorPose(Eigen::Matrix4f oldPose)
Definition NaturalIKTest.cpp:978
armarx::NaturalIKTest::onDisconnectComponent
virtual void onDisconnectComponent() override
Definition NaturalIKTest.cpp:101
armarx::NaturalIKTest::buildGui
RemoteGui::WidgetPtr buildGui()
Definition NaturalIKTest.cpp:108
armarx::NaturalIKTest::createPropertyDefinitions
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition NaturalIKTest.cpp:1017
armarx::NaturalIKTest::onConnectComponent
virtual void onConnectComponent() override
Definition NaturalIKTest.cpp:85
armarx::NaturalIKTest::mirrorOri
Eigen::Quaternionf mirrorOri(Eigen::Quaternionf oriOld)
Definition NaturalIKTest.cpp:999
armarx::NaturalIKTest::onExitComponent
virtual void onExitComponent() override
Definition NaturalIKTest.cpp:1012
armarx::NaturalIKTest::processGui
void processGui(RemoteGui::TabProxy &prx)
Definition NaturalIKTest.cpp:187
armarx::NaturalIKTest::getDefaultName
virtual std::string getDefaultName() const override
Definition NaturalIKTest.cpp:66
armarx::NaturalIK
The NaturalIK class.
Definition NaturalIK.h:53
armarx::NaturalIK::solveSoechtingIK
NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f &targetPos, std::optional< float > minElbowHeight=std::nullopt)
Definition NaturalIK.cpp:43
armarx::NaturalIK::calculateIK
NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f &targetPose, ArmJoints arm, NaturalIK::Parameters params=NaturalIK::Parameters())
Definition NaturalIK.cpp:88
armarx::NaturalIK::calculateIKpos
NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f &targetPos, ArmJoints arm, NaturalIK::Parameters params=NaturalIK::Parameters())
Definition NaturalIK.cpp:105
armarx::NaturalIK::setUpperArmLength
void setUpperArmLength(float value)
Definition NaturalIK.cpp:239
armarx::NaturalIK::setLowerArmLength
void setLowerArmLength(float value)
Definition NaturalIK.cpp:251
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition PropertyDefinitionContainer.h:367
armarx::PropertyDefinitionContainer::defineOptionalProperty
PropertyDefinition< PropertyType > & defineOptionalProperty(const std::string &name, PropertyType defaultValue, const std::string &description="", PropertyDefinitionBase::PropertyConstness constness=PropertyDefinitionBase::eConstant)
Definition PropertyDefinitionContainer.h:582
armarx::RemoteGui::TabProxy::getButtonClicked
bool getButtonClicked(std::string const &name)
Definition WidgetProxy.cpp:12
armarx::RemoteGui::TabProxy::getValue
ValueProxy< T > getValue(std::string const &name)
Definition WidgetProxy.h:167
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition TimeUtil.cpp:42
armarx::natik
Brief description of class natik.
Definition natik.h:39
armarx::targets
Brief description of class targets.
Definition targets.h:39
armarx::viz::Client::layer
virtual Layer layer(std::string const &name) const
Definition Client.cpp:80
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition Client.cpp:89
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition ElementOps.h:218
armarx::viz::ElementOps::position
DerivedT & position(float x, float y, float z)
Definition ElementOps.h:136
armarx::viz::Robot::useFullModel
Robot & useFullModel()
Definition Robot.h:42
armarx::viz::Robot::joints
Robot & joints(std::map< std::string, float > const &values)
Definition Robot.h:74
armarx::viz::Robot::file
Robot & file(std::string const &project, std::string const &filename)
Definition Robot.h:16
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
The logging level for always important information, but expected behaviour (in contrast to ARMARX_WAR...
Definition Logging.h:190
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition EigenForwardDeclarations.h:61
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx::RemoteGui::makeCheckBox
detail::CheckBoxBuilder makeCheckBox(std::string const &name)
Definition BoolWidgets.h:27
armarx::RemoteGui::makeFloatSlider
detail::FloatSliderBuilder makeFloatSlider(std::string const &name)
Definition FloatWidgets.h:64
armarx::RemoteGui::makeVBoxLayout
detail::VBoxLayoutBuilder makeVBoxLayout(std::string const &name="")
Definition LayoutWidgets.h:286
armarx::RemoteGui::makeSimpleGridLayout
detail::SimpleGridLayoutBuilder makeSimpleGridLayout(std::string const &name="")
Definition LayoutWidgets.h:274
armarx::RemoteGui::makeButton
detail::ButtonBuilder makeButton(std::string const &name)
Definition IntegerWidgets.h:52
armarx::RemoteGui::makeLabel
detail::LabelBuilder makeLabel(std::string const &name)
Definition StaticWidgets.h:18
armarx::RemoteGui::makeGroupBox
detail::GroupBoxBuilder makeGroupBox(std::string const &name="")
Definition LayoutWidgets.h:292
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::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition VectorHelpers.h:327
armarx::IKStats::IKStats
IKStats(const std::string &name)
Definition NaturalIKTest.cpp:248
armarx::IKStats::averageElb
Eigen::Vector3f averageElb()
Definition NaturalIKTest.cpp:258
armarx::IKStats::elbow
std::vector< Eigen::Vector3f > elbow
Definition NaturalIKTest.cpp:255
armarx::NaturalDiffIK::Result::ikSteps
std::vector< IKStep > ikSteps
Definition NaturalDiffIK.h:89
armarx::NaturalIK::ArmJoints::elbow
VirtualRobot::RobotNodePtr elbow
Definition NaturalIK.h:68
armarx::NaturalIK::ArmJoints::rns
VirtualRobot::RobotNodeSetPtr rns
Definition NaturalIK.h:67
armarx::NaturalIK::ArmJoints::tcp
VirtualRobot::RobotNodePtr tcp
Definition NaturalIK.h:70
armarx::NaturalIK::Parameters::diffIKparams
NaturalDiffIK::Parameters diffIKparams
Definition NaturalIK.h:61
armarx::RemoteGui::detail::ChildrenMixin::addChild
Derived & addChild(WidgetPtr const &child)
Definition LayoutWidgets.h:27
armarx::RemoteGui::detail::LabelMixin::label
Derived & label(std::string const &label)
Definition Basic.h:216
armarx::ShoulderAngles::fwd
NaturalIK::SoechtingForwardPositions fwd
Definition NaturalIKTest.cpp:621
armarx::Side::layer
viz::Layer layer
Definition NaturalIKTest.cpp:614
armarx::Side::shoulder
Eigen::Vector3f shoulder
Definition NaturalIKTest.cpp:615
armarx::SoftPositionConstraint::tcp
VirtualRobot::SceneObjectPtr tcp
Definition NaturalIKTest.cpp:288
armarx::SoftPositionConstraint::optimizationGradient
Eigen::VectorXf optimizationGradient(unsigned int id) override
Definition NaturalIKTest.cpp:291
armarx::SoftPositionConstraint::SoftPositionConstraint
SoftPositionConstraint(const VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &nodeSet, const VirtualRobot::SceneObjectPtr &node, const VirtualRobot::SceneObjectPtr &tcp, const Eigen::Vector3f &target, VirtualRobot::IKSolver::CartesianSelection cartesianSelection=VirtualRobot::IKSolver::All, float tolerance=3.0f)
Definition NaturalIKTest.cpp:272
armarx::viz::Arrow::fromTo
Arrow & fromTo(const Eigen::Vector3f &from, const Eigen::Vector3f &to)
Definition Elements.h:219
armarx::viz::Box::size
Box & size(Eigen::Vector3f const &s)
Definition Elements.h:52
armarx::viz::Cylinder::fromTo
Cylinder & fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
Definition Elements.cpp:93
armarx::viz::Layer::add
void add(ElementT const &element)
Definition Layer.h:31