changed logging

source/RobotAPI/components/units/TCPControlUnit.cpp

@@ -348,7 +348,7 @@ namespace armarx
             {
                 //ARMARX_INFO << deactivateSpam(1) << "orientationVelocity before ChangeFrame: " << data.orientationVelocity->output();
                 data.orientationVelocity = FramedVector3::ChangeFrame(localRobot, *data.orientationVelocity, refFrame);
-                ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
+                ARMARX_INFO << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
                 Eigen::Matrix3f rot;
                 rot = Eigen::AngleAxisf(data.orientationVelocity->z*cycleTime*0.001, Eigen::Vector3f::UnitZ())
                     * Eigen::AngleAxisf(data.orientationVelocity->y*cycleTime*0.001, Eigen::Vector3f::UnitY())
@@ -700,12 +700,12 @@ namespace armarx
             float d = dTheta.norm();
             float posDist = getMeanErrorPosition();
             float oriErr = getErrorRotation(rns->getTCP());
-            if (dTheta.norm()<mininumChange)
-            {
-//                if (verbose)
-                    ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
-                break;
-            }
+//            if (dTheta.norm()<mininumChange)
+//            {
+////                if (verbose)
+//                    ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
+//                break;
+//            }
 
             if (checkImprovement && posDist>lastDist)
             {
@@ -807,7 +807,7 @@ namespace armarx
 //        applyDOFWeightsToJacobian(Jacobian);
         ARMARX_INFO_S << VAROUT(Jacobian);
         MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
-        ARMARX_INFO_S << VAROUT(Jacobian);
+        ARMARX_INFO_S << VAROUT(pseudo);
         ARMARX_INFO_S << VAROUT(error);
         return pseudo * error;
     }