Commits

Martin Felis committed 7fb63c5

removed unused model flag for use of experimental floating base

  • Participants
  • Parent commits 7afe0cf
  • Branches dev

Comments (0)

Files changed (6)

 	for (i = 0; i < CS.size(); i++) {
 		CS.constraint_impulse[i] = CS.x[model.dof_count + i];
 	}
-
 }
 
 /** \brief Compute only the effects of external forces on the generalized accelerations
 		) {
 	LOG << "-------- " << __func__ << " ------" << std::endl;
 
-//	LOG << "Q    = " << Q.transpose() << std::endl;
-//	LOG << "QDot = " << QDot.transpose() << std::endl;
-//	assert (ContactData.size() == 1);
-
 	assert (CS.f_ext_constraints.size() == model.mBodies.size());
 	assert (CS.QDDot_0.size() == model.dof_count);
 	assert (CS.QDDot_t.size() == model.dof_count);
 		) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 
-	if (model.experimental_floating_base) {
-		assert (0 && "Experimental floating base not supported");
-	}
-
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
 	unsigned int i = 0;
 		) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 
-	if (model.experimental_floating_base) {
-		assert (0 && !"InverseDynamics not supported for experimental floating base models!");
-	}
-
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
 	unsigned int i;

src/Kinematics.cc

 
 	unsigned int i;
 
-	if (model.experimental_floating_base) {
-		assert (0 && !"UpdateKinematics not supported yet for experimental floating bases");
-	}
-	
 	SpatialVector spatial_gravity (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
 
 	model.a[0].setZero();
 
 	unsigned int i;
 
-	if (model.experimental_floating_base) {
-		assert (0 && !"UpdateKinematics not supported yet for experimental floating bases");
-	}
-
 	if (Q) {
 		for (i = 1; i < model.mBodies.size(); i++) {
 			SpatialVector v_J;
 		bool update_kinematics
 	) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
-	if (model.experimental_floating_base) {
-		assert (0 && "CalcPointJacobian() not yet supported for experimental floating base models");
-	};
 
 	// update the Kinematics if necessary
 	if (update_kinematics) {
 	LOG << "-------- " << __func__ << " --------" << std::endl;
 	unsigned int i;
 
-	if (model.experimental_floating_base) {
-		assert (0 && !"floating base not supported");
-	}
-		
 	assert (body_id > 0 && body_id < model.mBodies.size());
 	assert (model.mBodies.size() == Q.size() + 1);
 	assert (model.mBodies.size() == QDot.size() + 1);
 	model.v[0].setZero();
 	model.a[0].setZero();
 
-	if (model.experimental_floating_base) {
-		assert (0 && !"floating base not supported");
-	}
-
 	if (update_kinematics)
 		UpdateKinematics (model, Q, QDot, QDDot);
 
 using namespace RigidBodyDynamics::Math;
 
 void Model::Init() {
-	experimental_floating_base = false;
-
 	Body root_body;
 	Joint root_joint;
 
 unsigned int Model::SetFloatingBaseBody (const Body &body) {
 	assert (lambda.size() >= 0);
 
-	if (experimental_floating_base) {
-		// we also will have 6 more degrees of freedom
-		dof_count += 6;
+	// Add five zero weight bodies and append the given body last. Order of
+	// the degrees of freedom is:
+	// 		tx ty tz rz ry rx
+	//
 
-		// parent is the maximum possible value to mark it as having no parent
-		lambda.at(0) = std::numeric_limits<unsigned int>::max();
+	unsigned body_tx_id;
+	Body body_tx (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+	Joint joint_tx (JointTypePrismatic, Vector3d (1., 0., 0.));
+	body_tx_id = this->AddBody(0, Xtrans (Vector3d (0., 0., 0.)), joint_tx, body_tx);
 
-		// Bodies
-		X_lambda.at(0) = SpatialTransform();
-		X_base.at(0) = SpatialTransform();
-		mBodies.at(0) = body;
-		return 0;
-	} else {
-		// Add five zero weight bodies and append the given body last. Order of
-		// the degrees of freedom is:
-		// 		tx ty tz rz ry rx
-		//
+	unsigned body_ty_id;
+	Body body_ty (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+	Joint joint_ty (JointTypePrismatic, Vector3d (0., 1., 0.));
+	body_ty_id = this->AddBody(body_tx_id, Xtrans (Vector3d (0., 0., 0.)), joint_ty, body_ty);
 
-		unsigned body_tx_id;
-		Body body_tx (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
-		Joint joint_tx (JointTypePrismatic, Vector3d (1., 0., 0.));
-		body_tx_id = this->AddBody(0, Xtrans (Vector3d (0., 0., 0.)), joint_tx, body_tx);
+	unsigned body_tz_id;
+	Body body_tz (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+	Joint joint_tz (JointTypePrismatic, Vector3d (0., 0., 1.));
+	body_tz_id = this->AddBody(body_ty_id, Xtrans (Vector3d (0., 0., 0.)), joint_tz, body_tz);
 
-		unsigned body_ty_id;
-		Body body_ty (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
-		Joint joint_ty (JointTypePrismatic, Vector3d (0., 1., 0.));
-		body_ty_id = this->AddBody(body_tx_id, Xtrans (Vector3d (0., 0., 0.)), joint_ty, body_ty);
+	unsigned body_rz_id;
+	Body body_rz (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+	Joint joint_rz (JointTypeRevolute, Vector3d (0., 0., 1.));
+	body_rz_id = this->AddBody(body_tz_id, Xtrans (Vector3d (0., 0., 0.)), joint_rz, body_rz);
 
-		unsigned body_tz_id;
-		Body body_tz (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
-		Joint joint_tz (JointTypePrismatic, Vector3d (0., 0., 1.));
-		body_tz_id = this->AddBody(body_ty_id, Xtrans (Vector3d (0., 0., 0.)), joint_tz, body_tz);
+	unsigned body_ry_id;
+	Body body_ry (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+	Joint joint_ry (JointTypeRevolute, Vector3d (0., 1., 0.));
+	body_ry_id = this->AddBody(body_rz_id, Xtrans (Vector3d (0., 0., 0.)), joint_ry, body_ry);
 
-		unsigned body_rz_id;
-		Body body_rz (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
-		Joint joint_rz (JointTypeRevolute, Vector3d (0., 0., 1.));
-		body_rz_id = this->AddBody(body_tz_id, Xtrans (Vector3d (0., 0., 0.)), joint_rz, body_rz);
+	unsigned body_rx_id;
+	Joint joint_rx (JointTypeRevolute, Vector3d (1., 0., 0.));
+	body_rx_id = this->AddBody(body_ry_id, Xtrans (Vector3d (0., 0., 0.)), joint_rx, body);
 
-		unsigned body_ry_id;
-		Body body_ry (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
-		Joint joint_ry (JointTypeRevolute, Vector3d (0., 1., 0.));
-		body_ry_id = this->AddBody(body_rz_id, Xtrans (Vector3d (0., 0., 0.)), joint_ry, body_ry);
-
-		unsigned body_rx_id;
-		Joint joint_rx (JointTypeRevolute, Vector3d (1., 0., 0.));
-		body_rx_id = this->AddBody(body_ry_id, Xtrans (Vector3d (0., 0., 0.)), joint_rx, body);
-
-		return body_rx_id;
-	}
+	return body_rx_id;
 }
 
 unsigned int Model::GetBodyId (const char *id) const {
 	/// \brief Contains the ids of all the children of a given body
 	std::vector<std::vector<unsigned int> >mu;
 
-	/** \brief Use floating base extension as described in RBDA chapter 9.4
-	 *
-	 * \warning This function is experimental and produces wrong results. Do
-	 * \warning not use!
-	 */
-	bool experimental_floating_base;
-
 	/** \brief number of degrees of freedoms of the model
 	 *
 	 * This value contains the number of entries in the generalized state (q)

tests/ModelTests.cc

 	CHECK_EQUAL (2u, model->mBodies.size());
 }
 
-TEST_FIXTURE(ModelFixture, TestExperimentalFloatingBodyDimensions) {
-	Body body;
-
-	model->experimental_floating_base = true;
-	model->SetFloatingBaseBody(body);
-
-	CHECK_EQUAL (1u, model->lambda.size());
-	CHECK_EQUAL (1u, model->mu.size());
-	CHECK_EQUAL (6u, model->dof_count);
-
-	CHECK_EQUAL (1u, model->v.size());
-	CHECK_EQUAL (1u, model->a.size());
-	
-	CHECK_EQUAL (1u, model->mJoints.size());
-	CHECK_EQUAL (1u, model->S.size());
-
-	CHECK_EQUAL (1u, model->c.size());
-	CHECK_EQUAL (1u, model->IA.size());
-	CHECK_EQUAL (1u, model->pA.size());
-	CHECK_EQUAL (1u, model->U.size());
-	CHECK_EQUAL (1u, model->d.size());
-	CHECK_EQUAL (1u, model->u.size());
-
-	SpatialVector spatial_zero;
-	spatial_zero.setZero();
-	
-	CHECK_EQUAL (1u, model->X_lambda.size());
-	CHECK_EQUAL (1u, model->X_base.size());
-	CHECK_EQUAL (1u, model->mBodies.size());
-}
-
 TEST_FIXTURE(ModelFixture, TestFloatingBodyDimensions) {
 	Body body;