Fix bug in Contact6d::setRegularizationTaskWeightVector

andreadelprete · andreadelprete · commit 71d10d132522 · 2020-11-25T11:12:42.000+01:00
diff --git a/cmake b/cmake
@@ -1 +1 @@
-Subproject commit 32015cb28d977b592227675665d17d11531ef418
+Subproject commit 000a90190d1dca8b028f437095f2fddff0f9839b
diff --git a/src/contacts/contact-6d.cpp b/src/contacts/contact-6d.cpp
@@ -138,7 +138,7 @@ typedef Eigen::Matrix<double,6,6> Matrix6;
   Matrix6 A = Matrix6::Zero();
   A.diagonal() = m_weightForceRegTask;
   m_forceRegTask.setMatrix(A*m_forceGenMat);
-  m_forceRegTask.setVector(m_fRef);
+  m_forceRegTask.setVector(A*m_fRef);
 }
 
 void Contact6d:: updateForceGeneratorMatrix()

Original file line number	Diff line number	Diff line change
`@@ -138,7 +138,7 @@ typedef Eigen::Matrix<double,6,6> Matrix6;`
`138`	`138`	`Matrix6 A = Matrix6::Zero();`
`139`	`139`	`A.diagonal() = m_weightForceRegTask;`
`140`	`140`	`m_forceRegTask.setMatrix(A*m_forceGenMat);`
`141`		`- m_forceRegTask.setVector(m_fRef);`
	`141`	`+ m_forceRegTask.setVector(A*m_fRef);`
`142`	`142`	`}`
`143`	`143`
`144`	`144`	`void Contact6d:: updateForceGeneratorMatrix()`