Martin  Felis avatar Martin Felis committed efdc494

removed generalized state information from the model structure

Comments (0)

Files changed (7)

  */
 void ForwardDynamicsApplyConstraintForces (
 		Model &model,
+		const VectorNd &Tau,
 		ConstraintSet &CS,
 		VectorNd &QDDot
 		) {
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 		CS.d_U[i] = CS.d_IA[i] * model.S[i];
 		CS.d_d[i] = model.S[i].dot(model.U[i]);
-		CS.d_u[i] = model.tau[i] - model.S[i].dot(CS.d_pA[i]);
+		CS.d_u[i] = Tau[i - 1] - model.S[i].dot(CS.d_pA[i]);
 
 		unsigned int lambda = model.lambda[i];
 		if (lambda != 0) {
 
 	{
 		SUPPRESS_LOGGING;
-		ForwardDynamicsApplyConstraintForces (model, CS, QDDot);
+		ForwardDynamicsApplyConstraintForces (model, Tau, CS, QDDot);
 	}
 }
 
 
 	unsigned int i = 0;
 
-	// Copy state values from the input to the variables in model
-	CopyDofVectorToModelStateVector (model, model.q, Q);
-	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
-	CopyDofVectorToModelStateVector (model, model.tau, Tau);
-
 	LOG << "Q          = " << Q.transpose() << std::endl;
 	LOG << "QDot       = " << QDot.transpose() << std::endl;
 	LOG << "Tau        = " << Tau.transpose() << std::endl;
 	LOG << "---" << std::endl;
-	LOG << "model.q    = " << model.q.transpose() << std::endl;
-	LOG << "model.qdot = " << model.qdot.transpose() << std::endl;
-	LOG << "model.tau  = " << model.tau.transpose() << std::endl;
-	LOG << "---" << std::endl;
 
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 		SpatialVector c_J;
 		unsigned int lambda = model.lambda[i];
 
-		jcalc (model, i, X_J, model.S[i], v_J, c_J, model.q[i], model.qdot[i]);
+		jcalc (model, i, X_J, model.S[i], v_J, c_J, Q[i - 1], QDot[i - 1]);
 		LOG << "X_T (" << i << "):" << std::endl << model.X_T[i] << std::endl;
 
 		model.X_lambda[i] = X_J * model.X_T[i];
 	for (i = model.mBodies.size() - 1; i > 0; i--) {
 		model.U[i] = model.IA[i] * model.S[i];
 		model.d[i] = model.S[i].dot(model.U[i]);
-		model.u[i] = model.tau[i] - model.S[i].dot(model.pA[i]);
+		model.u[i] = Tau[i - 1] - model.S[i].dot(model.pA[i]);
 
 		unsigned int lambda = model.lambda[i];
 		if (lambda != 0) {
 
 		model.a[i] = X_lambda.apply(model.a[lambda]) + model.c[i];
 
-		model.qddot[i] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
-		model.a[i] = model.a[i] + model.S[i] * model.qddot[i];
+		QDDot[i - 1] = (1./model.d[i]) * (model.u[i] - model.U[i].dot(model.a[i]));
+		model.a[i] = model.a[i] + model.S[i] * QDDot[i - 1];
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
 		LOG << "a[" << i << "] = " << model.a[i].transpose() << std::endl;
 	}
 
-	LOG << "qddot = " << model.qddot.transpose() << std::endl;
-
-	// copy back values
-	CopyModelStateVectorToDofVector (model, QDDot, model.qddot);
+	LOG << "QDDot = " << QDDot.transpose() << std::endl;
 }
 
 void ForwardDynamicsLagrangian (
 
 	unsigned int i;
 
-	// Copy state values from the input to the variables in model
-//	CopyDofVectorToModelStateVector (model, model.q, Q);
-//	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
-//	CopyDofVectorToModelStateVector (model, model.qddot, QDDot);
-
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 	model.a[0] = spatial_gravity * -1.;
 	}
 
 	LOG << "-- second loop" << std::endl;
-	LOG << "tau = " << model.tau.transpose() << std::endl;
+	LOG << "Tau = " << Tau.transpose() << std::endl;
 	for (i = 0; i < model.mBodies.size(); i++) {
 		LOG << "f[" << i << "] = " << model.f[i].transpose() << std::endl;
 	}
 	for (i = 0; i < model.mBodies.size(); i++) {
 		LOG << "S[" << i << "] = " << model.S[i].transpose() << std::endl;
 	}
-
-	// copy back values
-	// CopyModelStateVectorToDofVector (model, Tau, model.tau);
 }
 
 void CompositeRigidBodyAlgorithm (Model& model, const VectorNd &Q, MatrixNd &H, bool update_kinematics) {

src/Kinematics.cc

 		assert (0 && !"UpdateKinematics not supported yet for experimental floating bases");
 	}
 	
-	// Copy positions, velocities, and accelerations while taking account for
-	// fixed joints
-	CopyDofVectorToModelStateVector (model, model.q, Q);
-	CopyDofVectorToModelStateVector (model, model.qdot, QDot);
-	CopyDofVectorToModelStateVector (model, model.qddot, QDDot);
-
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
 	model.a[0].setZero();
 		Joint joint = model.mJoints[i];
 		unsigned int lambda = model.lambda[i];
 
-		jcalc (model, i, X_J, model.S[i], v_J, c_J, model.q[i], model.qdot[i]);
+		jcalc (model, i, X_J, model.S[i], v_J, c_J, Q[i - 1], QDot[i - 1]);
 
 		model.X_lambda[i] = X_J * model.X_T[i];
 
 		}
 		
 		model.a[i] = model.X_lambda[i].apply(model.a[lambda]) + model.c[i];
-		model.a[i] = model.a[i] + model.S[i] * model.qddot[i];
+		model.a[i] = model.a[i] + model.S[i] * QDDot[i - 1];
 	}
 
 	for (i = 1; i < model.mBodies.size(); i++) {
 	}
 
 	if (Q) {
-		CopyDofVectorToModelStateVector (model, model.q, *Q);
-	}
-
-	if (QDot) {
-		CopyDofVectorToModelStateVector (model, model.qdot, *QDot);
-	}
-
-	if (QDDot) {
-		CopyDofVectorToModelStateVector (model, model.qddot, *QDDot);
-	}
-
-	if (Q) {
 		for (i = 1; i < model.mBodies.size(); i++) {
 			SpatialVector v_J;
 			SpatialVector c_J;
 			Joint joint = model.mJoints[i];
 			unsigned int lambda = model.lambda[i];
 
-			jcalc (model, i, X_J, model.S[i], v_J, c_J, model.q[i], model.qdot[i]);
+			jcalc (model, i, X_J, model.S[i], v_J, c_J, (*Q)[i - 1], 0.);
 
 			model.X_lambda[i] = X_J * model.X_T[i];
 
 			Joint joint = model.mJoints[i];
 			unsigned int lambda = model.lambda[i];
 
-			jcalc (model, i, X_J, model.S[i], v_J, c_J, model.q[i], model.qdot[i]);
+			jcalc (model, i, X_J, model.S[i], v_J, c_J, (*Q)[i - 1], (*QDot)[i - 1]);
 
 			if (lambda != 0) {
 				model.v[i] = model.X_lambda[i].apply(model.v[lambda]) + v_J;
 				model.a[i].setZero();
 			}
 
-			model.a[i] = model.a[i] + model.S[i] * model.qddot[i];
+			model.a[i] = model.a[i] + model.S[i] * (*QDDot)[i - 1];
 		}
 	}
 }
 	}
 		
 	assert (body_id > 0 && body_id < model.mBodies.size());
-	assert (model.q.size() == Q.size() + 1);
-	assert (model.qdot.size() == QDot.size() + 1);
+	assert (model.mBodies.size() == Q.size() + 1);
+	assert (model.mBodies.size() == QDot.size() + 1);
 
 	// Reset the velocity of the root body
 	model.v[0].setZero();
 
 /** \defgroup kinematics_group Kinematics
  * @{
+ *
+ * \note Please note that in the Rigid %Body Dynamics Library all angles
+ * are specified in radians.
+ *
  */
 
 /** \brief Updates and computes velocities and accelerations of the bodies
 	gravity = Vector3d (0., -9.81, 0.);
 
 	// state information
-	q = VectorNd::Zero(1);
-	qdot = VectorNd::Zero(1);
-	qddot = VectorNd::Zero(1);
-	tau = VectorNd::Zero(1);
-
 	v.push_back(zero_spatial);
 	a.push_back(zero_spatial);
 
 	// structural information
 	lambda.push_back(parent_id);
 	mu.push_back(std::vector<unsigned int>());
-	mu.at(parent_id).push_back(q.size());
+	mu.at(parent_id).push_back(mBodies.size());
 
 	// Bodies
 	X_lambda.push_back(SpatialTransform());
 	dof_count++;
 
 	// state information
-	q = VectorNd::Zero (mBodies.size());
-	qdot = VectorNd::Zero (mBodies.size());
-	qddot = VectorNd::Zero (mBodies.size());
-	tau = VectorNd::Zero (mBodies.size());
-
 	v.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 	a.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
 
 		const Body &body,
 		std::string body_name 
 		) {
-	unsigned int prev_body_id = q.size() - 1;
+	unsigned int prev_body_id = mBodies.size() - 1;
 	return Model::AddBody (prev_body_id, joint_frame,
 			joint, body, body_name);
 }
 		// we also will have 6 more degrees of freedom
 		dof_count += 6;
 
-		q.resize (dof_count + 1);
-		qdot.resize (dof_count + 1);
-		qddot.resize (dof_count + 1);
-		tau.resize (dof_count + 1);
-
 		// parent is the maximum possible value to mark it as having no parent
 		lambda.at(0) = std::numeric_limits<unsigned int>::max();
 
  *
  * The model structure contains all the parameters of the rigid multi-body
  * model such as joint informations, mass and inertial parameters of the
- * rigid bodies, etc.
- *
- * Furthermore the current state of the model, such as the
- * generalized positions, velocities and accelerations is also stored
- * within the model. Angles are always stored as radians. This internal
- * state is used when functions such as \link
- * RigidBodyDynamics::Kinematics::CalcBodyToBaseCoordinates
- * Kinematics::CalcBodyToBaseCoordinates(...)\endlink, \link
- * RigidBodyDynamics::Kinematics::CalcPointJacobian
- * Kinematics::CalcPointJacobian\endlink are called.
+ * rigid bodies, etc. It also contains storage for the transformations and
+ * current state, such as velocity and acceleration of each body.
  *
  * \section joint_models Joint Models
  *
  *
  * \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
 	Math::Vector3d gravity;
 
 	// State information
-
-	/** \brief The joint position
-	 * 
-	 * Warning: to have an easier numbering in the algorithm the state vector
-	 * has #Bodies + 1 elements. However element with index 0 is not used!
-	 * 
-	 * q[0] - unused <br>
-	 * q[1] - joint 1 <br>
-	 * q[2] - joint 2 <br>
-	 * ... <br>
-	 * q[#Bodies] - joint #Bodies <br>
-	 *
-	 * \note The dimensions of q, qdot, qddot, and tau are increased whenever
-	 * a body is added. This is also true for bodies that are added with a
-	 * fixed joint. To query the actual number of degrees of freedom use
-	 * Model::dof_count.
-	 */
-	Math::VectorNd q;
-	/// \brief The joint velocity
-	Math::VectorNd qdot;
-	/// \brief The joint acceleration
-	Math::VectorNd qddot;
-	/// \brief The force / torque applied at joint i
-	Math::VectorNd tau;
 	/// \brief The spatial velocity of body i
 	std::vector<Math::SpatialVector> v;
 	/// \brief The spatial acceleration of body i
 	void Init ();
 };
 
-/** \brief Copies values from a DoF-vector to a model state vector.
- *
- * All values for the generalized vectors such as Q, QDot, QDDot, and Tau
- * are copied to the values within the model structure. As the model
- * structure uses a different numbering, all values have to be shifted by
- * one.
- */
-inline void CopyDofVectorToModelStateVector (const Model &model, Math::VectorNd &dest_model_state, const Math::VectorNd &dof_src) {
-	assert (dest_model_state.size() == model.mBodies.size());
-	assert (dof_src.size() == model.dof_count);
-
-	for (unsigned int i = 0; i < model.dof_count; i++)
-		dest_model_state[i + 1] = dof_src[i];
-}
-
-/** \brief Inverse operation to CopyDofVectorToModelStateVector() */
-inline void CopyModelStateVectorToDofVector (const Model &model, Math::VectorNd &dof_dest, const Math::VectorNd &src_model_state) {
-	assert (dof_dest.size() == model.dof_count);
-	assert (src_model_state.size() == model.mBodies.size());
-
-	for (unsigned int i = 0; i < model.dof_count; i++)
-		dof_dest[i] = src_model_state[i + 1];	
-}
-
 /** @} */
 
 }

tests/ModelTests.cc

 	CHECK_EQUAL (1u, model->mu.size());
 	CHECK_EQUAL (0u, model->dof_count);
 
-	CHECK_EQUAL (1u, model->q.size());
-	CHECK_EQUAL (1u, model->qdot.size());
-	CHECK_EQUAL (1u, model->qddot.size());
-	CHECK_EQUAL (1u, model->tau.size());
 	CHECK_EQUAL (1u, model->v.size());
 	CHECK_EQUAL (1u, model->a.size());
 	
 	CHECK_EQUAL (2u, model->mu.size());
 	CHECK_EQUAL (1u, model->dof_count);
 
-	CHECK_EQUAL (2u, model->q.size());
-	CHECK_EQUAL (2u, model->qdot.size());
-	CHECK_EQUAL (2u, model->qddot.size());
-	CHECK_EQUAL (2u, model->tau.size());
 	CHECK_EQUAL (2u, model->v.size());
 	CHECK_EQUAL (2u, model->a.size());
 	
 	CHECK_EQUAL (1u, model->mu.size());
 	CHECK_EQUAL (6u, model->dof_count);
 
-	CHECK_EQUAL (7u, model->q.size());
-	CHECK_EQUAL (7u, model->qdot.size());
-	CHECK_EQUAL (7u, model->qddot.size());
-	CHECK_EQUAL (7u, model->tau.size());
 	CHECK_EQUAL (1u, model->v.size());
 	CHECK_EQUAL (1u, model->a.size());
 	
 	CHECK_EQUAL (7u, model->mu.size());
 	CHECK_EQUAL (6u, model->dof_count);
 
-	CHECK_EQUAL (7u, model->q.size());
-	CHECK_EQUAL (7u, model->qdot.size());
-	CHECK_EQUAL (7u, model->qddot.size());
-	CHECK_EQUAL (7u, model->tau.size());
 	CHECK_EQUAL (7u, model->v.size());
 	CHECK_EQUAL (7u, model->a.size());
 	
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.