Adding verbose information and switch.

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@275 042f3d55-54a8-47e9-b7fb-15903f145c44

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

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;

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

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.

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

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;