DH Chain Base Offset (#71)

* Added fixed base offset transform to DH chain; removed fixed DH joint type

* Added two-axis positioner DH chain

* Revised error checking in FK function

* Removed const specifiers from DH chain transforms and base transform to support move assignment

rct_optimizations/include/rct_optimizations/dh_chain.h

@@ -14,7 +14,6 @@ enum class DHJointType : unsigned
 {
   LINEAR,
   REVOLUTE,
-  FIXED
 };
 
 template<typename T>
@@ -66,7 +65,7 @@ struct DHTransform
       updated_params(1) += joint_value;
       break;
     default:
-      break;
+      throw std::runtime_error("Unknown DH joint type");
     }
 
     // Perform the DH transformations
@@ -100,11 +99,15 @@ struct DHTransform
    *  r: The linear offset in X
    *  alpha: The rotational offset about X
    */
-  Eigen::Vector4d params;
-  DHJointType type; /** @brief The type of actuation of the joint */
-  double max = M_PI; /** @brief Joint max */
-  double min = -M_PI; /** @brief Joint min */
-  std::string name; /** @brief Label for this transform */
+  const Eigen::Vector4d params;
+  /** @brief The type of actuation of the joint */
+  const DHJointType type;
+  /** @brief Joint maximum value */
+  double max = M_PI;
+  /** @brief Joint minimum value */
+  double min = -M_PI;
+  /** @brief Label for this transform */
+  std::string name;
 };
 
 /**
@@ -113,37 +116,51 @@ struct DHTransform
 class DHChain
 {
 public:
-    DHChain(std::vector<DHTransform> transforms);
+  DHChain(std::vector<DHTransform> transforms,
+          const Eigen::Isometry3d& base_offset = Eigen::Isometry3d::Identity());
 
   /**
-   * @brief Calculates forward kinematics for the robot with the joints provided.
+   * @brief Calculates forward kinematics for the chain with the joints provided.
    * Note: the transform to the n-th link is calculated, where n is the size of @ref joint_values
-   * @param joint_values - The joint values with which to calculate forward kinematics.
+   * @param joint_values - The joint values with which to calculate forward kinematics (size: [<= @ref dof()])
    * @return
-   * @throws Exception if the size of joint values is larger than the number of DH transforms in the robot
+   * @throws Exception if the size of joint values is larger than the number of DH transforms in the chain
    */
   template<typename T>
   Isometry3<T> getFK(const Eigen::Matrix<T, Eigen::Dynamic, 1> &joint_values) const
   {
-    Isometry3<T> transform(Isometry3<T>::Identity());
-    for (Eigen::Index i = 0; i < joint_values.size(); ++i)
-    {
-      transform = transform * transforms_.at(i).createRelativeTransform(joint_values[i]);
-    }
-    return transform;
+    Eigen::Matrix<T, Eigen::Dynamic, 4> offsets = Eigen::Matrix<T, Eigen::Dynamic, 4>::Zero(dof(), 4);
+    return getFK(joint_values, offsets);
   }
 
   /**
-   * @brief Override function of @ref getFK but using a data pointer for easier integration with Ceres
-   * @param joint_values
-   * @param offsets
+   * @brief Calculates the forward kinematics for the chain given a set of joint values and DH parameter offsets
+   * Note: the transform to the n-th link is calculated, where n is the size of @ref joint_values
+   * @param joint_values - The joint values with which to calculate the forward kinematics (size: [<= @ref dof()])
+   * @param offsets - The DH parameter offsets to apply when calculating the forward kinematics (size: [@ref dof() x 4])
    * @return
+   * @throws Exception if the size of @ref joint_values is larger than the number of DH transforms in the chain
+   * or if the size of @ref joint_values is larger than the rows of DH offsets
    */
   template<typename T>
   Isometry3<T> getFK(const Eigen::Matrix<T, Eigen::Dynamic, 1>& joint_values,
                      const Eigen::Matrix<T, Eigen::Dynamic, 4>& offsets) const
   {
-    Isometry3<T> transform(Isometry3<T>::Identity());
+    if (joint_values.size() > dof())
+    {
+      std::stringstream ss;
+      ss << "Joint values size (" << joint_values.size() << ") is larger than the chain DoF (" << dof() << ")";
+      throw std::runtime_error(ss.str());
+    }
+    else if (joint_values.size() > offsets.rows())
+    {
+      std::stringstream ss;
+      ss << "Joint values size (" << joint_values.size() << ") is larger than the rows of DH offsets ("
+         << offsets.rows() << ")";
+      throw std::runtime_error(ss.str());
+    }
+
+    Isometry3<T> transform(base_offset_.cast<T>());
     for (Eigen::Index i = 0; i < joint_values.size(); ++i)
     {
       const Eigen::Matrix<T, 1, 4> &offset = offsets.row(i);
@@ -165,7 +182,7 @@ class DHChain
   std::size_t dof() const;
 
   /**
-   * @brief Gets a dof x 4 matrix of the DH parameters
+   * @brief Gets a @ref dof() x 4 matrix of the DH parameters
    * @return
    */
   Eigen::MatrixX4d getDHTable() const;
@@ -176,11 +193,17 @@ class DHChain
    */
   std::vector<DHJointType> getJointTypes() const;
 
+  /**
+   * @brief Returns the labels of the DH parameters for each DH transform in the chain
+   * @return
+   */
   std::vector<std::array<std::string, 4>> getParamLabels() const;
 
 protected:
   /** @brief The DH transforms that make up the chain */
   std::vector<DHTransform> transforms_;
+  /** @brief Fixed transform offset to the beginning of the chain */
+  Eigen::Isometry3d base_offset_;
 };
 
 } // namespace rct_optimizations

rct_optimizations/src/rct_optimizations/dh_chain.cpp

@@ -40,8 +40,10 @@ std::array<std::string, 4> DHTransform::getParamLabels() const
 
 // DH Chain
 
-DHChain::DHChain(std::vector<DHTransform> transforms)
+DHChain::DHChain(std::vector<DHTransform> transforms,
+                 const Eigen::Isometry3d& base_offset)
   : transforms_(std::move(transforms))
+  , base_offset_(base_offset)
 {
 }
 

rct_optimizations/test/dh_parameter_utest.cpp

@@ -4,18 +4,30 @@
 
 using namespace rct_optimizations;
 
-TEST(DHChain, FKTest)
+TEST(DHChain, FKTestABBIRB2400)
 {
   DHChain robot = test::createABBIRB2400();
-  Eigen::Isometry3d transform = robot.getFK<double>(Eigen::VectorXd::Zero(6));
+  Eigen::Isometry3d transform = robot.getFK<double>(Eigen::VectorXd::Zero(robot.dof()));
   std::cout << transform.matrix() << std::endl;
 
   Eigen::Isometry3d desired(Eigen::Isometry3d::Identity());
   desired.translate(Eigen::Vector3d(0.940, 0.0, 1.455));
   desired.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));
   std::cout << desired.matrix() << std::endl;
 
-  EXPECT_TRUE(transform.isApprox(desired, std::numeric_limits<double>::epsilon()));
+  EXPECT_TRUE(transform.isApprox(desired));
+}
+
+TEST(DHChain, FKTestPositioner)
+{
+  DHChain robot = test::createTwoAxisPositioner();
+  Eigen::Isometry3d transform = robot.getFK<double>(Eigen::VectorXd::Zero(robot.dof()));
+
+  Eigen::Isometry3d desired = Eigen::Isometry3d::Identity();
+  desired.translate(Eigen::Vector3d(2.2, 0.0, 1.125));
+  desired.rotate(Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitZ()));
+
+  EXPECT_TRUE(transform.isApprox(desired));
 }
 
 TEST(DHChain, NoisyFKTest)
@@ -28,25 +40,25 @@ TEST(DHChain, NoisyFKTest)
 
   Eigen::MatrixX4d dh_offsets = Eigen::MatrixX4d(robot.dof(), 4).unaryExpr([&](float) { return dist(mt_rand); });
 
-  Eigen::Isometry3d transform = robot.getFK<double>(Eigen::VectorXd::Zero(6), dh_offsets);
+  Eigen::Isometry3d transform = robot.getFK<double>(Eigen::VectorXd::Zero(robot.dof()), dh_offsets);
   std::cout << "FK transform with noise\n" << transform.matrix() << std::endl;
 
   Eigen::Isometry3d desired(Eigen::Isometry3d::Identity());
   desired.translate(Eigen::Vector3d(0.940, 0.0, 1.455));
   desired.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));
   std::cout << "Desired FK transform \n" << desired.matrix() << std::endl;
 
-  EXPECT_FALSE(transform.isApprox(desired, std::numeric_limits<double>::epsilon()));
+  EXPECT_FALSE(transform.isApprox(desired));
 }
 
 TEST(DHChain, FKWithJointSubsetTest)
 {
   DHChain robot = test::createABBIRB2400();
-  for (std::size_t i = 0; i < 6; ++i)
+  for (std::size_t i = 0; i < robot.dof(); ++i)
   {
     EXPECT_NO_THROW(robot.getFK<double>(Eigen::VectorXd::Zero(i)));
   }
-  EXPECT_THROW(robot.getFK<double>(Eigen::VectorXd::Zero(7)), std::out_of_range);
+  EXPECT_THROW(robot.getFK<double>(Eigen::VectorXd::Zero(robot.dof() + 1)), std::runtime_error);
 }
 
 TEST(DHChain, generateObservations3D)
@@ -71,7 +83,7 @@ class DHChainObservationGeneration : public ::testing::Test
     , target(7, 5, 0.025)
   {
     // Create a transform to the tool0 of the robot at it's all-zero position
-    camera_base_to_target_base = camera_chain.getFK<double>(Eigen::VectorXd::Zero(6));
+    camera_base_to_target_base = camera_chain.getFK<double>(Eigen::VectorXd::Zero(camera_chain.dof()));
 
     // Place the target 0.5m in front of the robot's all-zero position, facing back at the robot
     camera_base_to_target_base.translate(Eigen::Vector3d(0.0, 0.0, 0.5));

rct_optimizations/test/include/rct_optimizations_tests/utilities.h

@@ -88,6 +88,12 @@ DHChain createChain(const Eigen::MatrixXd &dh, const std::vector<DHJointType> &j
  */
 DHChain createABBIRB2400();
 
+/**
+ * @brief Creates a DH parameter-based robot representation of a generic two-axis part positioner with arbitrary base offset
+ * @return
+ */
+DHChain createTwoAxisPositioner();
+
 /**
  * @brief Creates a kinematic chain whose DH parameters are pertubed with Gaussian noise relative to the reference kinematic chain
  * @param in - reference kinematic chain

rct_optimizations/test/src/utilities.cpp

@@ -57,26 +57,58 @@ DHChain createChain(const Eigen::MatrixXd &dh, const std::vector<DHJointType> &j
 
 DHChain createABBIRB2400()
 {
-  std::vector<DHTransform> joints;
-  joints.reserve(6);
+  std::vector<DHTransform> transforms;
+  transforms.reserve(6);
 
-  Eigen::Vector4d t1, t2, t3, t4, t5, t6;
+  Eigen::Vector4d p1, p2, p3, p4, p5, p6;
 
-  t1 << 0.615, 0.0, 0.100, -M_PI / 2.0;
-  t2 << 0.0, -M_PI / 2.0, 0.705, 0.0;
-  t3 << 0.0, 0.0, 0.135, -M_PI / 2.0;
-  t4 << 0.755, 0.0, 0.0, M_PI / 2.0;
-  t5 << 0.0, 0.0, 0.0, -M_PI / 2.0;
-  t6 << 0.085, M_PI, 0.0, 0.0;
+  p1 << 0.615, 0.0, 0.100, -M_PI / 2.0;
+  p2 << 0.0, -M_PI / 2.0, 0.705, 0.0;
+  p3 << 0.0, 0.0, 0.135, -M_PI / 2.0;
+  p4 << 0.755, 0.0, 0.0, M_PI / 2.0;
+  p5 << 0.0, 0.0, 0.0, -M_PI / 2.0;
+  p6 << 0.085, M_PI, 0.0, 0.0;
 
-  joints.push_back(DHTransform(t1, DHJointType::REVOLUTE, "j1"));
-  joints.push_back(DHTransform(t2, DHJointType::REVOLUTE, "j2"));
-  joints.push_back(DHTransform(t3, DHJointType::REVOLUTE, "j3"));
-  joints.push_back(DHTransform(t4, DHJointType::REVOLUTE, "j4"));
-  joints.push_back(DHTransform(t5, DHJointType::REVOLUTE, "j5"));
-  joints.push_back(DHTransform(t6, DHJointType::REVOLUTE, "j6"));
+  transforms.emplace_back(p1, DHJointType::REVOLUTE, "j1");
+  transforms.emplace_back(p2, DHJointType::REVOLUTE, "j2");
+  transforms.emplace_back(p3, DHJointType::REVOLUTE, "j3");
+  transforms.emplace_back(p4, DHJointType::REVOLUTE, "j4");
+  transforms.emplace_back(p5, DHJointType::REVOLUTE, "j5");
+  transforms.emplace_back(p6, DHJointType::REVOLUTE, "j6");
 
-  return DHChain(joints);
+  return DHChain(transforms);
+}
+
+DHChain createTwoAxisPositioner()
+{
+  std::vector<DHTransform> transforms;
+  transforms.reserve(2);
+
+  Eigen::Vector4d p1, p2;
+  p1 << 0.0, 0.0, 0.0, -M_PI / 2.0;
+  p2 << -0.475, -M_PI / 2.0, 0.0, 0.0;
+
+  // Add the first DH transform
+  {
+    DHTransform t(p1, DHJointType::REVOLUTE, "j1");
+    t.max = M_PI;
+    t.min = -M_PI;
+    transforms.push_back(t);
+  }
+  // Add the second DH transform
+  {
+    DHTransform dh_transform(p2, DHJointType::REVOLUTE, "j2");
+    dh_transform.max = 2.0 * M_PI;
+    dh_transform.min = -2.0 * M_PI;
+    transforms.push_back(dh_transform);
+  }
+
+  // Set an arbitrary base offset
+  Eigen::Isometry3d base_offset(Eigen::Isometry3d::Identity());
+  base_offset.translate(Eigen::Vector3d(2.2, 0.0, 1.6));
+  base_offset.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()));
+
+  return DHChain(transforms, base_offset);
 }
 
 DHChain perturbDHChain(const DHChain &in, const double stddev)