Commits

Martin Felis committed f7f525e

Added support for multi-dof joints

  • Participants
  • Parent commits 77e81e2
  • Branches dev

Comments (0)

Files changed (5)

File src/Joint.cc

 
 		return;
 	} else if (model.mJoints[joint_id].mJointType == JointTypeRevolute) {
-		// Only rotations around coordinate axes are supported so far!
-		if (S == SpatialVector(1., 0., 0., 0., 0., 0.)) {
-			XJ = Xrotx (q);
-		} else if (S == SpatialVector(0., 1., 0., 0., 0., 0.)) {
-			XJ = Xroty (q);
-		} else if (S == SpatialVector(0., 0., 1., 0., 0., 0.)) {
-			XJ = Xrotz (q);
-		} else {
-			assert (0 && !"Invalid joint axis!");
-		}
+		XJ = Xrot (q, Vector3d (
+					model.mJoints[joint_id].mJointAxes[0][0],
+					model.mJoints[joint_id].mJointAxes[0][1],
+					model.mJoints[joint_id].mJointAxes[0][2]
+					));
 	} else if (model.mJoints[joint_id].mJointType == JointTypePrismatic) {
 		XJ = Xtrans ( Vector3d (
 					model.mJoints[joint_id].mJointAxes[0][3] * q,
 	JointTypePrismatic,
 
 	JointType2DoF,
+	JointType3DoF,
+	JointType4DoF,
+	JointType5DoF,
+	JointType6DoF
 };
 
-/** \brief Describes a joint relative to the predecessor body 
+/** \brief Describes a joint relative to the predecessor body.
  *
  * This class contains all information required for one single joint. This
  * contains the joint type and the axis of the joint.
 		}
 	}
 
-	/** \brief Constructs a joint from the given cartesian parameters
+	/** \brief Constructs a joint from the given cartesian parameters.
 	 *
 	 * This constructor creates all the required spatial values for the given
 	 * cartesian parameters.
 		}
 	}
 
+	/** \brief Constructs a 2 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1
+			) {
+		mJointType = JointType2DoF;
+		mDoFCount = 2;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+	}
+
+	/** \brief Constructs a 3 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2
+			) {
+		mJointType = JointType3DoF;
+		mDoFCount = 3;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+	}
+
+	/** \brief Constructs a 4 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 * \param axis_3 Motion subspace for axis 3
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2,
+			const Math::SpatialVector &axis_3
+			) {
+		mJointType = JointType4DoF;
+		mDoFCount = 4;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+		mJointAxes[3] = axis_3;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+		validate_spatial_axis (mJointAxes[3]);
+	}
+
+	/** \brief Constructs a 5 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 * \param axis_3 Motion subspace for axis 3
+	 * \param axis_4 Motion subspace for axis 4
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2,
+			const Math::SpatialVector &axis_3,
+			const Math::SpatialVector &axis_4
+			) {
+		mJointType = JointType5DoF;
+		mDoFCount = 5;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+		mJointAxes[3] = axis_3;
+		mJointAxes[4] = axis_4;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+		validate_spatial_axis (mJointAxes[3]);
+		validate_spatial_axis (mJointAxes[4]);
+	}
+
+	/** \brief Constructs a 6 DoF joint with the given motion subspaces.
+	 *
+	 * The motion subspaces are of the format:
+	 * \f[ (r_x, r_y, r_z, t_x, t_y, t_z) \f]
+	 *
+	 * \note So far only pure rotations or pure translations are supported.
+	 *
+	 * \param axis_0 Motion subspace for axis 0
+	 * \param axis_1 Motion subspace for axis 1
+	 * \param axis_2 Motion subspace for axis 2
+	 * \param axis_3 Motion subspace for axis 3
+	 * \param axis_4 Motion subspace for axis 4
+	 * \param axis_5 Motion subspace for axis 5
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0,
+			const Math::SpatialVector &axis_1,
+			const Math::SpatialVector &axis_2,
+			const Math::SpatialVector &axis_3,
+			const Math::SpatialVector &axis_4,
+			const Math::SpatialVector &axis_5
+			) {
+		mJointType = JointType6DoF;
+		mDoFCount = 6;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+
+		mJointAxes[0] = axis_0;
+		mJointAxes[1] = axis_1;
+		mJointAxes[2] = axis_2;
+		mJointAxes[3] = axis_3;
+		mJointAxes[4] = axis_4;
+		mJointAxes[5] = axis_5;
+
+		validate_spatial_axis (mJointAxes[0]);
+		validate_spatial_axis (mJointAxes[1]);
+		validate_spatial_axis (mJointAxes[2]);
+		validate_spatial_axis (mJointAxes[3]);
+		validate_spatial_axis (mJointAxes[4]);
+		validate_spatial_axis (mJointAxes[5]);
+	}
+
+	/** \brief Checks whether we have pure rotational or translational axis.
+	 *
+	 * This function is mainly used to print out warnings when specifying an
+	 * axis that might not be intended.
+	 */
+	bool validate_spatial_axis (Math::SpatialVector &axis) {
+		if (fabs(axis.norm() - 1.0) > 1.0e-8) {
+			std::cerr << "Warning: joint axis is not unit!" << std::endl;
+			return false;
+		}
+
+		bool axis_rotational = false;
+		bool axis_translational = false;
+
+		Math::Vector3d rotation (axis[0], axis[1], axis[2]);
+		if (fabs(rotation.norm() - 1.0) < 1.0e-8)
+			axis_rotational = true;
+
+		Math::Vector3d translation (axis[3], axis[4], axis[5]);
+		if (fabs(translation.norm() - 1.0) < 1.0e-8)
+			axis_translational = true;
+
+		return axis_rotational || axis_translational;
+	}
+
 	/// \brief The spatial axis of the joint
 	Math::SpatialVector* mJointAxes;
 	/// \brief Type of joint (rotational or prismatic)

File src/Model.cc

 		const Joint &joint,
 		const Body &body,
 		std::string body_name) {
+	// Here we emulate multi DoF joints by simply adding nullbodies. This
+	// may be changed in the future, but so far it is reasonably fast.
+	if (joint.mJointType != JointTypePrismatic 
+			&& joint.mJointType != JointTypeRevolute
+			&& joint.mJointType != JointTypeFixed) {
+		unsigned int joint_count;
+		if (joint.mJointType == JointType2DoF)
+			joint_count = 2;
+		else if (joint.mJointType == JointType3DoF)
+			joint_count = 3;
+		else if (joint.mJointType == JointType4DoF)
+			joint_count = 4;
+		else if (joint.mJointType == JointType5DoF)
+			joint_count = 5;
+		else if (joint.mJointType == JointType6DoF)
+			joint_count = 6;
+		else {
+			std::cerr << "Error: Invalid joint type: " << joint.mJointType << std::endl;
+			assert (0 && !"Invalid joint type!");
+		}
+
+		Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
+		unsigned int null_parent = parent_id;
+		SpatialTransform null_transform;
+
+		for (unsigned int j = 0; j < joint_count; j++) {
+			Joint single_dof_joint;
+
+			Vector3d rotation (
+					joint.mJointAxes[j][0],
+					joint.mJointAxes[j][1],
+					joint.mJointAxes[j][2]);
+			Vector3d translation (
+					joint.mJointAxes[j][3],
+					joint.mJointAxes[j][4],
+					joint.mJointAxes[j][5]);
+
+			if (rotation == Vector3d (0., 0., 0.)) {
+				single_dof_joint = Joint (JointTypePrismatic, translation);
+			} else if (translation == Vector3d (0., 0., 0.)) {
+				single_dof_joint = Joint (JointTypeRevolute, rotation);
+			}
+
+			if (j == 0)
+				// if we are adding the first joint, we have to use the original
+				// transformation!
+				null_parent = AddBody (null_parent, joint_frame, single_dof_joint, null_body);
+			else if (j == joint_count - 1)
+				// if we are at the last we must add the real body
+				return AddBody (null_parent, null_transform, single_dof_joint, body, body_name);
+			else
+				// otherwise we just add an intermediate body
+				null_parent = AddBody (null_parent, null_transform, single_dof_joint, null_body);
+		}
+	}
+
 	assert (lambda.size() > 0);
 	assert (joint.mJointType != JointTypeUndefined);
 
  * \link RigidBodyDynamics::Model::AddBody Model::AddBody(...)\endlink,
  * \link RigidBodyDynamics::Model::AppendBody Model::AppendBody(...)\endlink, and
  * \link RigidBodyDynamics::Model::GetBodyId Model::GetBodyId(...)\endlink,
- * are used to initialize and construct the \ref model_structure.
+ * are used to initialize and construct the \ref model_structure. To
+ * create a model with a floating base (a.k.a a model with a free-flyer
+ * joint) it is recommended to use
+ * \link RigidBodyDynamics::Model::SetFloatingBaseBody Model::SetFloatingBaseBody(...)\endlink.
  *
  * Once this is done, the model structure can be used with the functions of \ref
  * 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
 	 * \li rotation Y
 	 * \li rotation X
 	 *
+	 * To specify a different ordering, it is recommended to create a 6 DoF
+	 * joint. See \link RigidBodyDynamics::Joint Joint\endlink for more
+	 * information.
+	 *
 	 * \param body Properties of the floating base body.
 	 *
 	 *  \returns id of the body with 6 DoF

File tests/ModelTests.cc

 
 	CHECK_ARRAY_CLOSE (base_coords.data(), base_coords_back.data(), 3, TEST_PREC);
 }
+
+TEST ( Model2DoFJoint ) {
+	// the standard modeling using a null body
+	Body null_body;
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	Joint joint_rot_x (
+			JointTypeRevolute,
+			Vector3d(1., 0., 0.)
+			);
+
+	Model model_std;
+	model_std.Init();
+	model_std.gravity = Vector3d (0., -9.81, 0.);
+
+	model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); 
+	model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); 
+
+	// using a model with a 2 DoF joint
+	Joint joint_rot_zx (
+		SpatialVector (0., 0., 1., 0., 0., 0.),
+		SpatialVector (1., 0., 0., 0., 0., 0.)
+		);
+
+	Model model_2;
+	model_2.Init();
+	model_2.gravity = Vector3d (0., -9.81, 0.);
+
+	model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zx, body);
+
+	VectorNd Q = VectorNd::Zero(model_std.dof_count);
+	VectorNd QDot = VectorNd::Zero(model_std.dof_count);
+	VectorNd Tau = VectorNd::Zero(model_std.dof_count);
+
+	VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
+	VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
+
+	ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std);
+	ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2);
+
+	CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC);
+}
+
+TEST ( Model3DoFJoint ) {
+	// the standard modeling using a null body
+	Body null_body;
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	Joint joint_rot_y (
+			JointTypeRevolute,
+			Vector3d(0., 1., 0.)
+			);
+	Joint joint_rot_x (
+			JointTypeRevolute,
+			Vector3d(1., 0., 0.)
+			);
+
+	Model model_std;
+	model_std.Init();
+	model_std.gravity = Vector3d (0., -9.81, 0.);
+
+	unsigned int body_id;
+
+	// in total we add two bodies to make sure that the transformations are
+	// correct.
+	model_std.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); 
+	model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); 
+	body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); 
+
+	model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); 
+	model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); 
+	body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); 
+
+	// using a model with a 2 DoF joint
+	Joint joint_rot_zyx (
+		SpatialVector (0., 0., 1., 0., 0., 0.),
+		SpatialVector (0., 1., 0., 0., 0., 0.),
+		SpatialVector (1., 0., 0., 0., 0., 0.)
+		);
+
+	Model model_2;
+	model_2.Init();
+	model_2.gravity = Vector3d (0., -9.81, 0.);
+
+	// in total we add two bodies to make sure that the transformations are
+	// correct.
+	body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
+	body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
+
+	VectorNd Q = VectorNd::Zero(model_std.dof_count);
+	VectorNd QDot = VectorNd::Zero(model_std.dof_count);
+	VectorNd Tau = VectorNd::Zero(model_std.dof_count);
+
+	VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
+	VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
+
+	ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std);
+	ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2);
+
+	CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC);
+}
+
+TEST ( Model6DoFJoint ) {
+	// the standard modeling using a null body
+	Body null_body;
+	Body body(1., Vector3d (1., 0.4, 0.4), Vector3d (1., 1., 1.));
+
+	Joint joint_rot_z (
+			JointTypeRevolute,
+			Vector3d(0., 0., 1.)
+			);
+	Joint joint_rot_y (
+			JointTypeRevolute,
+			Vector3d(0., 1., 0.)
+			);
+	Joint joint_rot_x (
+			JointTypeRevolute,
+			Vector3d(1., 0., 0.)
+			);
+
+	Model model_std;
+	model_std.Init();
+	model_std.gravity = Vector3d (0., -9.81, 0.);
+
+	unsigned int body_id;
+
+	// in total we add two bodies to make sure that the transformations are
+	// correct.
+	body_id = model_std.SetFloatingBaseBody (body);
+
+	model_std.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body); 
+	model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body); 
+	body_id = model_std.AppendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body); 
+
+	// using a model with a 2 DoF joint
+	Joint joint_floating_base (
+		SpatialVector (0., 0., 0., 1., 0., 0.),
+		SpatialVector (0., 0., 0., 0., 1., 0.),
+		SpatialVector (0., 0., 0., 0., 0., 1.),
+		SpatialVector (0., 0., 1., 0., 0., 0.),
+		SpatialVector (0., 1., 0., 0., 0., 0.),
+		SpatialVector (1., 0., 0., 0., 0., 0.)
+		);
+
+	Joint joint_rot_zyx (
+		SpatialVector (0., 0., 1., 0., 0., 0.),
+		SpatialVector (0., 1., 0., 0., 0., 0.),
+		SpatialVector (1., 0., 0., 0., 0., 0.)
+		);
+
+	Model model_2;
+	model_2.Init();
+	model_2.gravity = Vector3d (0., -9.81, 0.);
+
+	// in total we add two bodies to make sure that the transformations are
+	// correct.
+	body_id = model_2.AddBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_floating_base, body);
+	body_id = model_2.AddBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
+
+	VectorNd Q = VectorNd::Zero(model_std.dof_count);
+	VectorNd QDot = VectorNd::Zero(model_std.dof_count);
+	VectorNd Tau = VectorNd::Zero(model_std.dof_count);
+
+	VectorNd QDDot_2 = VectorNd::Zero(model_std.dof_count);
+	VectorNd QDDot_std = VectorNd::Zero(model_std.dof_count);
+
+	ForwardDynamics (model_std, Q, QDot, Tau, QDDot_std);
+	ForwardDynamics (model_2, Q, QDot, Tau, QDDot_2);
+
+	CHECK_ARRAY_CLOSE (QDDot_std.data(), QDDot_2.data(), model_std.dof_count, TEST_PREC);
+}