Check that robot control is not NaN.

jmirabel · olivier-stasse · commit 59242573d56a · 2018-10-24T05:55:35.000+02:00
diff --git a/src/tools/device.cpp b/src/tools/device.cpp
@@ -526,6 +526,13 @@ void Device::integrate( const double & dt )
 {
   const Vector & controlIN = controlSIN.accessCopy();
 
+  if (sanityCheck_ && controlIN.hasNaN())
+  {
+    dgRTLOG () << "Device::integrate: Control has NaN values: " << '\n'
+               << controlIN.transpose() << '\n';
+    return;
+  }
+
   if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION)
   {
     assert(state_.size()==controlIN.size()+6);

Original file line number	Diff line number	Diff line change
`@@ -526,6 +526,13 @@ void Device::integrate( const double & dt )`
`526`	`526`	`{`
`527`	`527`	`const Vector & controlIN = controlSIN.accessCopy();`
`528`	`528`
	`529`	`+ if (sanityCheck_ && controlIN.hasNaN())`
	`530`	`+ {`
	`531`	`+ dgRTLOG () << "Device::integrate: Control has NaN values: " << '\n'`
	`532`	`+ << controlIN.transpose() << '\n';`
	`533`	`+ return;`
	`534`	`+ }`
	`535`	`+`
`529`	`536`	`if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION)`
`530`	`537`	`{`
`531`	`538`	`assert(state_.size()==controlIN.size()+6);`