From 5467b75323b870d01a10254948bb3411159e5297 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Mon, 8 Jun 2020 15:33:01 +0200
Subject: [PATCH] NaturalIKTest
---
.../components/NaturalIKTest/NaturalIKTest.cpp | 13 +++++++++++++
.../RobotAPI/libraries/diffik/CompositeDiffIK.cpp | 7 ++++++-
source/RobotAPI/libraries/diffik/CompositeDiffIK.h | 1 +
3 files changed, 20 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 2daf4424e..f230c8305 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -582,6 +582,7 @@ namespace armarx
ik_L.setLowerArmLength(lower_arm_length);
+
NaturalIK::ArmJoints arm_R;
arm_R.rns = rns_R;
arm_R.elbow = elb_R;
@@ -664,6 +665,7 @@ namespace armarx
Eigen::Matrix4f target_R = math::Helpers::Pose(p.target + offset, targetOri);
+
NaturalDiffIK::Result ikResult;
if (p.setOri)
{
@@ -686,6 +688,17 @@ namespace armarx
}
//ARMARX_IMPORTANT << ss.str();
+ CompositeDiffIK cik_R(rns_R);
+ cik_R.addTarget(arm_R.tcp, target_R, VirtualRobot::IKSolver::CartesianSelection::All);
+ CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rns_R));
+ nsjt->set(2, 0.2, 1);
+ cik_R.addNullspaceGradient(nsjt);
+ CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
+ nsjla->set(2, 0);
+ cik_R.addNullspaceGradient(nsjla);
+ CompositeDiffIK::Paramaters cp;
+ CompositeDiffIK cikResult = cik_R.solve(cp);
+
vizrobot.joints(arm_R.rns->getJointValueMap());
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
index daefa367e..0e72540bd 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
@@ -34,11 +34,16 @@ CompositeDiffIK::CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns)
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
}
-void CompositeDiffIK::addTarget(const CompositeDiffIK::TargetPtr& target)
+void CompositeDiffIK::addTarget(const TargetPtr& target)
{
targets.emplace_back(target);
}
+void CompositeDiffIK::addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ addTarget(std::make_shared<Target>(rns, tcp, target, mode));
+}
+
void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
{
nullspaceGradients.emplace_back(gradient);
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
index f306fb6ed..5bf5f383b 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -153,6 +153,7 @@ namespace armarx
CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns);
void addTarget(const TargetPtr& target);
+ void addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
void addNullspaceGradient(const NullspaceGradientPtr& gradient);
Result solve(Parameters params);
--
GitLab