diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index c18cbd5a5d7e14d04aa624a5f6bfccc4d2a41717..bec99b20f39df8b5a0976372c02922b5b2fc479c 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -42,8 +42,6 @@ namespace VirtualRobot
positionMaxStep = -1.0f;
tmpUpdateErrorDelta.resize(6);
-
- radianToMMfactor = _rns->getRobot()->getRadianToMMfactor();
}
diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp
index bd6ecf1a33eb6d89da4cca2051881938ae60f88d..573997e997423fb1f95c705cf44b53a2607dbbe5 100644
--- a/VirtualRobot/IK/JacobiProvider.cpp
+++ b/VirtualRobot/IK/JacobiProvider.cpp
@@ -16,6 +16,9 @@ namespace VirtualRobot
name("JacobiProvvider"), rns(rns), inverseMethod(invJacMethod)
{
initialized = false;
+ dampedSvdLambda = 0.1;
+ jacobiMMRegularization = 50;
+ jacobiRadianRegularization = 1;
}
JacobiProvider::~JacobiProvider()
@@ -32,7 +35,7 @@ namespace VirtualRobot
return getJacobianMatrix(tcp).cast<double>();
}
- Eigen::MatrixXf JacobiProvider::getPseudoInverseJacobianMatrix(SceneObjectPtr tcp)
+ Eigen::MatrixXf JacobiProvider::getPseudoInverseJacobianMatrix(SceneObjectPtr tcp, const Eigen::VectorXf regularization)
{
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
@@ -41,7 +44,7 @@ namespace VirtualRobot
#ifdef CHECK_PERFORMANCE
clock_t startT2 = clock();
#endif
- Eigen::MatrixXf res = computePseudoInverseJacobianMatrix(Jacobian);
+ Eigen::MatrixXf res = computePseudoInverseJacobianMatrix(Jacobian, regularization);
#ifdef CHECK_PERFORMANCE
clock_t endT = clock();
float diffClock1 = (float)(((float)(startT2 - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
@@ -51,7 +54,7 @@ namespace VirtualRobot
return res;
}
- Eigen::MatrixXd JacobiProvider::getPseudoInverseJacobianMatrixD(SceneObjectPtr tcp)
+ Eigen::MatrixXd JacobiProvider::getPseudoInverseJacobianMatrixD(SceneObjectPtr tcp, const Eigen::VectorXd regularization)
{
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
@@ -60,7 +63,7 @@ namespace VirtualRobot
#ifdef CHECK_PERFORMANCE
clock_t startT2 = clock();
#endif
- Eigen::MatrixXd res = computePseudoInverseJacobianMatrixD(Jacobian);
+ Eigen::MatrixXd res = computePseudoInverseJacobianMatrixD(Jacobian, regularization);
#ifdef CHECK_PERFORMANCE
clock_t endT = clock();
float diffClock1 = (float)(((float)(startT2 - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
@@ -70,56 +73,87 @@ namespace VirtualRobot
return res;
}
+ Eigen::VectorXf JacobiProvider::getJacobiRegularization(IKSolver::CartesianSelection mode)
+ {
+ Eigen::VectorXf regularization(6);
+
+ int i = 0;
+
+ if (mode & IKSolver::X)
+ {
+ regularization(i++) = 1 / jacobiMMRegularization;
+ }
+
+ if (mode & IKSolver::Y)
+ {
+ regularization(i++) = 1 / jacobiMMRegularization;
+ }
+
+ if (mode & IKSolver::Z)
+ {
+ regularization(i++) = 1 / jacobiMMRegularization;
+ }
- Eigen::MatrixXf JacobiProvider::getPseudoInverseJacobianMatrix()
+ if (mode & IKSolver::Orientation)
+ {
+ regularization(i++) = 1 / jacobiRadianRegularization;
+ regularization(i++) = 1 / jacobiRadianRegularization;
+ regularization(i++) = 1 / jacobiRadianRegularization;
+ }
+ return regularization.topRows(i);
+
+ }
+
+
+ Eigen::MatrixXf JacobiProvider::getPseudoInverseJacobianMatrix(const Eigen::VectorXf regularization)
{
MatrixXf Jacobian = this->getJacobianMatrix();
- return computePseudoInverseJacobianMatrix(Jacobian);
+ return computePseudoInverseJacobianMatrix(Jacobian, regularization);
//return getPseudoInverseJacobianMatrix(rns->getTCP());
}
- Eigen::MatrixXd JacobiProvider::getPseudoInverseJacobianMatrixD()
+ Eigen::MatrixXd JacobiProvider::getPseudoInverseJacobianMatrixD(const Eigen::VectorXd regularization)
{
MatrixXd Jacobian = this->getJacobianMatrixD();
- return computePseudoInverseJacobianMatrixD(Jacobian);
+ return computePseudoInverseJacobianMatrixD(Jacobian, regularization);
}
- Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m) const
+ Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, const VectorXf regularization) const
{
- return computePseudoInverseJacobianMatrix(m, 0.0f);
+ return computePseudoInverseJacobianMatrix(m, 0.0f, regularization);
}
- MatrixXd JacobiProvider::computePseudoInverseJacobianMatrixD(const MatrixXd& m) const
+ MatrixXd JacobiProvider::computePseudoInverseJacobianMatrixD(const MatrixXd& m, const Eigen::VectorXd regularization) const
{
- return computePseudoInverseJacobianMatrixD(m, 0.0);
+ return computePseudoInverseJacobianMatrixD(m, 0.0, regularization);
}
- Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, float invParameter) const
+ Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, float invParameter, const VectorXf regularization) const
{
Eigen::MatrixXf result(m.cols(), m.rows());
- updatePseudoInverseJacobianMatrix(result, m, invParameter);
+ updatePseudoInverseJacobianMatrix(result, m, invParameter, regularization);
return result;
}
- Eigen::MatrixXd JacobiProvider::computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m, double invParameter) const
+ Eigen::MatrixXd JacobiProvider::computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m, double invParameter, const Eigen::VectorXd regularization) const
{
Eigen::MatrixXd result(m.cols(), m.rows());
- updatePseudoInverseJacobianMatrixD(result, m, invParameter);
+ updatePseudoInverseJacobianMatrixD(result, m, invParameter, regularization);
return result;
}
- void JacobiProvider::updatePseudoInverseJacobianMatrix(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter) const
+ void JacobiProvider::updatePseudoInverseJacobianMatrix(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter, VectorXf regularization) const
{
Eigen::MatrixXf m2 = m;
- Eigen::VectorXf R(6);
- R << 1, 1, 1, radianToMMfactor, radianToMMfactor, radianToMMfactor;
- //std::cout << "m2: " << m2 << std::endl;
- m2 = R.asDiagonal() * m2;
- //std::cout << "m2: " << m2 << std::endl;
+ VR_ASSERT(regularization.rows() == 0 || regularization.rows() == m2.rows())
+ if(regularization.rows() != m2.rows())
+ {
+ regularization = VectorXf::Ones(m2.rows());
+ }
+ //std::cout << "regularization: " << regularization.transpose() << std::endl;
+ m2 = regularization.asDiagonal() * m2;
updatePseudoInverseJacobianMatrixInternal(invJac, m2, invParameter);
- //std::cout << "invJac: " << invJac << std::endl;
- invJac = invJac * R.asDiagonal();
- //std::cout << "invJac: " << invJac << std::endl;
+ invJac = invJac * regularization.asDiagonal();
}
void JacobiProvider::updatePseudoInverseJacobianMatrixInternal(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter) const
@@ -180,14 +214,14 @@ namespace VirtualRobot
case eSVDDamped:
{
- float pinvtoler = 1.0f;
+ float lambda = dampedSvdLambda;
if (invParameter != 0.0f)
{
- pinvtoler = invParameter;
+ lambda = invParameter;
}
- invJac = MathTools::getPseudoInverseDamped(m, pinvtoler);
+ invJac = MathTools::getPseudoInverseDamped(m, lambda);
break;
}
@@ -203,12 +237,17 @@ namespace VirtualRobot
#endif
}
- void JacobiProvider::updatePseudoInverseJacobianMatrixD(Eigen::MatrixXd& invJac, const Eigen::MatrixXd& m, double invParameter) const
+ void JacobiProvider::updatePseudoInverseJacobianMatrixD(Eigen::MatrixXd& invJac, const Eigen::MatrixXd& m, double invParameter, Eigen::VectorXd regularization) const
{
Eigen::MatrixXd m2 = m;
- m2.block(3, 0, 3, m2.cols()) *= radianToMMfactor;
+ VR_ASSERT(regularization.rows() == 0 || regularization.rows() == m2.rows())
+ if(regularization.rows() != m2.rows())
+ {
+ regularization = VectorXd::Ones(m2.rows());
+ }
+ m2 = regularization.asDiagonal() * m2;
updatePseudoInverseJacobianMatrixDInternal(invJac, m2, invParameter);
- invJac.block(0, 3, m2.rows(), 3) *= radianToMMfactor;
+ invJac = invJac * regularization.asDiagonal();
}
void JacobiProvider::updatePseudoInverseJacobianMatrixDInternal(Eigen::MatrixXd& invJac, const Eigen::MatrixXd& m, double invParameter) const
@@ -292,6 +331,36 @@ namespace VirtualRobot
#endif
}
+ float JacobiProvider::getJacobiRadianRegularization() const
+ {
+ return jacobiRadianRegularization;
+ }
+
+ void JacobiProvider::setJacobiRadianRegularization(float value)
+ {
+ jacobiRadianRegularization = value;
+ }
+
+ float JacobiProvider::getJacobiMMRegularization() const
+ {
+ return jacobiMMRegularization;
+ }
+
+ void JacobiProvider::setJacobiMMRegularization(float value)
+ {
+ jacobiMMRegularization = value;
+ }
+
+ float JacobiProvider::getDampedSvdLambda() const
+ {
+ return dampedSvdLambda;
+ }
+
+ void JacobiProvider::setDampedSvdLambda(float value)
+ {
+ dampedSvdLambda = value;
+ }
+
VirtualRobot::RobotNodeSetPtr JacobiProvider::getRobotNodeSet()
@@ -311,16 +380,6 @@ namespace VirtualRobot
cout << "RNS:" << rns->getName() << " with " << rns->getSize() << " joints" << endl;
}
- float JacobiProvider::getRadianToMMfactor() const
- {
- return radianToMMfactor;
- }
-
- void JacobiProvider::setRadianToMMfactor(float value)
- {
- radianToMMfactor = value;
- }
-
bool JacobiProvider::isInitialized()
{
return initialized;
diff --git a/VirtualRobot/IK/JacobiProvider.h b/VirtualRobot/IK/JacobiProvider.h
index cc289092dd165b9e937e767ff2cc8c7e34241c73..4b2b881702b2f520793adb6b57fd78f015cb7208 100644
--- a/VirtualRobot/IK/JacobiProvider.h
+++ b/VirtualRobot/IK/JacobiProvider.h
@@ -26,6 +26,7 @@
#include "../VirtualRobot.h"
#include "../Nodes/RobotNode.h"
#include "../RobotNodeSet.h"
+#include "IKSolver.h"
#include <string>
#include <vector>
@@ -61,16 +62,18 @@ namespace VirtualRobot
virtual Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) = 0;
virtual Eigen::MatrixXd getJacobianMatrixD(SceneObjectPtr tcp);
- virtual Eigen::MatrixXf computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m) const;
- virtual Eigen::MatrixXd computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m) const;
- virtual Eigen::MatrixXf computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, float invParameter) const;
- virtual Eigen::MatrixXd computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m, double invParameter) const;
- virtual void updatePseudoInverseJacobianMatrix(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter = 0.0f) const;
- virtual void updatePseudoInverseJacobianMatrixD(Eigen::MatrixXd& invJac, const Eigen::MatrixXd& m, double invParameter = 0.0) const;
- virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix();
- virtual Eigen::MatrixXd getPseudoInverseJacobianMatrixD();
- virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix(SceneObjectPtr tcp);
- virtual Eigen::MatrixXd getPseudoInverseJacobianMatrixD(SceneObjectPtr tcp);
+ virtual Eigen::MatrixXf computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, const Eigen::VectorXf regularization = Eigen::VectorXf()) const;
+ virtual Eigen::MatrixXd computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m, const Eigen::VectorXd regularization = Eigen::VectorXd()) const;
+ virtual Eigen::MatrixXf computePseudoInverseJacobianMatrix(const Eigen::MatrixXf& m, float invParameter, const Eigen::VectorXf regularization = Eigen::VectorXf()) const;
+ virtual Eigen::MatrixXd computePseudoInverseJacobianMatrixD(const Eigen::MatrixXd& m, double invParameter, const Eigen::VectorXd regularization = Eigen::VectorXd()) const;
+ virtual void updatePseudoInverseJacobianMatrix(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter = 0.0f, Eigen::VectorXf regularization = Eigen::VectorXf()) const;
+ virtual void updatePseudoInverseJacobianMatrixD(Eigen::MatrixXd& invJac, const Eigen::MatrixXd& m, double invParameter = 0.0, Eigen::VectorXd regularization = Eigen::VectorXd()) const;
+ virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix(const Eigen::VectorXf regularization = Eigen::VectorXf());
+ virtual Eigen::MatrixXd getPseudoInverseJacobianMatrixD(const Eigen::VectorXd regularization = Eigen::VectorXd());
+ virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix(SceneObjectPtr tcp, const Eigen::VectorXf regularization = Eigen::VectorXf());
+ virtual Eigen::MatrixXd getPseudoInverseJacobianMatrixD(SceneObjectPtr tcp, const Eigen::VectorXd regularization = Eigen::VectorXd());
+
+ virtual Eigen::VectorXf getJacobiRegularization(IKSolver::CartesianSelection mode = IKSolver::All);
VirtualRobot::RobotNodeSetPtr getRobotNodeSet();
@@ -93,8 +96,15 @@ namespace VirtualRobot
* \brief print Print current status of the IK solver
*/
virtual void print();
- float getRadianToMMfactor() const;
- void setRadianToMMfactor(float value);
+
+ float getDampedSvdLambda() const;
+ void setDampedSvdLambda(float value);
+
+ float getJacobiMMRegularization() const;
+ void setJacobiMMRegularization(float value);
+
+ float getJacobiRadianRegularization() const;
+ void setJacobiRadianRegularization(float value);
protected:
virtual void updatePseudoInverseJacobianMatrixInternal(Eigen::MatrixXf& invJac, const Eigen::MatrixXf& m, float invParameter = 0.0f) const;
@@ -105,7 +115,9 @@ namespace VirtualRobot
InverseJacobiMethod inverseMethod;
bool initialized;
Eigen::VectorXf jointWeights;
- float radianToMMfactor = 1;
+ float dampedSvdLambda;
+ float jacobiMMRegularization;
+ float jacobiRadianRegularization;
};
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index fbea755c41fd5ab6db769820f61ac21a1674a7b3..9247c482f8f8771c66a210e40ca3b5b1c2eda705 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -432,7 +432,7 @@ namespace VirtualRobot
//rootNode->updatePose(globalPose);
}
- float Robot::getRadianToMMfactor() const
+ /*float Robot::getRadianToMMfactor() const
{
return radianToMMfactor;
}
@@ -440,7 +440,7 @@ namespace VirtualRobot
void Robot::setRadianToMMfactor(float value)
{
radianToMMfactor = value;
- }
+ }*/
/**
* This method stores all nodes belonging to the robot in \p storeNodes.
@@ -847,6 +847,7 @@ namespace VirtualRobot
result->setGlobalPose(getGlobalPose());
result->filename = filename;
result->type = type;
+ //result->radianToMMfactor = radianToMMfactor;
return result;
}
@@ -1099,6 +1100,7 @@ namespace VirtualRobot
{
std::stringstream ss;
ss << "<?xml version='1.0' encoding='UTF-8'?>" << endl << endl;
+ //ss << "<Robot Type='" << this->type << "' RootNode='" << this->getRootNode()->getName() << "' RadianToMMfactor='" << this->radianToMMfactor << "'>" << endl << endl;
ss << "<Robot Type='" << this->type << "' RootNode='" << this->getRootNode()->getName() << "'>" << endl << endl;
std::vector<RobotNodePtr> nodes = getRobotNodes();
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 1526af5ff8c1b37af376f73a435a92a9d01ca219..bd96e85958bf022c1b9cd5a825c3bb20e6f642b0 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -402,8 +402,8 @@ namespace VirtualRobot
float getScaling();
void setScaling(float scaling);
- float getRadianToMMfactor() const;
- void setRadianToMMfactor(float value);
+ //float getRadianToMMfactor() const;
+ //void setRadianToMMfactor(float value);
protected:
Robot();
@@ -426,7 +426,7 @@ namespace VirtualRobot
mutable boost::recursive_mutex mutex;
bool use_mutex;
- float radianToMMfactor = 10;
+ //float radianToMMfactor = 10;
};
diff --git a/VirtualRobot/RobotNodeSet.cpp b/VirtualRobot/RobotNodeSet.cpp
index f59d3ccad3acc3d4102bc41fd8d243c744a980d4..d5188588c350aeb105c061eb13a769684fedc8df 100644
--- a/VirtualRobot/RobotNodeSet.cpp
+++ b/VirtualRobot/RobotNodeSet.cpp
@@ -366,6 +366,13 @@ namespace VirtualRobot
return res;
}
+ Eigen::VectorXf RobotNodeSet::getJointValuesEigen() const
+ {
+ Eigen::VectorXf res;
+ getJointValues(res);
+ return res;
+ }
+
void RobotNodeSet::respectJointLimits(std::vector<float>& jointValues) const
{
diff --git a/VirtualRobot/RobotNodeSet.h b/VirtualRobot/RobotNodeSet.h
index 4539c166df6abdd5aa651ace0d5c027e022052b9..6f884b18bb5a9fc821009e2e5098b49207f07244 100644
--- a/VirtualRobot/RobotNodeSet.h
+++ b/VirtualRobot/RobotNodeSet.h
@@ -92,6 +92,7 @@ namespace VirtualRobot
virtual unsigned int getSize() const;
std::vector<float> getJointValues() const;
+ Eigen::VectorXf getJointValuesEigen() const;
void getJointValues(std::vector<float>& fillVector) const;
void getJointValues(Eigen::VectorXf& fillVector) const;
void getJointValues(RobotConfigPtr fillVector) const;
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 2f020f5f707b95d73b043a23758928f470e566a9..35e140d30f97aafe9595cd2679ca6472031de823 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -972,11 +972,11 @@ namespace VirtualRobot
// build robot
RobotPtr robo(new LocalRobot(robotName, robotType));
- attr = robotXMLNode->first_attribute("RadianToMMfactor", 0, false);
+ /*attr = robotXMLNode->first_attribute("RadianToMMfactor", 0, false);
if(attr)
{
robo->setRadianToMMfactor(atof(attr->value()));
- }
+ }*/
return robo;
}
diff --git a/VirtualRobot/tests/VirtualRobotJacobianTest.cpp b/VirtualRobot/tests/VirtualRobotJacobianTest.cpp
index f469b87965bf0176942e3606343f04b77ab4891d..435d8d498e8ad2cb39db69a1d3eef4f48dc93a9f 100644
--- a/VirtualRobot/tests/VirtualRobotJacobianTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotJacobianTest.cpp
@@ -18,7 +18,7 @@
#include <Eigen/Geometry>
#include <VirtualRobot/RuntimeEnvironment.h>
-
+#include <algorithm>
BOOST_AUTO_TEST_SUITE(RobotNode)
@@ -108,8 +108,9 @@ BOOST_AUTO_TEST_CASE(testJacobianRevoluteJoint)
}
-BOOST_AUTO_TEST_CASE(testJacobianRadianToMMfactor)
+BOOST_AUTO_TEST_CASE(testJacobianRegularization)
{
+ std::cout << "testJacobianRegularization" << std::endl;
std::string filename = "robots/ArmarIII/ArmarIII.xml";
bool fileOK = RuntimeEnvironment::getDataFileAbsolute(filename);
BOOST_REQUIRE(fileOK);
@@ -119,49 +120,137 @@ BOOST_AUTO_TEST_CASE(testJacobianRadianToMMfactor)
std::cout << "robot loaded" << std::endl;
{
- robot->setRadianToMMfactor(1);
VirtualRobot::DifferentialIK ik(rns, RobotNodePtr(), JacobiProvider::eSVDDamped);
+ ik.setDampedSvdLambda(1);
+ //ik.setPseudoInverseMMscaling(1);
+ //ik.setPseudoInverseRadianScaling(1);
Eigen::MatrixXf jacobi = ik.getJacobianMatrix(rns->getTCP());
- Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi);
+ Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi, Eigen::VectorXf());
Eigen::MatrixXf test = jacobi * invjac;
+
+ Eigen::MatrixXf errMat = ik.getJacobiRegularization().asDiagonal() * (test - Eigen::MatrixXf::Identity(6, 6));
std::cout << test << std::endl;
+ std::cout << errMat << std::endl;
- float error = (test - Eigen::MatrixXf::Identity(6, 6)).squaredNorm();
+ float error = errMat.norm();
std::cout << "error: " << error << std::endl;
- //check that error is big for SVDDamped when radians are not scaled up
+ //check that error is big for SVDDamped when jacobi is not regularized and lambda is too big
BOOST_CHECK_GE(error, 0.1);
}
{
- robot->setRadianToMMfactor(10);
VirtualRobot::DifferentialIK ik(rns, RobotNodePtr(), JacobiProvider::eSVDDamped);
Eigen::MatrixXf jacobi = ik.getJacobianMatrix(rns->getTCP());
- Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi);
+ Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi, ik.getJacobiRegularization());
Eigen::MatrixXf test = jacobi * invjac;
+
+ Eigen::MatrixXf errMat = ik.getJacobiRegularization().asDiagonal() * (test - Eigen::MatrixXf::Identity(6, 6));
std::cout << test << std::endl;
+ std::cout << errMat << std::endl;
- float error = (test - Eigen::MatrixXf::Identity(6, 6)).squaredNorm();
+ float error = errMat.norm();
std::cout << "error: " << error << std::endl;
//check that error is small when radians are scaled up
- BOOST_CHECK_LE(error, 0.01);
+ BOOST_CHECK_LE(error, 0.1);
//check that error is still existant for SVDDamped
- BOOST_CHECK_GE(error, 1e-6);
+ BOOST_CHECK_GE(error, 1e-3);
}
{
- robot->setRadianToMMfactor(10);
+ //robot->setRadianToMMfactor(10);
VirtualRobot::DifferentialIK ik(rns, RobotNodePtr(), JacobiProvider::eSVD);
Eigen::MatrixXf jacobi = ik.getJacobianMatrix(rns->getTCP());
- Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi);
+ Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi, ik.getJacobiRegularization());
Eigen::MatrixXf test = jacobi * invjac;
std::cout << test << std::endl;
- float error = (test - Eigen::MatrixXf::Identity(6, 6)).squaredNorm();
+ float error = (test - Eigen::MatrixXf::Identity(6, 6)).norm();
std::cout << "error: " << error << std::endl;
//check that error is very small for SVD
BOOST_CHECK_LE(error, 0.01);
}
}
+BOOST_AUTO_TEST_CASE(testJacobianSingularity)
+{
+ std::string filename = "robots/ArmarIII/ArmarIII.xml";
+ bool fileOK = RuntimeEnvironment::getDataFileAbsolute(filename);
+ BOOST_REQUIRE(fileOK);
+
+ RobotPtr robot = RobotIO::loadRobot(filename);
+ RobotNodeSetPtr rns = robot->getRobotNodeSet("RightArm");
+
+ std::cout << "robot loaded" << std::endl;
+
+ Eigen::VectorXf initialJointAngles = rns->getJointValuesEigen();
+
+ Eigen::VectorXf vel(6);
+ vel << 0, 10, 0, 0, 0, 0;
+
+ {
+ rns->setJointValues(initialJointAngles);
+ VirtualRobot::DifferentialIK ik(rns, RobotNodePtr(), JacobiProvider::eSVD);
+ float maxError = 0;
+ for(int i = 0; i < 40; i++)
+ {
+ Eigen::MatrixXf jacobi = ik.getJacobianMatrix(rns->getTCP());
+ Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi, ik.getJacobiRegularization());
+ Eigen::VectorXf jointVel = invjac * vel;
+ //std::cout << jacobi << std::endl;
+ //std::cout << (jacobi * jointVel).transpose() << std::endl;
+ //std::cout << rns->getTCP()->getPositionInRootFrame().transpose() << std::endl;
+ Eigen::Vector3f oldPos = rns->getTCP()->getPositionInRootFrame();
+ rns->setJointValues(rns->getJointValuesEigen() + jointVel);
+ //std::cout << (rns->getTCP()->getPositionInRootFrame() - oldPos).transpose() << std::endl;
+ Eigen::Vector3f diff = rns->getTCP()->getPositionInRootFrame() - oldPos;
+ //std::cout << (vel.topRows(3) - diff).norm() << " " << diff.norm() << std::endl;
+ maxError = std::max(maxError, diff.norm());
+ //std::cout << jointVel.transpose() << std::endl;
+ }
+ std::cout << "maxError: " << maxError << std::endl;
+ BOOST_CHECK_GE(maxError, 50);
+
+ }
+ std::cout << "#### eSVDDamped" << std::endl;
+ {
+ rns->setJointValues(initialJointAngles);
+ VirtualRobot::DifferentialIK ik(rns, RobotNodePtr(), JacobiProvider::eSVDDamped);
+ ik.setDampedSvdLambda(0.2);
+ float maxError = 0;
+ for(int i = 0; i < 40; i++)
+ {
+ Eigen::MatrixXf jacobi = ik.getJacobianMatrix(rns->getTCP());
+ Eigen::MatrixXf invjac = ik.computePseudoInverseJacobianMatrix(jacobi, ik.getJacobiRegularization());
+ Eigen::VectorXf jointVel = invjac * vel;
+ //std::cout << (jacobi * jointVel).transpose() << std::endl;
+ //std::cout << rns->getTCP()->getPositionInRootFrame().transpose() << std::endl;
+ Eigen::Vector3f oldPos = rns->getTCP()->getPositionInRootFrame();
+ rns->setJointValues(rns->getJointValuesEigen() + jointVel);
+ //std::cout << (rns->getTCP()->getPositionInRootFrame() - oldPos).norm() << " " << (rns->getTCP()->getPositionInRootFrame() - oldPos).transpose() << std::endl;
+ Eigen::Vector3f diff = rns->getTCP()->getPositionInRootFrame() - oldPos;
+ //std::cout << (vel.topRows(3) - diff).norm() << " " << diff.norm() << std::endl;
+ maxError = std::max(maxError, diff.norm());
+ //std::cout << (rns->getTCP()->getPositionInRootFrame() - oldPos).transpose() << std::endl;
+ //std::cout << jointVel.transpose() << std::endl;
+ }
+ std::cout << "maxError: " << maxError << std::endl;
+ BOOST_CHECK_LE(maxError, 50);
+ }
+
+ /*std::cout << "#### eSVDDamped + calculateJointVelocity" << std::endl;
+ {
+ rns->setJointValues(initialJointAngles);
+ VirtualRobot::DifferentialIK ik(rns, RobotNodePtr(), JacobiProvider::eSVDDamped);
+ for(int i = 0; i < 40; i++)
+ {
+ Eigen::VectorXf jointVel = ik.calculateJointVelocity(rns->getTCP(), vel);
+ Eigen::Vector3f oldPos = rns->getTCP()->getPositionInRootFrame();
+ rns->setJointValues(rns->getJointValuesEigen() + jointVel);
+ std::cout << (rns->getTCP()->getPositionInRootFrame() - oldPos).transpose() << std::endl;
+ }
+
+ }*/
+}
+
BOOST_AUTO_TEST_SUITE_END()