diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp index c3abb9d2ad419e99c907ae28526467ddd0eef3ce..e9e52f4d239a415223fae72a58c44d4f0ae339ea 100644 --- a/VirtualRobot/IK/DifferentialIK.cpp +++ b/VirtualRobot/IK/DifferentialIK.cpp @@ -32,6 +32,7 @@ DifferentialIK::DifferentialIK(RobotNodeSetPtr _rns, RobotNodePtr _coordSystem) parents[nodes[i]] = p; } convertMMtoM = false; + verbose = false; } @@ -335,27 +336,32 @@ bool DifferentialIK::computeSteps(float stepSize, float minumChange, int maxNSte // check tolerances if (checkTolerances()) { - VR_INFO << "Tolerances ok, loop:" << step << endl; + if (verbose) + VR_INFO << "Tolerances ok, loop:" << step << endl; return true; } float d = dTheta.norm(); if (dTheta.norm()<minumChange) { - VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl; + if (verbose) + VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl; return false; } if (checkImprovement && d>lastDist) { - VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << ", last loop's norm:" << lastDist << "), loop:" << step << endl; + if (verbose) + VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << ", last loop's norm:" << lastDist << "), loop:" << step << endl; return false; } lastDist = d; step++; } - - VR_INFO << "IK failed, loop:" << step << endl; - VR_INFO << "pos error:" << getErrorPosition() << endl; - VR_INFO << "rot error:" << getErrorRotation() << endl; + if (verbose) + { + VR_INFO << "IK failed, loop:" << step << endl; + VR_INFO << "pos error:" << getErrorPosition() << endl; + VR_INFO << "rot error:" << getErrorRotation() << endl; + } return false; } @@ -369,4 +375,9 @@ void DifferentialIK::convertModelScalingtoM( bool enable ) convertMMtoM = enable; } +void DifferentialIK::setVerbose( bool enable ) +{ + verbose = enable; +} + } // namespace VirtualRobot diff --git a/VirtualRobot/IK/DifferentialIK.h b/VirtualRobot/IK/DifferentialIK.h index 11e7bd54a5bbacc145179894f738894919157cf2..ddd347bb664c8b99f3e5ec22579854972545a83f 100644 --- a/VirtualRobot/IK/DifferentialIK.h +++ b/VirtualRobot/IK/DifferentialIK.h @@ -243,6 +243,8 @@ public: Standard: disabled */ void convertModelScalingtoM(bool enable); + + void setVerbose(bool enable); protected: @@ -268,6 +270,8 @@ protected: std::vector <RobotNodePtr> nodes; std::map< RobotNodePtr, std::vector<RobotNodePtr> > parents; + bool verbose; + }; typedef boost::shared_ptr<DifferentialIK> DifferentialIKPtr; diff --git a/VirtualRobot/IK/GenericIKSolver.cpp b/VirtualRobot/IK/GenericIKSolver.cpp index 0d057f9f0f54fd384dfa08f043de11cda0db6d84..e9153412f9c95939225f079e466ca94cf228e858 100644 --- a/VirtualRobot/IK/GenericIKSolver.cpp +++ b/VirtualRobot/IK/GenericIKSolver.cpp @@ -106,5 +106,11 @@ bool GenericIKSolver::_sampleSolution( const Eigen::Matrix4f &globalPose, Cartes return solve(globalPose,selection,maxLoops); } +void GenericIKSolver::setVerbose( bool enable ) +{ + verbose = enable; + jacobian->setVerbose(verbose); +} + } // namespace VirtualRobot diff --git a/VirtualRobot/IK/GenericIKSolver.h b/VirtualRobot/IK/GenericIKSolver.h index 29cb7d4ad0c56a362d1ad972681e0a4e956887d7..c673a812a7f96f862cd6413bf5358af27f1e1d99 100644 --- a/VirtualRobot/IK/GenericIKSolver.h +++ b/VirtualRobot/IK/GenericIKSolver.h @@ -63,6 +63,8 @@ public: void setupJacobian(float stepSize, int maxLoops); + void setVerbose(bool enable); + protected: //! This method is called by the constructor and can be used in derived classes for initialization. diff --git a/VirtualRobot/IK/IKSolver.cpp b/VirtualRobot/IK/IKSolver.cpp index 20e7f3d2c3df5286e105afaf11f9b1d02fcf0864..a765d4f89a50f2c45ccc5b59c072a389e31406b8 100644 --- a/VirtualRobot/IK/IKSolver.cpp +++ b/VirtualRobot/IK/IKSolver.cpp @@ -27,6 +27,7 @@ namespace VirtualRobot IKSolver::IKSolver(RobotNodeSetPtr rns) : rns(rns) { + verbose = false; THROW_VR_EXCEPTION_IF(!rns, "Null data"); tcp = rns->getTCP(); THROW_VR_EXCEPTION_IF(!tcp, "no tcp"); @@ -240,5 +241,10 @@ VirtualRobot::RobotNodeSetPtr IKSolver::getRobotNodeSet() return rns; } +void IKSolver::setVerbose( bool enable ) +{ + verbose = enable; +} + } // namespace VirtualRobot diff --git a/VirtualRobot/IK/IKSolver.h b/VirtualRobot/IK/IKSolver.h index f838d1666e0d7d8ee8b0427847bce86759a74a38..2e36d7d85e11dfc9e61f030cdc73e5eb178c1364 100644 --- a/VirtualRobot/IK/IKSolver.h +++ b/VirtualRobot/IK/IKSolver.h @@ -148,6 +148,8 @@ public: RobotNodeSetPtr getRobotNodeSet(); RobotNodePtr getTcp(); + void setVerbose(bool enable); + protected: @@ -163,6 +165,8 @@ protected: float maxErrorOrientationRad; ReachabilityPtr reachabilitySpace; + + bool verbose; }; typedef boost::shared_ptr<IKSolver> IKSolverPtr;