From 2f35223a22736e53e5049404f8bc21171091f939 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 2 May 2016 20:31:31 +0200
Subject: [PATCH] Removed non-working joint limit avoidance from TCPControlUnit
---
.../components/units/TCPControlUnit.cpp | 40 -------------------
1 file changed, 40 deletions(-)
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 7b0c9430d..f0a9068ef 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -493,50 +493,10 @@ namespace armarx
dIK->computeSteps(jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
- MatrixXf fullJac = dIK->calcFullJacobian();
- MatrixXf fullJacInv = dIK->computePseudoInverseJacobianMatrix(fullJac);
- Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet, fullJac, fullJacInv);
-
- // for debugging only!
- /*{
- MatrixXf posJac = fullJac.block(0, 0, fullJac.rows(), 3);
- MatrixXf oriJac = fullJac.block(0, 3, fullJac.rows(), 3);
- JacobiSVD<MatrixXf> svdPos(posJac);
- JacobiSVD<MatrixXf> svdOri(oriJac);
- if (svdPos.singularValues()[2] < 90)
- {
- ARMARX_IMPORTANT << deactivateSpam() << "The robot is close to a singularity! Minimal singular value of the position Jacobian: " << svdPos.singularValues()[2];
- }
- if (svdOri.singularValues()[2] < 0.6)
- {
- ARMARX_IMPORTANT << deactivateSpam() << "The robot is close to a singularity! Minimal singular value of the orientation Jacobian: " << svdOri.singularValues()[2];
- }
- }*/
-
- // added by David S
- const float maxJointLimitCompensationRatio = 1.0f;
-
- // if (jointLimitCompensation(0) == jointLimitCompensation(0))
- // {
- // // if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio * jointDelta.norm())
- // // {
- // // jointLimitCompensation = maxJointLimitCompensationRatio * jointDelta.norm() / jointLimitCompensation.norm() * jointLimitCompensation;
- // // }
-
- // jointDelta += jointLimitCompensation;
- // }
-
jointDelta /= (cycleTime * 0.001);
jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
- // Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
- // const Eigen::Matrix4f mat = robotNodeSet->getTCP()->getGlobalPose() * lastTCPPose.inverse();
- // const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
- // Eigen::Vector3f rpy;
- // VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
- // ARMARX_DEBUG << deactivateSpam(2) << "Current TCP Position: \n" << currentTCPPosition;
- // ARMARX_VERBOSE << deactivateSpam(2)<< "ActualTCPVelocity: " << (rpy/(0.001*cycleTime));
lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
// build name value map
--
GitLab