Martin Felis avatar Martin Felis committed eb29fe7

nonlinear forces added

Comments (0)

Files changed (1)

example/codegen_example.cc

 	
 	body_b_id = model->AddBody(body_a_id, Xtrans(Vector3d(1., 0., 0.)), joint_b, body_b);
 	
-	body_c = Body (0., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.));
+	body_c = Body (1., Vector3d (0.5, 0., 0.), Vector3d (1., 1., 1.));
 		joint_c = Joint (
 		JointTypeRevolute,
 		Vector3d (0., 0., 1.)
 	q_sym[0] = t;
 
 	MatrixNd mass_matrix_sym = MatrixNd::Zero (model->dof_count, model->dof_count);
-	mass_matrix_sym(0,0) = q_sym[0];
-	mass_matrix_sym(1,1) = q_sym[1];
-	mass_matrix_sym(2,2) = q_sym[2];
-
 	ForwardKinematics (*model, q_sym, qdot_sym, qddot_sym);
 
 	CompositeRigidBodyAlgorithm (*model, q_sym, mass_matrix_sym);
-	cout << "### JOINT SPACE INERTIA ###" << endl;
+	cout << "### JOINT SPACE INERTIA M(q) ###" << endl;
 	cout << mass_matrix_sym << endl;
 
 	cout << endl;
 
+	VectorNd coriolis_sym = VectorNd::Zero (model->dof_count);
+	InverseDynamics (*model, q_sym, qdot_sym, qddot_sym, tau_sym);
+
+	cout << "### CORIOLIS FORCES N(q, qdot) ###" << endl;
+	cout << tau_sym << endl;
+
+	cout << endl;
+
  	ForwardDynamics (*model, q_sym, qdot_sym, tau_sym, qddot_sym);
 
-	cout << "### FORWARD DYNAMICS ###" << endl;
+	cout << "### FORWARD DYNAMICS qddot = FD (q, qdot, tau) ###" << endl;
 	cout << qddot_sym << endl;
 
 	delete model;
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.