Commits

Martin Felis committed ef0c097

added multi dof joint documentation, removed some old commented code

Comments (0)

Files changed (8)

 	src/rbdl_version.cc
 	src/rbdl_mathutils.cc
 	src/Logging.cc
-	src/Body.cc
 	src/Joint.cc
 	src/Model.cc
 	src/Kinematics.cc

src/Body.cc

-/*
- * RBDL - Rigid Body Library
- * Copyright (c) 2011 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
- *
- * Licensed under the zlib license. See LICENSE for more details.
- */
-
-#include <iostream>
-#include <assert.h>
-
-// #include "Body.h"
-
-using namespace std;
-
-
 		}
 		CS.f_ext_constraints[body_id].setZero();
 
-		////////////////////////////
 		CS.QDDot_t += CS.QDDot_0;
 
 		// compute the resulting acceleration
 			CS.K(ci,cj) = CS.normal[cj].dot(point_accel_t - CS.point_accel_0[cj]);
 			LOG << "point_accel_t = " << point_accel_t.transpose() << std::endl;
 		}
-		//////////////////////////////////
-
-		/*
-		// update the spatial accelerations due to the test force
-		for (unsigned j = 1; j < model.mBodies.size(); j++) {
-			if (model.lambda[j] != 0) {
-				model.a[j] = model.X_lambda[j] * model.a[model.lambda[j]] + model.c[j];
-			}	else {
-				model.a[j].setZero();
-			}
-
-			model.a[j] = model.a[j] + model.S[j] * QDDot_t[j - 1];
-		}
-		*/
-
-		/////////////i
-		/*
-
-		QDDot_t += QDDot_0;
-
-		{
-			SUPPRESS_LOGGING;
-			UpdateKinematicsCustom (model, NULL, NULL, &QDDot_t);
-		}
-		
-		LOG << "SUUUHUUM = " << (QDDot_t).transpose() << std::endl;
-	
-		for (unsigned int cj = 0; cj < ContactData.size(); cj++) {
-			static SpatialVector point_spatial_acc;
-			{
-				SUPPRESS_LOGGING;
-
-				// computation of the net effect (acceleration due to the test
-				// force)
-
-				// method 1: simply compute the acceleration by calling
-				// CalcPointAcceleration() (slow)
-				point_accel_t = CalcPointAcceleration (model, Q, QDot, QDDot_t, ContactData[cj].body_id, ContactData[cj].point, false);
-
-				// method 2: transforming the spatial acceleration
-				// appropriately.(faster: 
-//				point_global = CalcBodyToBaseCoordinates (model, Q, ContactData[cj].body_id, ContactData[cj].point, false);
-//				point_spatial_acc = Xtrans (point_global) * (spatial_inverse(model.X_base[ContactData[cj].body_id]) * model.a[ContactData[cj].body_id]);
-//				point_accel_t.set (point_spatial_acc[3], point_spatial_acc[4], point_spatial_acc[5]);
-
-				// method 3: reduce 1 Matrix-Matrix computation:
-				// \todo currently broken!
-//				point_global = CalcBodyToBaseCoordinates (model, Q, ContactData[cj].body_id, ContactData[cj].point, false);
-//				point_spatial_acc = spatial_inverse(model.X_base[ContactData[cj].body_id]) * model.a[ContactData[cj].body_id];
-//
-//				Matrix3d rx (0., point_global[2], -point_global[1], -point_global[2], 0, point_global[0], point_global[1], -point_global[0], 0.);
-//				Matrix3d R = model.X_base[ContactData[cj].body_id].block<3,3>(0,0).transpose();
-//				point_accel_t = rx * R * Vector3d (point_spatial_acc[0], point_spatial_acc[1], point_spatial_acc[2]) + Vector3d(point_spatial_acc[3], point_spatial_acc[4], point_spatial_acc[5]) ;
-			}
-
-			LOG << "point_spatial_a= " << point_spatial_acc.transpose() << std::endl;
-			LOG << "point_accel_0  = [" << point_accel_0.transpose() << " ]" << std::endl;
-			K(ci,cj) = ContactData[cj].normal.dot(point_accel_t);
-			LOG << "point_accel_t = [" << (point_accel_t).transpose() << " ]" << std::endl;
-		}
-		*/
-		////////////////
 	}
 
 	LOG << "K = " << std::endl << CS.K << std::endl;
 	LOG << "model.tau  = " << model.tau.transpose() << std::endl;
 	LOG << "---" << std::endl;
 
-
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 
 			model.f[i] -= model.X_base[i].toMatrixAdjoint() * (*f_ext)[i];
 
 		if (model.mJoints[i].mJointType == JointTypeFixed) {
+			model.f[lambda].setZero();
 		}
 	}
 
 
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 		model.tau[i] = model.S[i].dot(model.f[i]);
+
 		unsigned int lambda = model.lambda[i];
+
 		if (lambda != 0) {
 			model.f[lambda] = model.f[lambda] + model.X_lambda[i].toMatrixTranspose() * model.f[i];
 		}
 		LOG << "S[" << i << "] = " << model.S[i].transpose() << std::endl;
 	}
 
-
 	// copy back values
 	CopyModelStateVectorToDofVector (model, Tau, model.tau);
 }
 	// exception if we calculate it for the root body
 	assert (joint_id > 0);
 
-//	Joint joint = model.mJoints[joint_id];
-
 	// Calculate the spatial joint velocity
 	v_J = model.S.at(joint_id);
 
  * kinematics_group, \ref dynamics_group, \ref contacts_group, to perform
  * computations.
  *
- * See also \link RigidBodyDynamics::Joint Joint\endlink for joint
- * modeling.
- *
  * \section model_structure Model Structure
  *
  * The model structure contains all the parameters of the rigid multi-body
  * RigidBodyDynamics::Kinematics::CalcPointJacobian
  * Kinematics::CalcPointJacobian\endlink are called.
  *
+ * \section joint_models Joint Models
+ *
+ * The Rigid Body Dynamics Library supports models with multiple degrees of
+ * freedom. When a joint with more than one degrees of freedom is used,
+ * additional virtual bodies with zero mass that are connected by 1 degree
+ * of freedom joints to simulate the multiple degrees of freedom joint. Even
+ * though this adds some overhead in terms of memory usage, it allows to
+ * exploit fast computations on fixed size elements of the underlying math
+ * library Eigen3.
+ *
+ * Joints are defined by their motion subspace. For each degree of freedom
+ * a one dimensional motion subspace is specified as a Math::SpatialVector.
+ * This vector follows the following convention:
+ *   \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+ *
+ * To specify a planar joint with three degrees of freedom for which the
+ * first two are translations in \f$x\f$ and \f$y\f$ direction and the last
+ * is a rotation around \f$z\f$, the following joint definition can be used:
+ *
+ * \code
+ * Joint planar_joint = Joint ( 
+ *     Math::SpatialVector (0., 0., 0., 1., 0., 0.),
+ *     Math::SpatialVector (0., 0., 0., 0., 1., 0.),
+ *     Math::SpatialVector (0., 0., 1., 0., 0., 0.)
+ *     );
+ * \endcode
+ *
+ * See also: \link RigidBodyDynamics::Joint Joint\endlink.
+ *
  * \note Please note that in the Rigid %Body Dynamics Library all angles
  * are specified in radians.
+ *
+ *
  */
 
 /** \brief Contains all information about the rigid body model

tests/DynamicsTests.cc

 
 	model->AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint, null_body);
 	model->AppendBody(Xtrans(Vector3d(0., 0., 0.)), fixed_joint, body);
-	model->AppendBody(Xtrans(Vector3d(1., 0., 0.)), joint, null_body);
+	model->AppendBody(Xtrans(Vector3d(1., 0., 0.)), joint, body);
 	model->AppendBody(Xtrans(Vector3d(0., 0., 0.)), fixed_joint, body);
 
 	// proper initialization of Q, QDot, QDDot, Tau

tests/ModelTests.cc

 	// tests/CompositeRigidBodyTests.cc
 }
 
-/*
 TEST_FIXTURE(ModelFixture, TestCalcVelocitiesSimple) {
 	Body body(1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
 	Joint joint (
 	CHECK_EQUAL (spatial_body_velocity, model->v.at(2));
 	// std::cout << LogOutput.str() << std::endl;
 }
-*/
 
 TEST_FIXTURE ( ModelFixture, TestTransformBaseToLocal ) {
 	Body body;