From 5049e4dd733e07d062488261d27a3da090c92698 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 8 Nov 2024 15:27:44 +0100 Subject: [PATCH] Fix jacobian calculation (#3069) * Fix faulty Jacobian * Update tests * Format * More verbose error message Signed-off-by: Sebastian Jahr * Fix columns --------- Signed-off-by: Sebastian Jahr --- moveit_core/robot_state/src/robot_state.cpp | 17 ++- .../robot_state/test/robot_state_test.cpp | 109 +++++++++++++++++- 2 files changed, 115 insertions(+), 11 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 95714bc7e2..9f36336dda 100644 --- 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 index cafb5bf01f..bea2b7e229 100644 --- 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& 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& 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"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + constexpr char robot_srdf[] = R"xml( + + + + + + + + + + )xml"; + + const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf); + ASSERT_TRUE(urdf_model); + const auto srdf_model = std::make_shared(); + ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf)); + const auto robot_model = std::make_shared(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.