diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index de66c0591d66119057275be998bbfa6faccc854b..c3cd6fb23720fb82f17def66b3c9a434a671c21d 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -32,8 +32,9 @@
using namespace armarx;
-CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod)
- : rns(rns)
+CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
+ : rns(rns),
+ considerJointLimits(considerJointLimits)
{
//ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
@@ -55,9 +56,6 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
//ARMARX_IMPORTANT << VAROUT(tcp.get());
//ARMARX_IMPORTANT << VAROUT(ik.get());
jacobi = ik->getJacobianMatrix(tcp, mode);
- //ARMARX_IMPORTANT << "hi";
- //ARMARX_IMPORTANT << jacobi;
- inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
// ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
@@ -79,12 +77,22 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
//nsv /= kernel.cols();
- Eigen::VectorXf jointVel = inv * cartesianVel;
+
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+
+
+ if (considerJointLimits)
+ {
+ adjustJacobiForJointsAtJointLimits(mode, cartesianVel);
+ }
+
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
jointVel += nsv;
- if(maximumJointVelocities.rows() > 0)
+ if (maximumJointVelocities.rows() > 0)
{
jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
}
@@ -155,3 +163,51 @@ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobo
}
return regularization.topRows(i);
}
+
+bool CartesianVelocityController::adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy)
+{
+
+ bool modifiedJacobi = false;
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ size_t size = rns->getSize();
+
+ for (size_t i = 0; i < size; ++i)
+ {
+ auto& node = rns->getNode(i);
+
+ if (std::abs(jointVel(i)) < 0.001f)
+ {
+ continue;
+ }
+
+ if ((std::abs(node->getJointValue() - node->getJointLimitHigh()) < jointLimitCheckAccuracy && jointVel(i) > 0)
+ || (std::abs(node->getJointValue() - node->getJointLimitLow()) < jointLimitCheckAccuracy && jointVel(i) < 0))
+ {
+ for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
+ {
+ jacobi(k, i) = 0.0f;
+ }
+ // ARMARX_INFO << deactivateSpam(0.5, node->getName()) << " joint " << node->getName() << " is at limit -\n" << inv << "\n initial inv jacobi:\n" << initialInvJac << "\n target cart vel:\n" << cartesianVel << "\njacobi:\n" << jacobi;
+ modifiedJacobi = true;
+ }
+ }
+
+ if (modifiedJacobi)
+ {
+ // calculate inverse jacobi again since atleast one joint would be driven into the limit
+ inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ }
+
+ return modifiedJacobi;
+}
+
+bool CartesianVelocityController::getConsiderJointLimits() const
+{
+ return considerJointLimits;
+}
+
+void CartesianVelocityController::setConsiderJointLimits(bool value)
+{
+ considerJointLimits = value;
+}
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 01398dd59581ffd1a36749e95f8a3287fbde476f..3826e0471d9eada001b8cb13b07cdde7bde8b25c 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -40,7 +40,8 @@ namespace armarx
{
public:
CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
- const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped);
+ const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
+ bool considerJointLimits = true);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
@@ -49,6 +50,9 @@ namespace armarx
void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
+ bool getConsiderJointLimits() const;
+ void setConsiderJointLimits(bool value);
+
Eigen::MatrixXf jacobi;
Eigen::MatrixXf inv;
VirtualRobot::RobotNodeSetPtr rns;
@@ -58,7 +62,8 @@ namespace armarx
private:
Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);
-
+ bool adjustJacobiForJointsAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, float jointLimitCheckAccuracy = 0.001f);
+ bool considerJointLimits = true;
float cartesianMMRegularization;
float cartesianRadianRegularization;
};
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index f50dedc362145b2ef1aeb812f98233ba0f6da8d4..216c48d906af3f8ea6e14f3a2a89424a1ce13c22 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -76,3 +76,82 @@ BOOST_AUTO_TEST_CASE(test1)
BOOST_CHECK_LE((targetTcpVel - tcpVel).norm(), 0.01f);
}
+
+BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
+{
+ const std::string project = "RobotAPI";
+ armarx::CMakePackageFinder finder(project);
+
+ if (!finder.packageFound())
+ {
+ ARMARX_ERROR_S << "ArmarX Package " << project << " has not been found!";
+ }
+ else
+ {
+ armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+ }
+ std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
+ ArmarXDataPath::getAbsolutePath(fn, fn);
+ std::string robotFilename = fn;
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("TorsoRightArm");
+ CartesianVelocityControllerPtr h(new CartesianVelocityController(rns, nullptr, VirtualRobot::JacobiProvider::eSVDDamped));
+ const Eigen::VectorXf oldJV = rns->getJointValuesEigen();
+ ARMARX_INFO << VAROUT(oldJV);
+ Eigen::VectorXf cartesianVel(6);
+ srand(123);
+
+ for (int i = 0; i < 10000; ++i)
+ {
+
+
+ cartesianVel << rand() % 100, rand() % 100, rand() % 100, 0.01 * (rand() % 100), 0.01 * (rand() % 100), 0.01 * (rand() % 100);
+ // cartesianVel << 100,0,0;//, 0.01*(rand()%100), 0.01*(rand()%100), 0.01*(rand()%100);
+ auto mode = VirtualRobot::IKSolver::CartesianSelection::All;
+ Eigen::MatrixXf jacobi = h->ik->getJacobianMatrix(rns->getTCP(), mode);
+ Eigen::MatrixXf inv = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+ int jointIndex = rand() % rns->getSize();
+ jacobi.block(0, jointIndex, jacobi.rows(), 1) = Eigen::VectorXf::Zero(jacobi.rows());
+ Eigen::MatrixXf inv2 = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+
+ // ARMARX_INFO << "diff\n" << (inv-inv2);
+ rns->setJointValues(oldJV);
+ Eigen::Vector3f posBefore = h->tcp->getPositionInRootFrame();
+
+ float accuracy = 0.001f;
+
+ Eigen::VectorXf jointVel = inv * cartesianVel;
+ rns->setJointValues(oldJV + jointVel * accuracy);
+ Eigen::VectorXf resultCartesianVel = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+
+
+
+ Eigen::VectorXf jointVel2 = inv2 * cartesianVel;
+ rns->setJointValues(oldJV + jointVel2 * accuracy);
+ Eigen::VectorXf resultCartesianVel2 = ((h->tcp->getPositionInRootFrame() - posBefore) / accuracy);
+
+ Eigen::Vector3f diff = (resultCartesianVel - cartesianVel);
+ Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel);
+
+ if (!((diff - diff2).norm() < 0.5 || diff2.norm() < diff.norm()))
+ {
+ ARMARX_INFO << "Target cartesian velo:\n" << cartesianVel;
+ ARMARX_INFO << "jacobi\n" << jacobi;
+ ARMARX_INFO << "inv\n" << inv;
+ ARMARX_INFO << "inv2\n" << inv2;
+ ARMARX_INFO << "\n\nResulting joint cart velocity: " << resultCartesianVel << "\n jointVel:\n" << jointVel;
+ ARMARX_INFO << "\n\nResulting joint cart velocity with joint limit avoidance: " << resultCartesianVel2 << "\n jointVel:\n" << jointVel2;
+
+ ARMARX_INFO << "Diff:\n" << diff;
+ ARMARX_INFO << "Diff2:\n" << diff2;
+ }
+ else
+ {
+ ARMARX_INFO << "Success for \n" << cartesianVel;
+ }
+
+
+ BOOST_REQUIRE((diff - diff2).norm() < 1 || diff2.norm() < diff.norm());
+
+ }
+}