Fix jacobian calculation (#3069)

* Fix faulty Jacobian * Update tests * Format * More verbose error message Signed-off-by: Sebastian Jahr <[email protected]> * Fix columns --------- Signed-off-by: Sebastian Jahr <[email protected]>
moveit · Nov 8, 2024 · 5049e4d · 5049e4d
1 parent 1a77d7d
commit 5049e4d
Show file tree

Hide file tree

Showing 2 changed files with 115 additions and 11 deletions.
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -1496,19 +1496,25 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
   const int rows = use_quaternion_representation ? 7 : 6;
   const int columns = group->getVariableCount();
   jacobian.resize(rows, columns);
+  jacobian.setZero();
 
   // Get the tip pose with respect to the group root link. Append the user-requested offset 'reference_point_position'.
   const Eigen::Isometry3d root_pose_tip = root_pose_world * getGlobalLinkTransform(link);
   const Eigen::Vector3d tip_point = root_pose_tip * reference_point_position;
 
-  // Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian
-  // displacement at the tip. So we build the Jacobian incrementally joint by joint.
-  std::size_t active_joints = group->getActiveJointModels().size();
+  // Initialize the column index of the Jacobian matrix.
   int i = 0;
-  for (std::size_t joint = 0; joint < active_joints; ++joint)
+
+  // Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian
+  // displacement at the tip. So we build the Jacobian incrementally joint by joint up to the parent joint of the reference link.
+  for (const JointModel* joint_model : joint_models)
   {
+    // Stop looping if we reached the child joint of the reference link.
+    if (joint_model->getParentLinkModel() == link)
+    {
+      break;
+    }
     // Get the child link for the current joint, and its pose with respect to the group root link.
-    const JointModel* joint_model = joint_models[joint];
     const LinkModel* child_link_model = joint_model->getChildLinkModel();
     const Eigen::Isometry3d& root_pose_link = root_pose_world * getGlobalLinkTransform(child_link_model);
 
@@ -1540,6 +1546,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
       RCLCPP_ERROR(getLogger(), "Unknown type of joint in Jacobian computation");
       return false;
     }
+
     i += joint_model->getVariableCount();
   }
 

diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp
@@ -61,7 +61,8 @@ Eigen::VectorXd makeVector(const std::vector<double>& values)
 
 // Checks the validity of state.getJacobian() at the given 'joint_values' and 'joint_velocities'.
 void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group,
-                   const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities)
+                   const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities,
+                   const moveit::core::LinkModel* reference_link = nullptr)
 {
   // Using the Jacobian, compute the Cartesian velocity vector at which the end-effector would move, with the given
   // joint velocities.
@@ -73,9 +74,36 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod
   const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
   const Eigen::Isometry3d root_pose_world = state.getGlobalLinkTransform(root_link_model).inverse();
 
-  const Eigen::Isometry3d tip_pose_initial =
-      root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back());
-  const Eigen::MatrixXd jacobian = state.getJacobian(&joint_model_group);
+  if (!reference_link)
+  {
+    reference_link = joint_model_group.getLinkModels().back();
+  }
+  const Eigen::Isometry3d tip_pose_initial = root_pose_world * state.getGlobalLinkTransform(reference_link);
+  Eigen::MatrixXd jacobian;
+  state.getJacobian(&joint_model_group, reference_link, Eigen::Vector3d::Zero(), jacobian);
+
+  // Verify that only elements of the Jacobian contain values that correspond to joints that are being used based on the reference link.
+  const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group.getJointModels();
+  auto it = std::find_if(joint_models.begin(), joint_models.end(), [&](const moveit::core::JointModel* jm) {
+    return jm->getParentLinkModel() == reference_link;
+  });
+  if (it != joint_models.end())
+  {
+    std::size_t index = 0;
+    for (auto jt = joint_models.begin(); jt != it; ++jt)
+    {
+      index += (*jt)->getVariableCount();
+    }
+
+    EXPECT_TRUE(jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index).isZero())
+        << "Jacobian contains non-zero values for joints that are not used based on the reference link "
+        << reference_link->getName() << ". This is the faulty Jacobian: " << '\n'
+        << jacobian << '\n'
+        << "The columns " << index << " to " << jacobian.cols() << " should be zero. Instead the values are: " << '\n'
+        << jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index);
+  }
+
+  // Compute the Cartesian velocity vector using the Jacobian.
   const Eigen::VectorXd cartesian_velocity = jacobian * joint_velocities;
 
   // Compute the instantaneous displacement that the end-effector would achieve if the given joint
@@ -84,8 +112,7 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod
   const Eigen::VectorXd delta_joint_angles = time_step * joint_velocities;
   state.setJointGroupPositions(&joint_model_group, joint_values + delta_joint_angles);
   state.updateLinkTransforms();
-  const Eigen::Isometry3d tip_pose_after_delta =
-      root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back());
+  const Eigen::Isometry3d tip_pose_after_delta = root_pose_world * state.getGlobalLinkTransform(reference_link);
   const Eigen::Vector3d displacement = tip_pose_after_delta.translation() - tip_pose_initial.translation();
 
   // The Cartesian velocity vector obtained via the Jacobian should be aligned with the instantaneous robot motion, i.e.
@@ -843,6 +870,76 @@ TEST(getJacobian, RevoluteJoints)
   checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }));
 }
 
+TEST(getJacobian, RevoluteJointsButDifferentLink)
+{
+  // Robot URDF with four revolute joints.
+  constexpr char robot_urdf[] = R"(
+      <?xml version="1.0" ?>
+      <robot name="one_robot">
+        <link name="base_link"/>
+        <joint name="joint_a_revolute" type="revolute">
+            <axis xyz="0 0 1"/>
+            <parent link="base_link"/>
+            <child link="link_a"/>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
+        </joint>
+        <link name="link_a"/>
+        <joint name="joint_b_revolute" type="revolute">
+          <axis xyz="0 0 1"/>
+          <parent link="link_a"/>
+          <child link="link_b"/>
+          <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
+          <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
+        </joint>
+        <link name="link_b"/>
+        <joint name="joint_c_revolute" type="revolute">
+          <axis xyz="0 1 0"/>
+          <parent link="link_b"/>
+          <child link="link_c"/>
+          <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
+          <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
+        </joint>
+        <link name="link_c"/>
+        <joint name="joint_d_revolute" type="revolute">
+          <axis xyz="1 0 0"/>
+          <parent link="link_c"/>
+          <child link="link_d"/>
+          <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
+          <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
+        </joint>
+        <link name="link_d"/>
+      </robot>
+    )";
+
+  constexpr char robot_srdf[] = R"xml(
+      <?xml version="1.0" ?>
+      <robot name="one_robot">
+        <group name="base_to_tip">
+          <joint name="joint_a_revolute"/>
+          <joint name="joint_b_revolute"/>
+          <joint name="joint_c_revolute"/>
+          <joint name="joint_d_revolute"/>
+        </group>
+      </robot>
+      )xml";
+
+  const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
+  ASSERT_TRUE(urdf_model);
+  const auto srdf_model = std::make_shared<srdf::Model>();
+  ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
+  const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
+
+  moveit::core::RobotState state(robot_model);
+  const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
+
+  // Some made-up numbers, at zero and non-zero robot configurations.
+  checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }),
+                state.getLinkModel("link_c"));
+  checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }),
+                state.getLinkModel("link_c"));
+}
+
 TEST(getJacobian, RevoluteAndPrismaticJoints)
 {
   // Robot URDF with three revolute joints and one prismatic joint.