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