Fix bug in tesseract kdl kinematics

Author: Levi-Armstrong

tesseract/tesseract_kinematics/include/tesseract_kinematics/kdl/kdl_utils.h

@@ -181,9 +181,6 @@ inline bool parseSceneGraph(KDLChainData& results,
 
   results.joint_list.resize(results.robot_chain.getNrOfJoints());
   results.joint_limits.resize(results.robot_chain.getNrOfJoints(), 2);
-  std::vector<int> joint_too_segment;
-  joint_too_segment.resize(results.robot_chain.getNrOfJoints());
-  joint_too_segment.back() = -1;
 
   results.segment_index[results.base_name] = 0;
   results.link_list.push_back(results.base_name);
@@ -195,6 +192,9 @@ inline bool parseSceneGraph(KDLChainData& results,
     const KDL::Joint& jnt = seg.getJoint();
     results.link_list.push_back(seg.getName());
 
+    // When requesting forward kin for link_name it is index + 1
+    results.segment_index[seg.getName()] = static_cast<int>(i + 1);
+
     if (found)
       results.active_link_list.push_back(seg.getName());
 
@@ -211,8 +211,6 @@ inline bool parseSceneGraph(KDLChainData& results,
     const tesseract_scene_graph::Joint::ConstPtr& joint = scene_graph.getJoint(jnt.getName());
     results.joint_limits(j, 0) = joint->limits->lower;
     results.joint_limits(j, 1) = joint->limits->upper;
-    if (j > 0)
-      joint_too_segment[j - 1] = static_cast<int>(i);
 
     // Need to set limits for continuous joints. TODO: This may not be required
     // by the optization library but may be nice to have
@@ -226,38 +224,6 @@ inline bool parseSceneGraph(KDLChainData& results,
     ++j;
   }
 
-  for (unsigned i = 0; i < results.robot_chain.getNrOfSegments(); ++i)
-  {
-    bool found = false;
-    const KDL::Segment& seg = results.robot_chain.getSegment(i);
-    tesseract_scene_graph::Link::ConstPtr link_model = scene_graph.getLink(seg.getName());
-    while (!found)
-    {
-      // Check if the link is the root
-      std::vector<tesseract_scene_graph::Joint::ConstPtr> parent_joints =
-          scene_graph.getInboundJoints(link_model->getName());
-      if (parent_joints.empty())
-      {
-        results.segment_index[seg.getName()] = 0;
-        break;
-      }
-
-      std::string joint_name = parent_joints[0]->getName();
-      std::vector<std::string>::const_iterator it =
-          std::find(results.joint_list.begin(), results.joint_list.end(), joint_name);
-      if (it != results.joint_list.end())
-      {
-        unsigned joint_index = static_cast<unsigned>(it - results.joint_list.begin());
-        results.segment_index[seg.getName()] = joint_too_segment[joint_index];
-        found = true;
-      }
-      else
-      {
-        link_model = scene_graph.getSourceLink(joint_name);
-      }
-    }
-  }
-
   return true;
 }
 }  // namespace tesseract_kinematics

tesseract/tesseract_kinematics/test/kdl_kinematics_unit.cpp

@@ -63,6 +63,83 @@ void runFwdKinTest(tesseract_kinematics::ForwardKinematics& kin)
   ///////////////////////////
   // Test forward kinematics
   ///////////////////////////
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_1"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = 0;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_2"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = -0.00043624;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0.36;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_3"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = -0.00043624;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0.36;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_4"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = 0;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0.36 + 0.42;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_5"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = 0;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0.36 + 0.42;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_6"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = 0;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0.36 + 0.42 + 0.4;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
+  {
+    pose.setIdentity();
+    EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "link_7"));
+    Eigen::Isometry3d result;
+    result.setIdentity();
+    result.translation()[0] = 0;
+    result.translation()[1] = 0;
+    result.translation()[2] = 0.36 + 0.42 + 0.4;
+    EXPECT_TRUE(pose.isApprox(result));
+  }
+
   pose.setIdentity();
   EXPECT_TRUE(kin.calcFwdKin(pose, jvals, "tool0"));
   Eigen::Isometry3d result;
@@ -73,6 +150,29 @@ void runFwdKinTest(tesseract_kinematics::ForwardKinematics& kin)
   EXPECT_TRUE(pose.isApprox(result));
 }
 
+void runJacobianTestHelper(tesseract_kinematics::ForwardKinematics& kin,
+                           const Eigen::VectorXd& jvals,
+                           const std::string& link_name,
+                           const Eigen::Vector3d& link_point,
+                           const Eigen::Isometry3d& change_base)
+{
+  Eigen::MatrixXd jacobian, numerical_jacobian;
+  Eigen::Isometry3d pose;
+  kin.calcFwdKin(pose, jvals, link_name);
+
+  jacobian.resize(6, 7);
+  EXPECT_TRUE(kin.calcJacobian(jacobian, jvals, link_name));
+  tesseract_kinematics::jacobianChangeBase(jacobian, change_base);
+  tesseract_kinematics::jacobianChangeRefPoint(jacobian, (change_base * pose).linear() * link_point);
+
+  numerical_jacobian.resize(6, 7);
+  tesseract_kinematics::numericalJacobian(numerical_jacobian, change_base, kin, jvals, link_name, link_point);
+
+  for (int i = 0; i < 6; ++i)
+    for (int j = 0; j < 7; ++j)
+      EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
+}
+
 void runJacobianTest(tesseract_kinematics::ForwardKinematics& kin)
 {
   //////////////////////////////////////////////////////////////////
@@ -93,17 +193,16 @@ void runJacobianTest(tesseract_kinematics::ForwardKinematics& kin)
   ///////////////////////////
   // Test Jacobian
   ///////////////////////////
-  jacobian.resize(6, 7);
-  EXPECT_TRUE(kin.calcJacobian(jacobian, jvals, "tool0"));
-
   Eigen::Vector3d link_point(0, 0, 0);
-  numerical_jacobian.resize(6, 7);
-  tesseract_kinematics::numericalJacobian(
-      numerical_jacobian, Eigen::Isometry3d::Identity(), kin, jvals, "tool0", link_point);
-
-  for (int i = 0; i < 6; ++i)
-    for (int j = 0; j < 7; ++j)
-      EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
+  runJacobianTestHelper(kin, jvals, "base_link", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_1", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_2", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_3", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_4", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_5", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_6", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "link_7", link_point, Eigen::Isometry3d::Identity());
+  runJacobianTestHelper(kin, jvals, "tool0", link_point, Eigen::Isometry3d::Identity());
 
   ///////////////////////////
   // Test Jacobian at Point
@@ -115,17 +214,15 @@ void runJacobianTest(tesseract_kinematics::ForwardKinematics& kin)
     Eigen::Vector3d link_point(0, 0, 0);
     link_point[k] = 1;
 
-    // calcJacobian requires the link point to be in the base frame for which the jacobian is calculated.
-    EXPECT_TRUE(kin.calcJacobian(jacobian, jvals, "tool0"));
-    tesseract_kinematics::jacobianChangeRefPoint(jacobian, pose.linear() * link_point);
-
-    numerical_jacobian.resize(6, 7);
-    tesseract_kinematics::numericalJacobian(
-        numerical_jacobian, Eigen::Isometry3d::Identity(), kin, jvals, "tool0", link_point);
-
-    for (int i = 0; i < 6; ++i)
-      for (int j = 0; j < 7; ++j)
-        EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
+    runJacobianTestHelper(kin, jvals, "base_link", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_1", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_2", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_3", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_4", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_5", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_6", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "link_7", link_point, Eigen::Isometry3d::Identity());
+    runJacobianTestHelper(kin, jvals, "tool0", link_point, Eigen::Isometry3d::Identity());
   }
 
   ///////////////////////////////////////////
@@ -143,15 +240,15 @@ void runJacobianTest(tesseract_kinematics::ForwardKinematics& kin)
     change_base.translation() = Eigen::Vector3d(0, 0, 0);
     change_base.translation()[k] = 1;
 
-    EXPECT_TRUE(kin.calcJacobian(jacobian, jvals, "tool0"));
-    tesseract_kinematics::jacobianChangeBase(jacobian, change_base);
-
-    numerical_jacobian.resize(6, 7);
-    tesseract_kinematics::numericalJacobian(numerical_jacobian, change_base, kin, jvals, "tool0", link_point);
-
-    for (int i = 0; i < 6; ++i)
-      for (int j = 0; j < 7; ++j)
-        EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
+    runJacobianTestHelper(kin, jvals, "base_link", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_1", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_2", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_3", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_4", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_5", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_6", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_7", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "tool0", link_point, change_base);
   }
 
   ///////////////////////////////////////////
@@ -170,18 +267,15 @@ void runJacobianTest(tesseract_kinematics::ForwardKinematics& kin)
     change_base(1, 1) = 0;
     change_base.translation() = link_point;
 
-    kin.calcFwdKin(pose, jvals, "tool0");
-    // calcJacobian requires the link point to be in the base frame for which the jacobian is calculated.
-    EXPECT_TRUE(kin.calcJacobian(jacobian, jvals, "tool0"));
-    tesseract_kinematics::jacobianChangeBase(jacobian, change_base);
-    tesseract_kinematics::jacobianChangeRefPoint(jacobian, (change_base * pose).linear() * link_point);
-
-    numerical_jacobian.resize(6, 7);
-    tesseract_kinematics::numericalJacobian(numerical_jacobian, change_base, kin, jvals, "tool0", link_point);
-
-    for (int i = 0; i < 6; ++i)
-      for (int j = 0; j < 7; ++j)
-        EXPECT_NEAR(numerical_jacobian(i, j), jacobian(i, j), 1e-3);
+    runJacobianTestHelper(kin, jvals, "base_link", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_1", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_2", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_3", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_4", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_5", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_6", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "link_7", link_point, change_base);
+    runJacobianTestHelper(kin, jvals, "tool0", link_point, change_base);
   }
 }