diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 9d84fcf72b459f97cf17477113b4ea4b688c7d45..050bdb775a65ff522de0380c250cb7f0644a165e 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -32,6 +32,16 @@
#include <random>
+#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
+
+#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
+#include <VirtualRobot/IK/constraints/PoseConstraint.h>
+#include <VirtualRobot/IK/constraints/PositionConstraint.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+
+#include <RobotAPI/libraries/diffik/CompositeDiffIK.h>
+
namespace armarx
{
NaturalIKTestPropertyDefinitions::NaturalIKTestPropertyDefinitions(std::string prefix) :
@@ -183,7 +193,10 @@ namespace armarx
struct IKStats
{
+ IKStats(const std::string& name) : name(name) {}
+ std::string name;
int solved = 0;
+ float duration = 0;
std::vector<Eigen::Vector3f> elbow;
Eigen::Vector3f averageElb()
{
@@ -196,6 +209,51 @@ namespace armarx
}
};
+ struct SoftPositionConstraint : public VirtualRobot::PositionConstraint
+ {
+ public:
+ 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)
+ : PositionConstraint(robot, nodeSet, node, target, cartesianSelection, tolerance), tcp(tcp)
+ {
+ optimizationFunctions.clear();
+
+ addOptimizationFunction(0, true);
+ }
+ VirtualRobot::SceneObjectPtr tcp;
+
+
+ Eigen::VectorXf optimizationGradient(unsigned int id) override
+ {
+ int size = nodeSet->getSize();
+
+ Eigen::MatrixXf J = ik->getJacobianMatrix(tcp);//.block(0, 0, 3, size);
+ //Eigen::Vector3f d = eef->getGlobalPose().block<3,1>(0,3) - target;
+
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(J);
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+ Eigen::VectorXf grad = PositionConstraint::optimizationGradient(id);
+
+ Eigen::VectorXf mapped = Eigen::VectorXf::Zero(grad.size());
+ for (int i = 0; i < nullspace.cols(); i++)
+ {
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ mapped += nullspace.col(i) * nullspace.col(i).dot(grad) / nullspace.col(i).squaredNorm();
+ }
+ }
+ return mapped;
+ }
+
+ bool checkTolerances() override
+ {
+ return true;
+ }
+ };
+
void NaturalIKTest::testTaskRun()
{
CMakePackageFinder finder("Armar6RT");
@@ -241,23 +299,34 @@ namespace armarx
arm_R.tcp = rns_R->getTCP();
std::vector<NaturalIK::Parameters> ikParamList;
- std::vector<IKStats> ikStats = {IKStats(), IKStats()};
+ std::vector<IKStats> ikStats = {IKStats("NaturalIK"), IKStats("SimpleDiffIK"), IKStats("OptIK"), IKStats("DiffIK"), IKStats("CompositeIK")};
+
+ NaturalIK::Parameters p0;
+ p0.diffIKparams.resetRnsValues = false;
+ p0.diffIKparams.ikStepLengthInitial = 1;
+ p0.diffIKparams.ikStepLengthFineTune = 1;
+ p0.diffIKparams.maxJointAngleStep = 0.2f;
{
- NaturalIK::Parameters p;
+ NaturalIK::Parameters p = p0;
p.diffIKparams.resetRnsValues = false;
p.diffIKparams.elbowKp = 1;
p.diffIKparams.jointLimitAvoidanceKp = 0.1;
+ p.diffIKparams.ikStepLengthInitial = 1;
ikParamList.emplace_back(p);
}
{
- NaturalIK::Parameters p;
+ NaturalIK::Parameters p = p0;
p.diffIKparams.resetRnsValues = false;
p.diffIKparams.elbowKp = 0;
p.diffIKparams.jointLimitAvoidanceKp = 1;
ikParamList.emplace_back(p);
}
+ CompositeDiffIK::Parameters pc;
+ pc.resetRnsValues = false;
+
+
std::random_device rd; //Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
@@ -276,27 +345,27 @@ namespace armarx
}
}
- //rns_R->getNode(1)->setJointValue(0);
- //rns_R->getNode(3)->setJointValue(0);
- //rns_R->getNode(5)->setJointValue(0);
+ rns_R->getNode(1)->setJointValue(0);
+ rns_R->getNode(3)->setJointValue(0);
+ rns_R->getNode(5)->setJointValue(0);
+ initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
+ rns_R->getNode(1)->setJointValue(0);
+ rns_R->getNode(3)->setJointValue(0);
+ rns_R->getNode(5)->setJointValue(M_PI);
+ initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
+ rns_R->getNode(1)->setJointValue(0);
+ rns_R->getNode(3)->setJointValue(M_PI);
+ rns_R->getNode(5)->setJointValue(0);
+ initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
+ rns_R->getNode(1)->setJointValue(0);
+ rns_R->getNode(3)->setJointValue(M_PI);
+ rns_R->getNode(5)->setJointValue(M_PI);
initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
- //rns_R->getNode(1)->setJointValue(0);
- //rns_R->getNode(3)->setJointValue(0);
- //rns_R->getNode(5)->setJointValue(M_PI);
- //initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
- //rns_R->getNode(1)->setJointValue(0);
- //rns_R->getNode(3)->setJointValue(M_PI);
- //rns_R->getNode(5)->setJointValue(0);
- //initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
- //rns_R->getNode(1)->setJointValue(0);
- //rns_R->getNode(3)->setJointValue(M_PI);
- //rns_R->getNode(5)->setJointValue(M_PI);
- //initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
std::vector<Eigen::Matrix4f> targets;
- while (targets.size() < 1000)
+ while (targets.size() < 100)
{
for (size_t i = 0; i < rns_R->getSize(); i++)
{
@@ -318,9 +387,12 @@ namespace armarx
}
}
- for (size_t n = 0; n < targets.size(); n++)
+
+ IceUtil::Time start;
+ for (size_t i = 0; i < ikParamList.size(); i++)
{
- for (size_t i = 0; i < ikParamList.size(); i++)
+ start = TimeUtil::GetTime();
+ for (size_t n = 0; n < targets.size(); n++)
{
for (const Eigen::VectorXf& initjv : initialConfigurations)
{
@@ -334,12 +406,110 @@ namespace armarx
}
}
}
+ ikStats.at(i).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
}
+ start = TimeUtil::GetTime();
+
+ std::vector<VirtualRobot::RobotNodePtr> elbJoints_R;
+ for (int i = 0; i < 4; i++)
+ {
+ elbJoints_R.push_back(rns_R->getNode(i));
+ }
+ VirtualRobot::RobotNodeSetPtr rnsElb_R = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, "ELB_R", elbJoints_R, rns_R->getKinematicRoot(), elb_R);
+ for (size_t n = 0; n < targets.size(); n++)
+ {
+ rns_R->setJointValues(initialConfigurations.at(0));
+ VirtualRobot::ConstrainedOptimizationIK optIK(robot, rns_R);
+ //VirtualRobot::ConstraintPtr conPose(new VirtualRobot::PoseConstraint(robot, rns_R, tcp_R, targets.at(n)));
+ //optIK.addConstraint(conPose);
+ VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(robot, rns_R, tcp_R,
+ math::Helpers::GetPosition(targets.at(n)), VirtualRobot::IKSolver::CartesianSelection::All));
+ posConstraint->setOptimizationFunctionFactor(1);
+
+ VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(robot, rns_R, tcp_R,
+ math::Helpers::GetOrientation(targets.at(n)), VirtualRobot::IKSolver::CartesianSelection::All, VirtualRobot::MathTools::deg2rad(2)));
+ oriConstraint->setOptimizationFunctionFactor(1000);
+
+
+ Eigen::Vector3f elbowPos = ik_R.solveSoechtingIK(math::Helpers::GetPosition(targets.at(n))).elbow;
+
+ VirtualRobot::ConstraintPtr elbConstraint(new SoftPositionConstraint(robot, rns_R, elb_R, tcp_R,
+ elbowPos, VirtualRobot::IKSolver::CartesianSelection::All));
+ elbConstraint->setOptimizationFunctionFactor(0.01);
+ //elbConstraint->
+
+ //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationFunction(0));
+ //ARMARX_IMPORTANT << VAROUT(elbConstraint->optimizationGradient(0));
+
+ optIK.addConstraint(posConstraint);
+ optIK.addConstraint(oriConstraint);
+ //optIK.addConstraint(elbConstraint);
+
+ optIK.initialize();
+ bool reached = optIK.solve();
+ if (reached)
+ {
+ ikStats.at(2).solved = ikStats.at(2).solved + 1;
+ ikStats.at(2).elbow.emplace_back(elb_R->getPositionInRootFrame());
+ //float perr = CartesianPositionController::GetPositionError(targets.at(n), tcp_R);
+ //float oerr = 180 / M_PI * CartesianPositionController::GetOrientationError(targets.at(n), tcp_R);
+ //ARMARX_IMPORTANT << VAROUT(perr) << VAROUT(oerr);
+ //ARMARX_IMPORTANT << VAROUT(targets.at(n)) << VAROUT(tcp_R->getPoseInRootFrame());
+ }
+ }
+ ikStats.at(2).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
+
+ start = TimeUtil::GetTime();
+ for (size_t n = 0; n < targets.size(); n++)
+ {
+ rns_R->setJointValues(initialConfigurations.at(0));
+ VirtualRobot::DifferentialIK diffIK(rns_R);
+ diffIK.setGoal(targets.at(n));
+ bool reached = diffIK.solveIK();
+ if (reached)
+ {
+ ikStats.at(3).solved = ikStats.at(3).solved + 1;
+ ikStats.at(3).elbow.emplace_back(elb_R->getPositionInRootFrame());
+ }
+ }
+ ikStats.at(3).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
+
+ start = TimeUtil::GetTime();
+ for (size_t n = 0; n < targets.size(); n++)
+ {
+ for (const Eigen::VectorXf& initjv : initialConfigurations)
+ {
+ rns_R->setJointValues(initjv);
+ CompositeDiffIK ik(rns_R);
+ CompositeDiffIK::TargetPtr t(new CompositeDiffIK::Target(rns_R, tcp_R, targets.at(n), VirtualRobot::IKSolver::CartesianSelection::All));
+ ik.addTarget(t);
+ CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rns_R));
+ nsjt->set(2, 0.2, 1);
+ ik.addNullspaceGradient(nsjt);
+ CompositeDiffIK::Result result = ik.solve(pc);
+ if (result.reached)
+ {
+ ikStats.at(4).solved = ikStats.at(4).solved + 1;
+ ikStats.at(4).elbow.emplace_back(elb_R->getPositionInRootFrame());
+ break;
+ }
+ }
+ }
+ ikStats.at(4).duration = (TimeUtil::GetTime() - start).toSecondsDouble();
+
+ for (size_t i = 0; i < ikStats.size(); i++)
+ {
+ ARMARX_IMPORTANT << ikStats.at(i).name << " solved: " << ikStats.at(i).solved << ", T: " << ikStats.at(i).duration;
+ }
+
+ //ARMARX_IMPORTANT << "NaturalIK solved: " << ikStats.at(0).solved << ", T: " << durations.at(0);
+ //ARMARX_IMPORTANT << "SimpleDiffIK solved: " << ikStats.at(1).solved;
+ //ARMARX_IMPORTANT << "OptIK solved: " << ikStats.at(2).solved;
+ //ARMARX_IMPORTANT << "DiffIK solved: " << ikStats.at(3).solved;
- ARMARX_IMPORTANT << VAROUT(ikStats.at(0).solved);
- ARMARX_IMPORTANT << VAROUT(ikStats.at(0).averageElb());
- ARMARX_IMPORTANT << VAROUT(ikStats.at(1).solved);
- ARMARX_IMPORTANT << VAROUT(ikStats.at(1).averageElb());
+ //ARMARX_IMPORTANT << VAROUT(ikStats.at(0).averageElb());
+ //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).solved);
+ //ARMARX_IMPORTANT << VAROUT(ikStats.at(1).averageElb());
}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 2765f2985a9ac0df68913aa73fa58c25bc5da51d..fb8742a15cc65b0f8614744b66dbc0f1d4222388 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -115,6 +115,25 @@ namespace armarx
&& (!checkOri || GetOrientationError(targetPose, tcp) < thresholdOriReached);
}
+ bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
+ {
+ if (mode & VirtualRobot::IKSolver::Position)
+ {
+ if (GetPositionError(targetPose, tcp) > thresholdPosReached)
+ {
+ return false;
+ }
+ }
+ if (mode & VirtualRobot::IKSolver::Orientation)
+ {
+ if (GetOrientationError(targetPose, tcp) > thresholdOriReached)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 5ecf2c66e3f25dcb7354930e2c4a7a888617f006..dfc8c96351c9ea53cae128339babb472e31a1c60 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -54,6 +54,7 @@ namespace armarx
static float GetPositionError(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp);
static float GetOrientationError(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp);
static bool Reached(const Eigen::Matrix4f& targetPose, const VirtualRobot::RobotNodePtr& tcp, bool checkOri, float thresholdPosReached, float thresholdOriReached);
+ bool reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached);
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index a3a577198b9a6a80534013ef3aea1218aec7da12..1c2b0b28ee9607a989e25ca984e12fb893ce6ca4 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -63,12 +63,12 @@ namespace armarx
Eigen::VectorXf maximumJointVelocities;
void setJointCosts(const std::vector<float>& _jointCosts);
+ Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
private:
void calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::MatrixXf _jacobiWithCosts;
Eigen::MatrixXf _inv;
- Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
bool clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& _inv, float jointLimitCheckAccuracy = 0.001f);
bool _considerJointLimits = true;
float _cartesianMMRegularization;
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
index 05a777679813c9f2411dbe6cce56141ab75dae77..fce8840d1b6643ea53cc63d18f2703358eb37dde 100644
--- a/source/RobotAPI/libraries/diffik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -15,6 +15,7 @@ set(LIB_FILES
SimpleDiffIK.cpp
RobotPlacement.cpp
GraspTrajectory.cpp
+ CompositeDiffIK.cpp
)
set(LIB_HEADERS
NaturalDiffIK.h
@@ -22,6 +23,7 @@ set(LIB_HEADERS
DiffIKProvider.h
RobotPlacement.h
GraspTrajectory.h
+ CompositeDiffIK.h
)
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df655bd741aaacf783425877999319f4c44028aa
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
@@ -0,0 +1,264 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CompositeDiffIK.h"
+#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <VirtualRobot/math/Helpers.h>
+
+using namespace armarx;
+
+CompositeDiffIK::CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns)
+ : rns(rns)
+{
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+}
+
+void CompositeDiffIK::addTarget(const CompositeDiffIK::TargetPtr& target)
+{
+ targets.emplace_back(target);
+}
+
+void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
+{
+ nullspaceGradients.emplace_back(gradient);
+}
+
+
+CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
+{
+ if (params.resetRnsValues)
+ {
+ for (VirtualRobot::RobotNodePtr rn : rns->getAllRobotNodes())
+ {
+ if (rn->isLimitless())
+ {
+ rn->setJointValue(0);
+ }
+ else
+ {
+ rn->setJointValue((rn->getJointLimitHi() + rn->getJointLimitLo()) / 2);
+ }
+ }
+ }
+
+ int rows = 0;
+ int cols = rns->getSize();
+
+ for (const TargetPtr& target : targets)
+ {
+ rows += CartesianSelectionToSize(target->mode);
+ }
+
+ Eigen::VectorXf regularization = Eigen::VectorXf::Zero(rows);
+ {
+ CartesianVelocityController tmpVC(rns);
+ int row = 0;
+ for (const TargetPtr& target : targets)
+ {
+ int h = CartesianSelectionToSize(target->mode);
+ target->jacobi = Eigen::MatrixXf::Zero(h, cols);
+ regularization.block(row, 0, h, 1) = tmpVC.calculateRegularization(target->mode);
+ row += h;
+ }
+ }
+
+ Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
+
+ for (size_t i = 0; i <= params.steps; i++)
+ {
+ Eigen::MatrixXf jacobi = Eigen::MatrixXf::Zero(rows, cols);
+ Eigen::MatrixXf invJac = Eigen::MatrixXf::Zero(cols, rows);
+
+ Eigen::VectorXf cartesianVel = Eigen::VectorXf::Zero(rows);
+ {
+ int row = 0;
+ for (const TargetPtr& target : targets)
+ {
+ ik->updateJacobianMatrix(target->jacobi, target->tcp, target->mode);
+ int h = CartesianSelectionToSize(target->mode);
+ jacobi.block(row, 0, h, cols) = target->jacobi;
+ cartesianVel.block(row, 0, h, 1) = target->pCtrl.calculate(target->target, target->mode);
+ row += h;
+ }
+ }
+
+ ik->updatePseudoInverseJacobianMatrix(invJac, jacobi, 0, regularization);
+
+ Eigen::VectorXf nullspaceVel = Eigen::VectorXf::Zero(cols);
+
+ for (const NullspaceGradientPtr& nsGradient : nullspaceGradients)
+ {
+ nullspaceVel += nsGradient->kP * nsGradient->getGradient();
+ }
+ LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
+
+ Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+ Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+ Eigen::VectorXf nsv = Eigen::VectorXf::Zero(cols);
+ for (int i = 0; i < nullspace.cols(); i++)
+ {
+ float squaredNorm = nullspace.col(i).squaredNorm();
+ // Prevent division by zero
+ if (squaredNorm > 1.0e-32f)
+ {
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ }
+ }
+ Eigen::VectorXf jv = invJac * cartesianVel + nsv;
+ LimitInfNormTo(jv, params.maxJointAngleStep);
+
+ Eigen::VectorXf newJointValues = currentJointValues + jv;
+ rns->setJointValues(newJointValues);
+ currentJointValues = newJointValues;
+ }
+
+ bool allReached = true;
+ for (const TargetPtr& target : targets)
+ {
+ allReached = allReached && target->pCtrl.reached(target->target, target->mode, target->maxPosError, target->maxOriError);
+ }
+
+
+ //ARMARX_IMPORTANT << ss.str();
+
+ Result result;
+ result.jointValues = rns->getJointValuesEigen();
+ result.reached = allReached;
+ result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
+ result.minimumJointLimitMargin = FLT_MAX;
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ result.jointLimitMargins(i) = M_PI;
+ }
+ else
+ {
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ }
+ }
+
+ return result;
+}
+
+int CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode)
+{
+ switch (mode)
+ {
+ case VirtualRobot::IKSolver::CartesianSelection::Position:
+ return 3;
+ case VirtualRobot::IKSolver::CartesianSelection::Orientation:
+ return 3;
+ case VirtualRobot::IKSolver::CartesianSelection::All:
+ return 6;
+ default:
+ throw LocalException("mode not supported: ") << mode;
+ }
+}
+
+CompositeDiffIK::Target::Target(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
+ : tcp(tcp), target(target), mode(mode), pCtrl(tcp)
+{
+ jacobi = Eigen::MatrixXf::Zero(0, 0);
+}
+
+CompositeDiffIK::NullspaceTarget::NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
+ : tcp(tcp), target(target), mode(mode), pCtrl(tcp), vCtrl(rns, tcp)
+{
+
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceTarget::getGradient()
+{
+ Eigen::VectorXf cartesianVel = pCtrl.calculate(target, mode);
+ return vCtrl.calculate(cartesianVel, mode);
+}
+
+Eigen::VectorXf CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+{
+ float infNorm = vec.lpNorm<Eigen::Infinity>();
+ if (infNorm > maxValue)
+ {
+ vec = vec / infNorm * maxValue;
+ }
+ return vec;
+}
+
+CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns)
+ : rns(rns), target(rns->getJointValuesEigen()), scale(Eigen::VectorXf::Zero(rns->getSize()))
+{
+
+}
+
+CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& target, const Eigen::VectorXf& scale)
+ : rns(rns), target(target), scale(scale)
+{
+
+}
+
+void CompositeDiffIK::NullspaceJointTarget::set(int index, float target, float scale)
+{
+ this->target(index) = target;
+ this->scale(scale) = scale;
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceJointTarget::getGradient()
+{
+ return (target - rns->getJointValuesEigen()).cwiseProduct(scale);
+}
+
+CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns)
+ : rns(rns), scale(Eigen::VectorXf::Constant(rns->getSize(), 1))
+{
+
+}
+
+CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& scale)
+ : rns(rns), scale(scale)
+{
+
+}
+
+Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient()
+{
+ Eigen::VectorXf r(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ VirtualRobot::RobotNodePtr rn = rns->getNode(i);
+ if (rn->isLimitless())
+ {
+ r(i) = 0;
+ }
+ else
+ {
+ float f = math::Helpers::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ r(i) = cos(f * M_PI);
+ //r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
+ }
+ }
+ return r.cwiseProduct(scale);
+}
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
new file mode 100644
index 0000000000000000000000000000000000000000..5da910e5d178ab5bb12a3bf1a52135bc1ceb013a
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -0,0 +1,169 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include <memory>
+
+
+namespace armarx
+{
+
+
+ typedef std::shared_ptr<class CompositeDiffIK> CompositeDiffIKPtr;
+ class CompositeDiffIK
+ {
+ public:
+ class NullspaceGradient
+ {
+ public:
+ virtual Eigen::VectorXf getGradient() = 0;
+ float kP = 1;
+ };
+ typedef std::shared_ptr<class NullspaceGradient> NullspaceGradientPtr;
+
+ class NullspaceTarget : public NullspaceGradient
+ {
+ public:
+ NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::Matrix4f target;
+ VirtualRobot::IKSolver::CartesianSelection mode;
+ CartesianPositionController pCtrl;
+ CartesianVelocityController vCtrl;
+
+ Eigen::VectorXf getGradient() override;
+ };
+ typedef std::shared_ptr<class NullspaceTarget> NullspaceTargetPtr;
+
+ class NullspaceJointTarget : public NullspaceGradient
+ {
+ public:
+ NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns);
+ NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& target, const Eigen::VectorXf& scale);
+ void set(int index, float target, float scale);
+
+ VirtualRobot::RobotNodeSetPtr rns;
+ Eigen::VectorXf target;
+ Eigen::VectorXf scale;
+
+ Eigen::VectorXf getGradient() override;
+ };
+ typedef std::shared_ptr<class NullspaceJointTarget> NullspaceJointTargetPtr;
+
+ class NullspaceJointLimitAvoidance : public NullspaceGradient
+ {
+ public:
+ NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns);
+ NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& scale);
+
+ VirtualRobot::RobotNodeSetPtr rns;
+ Eigen::VectorXf scale;
+
+ Eigen::VectorXf getGradient() override;
+ };
+ typedef std::shared_ptr<class NullspaceJointLimitAvoidance> NullspaceJointLimitAvoidancePtr;
+
+
+ class Target
+ {
+ public:
+ Target(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+ VirtualRobot::RobotNodePtr tcp;
+ Eigen::Matrix4f target;
+ VirtualRobot::IKSolver::CartesianSelection mode;
+ Eigen::MatrixXf jacobi;
+ CartesianPositionController pCtrl;
+ float maxPosError = 10.f;
+ float maxOriError = 0.05f;
+ };
+ typedef std::shared_ptr<Target> TargetPtr;
+
+ struct Parameters
+ {
+ Parameters() {}
+ // IK params
+ size_t steps = 40;
+
+ float maxJointAngleStep = 0.1f;
+ bool resetRnsValues = true;
+ bool returnIKSteps = false;
+ };
+ struct IKStep
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f pdTcp;
+ Eigen::Vector3f odTcp;
+ Eigen::Vector3f pdElb;
+ Eigen::Matrix4f tcpPose;
+ Eigen::Matrix4f elbPose;
+ Eigen::VectorXf cartesianVel;
+ Eigen::VectorXf cartesianVelElb;
+ Eigen::VectorXf jvElb;
+ Eigen::VectorXf jvLA;
+ Eigen::VectorXf jv;
+ Eigen::VectorXf jvClamped;
+ };
+
+ struct Result
+ {
+ Eigen::VectorXf jointValues;
+ Eigen::Vector3f posDiff;
+ Eigen::Vector3f posDiffElbow;
+ Eigen::Vector3f oriDiff;
+ float posError;
+ float posErrorElbow;
+ float oriError;
+ bool reached;
+ Eigen::VectorXf jointLimitMargins;
+ float minimumJointLimitMargin;
+ Eigen::Vector3f elbowPosDiff;
+ std::vector<IKStep> ikSteps;
+ };
+
+ static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
+
+ CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns);
+
+ void addTarget(const TargetPtr& target);
+ void addNullspaceGradient(const NullspaceGradientPtr& gradient);
+
+ Result solve(Parameters params);
+
+ int CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode);
+
+
+ private:
+ VirtualRobot::RobotNodeSetPtr rns;
+ std::vector<TargetPtr> targets;
+ std::vector<NullspaceGradientPtr> nullspaceGradients;
+ VirtualRobot::DifferentialIKPtr ik;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index f9a6e2a1022e46cae08520a4313a503fc52e022c..4d30bc95d80d65a721bc2394fa8672d1a6c971bb 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -68,6 +68,9 @@ namespace armarx
//ARMARX_IMPORTANT << "start";
+ int posMet = 0;
+ int oriMet = 0;
+
std::vector<IKStep> ikSteps;
Eigen::VectorXf currentJointValues = rns->getJointValuesEigen();
for (size_t i = 0; i <= params.stepsInitial + params.stepsFineTune; i++)
@@ -121,6 +124,29 @@ namespace armarx
Eigen::VectorXf newJointValues = currentJointValues + jvClamped;
rns->setJointValues(newJointValues);
currentJointValues = newJointValues;
+
+ if (pdTcp.norm() > params.maxPosError)
+ {
+ posMet = 0;
+ }
+ else
+ {
+ posMet++;
+ }
+ if (odTcp.norm() > params.maxOriError)
+ {
+ oriMet = 0;
+ }
+ else
+ {
+ oriMet++;
+ }
+
+ // terminate early, if reached for at least 3 iterations
+ if (posMet > 2 && oriMet > 2)
+ {
+ break;
+ }
}
//ARMARX_IMPORTANT << ss.str();