Merge pull request #155 from Naoki-Hiraoka/PR-fixedrobot

[hrplib/hrpModel/ForwardDynamicsCBM.cpp] check equation num in solveUnknownAccels
fkanehiro · Jun 21, 2021 · eac8c03 · eac8c03
2 parents 2bfbea7 + bf8fce0
commit eac8c03
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/hrplib/hrpModel/ForwardDynamicsCBM.cpp b/hrplib/hrpModel/ForwardDynamicsCBM.cpp
@@ -707,7 +707,10 @@ void ForwardDynamicsMM::solveUnknownAccels(const Vector3& fext, const Vector3& t
         c1 -= d1;
 	c1 -= b1.col(0);
 
-        dvector a(M11.colPivHouseholderQr().solve(c1));
+        dvector a;
+        if(c1.size()!=0) {
+            a = M11.colPivHouseholderQr().solve(c1);
+        }
 
 	if(unknown_rootDof){
 		Link* root = body->rootLink();