1. Eric Cousineau
  2. rbdl

Commits

Martin Felis  committed 02a73fd

Joints with 1 DoF can now also be specified by its motion subspace

  • Participants
  • Parent commits f7f525e
  • Branches dev

Comments (0)

Files changed (2)

File src/Joint.h

View file
 	JointTypeRevolute,
 	JointTypePrismatic,
 
+	JointType1DoF,
 	JointType2DoF,
 	JointType3DoF,
 	JointType4DoF,
 		}
 	}
 
+	/** \brief Constructs a 1 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
+	 */
+	Joint (
+			const Math::SpatialVector &axis_0
+			) {
+		mJointType = JointType1DoF;
+		mDoFCount = 1;
+
+		mJointAxes = new Math::SpatialVector[mDoFCount];
+		mJointAxes[0] = axis_0;
+
+		validate_spatial_axis (mJointAxes[0]);
+	}
+
 	/** \brief Constructs a 2 DoF joint with the given motion subspaces.
 	 *
 	 * The motion subspaces are of the format:

File src/Model.cc

View file
 			&& joint.mJointType != JointTypeRevolute
 			&& joint.mJointType != JointTypeFixed) {
 		unsigned int joint_count;
-		if (joint.mJointType == JointType2DoF)
+		if (joint.mJointType == JointType1DoF)
+			joint_count = 1;
+		else if (joint.mJointType == JointType2DoF)
 			joint_count = 2;
 		else if (joint.mJointType == JointType3DoF)
 			joint_count = 3;
 
 		Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
 		unsigned int null_parent = parent_id;
-		SpatialTransform null_transform;
+		SpatialTransform joint_frame_transform;
 
 		for (unsigned int j = 0; j < joint_count; j++) {
 			Joint single_dof_joint;
 				single_dof_joint = Joint (JointTypeRevolute, rotation);
 			}
 
+			// the first joint has to be transformed by joint_frame, all the
+			// others must have a null transformation
 			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)
+				joint_frame_transform = joint_frame;
+			else
+				joint_frame_transform = SpatialTransform();
+
+			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);
+				return AddBody (null_parent, joint_frame_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);
+				null_parent = AddBody (null_parent, joint_frame_transform, single_dof_joint, null_body);
 		}
 	}