NaN bug fixed for now. Some torques got nan values. This seemed to st…

…em from a normalization of a 0-vector when calculating fsw. This fixes #5 and #8. WIP visualization of all the scores as a graph.
jarllarsson · Aug 2, 2014 · fa3b54b · fa3b54b
1 parent 1ae4dbf
commit fa3b54b
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 8 deletions.
diff --git a/multileg/src/util/ColorPalettes.h b/multileg/src/util/ColorPalettes.h
@@ -40,7 +40,9 @@ Color4f toColor4f(const Color3f& p_col);
 
 Color3f toColor3f(const Color4f& p_col);
 
-static const Color3f colarr[18] = { { 1.0f, 0.0f, 0.0f },
+#define colarrSz 18
+
+static const Color3f colarr[colarrSz] = { { 1.0f, 0.0f, 0.0f },
 { 1.0f, 0.5f, 0.0f },
 { 1.0f, 0.0f, 0.5f },
 { 0.0f, 0.7f, 0.0f },

diff --git a/multileg/src/winapp/App.cpp b/multileg/src/winapp/App.cpp
@@ -131,6 +131,7 @@ void App::run()
 	std::vector<float>* bestParams = NULL;
 	int optimizationIterationCount = 0;
 	double bestScore = FLT_MAX;
+	std::vector<double> allResults;
 	m_toolBar->addReadOnlyVariable(Toolbar::PERFORMANCE, "O-Score", Toolbar::DOUBLE, &bestScore);
 	m_toolBar->addReadOnlyVariable(Toolbar::PERFORMANCE, "O-Iter", Toolbar::INT, &optimizationIterationCount);
 #endif
@@ -381,9 +382,27 @@ void App::run()
 		{
 			if (!pumpMessage(msg))
 			{
+				// draw axes
 				m_debugDrawBatch->drawLine(glm::vec3(0.0f), glm::vec3(10.0f, 0.0f, 0.0f), colarr[0], colarr[1]);
 				m_debugDrawBatch->drawLine(glm::vec3(0.0f), glm::vec3(0.0f, 10.0f, 0.0f), colarr[3], colarr[4]);
 				m_debugDrawBatch->drawLine(glm::vec3(0.0f), glm::vec3(0.0f, 0.0f, 10.0f), dawnBringerPalRGB[COL_NAVALBLUE], dawnBringerPalRGB[COL_LIGHTBLUE]);
+
+#ifdef OPTIMIZATION
+				// draw test graph if optimizing
+				int vals = allResults.size();
+				if (vals > 1)
+				{
+					m_debugDrawBatch->drawLine(glm::vec3(0.0f, 20.0f, 0.0f), glm::vec3(vals-1, 20.0f, 0.0f), Color3f(0.0f, 0.0f, 0.0f), Color3f(1.0f, 1.0f, 1.0f));
+					for (int i = 0; i < vals - 1; i++)
+					{
+						m_debugDrawBatch->drawLine(
+							glm::vec3((float)i, 20.0f + allResults[i]*10.0f, 0.0f),
+							glm::vec3((float)i + 1.0f, 20.0f + allResults[i + 1]*10.0f, 0.0f),
+							colarr[i% colarrSz], colarr[(i + 1) % colarrSz]);
+					}
+				}
+#endif
+
 				// Start by rendering
 				render();
 
@@ -497,6 +516,7 @@ void App::run()
 		bestParams = new std::vector<float>(optimizationSystem->getWinnerParams());
 		bestScore = optimizationSystem->getWinnerScore();
 		optimizationIterationCount++;
+		allResults.push_back(bestScore);
 	#endif
 
 

diff --git a/multileg/src/winapp/ControllerOptimizationSystem.cpp b/multileg/src/winapp/ControllerOptimizationSystem.cpp
@@ -9,7 +9,7 @@ ControllerOptimizationSystem::ControllerOptimizationSystem()
 	addComponentType<ControllerComponent>();
 	addComponentType<ControllerMovementRecorderComponent>();
 	// settings
-	m_simTicks = 600;			
+	m_simTicks = 120;			
 	m_warmupTicks = 2;	
 	m_instantEval = false;
 	// playback

diff --git a/multileg/src/winapp/ControllerSystem.cpp b/multileg/src/winapp/ControllerSystem.cpp
@@ -828,13 +828,17 @@ glm::vec3 ControllerSystem::calculateFsw(ControllerComponent::LegFrame* p_lf, un
 	p_lf->m_footTrackingSpringDamper.setKp_KdEQTenPrcntKp(Kft);
 	glm::vec3 diff = getFootPos(p_lf,p_legIdx) - p_lf->m_footStrikePlacement[p_legIdx];
 	float error = glm::length(diff);
-	glm::vec3 normdiff = glm::normalize(diff);
-	float pdval = p_lf->m_footTrackingSpringDamper.drive(error, p_dt);
-	glm::vec3 res = -normdiff * pdval;
-	bool vecnanchk = glm::isnan(res) == glm::bool3(true, true, true);
-	if (vecnanchk)
+	glm::vec3 res;
+	if (error > 0.0f)
 	{
-		int i = 0;
+		glm::vec3 normdiff = glm::normalize(diff);
+		float pdval = p_lf->m_footTrackingSpringDamper.drive(error, p_dt);
+		glm::vec3 res = -normdiff * pdval;
+		bool vecnanchk = glm::isnan(res) == glm::bool3(true, true, true);
+		if (vecnanchk)
+		{
+			int i = 0;
+		}
 	}
 	return res;
 }