diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
index 2af62e09d977b13643124e5d2204fcccf54a0016..f35569f728c3412be25276ca6d6fce59da010786 100644
--- a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.cpp
@@ -29,7 +29,7 @@ namespace VirtualRobot {
CompositeDiffIK::CompositeDiffIK(const RobotNodeSetPtr& rns)
: rns(rns), robot(rns->getRobot())
{
- ik.reset(new DifferentialIK(rns, rns->getRobot()->getRootNode(), JacobiProvider::eSVDDamped));
+ ik.reset(new DifferentialIK(rns, rns->getRobot()->getRootNode(), JacobiProvider::eSVDDampedDynamic));
}
void CompositeDiffIK::addTarget(const TargetPtr& target)
@@ -121,7 +121,7 @@ CompositeDiffIK::Result CompositeDiffIK::getLastResult() {
bool allReached = true;
for (const TargetPtr& target : targets)
{
- allReached = allReached && target->pCtrl.reached(target->target, target->mode, target->maxPosError, target->maxOriError);
+ allReached = allReached && target->isReached();
}
Result result;
diff --git a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
index c341f2ce5dc1c47655a73b03618286318eb61376..71e27d22469016e0a6a246255140428d4687f6a8 100644
--- a/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
+++ b/VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h
@@ -177,6 +177,10 @@ namespace VirtualRobot
float maxPosError = 10.f;
float maxOriError = 0.05f;
std::vector<TargetStep> ikSteps;
+
+ bool isReached() {
+ return pCtrl.reached(target, mode, maxPosError, maxOriError);
+ }
};
typedef std::shared_ptr<Target> TargetPtr;
diff --git a/VirtualRobot/IK/HierarchicalIK.cpp b/VirtualRobot/IK/HierarchicalIK.cpp
index af3d203237d08feaf5044ea07f82a4e343b919ca..b42c4199358bbc7736349bdba32363c64b4bf694 100644
--- a/VirtualRobot/IK/HierarchicalIK.cpp
+++ b/VirtualRobot/IK/HierarchicalIK.cpp
@@ -137,6 +137,8 @@ namespace VirtualRobot
case JacobiProvider::eSVDDamped:
JAinv_i_min1 = MathTools::getPseudoInverseDampedD(JA_i_min1, invDamped_lamba);
break;
+ default:
+ THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
@@ -175,6 +177,8 @@ namespace VirtualRobot
case JacobiProvider::eSVDDamped:
Jinv_tilde_i = MathTools::getPseudoInverseDampedD(J_tilde_i, invDamped_lamba);
break;
+ default:
+ THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
//Eigen::MatrixXf Jinv_tilde_i = MathTools::getPseudoInverse(J_tilde_i, pinvtoler);
diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp
index aee5347f7b6fa1b72559ca53345b2b6ff5a0b804..b7907734284c9a043ca05f0bc7e3afa0ad50d262 100644
--- a/VirtualRobot/IK/JacobiProvider.cpp
+++ b/VirtualRobot/IK/JacobiProvider.cpp
@@ -226,6 +226,12 @@ namespace VirtualRobot
break;
}
+ case eSVDDampedDynamic:
+ {
+ invJac = MathTools::getDampedLeastSquareInverse(m, MathTools::getDamping(m));
+ break;
+ }
+
default:
THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
@@ -320,6 +326,12 @@ namespace VirtualRobot
break;
}
+ case eSVDDampedDynamic:
+ {
+ invJac = MathTools::getDampedLeastSquareInverse(m, MathTools::getDamping(m));
+ break;
+ }
+
default:
THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
diff --git a/VirtualRobot/IK/JacobiProvider.h b/VirtualRobot/IK/JacobiProvider.h
index 99b8bb21a6caf31980ad27fdc09a9e8f08afb547..0a2cb2f9af0984d47b80bb77e7ea63c3a1563494 100644
--- a/VirtualRobot/IK/JacobiProvider.h
+++ b/VirtualRobot/IK/JacobiProvider.h
@@ -44,9 +44,10 @@ namespace VirtualRobot
*/
enum InverseJacobiMethod
{
- eSVD, //<! PseudoInverse Jacobian. Performing SVD and setting very small eigen values to zero results in a quite stable inverting of the Jacobi. (default)
- eSVDDamped, //<! Using the damped PseudoInverse algorithm
- eTranspose //<! The Jacobi Transpose method is faster than SVD and works well for redundant kinematic chains.
+ eSVD, //<! PseudoInverse Jacobian. Performing SVD and setting very small eigen values to zero results in a quite stable inverting of the Jacobi. (default)
+ eSVDDamped, //<! Using the damped PseudoInverse algorithm
+ eSVDDampedDynamic, //<! Using the damped PseudoInverse algorithm with a different computation of the damping factor which is dynamically calculated when near a singularity
+ eTranspose //<! The Jacobi Transpose method is faster than SVD and works well for redundant kinematic chains.
};
/*!
diff --git a/VirtualRobot/IK/StackedIK.cpp b/VirtualRobot/IK/StackedIK.cpp
index f6c8549a076dc441cb4cefb16793a16c5aa51d20..28d302e0388869937c18e990361fee42149082ca 100644
--- a/VirtualRobot/IK/StackedIK.cpp
+++ b/VirtualRobot/IK/StackedIK.cpp
@@ -97,6 +97,8 @@ namespace VirtualRobot
case JacobiProvider::eSVDDamped:
J_inv = MathTools::getPseudoInverseDamped(jacobian, 1.0);
break;
+ default:
+ THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
// Compute IK step
diff --git a/VirtualRobot/Manipulability/AbstractManipulability.cpp b/VirtualRobot/Manipulability/AbstractManipulability.cpp
index fd130f3de6370a24db6f760a6bb405e571e60449..8e6725d0e62a1158ec840da3615a4f3382cdbab6 100644
--- a/VirtualRobot/Manipulability/AbstractManipulability.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulability.cpp
@@ -24,7 +24,7 @@
#include "AbstractManipulability.h"
#include <Eigen/Dense>
-#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
+#include <SimoxUtility/math/convert.h>
#include "VirtualRobot/Visualization/VisualizationFactory.h"
#include "VirtualRobot/Visualization/VisualizationNode.h"
@@ -102,7 +102,9 @@ VisualizationNodePtr AbstractManipulability::getManipulabilityVis(const Eigen::M
getEllipsoidOrientationAndScale(manipulability, orientation, scale);
scale *= scaling;
auto vis = visualizationFactory->createEllipse(scale(0), scale(1), scale(2), false);
- vis->setGlobalPose(simox::math::pos_quat_to_mat4f(position, orientation));
+ Eigen::Matrix4f pose = simox::math::pos_mat3f_to_mat4f(position, simox::math::mat4f_to_mat3f(getCoordinateSystem()).inverse() * simox::math::quat_to_mat3f(orientation));
+ vis->setUpdateVisualization(true);
+ vis->setGlobalPose(pose);
return vis;
}
diff --git a/VirtualRobot/Manipulability/AbstractManipulability.h b/VirtualRobot/Manipulability/AbstractManipulability.h
index 27e336aa91a402df6ab3728d5e74f75c45a06d91..d992f4023a2a1344bc26dd3e87f90f5aae57bfe7 100644
--- a/VirtualRobot/Manipulability/AbstractManipulability.h
+++ b/VirtualRobot/Manipulability/AbstractManipulability.h
@@ -80,6 +80,8 @@ public:
virtual Eigen::Vector3f getGlobalPosition() = 0;
+ virtual Eigen::Matrix4f getCoordinateSystem() = 0;
+
virtual Eigen::Vector3f getLocalPosition() = 0;
virtual std::vector<std::string> getJointNames() = 0;
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
index fb9c8c7c1ddfb1177b52860a1e8c01f44015919b..206b0716fd4d65c23b09d612281f1ab29986f6cd 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.cpp
@@ -136,14 +136,6 @@ Eigen::MatrixXd AbstractManipulabilityTracking::getDefaultGainMatrix(){
}
}
-Eigen::MatrixXd AbstractManipulabilityTracking::dampedLeastSquaresInverse(const Eigen::MatrixXd &jacobian, double dampingFactor) {
- Eigen::MatrixXd k2I = dampingFactor/*std::pow(dampingFactor, 2)*/ * Eigen::MatrixXd::Identity(jacobian.rows(), jacobian.rows());
- Eigen::MatrixXd JJT = jacobian * jacobian.transpose();
- Eigen::MatrixXd JJT_k2I = JJT + k2I;
- Eigen::MatrixXd dampedLeastSquaresInverse = jacobian.transpose() * JJT_k2I.inverse();
- return dampedLeastSquaresInverse;
-}
-
Eigen::MatrixXd AbstractManipulabilityTracking::logMap(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &manipulabilityCurrent) {
Eigen::MatrixXd m = manipulabilityCurrent.inverse() * manipulabilityDesired;
Eigen::EigenSolver<Eigen::MatrixXd> eigenSolver(m);
@@ -156,14 +148,6 @@ double AbstractManipulabilityTracking::computeDistance(const Eigen::MatrixXd &ma
return logMap(manipulabilityDesired, computeCurrentManipulability()).norm();
}
-double AbstractManipulabilityTracking::getDamping(const Eigen::MatrixXd &matrix) {
- auto svd = Eigen::JacobiSVD(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
- auto sV = svd.singularValues();
- double minEigenValue = sV(sV.rows()-1, sV.cols()-1);
- double damping = minEigenValue < 1e-2 ? 1e-2 : 1e-8; // c++ code sets damping to min singular value if smaller
- return damping;
-}
-
Eigen::MatrixXd AbstractManipulabilityTracking::getJointsLimitsWeightMatrix(const Eigen::VectorXd &jointAngles, const Eigen::VectorXd &jointLimitsLow, const Eigen::VectorXd &jointLimitsHigh) {
int nbJoints = jointAngles.size();
Eigen::VectorXd weights(nbJoints);
diff --git a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
index 1e0ac762800aaf575cdd56b0cf2f3984a42f876c..505e6494aee2cc2418476bc15233b0b5f70d6271 100644
--- a/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
+++ b/VirtualRobot/Manipulability/AbstractManipulabilityTracking.h
@@ -68,18 +68,9 @@ public:
Eigen::MatrixXd getDefaultGainMatrix();
- /*! @brief Calculates the damped least square inverse by J^T * (JJ^T + k^2 I)^{-1}
- * @jacobian Jacobian J
- * @dampingFactor Damping factor k
- */
- Eigen::MatrixXd dampedLeastSquaresInverse(const Eigen::MatrixXd &jacobian, double dampingFactor = 0.01);
-
Eigen::MatrixXd logMap(const Eigen::MatrixXd &manipulabilityDesired, const Eigen::MatrixXd &manipulabilityCurrent);
- double computeDistance(const Eigen::MatrixXd &manipulabilityDesired);
-
- /*! Calculates damping factor for singularity avoidance */
- double getDamping(const Eigen::MatrixXd &matrix);
+ double computeDistance(const Eigen::MatrixXd &manipulabilityDesired);
/* Calculate weight matrix for joint limits avoidance */
Eigen::MatrixXd getJointsLimitsWeightMatrix(const Eigen::VectorXd &jointAngles, const Eigen::VectorXd &jointLimitsLow, const Eigen::VectorXd &jointLimitsHigh);
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.cpp b/VirtualRobot/Manipulability/BimanualManipulability.cpp
index ede081a125dc811df4665fc62a0a812ae406180a..c833afe101a033067e75471af294b62763eb3d31 100644
--- a/VirtualRobot/Manipulability/BimanualManipulability.cpp
+++ b/VirtualRobot/Manipulability/BimanualManipulability.cpp
@@ -48,9 +48,9 @@ BimanualManipulability::BimanualManipulability(const RobotNodeSetPtr &rnsLeft, c
if (rnsLeft->getRobot() != rnsRight->getRobot()) {
throw VirtualRobotException("Left and right robot node set of bimanual manipulability do not map to the same robot!");
}
- ikLeft.reset(new DifferentialIK(rnsLeft, coordSystem, JacobiProvider::eSVDDamped));
+ ikLeft.reset(new DifferentialIK(rnsLeft, coordSystem, JacobiProvider::eSVDDampedDynamic));
ikLeft->convertModelScalingtoM(convertMMtoM);
- ikRight.reset(new DifferentialIK(rnsRight, coordSystem, JacobiProvider::eSVDDamped));
+ ikRight.reset(new DifferentialIK(rnsRight, coordSystem, JacobiProvider::eSVDDampedDynamic));
ikRight->convertModelScalingtoM(convertMMtoM);
}
@@ -96,6 +96,10 @@ Eigen::Vector3f BimanualManipulability::getGlobalPosition() {
else return (nodeLeft->getGlobalPosition() + nodeRight->getGlobalPosition()) / 2.0f;
}
+Eigen::Matrix4f BimanualManipulability::getCoordinateSystem() {
+ return coordSystem ? coordSystem->getGlobalPose() : Eigen::Matrix4f::Identity();
+}
+
Eigen::MatrixXd BimanualManipulability::computeBimanualGraspMatrix() {
Eigen::Vector3f endeffectorLeft = nodeLeft->getPositionInRootFrame();
Eigen::Vector3f endeffectorRight = nodeRight->getPositionInRootFrame();
diff --git a/VirtualRobot/Manipulability/BimanualManipulability.h b/VirtualRobot/Manipulability/BimanualManipulability.h
index 5697dd91552961e09c778362e873b068880029f1..9455b074167a0b07cb2e61747b73fef0ad6e69f0 100644
--- a/VirtualRobot/Manipulability/BimanualManipulability.h
+++ b/VirtualRobot/Manipulability/BimanualManipulability.h
@@ -66,6 +66,8 @@ public:
virtual Eigen::Vector3f getGlobalPosition() override;
+ virtual Eigen::Matrix4f getCoordinateSystem() override;
+
Eigen::MatrixXd computeBimanualGraspMatrix();
RobotNodeSetPtr getLeftRobotNodeSet();
diff --git a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
index 0e71662638d678cc726a55b2f0976a9d20c1e2fb..a8068674e44d5fcfbfe39d256e43f09c7d44ebdd 100644
--- a/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/BimanualManipulabilityTracking.cpp
@@ -26,6 +26,8 @@
#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
#include "BimanualManipulability.h"
+#include <VirtualRobot/MathTools.h>
+
namespace VirtualRobot
{
@@ -62,8 +64,8 @@ Eigen::VectorXf BimanualManipulabilityTracking::calculateVelocity(const Eigen::M
}
// Compute damped pseudo-inverse of manipulability Jacobian
- double dampingFactor = getDamping(matManipJ);
- Eigen::MatrixXd dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ double dampingFactor = MathTools::getDamping(matManipJ);
+ Eigen::MatrixXd dampedLSI = MathTools::getDampedLeastSquareInverse(matManipJ, dampingFactor);
// Compute joint velocity
Eigen::VectorXd dq = dampedLSI * (gainMatrix.rows() == 0 ? getDefaultGainMatrix() : gainMatrix) * manipulability_mandel;
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.cpp b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
index f08339e82ed9473bf65beb60006ffb27e07e655d..923199f16a18c94e0afdda3880e8e60c61a48bc7 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulability.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.cpp
@@ -65,7 +65,7 @@ SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNo
SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, const RobotNodePtr &node, Mode mode, Type type, const RobotNodePtr &coordSystem, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
: AbstractSingleChainManipulability(mode, type, weightMatrixInit), rns(rns), node(node), coordSystem(coordSystem)
{
- ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDamped));
+ ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDampedDynamic));
ik->convertModelScalingtoM(convertMMtoM);
}
@@ -89,6 +89,10 @@ Eigen::Vector3f SingleRobotNodeSetManipulability::getGlobalPosition() {
return rns->getTCP()->getGlobalPosition();
}
+Eigen::Matrix4f SingleRobotNodeSetManipulability::getCoordinateSystem() {
+ return coordSystem ? coordSystem->getGlobalPose() : Eigen::Matrix4f::Identity();
+}
+
std::vector<std::string> SingleRobotNodeSetManipulability::getJointNames() {
return rns->getNodeNames();
}
@@ -151,11 +155,15 @@ SingleChainManipulability::SingleChainManipulability(const Eigen::Matrix<double,
}
Eigen::Vector3f SingleChainManipulability::getLocalPosition() {
- return globalPosition;
+ return localPosition;
}
Eigen::Vector3f SingleChainManipulability::getGlobalPosition() {
- return localPosition;
+ return globalPosition;
+}
+
+Eigen::Matrix4f SingleChainManipulability::getCoordinateSystem() {
+ return Eigen::Matrix4f::Identity();
}
std::vector<std::string> SingleChainManipulability::getJointNames() {
diff --git a/VirtualRobot/Manipulability/SingleChainManipulability.h b/VirtualRobot/Manipulability/SingleChainManipulability.h
index e47d7027c3e254118730e0b28f0f6973298a7367..2eee9edbf5f12e09674cc2fd524f7c7ca046edb5 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulability.h
+++ b/VirtualRobot/Manipulability/SingleChainManipulability.h
@@ -65,6 +65,8 @@ public:
virtual Eigen::Vector3f getGlobalPosition() override;
+ virtual Eigen::Matrix4f getCoordinateSystem() override;
+
virtual std::vector<std::string> getJointNames() override;
virtual Eigen::VectorXd getJointAngles() override;
@@ -102,6 +104,8 @@ public:
virtual Eigen::Vector3f getGlobalPosition() override;
+ virtual Eigen::Matrix4f getCoordinateSystem() override;
+
virtual std::vector<std::string> getJointNames() override;
virtual Eigen::VectorXd getJointAngles() override;
diff --git a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
index 12f4b64ccc03cc6ded480763a06df14e0e2efa82..cf5308311fb0da8debcd9991b677cbeb33188dec 100644
--- a/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
+++ b/VirtualRobot/Manipulability/SingleChainManipulabilityTracking.cpp
@@ -25,6 +25,8 @@
#include "SingleChainManipulability.h"
+#include <VirtualRobot/MathTools.h>
+
namespace VirtualRobot
{
@@ -94,13 +96,13 @@ Eigen::VectorXf SingleChainManipulabilityTracking::calculateVelocity(const Eigen
Eigen::MatrixXd jointsWeightMatrix = getJointsLimitsWeightMatrix(manipulability->getJointAngles(), manipulability->getJointLimitsLow(), manipulability->getJointLimitsHigh());
matManipJ = matManipJ * jointsWeightMatrix;
// Compute pseudo-inverse of weighted manipulability Jacobian
- double dampingFactor = getDamping(matManipJ);
- dampedLSI = jointsWeightMatrix * dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ double dampingFactor = MathTools::getDamping(matManipJ);
+ dampedLSI = jointsWeightMatrix * MathTools::getDampedLeastSquareInverse(matManipJ, dampingFactor);
}
else{
// Compute pseudo-inverse of manipulability Jacobian
- double dampingFactor = getDamping(matManipJ);
- dampedLSI = dampedLeastSquaresInverse(matManipJ, dampingFactor);
+ double dampingFactor = MathTools::getDamping(matManipJ);
+ dampedLSI = MathTools::getDampedLeastSquareInverse(matManipJ, dampingFactor);
}
Eigen::MatrixXd manipulability_mandel = symMatrixToVector(manipulability_velocity);
diff --git a/VirtualRobot/MathTools.cpp b/VirtualRobot/MathTools.cpp
index 49ce02d9fb91857d60a47ae4537191b7c86ea056..2f31285c735a168ec588b1b0f14278e7f5be7d72 100644
--- a/VirtualRobot/MathTools.cpp
+++ b/VirtualRobot/MathTools.cpp
@@ -2257,6 +2257,22 @@ namespace VirtualRobot
pose = newGlobalPose;
}
+ double MathTools::getDamping(const Eigen::MatrixXd &matrix) {
+ auto svd = Eigen::JacobiSVD(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sV = svd.singularValues();
+ double minEigenValue = sV(sV.rows()-1, sV.cols()-1);
+ double damping = minEigenValue < 1e-2 ? 1e-2 : 1e-8; // c++ code sets damping to min singular value if smaller
+ return damping;
+ }
+
+ double MathTools::getDamping(const Eigen::MatrixXf &matrix) {
+ auto svd = Eigen::JacobiSVD(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ auto sV = svd.singularValues();
+ double minEigenValue = sV(sV.rows()-1, sV.cols()-1);
+ double damping = minEigenValue < 1e-2 ? 1e-2 : 1e-8; // c++ code sets damping to min singular value if smaller
+ return damping;
+ }
+
} // namespace
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index e2d37fb588f1e861dda21321498365ecfa2cec3b..74624441024d722bb024a4edeff6746fe3932db0 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -645,6 +645,23 @@ namespace VirtualRobot
Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT getPseudoInverseDamped(const Eigen::MatrixXf& m, float lambda = 1.0f);
Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT getPseudoInverseDampedD(const Eigen::MatrixXd& m, double lambda = 1.0);
+ /*! Calculates damping factor for singularity avoidance */
+ double VIRTUAL_ROBOT_IMPORT_EXPORT getDamping(const Eigen::MatrixXd &matrix);
+ double VIRTUAL_ROBOT_IMPORT_EXPORT getDamping(const Eigen::MatrixXf &matrix);
+
+ /*! @brief Calculates the damped least square inverse by J^T * (JJ^T + k^2 I)^{-1}
+ * @jacobian Jacobian J
+ * @squaredDampingFactor Damping factor k^2
+ */
+ template <typename T>
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> VIRTUAL_ROBOT_IMPORT_EXPORT getDampedLeastSquareInverse(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &jacobian, double squaredDampingFactor = 0.01f) {
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> k2I = squaredDampingFactor * Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Identity(jacobian.rows(), jacobian.rows());
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> JJT = jacobian * jacobian.transpose();
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> JJT_k2I = JJT + k2I;
+ Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> dampedLeastSquaresInverse = jacobian.transpose() * JJT_k2I.inverse();
+ return dampedLeastSquaresInverse;
+ }
+
/*!
Check if all entries of v are valid numbers (i.e. all entries of v are not NaN and not INF)
*/