Commits

Martin Felis committed 958a7fd

Fixed bodies can now have a fixed body as parent

  • Participants
  • Parent commits 914d2cb

Comments (0)

Files changed (3)

File src/Model.cc

 	lambda.push_back(0.);
 	mu.push_back(std::vector<unsigned int>());
 	dof_count = 0;
+	previously_added_body_id = 0;
 
 	gravity = Vector3d (0., -9.81, 0.);
 
 		const Joint &joint,
 		const Body &body,
 		std::string body_name) {
-	if (parent_id >= model.fixed_body_discriminator) {
-		std::cerr << "Error: cannot attach Body with a fixed joint to another fixed body!" << std::endl;
-		assert (0);
-		abort();
-	}
 	FixedBody fbody = FixedBody::CreateFromBody (body);
 	fbody.mMovableParent = parent_id;
 	fbody.mParentTransform = joint_frame;
 
+	if (model.IsFixedBodyId(parent_id)) {
+		FixedBody fixed_parent = model.mFixedBodies[parent_id - model.fixed_body_discriminator];
+
+		fbody.mMovableParent = fixed_parent.mMovableParent;
+		fbody.mParentTransform = fixed_parent.mParentTransform * joint_frame;
+	}
+
 	// merge the two bodies
-	Body parent_body = model.mBodies[parent_id];
-	parent_body.Join (joint_frame, body);
-	model.mBodies[parent_id] = parent_body;
+	Body parent_body = model.mBodies[fbody.mMovableParent];
+	parent_body.Join (fbody.mParentTransform, body);
+	model.mBodies[fbody.mMovableParent] = parent_body;
 
 	model.mFixedBodies.push_back (fbody);
 
 		model.mBodyNameMap[body_name] = model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
 	}
 
-	return model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
+	model.previously_added_body_id = model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
+
+	return model.previously_added_body_id;
 }
 
 unsigned int AddBodyMultiDofJoint (
 		else
 			joint_frame_transform = SpatialTransform();
 
-		if (j == joint_count - 1)
+		if (j == joint_count - 1) {
 			// if we are at the last we must add the real body
 			return model.AddBody (null_parent, joint_frame_transform, single_dof_joint, body, body_name);
+		}
 		else
 			// otherwise we just add an intermediate body
 			null_parent = model.AddBody (null_parent, joint_frame_transform, single_dof_joint, null_body);
 		assert (0);
 		abort();
 	}
+	
+	previously_added_body_id = mBodies.size() - 1;
 
-	return mBodies.size() - 1;
+	return previously_added_body_id;
 }
 
 unsigned int Model::AppendBody (
 		const Body &body,
 		std::string body_name 
 		) {
-	unsigned int prev_body_id = mBodies.size() - 1;
-	return Model::AddBody (prev_body_id, joint_frame,
+	return Model::AddBody (previously_added_body_id, joint_frame,
 			joint, body, body_name);
 }
 
 	 * velocity (qdot), acceleration (qddot), and force (tau) vector.
 	 */
 	unsigned int dof_count;
+	/// \brief Id of the previously added body, required for Model::AppendBody()
+	unsigned int previously_added_body_id;
 
 	/// \brief the cartesian vector of the gravity
 	Math::Vector3d gravity;

File tests/ModelTests.cc

 	CHECK_EQUAL (movable_body, model.lambda[appended_body_id]);
 }
 
+// Adds a fixed body to another fixed body.
+TEST ( ModelAppendFixedToFixedBody ) {
+	Body null_body;
+
+	double movable_mass = 1.1;
+	Vector3d movable_com (1., 0.4, 0.4);
+
+	double fixed_mass = 1.2;
+	Vector3d fixed_com (1.1, 0.5, 0.5);
+
+	Vector3d fixed_displacement (0., 1., 0.);
+
+	Body body(movable_mass, movable_com, Vector3d (1., 1., 1.));
+	Body fixed_body(fixed_mass, fixed_com, Vector3d (1., 1., 1.));
+
+	Model model;
+	model.Init();
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	unsigned int movable_body = model.AddBody (0, Xtrans(Vector3d(0., 0., 0.)), joint_rot_z, body);
+	unsigned int fixed_body_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body");
+	unsigned int fixed_body_2_id = model.AppendBody (Xtrans(fixed_displacement), Joint(JointTypeFixed), fixed_body, "fixed_body_2");
+	unsigned int appended_body_id = model.AppendBody (Xtrans(Vector3d(0., 1., 0.)), joint_rot_z, body, "appended_body");
+
+	CHECK_EQUAL (movable_body + 1, appended_body_id);
+	CHECK_EQUAL (movable_body, model.lambda[appended_body_id]);
+	CHECK_EQUAL (movable_mass + fixed_mass * 2., model.mBodies[movable_body].mMass);
+
+	CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_id - model.fixed_body_discriminator].mMovableParent);
+	CHECK_EQUAL (movable_body, model.mFixedBodies[fixed_body_2_id - model.fixed_body_discriminator].mMovableParent);
+
+	double new_mass = 3.5;
+	Vector3d new_com = (1. / new_mass) * (movable_mass * movable_com + fixed_mass * (fixed_com + fixed_displacement) + fixed_mass * (fixed_com + fixed_displacement * 2.));
+
+	CHECK_ARRAY_EQUAL (new_com.data(), model.mBodies[movable_body].mCenterOfMass.data(), 3);
+}
+
 TEST ( ModelGetBodyName ) {
 	Body null_body;
 	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));