Martin  Felis avatar Martin Felis committed b8c7aca

made sure that ID gets called before CRBA in FDContactsLagrangian

Comments (0)

Files changed (1)

 	bool solve_successful = LinSolveGaussElimPivot (H, C * -1. + Tau, QDDot);
 	assert (solve_successful);
 #endif
+
+	LOG << "x = " << QDDot << std::endl;
 }
 
 void InverseDynamics (
 		) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 
-	// Compute H
-	MatrixNd H (model.dof_count, model.dof_count);
-	CompositeRigidBodyAlgorithm (model, Q, H);
+	// Note: InverseDynamics must be called *before*
+	// CompositeRigidBodyAlgorithm() as the latter does not update
+	// transformations etc.!
 
 	// Compute C
 	VectorNd QDDot_zero (model.dof_count);
 
 	InverseDynamics (model, Q, QDot, QDDot_zero, C);
 
+	// Compute H
+	MatrixNd H (model.dof_count, model.dof_count);
+	CompositeRigidBodyAlgorithm (model, Q, H);
+
 	// Compute G
 	MatrixNd G (ContactData.size(), model.dof_count);
 
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.