Skip to content

Commit 5049e4d

Browse files
sjahrsjahr
authored
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]>
1 parent 1a77d7d commit 5049e4d

File tree

2 files changed

+115
-11
lines changed

2 files changed

+115
-11
lines changed

‎moveit_core/robot_state/src/robot_state.cpp

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1496,19 +1496,25 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
14961496
const int rows = use_quaternion_representation ? 7 : 6;
14971497
const int columns = group->getVariableCount();
14981498
jacobian.resize(rows, columns);
1499+
jacobian.setZero();
14991500

15001501
// Get the tip pose with respect to the group root link. Append the user-requested offset 'reference_point_position'.
15011502
const Eigen::Isometry3d root_pose_tip = root_pose_world * getGlobalLinkTransform(link);
15021503
const Eigen::Vector3d tip_point = root_pose_tip * reference_point_position;
15031504

1504-
// Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian
1505-
// displacement at the tip. So we build the Jacobian incrementally joint by joint.
1506-
std::size_t active_joints = group->getActiveJointModels().size();
1505+
// Initialize the column index of the Jacobian matrix.
15071506
int i = 0;
1508-
for (std::size_t joint = 0; joint < active_joints; ++joint)
1507+
1508+
// Here we iterate over all the group active joints, and compute how much each of them contribute to the Cartesian
1509+
// displacement at the tip. So we build the Jacobian incrementally joint by joint up to the parent joint of the reference link.
1510+
for (const JointModel* joint_model : joint_models)
15091511
{
1512+
// Stop looping if we reached the child joint of the reference link.
1513+
if (joint_model->getParentLinkModel() == link)
1514+
{
1515+
break;
1516+
}
15101517
// Get the child link for the current joint, and its pose with respect to the group root link.
1511-
const JointModel* joint_model = joint_models[joint];
15121518
const LinkModel* child_link_model = joint_model->getChildLinkModel();
15131519
const Eigen::Isometry3d& root_pose_link = root_pose_world * getGlobalLinkTransform(child_link_model);
15141520

@@ -1540,6 +1546,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
15401546
RCLCPP_ERROR(getLogger(), "Unknown type of joint in Jacobian computation");
15411547
return false;
15421548
}
1549+
15431550
i += joint_model->getVariableCount();
15441551
}
15451552

‎moveit_core/robot_state/test/robot_state_test.cpp

Lines changed: 103 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,8 @@ Eigen::VectorXd makeVector(const std::vector<double>& values)
6161

6262
// Checks the validity of state.getJacobian() at the given 'joint_values' and 'joint_velocities'.
6363
void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group,
64-
const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities)
64+
const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities,
65+
const moveit::core::LinkModel* reference_link = nullptr)
6566
{
6667
// Using the Jacobian, compute the Cartesian velocity vector at which the end-effector would move, with the given
6768
// joint velocities.
@@ -73,9 +74,36 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod
7374
const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
7475
const Eigen::Isometry3d root_pose_world = state.getGlobalLinkTransform(root_link_model).inverse();
7576

76-
const Eigen::Isometry3d tip_pose_initial =
77-
root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back());
78-
const Eigen::MatrixXd jacobian = state.getJacobian(&joint_model_group);
77+
if (!reference_link)
78+
{
79+
reference_link = joint_model_group.getLinkModels().back();
80+
}
81+
const Eigen::Isometry3d tip_pose_initial = root_pose_world * state.getGlobalLinkTransform(reference_link);
82+
Eigen::MatrixXd jacobian;
83+
state.getJacobian(&joint_model_group, reference_link, Eigen::Vector3d::Zero(), jacobian);
84+
85+
// Verify that only elements of the Jacobian contain values that correspond to joints that are being used based on the reference link.
86+
const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group.getJointModels();
87+
auto it = std::find_if(joint_models.begin(), joint_models.end(), [&](const moveit::core::JointModel* jm) {
88+
return jm->getParentLinkModel() == reference_link;
89+
});
90+
if (it != joint_models.end())
91+
{
92+
std::size_t index = 0;
93+
for (auto jt = joint_models.begin(); jt != it; ++jt)
94+
{
95+
index += (*jt)->getVariableCount();
96+
}
97+
98+
EXPECT_TRUE(jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index).isZero())
99+
<< "Jacobian contains non-zero values for joints that are not used based on the reference link "
100+
<< reference_link->getName() << ". This is the faulty Jacobian: " << '\n'
101+
<< jacobian << '\n'
102+
<< "The columns " << index << " to " << jacobian.cols() << " should be zero. Instead the values are: " << '\n'
103+
<< jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index);
104+
}
105+
106+
// Compute the Cartesian velocity vector using the Jacobian.
79107
const Eigen::VectorXd cartesian_velocity = jacobian * joint_velocities;
80108

81109
// 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
84112
const Eigen::VectorXd delta_joint_angles = time_step * joint_velocities;
85113
state.setJointGroupPositions(&joint_model_group, joint_values + delta_joint_angles);
86114
state.updateLinkTransforms();
87-
const Eigen::Isometry3d tip_pose_after_delta =
88-
root_pose_world * state.getGlobalLinkTransform(joint_model_group.getLinkModels().back());
115+
const Eigen::Isometry3d tip_pose_after_delta = root_pose_world * state.getGlobalLinkTransform(reference_link);
89116
const Eigen::Vector3d displacement = tip_pose_after_delta.translation() - tip_pose_initial.translation();
90117

91118
// 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)
843870
checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }));
844871
}
845872

873+
TEST(getJacobian, RevoluteJointsButDifferentLink)
874+
{
875+
// Robot URDF with four revolute joints.
876+
constexpr char robot_urdf[] = R"(
877+
<?xml version="1.0" ?>
878+
<robot name="one_robot">
879+
<link name="base_link"/>
880+
<joint name="joint_a_revolute" type="revolute">
881+
<axis xyz="0 0 1"/>
882+
<parent link="base_link"/>
883+
<child link="link_a"/>
884+
<origin rpy="0 0 0" xyz="0 0 0"/>
885+
<limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
886+
</joint>
887+
<link name="link_a"/>
888+
<joint name="joint_b_revolute" type="revolute">
889+
<axis xyz="0 0 1"/>
890+
<parent link="link_a"/>
891+
<child link="link_b"/>
892+
<origin rpy="0 0 0" xyz="0.0 0.5 0"/>
893+
<limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
894+
</joint>
895+
<link name="link_b"/>
896+
<joint name="joint_c_revolute" type="revolute">
897+
<axis xyz="0 1 0"/>
898+
<parent link="link_b"/>
899+
<child link="link_c"/>
900+
<origin rpy="0 0 0" xyz="0.2 0.2 0"/>
901+
<limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
902+
</joint>
903+
<link name="link_c"/>
904+
<joint name="joint_d_revolute" type="revolute">
905+
<axis xyz="1 0 0"/>
906+
<parent link="link_c"/>
907+
<child link="link_d"/>
908+
<origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
909+
<limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
910+
</joint>
911+
<link name="link_d"/>
912+
</robot>
913+
)";
914+
915+
constexpr char robot_srdf[] = R"xml(
916+
<?xml version="1.0" ?>
917+
<robot name="one_robot">
918+
<group name="base_to_tip">
919+
<joint name="joint_a_revolute"/>
920+
<joint name="joint_b_revolute"/>
921+
<joint name="joint_c_revolute"/>
922+
<joint name="joint_d_revolute"/>
923+
</group>
924+
</robot>
925+
)xml";
926+
927+
const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
928+
ASSERT_TRUE(urdf_model);
929+
const auto srdf_model = std::make_shared<srdf::Model>();
930+
ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
931+
const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
932+
933+
moveit::core::RobotState state(robot_model);
934+
const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
935+
936+
// Some made-up numbers, at zero and non-zero robot configurations.
937+
checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }),
938+
state.getLinkModel("link_c"));
939+
checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }),
940+
state.getLinkModel("link_c"));
941+
}
942+
846943
TEST(getJacobian, RevoluteAndPrismaticJoints)
847944
{
848945
// Robot URDF with three revolute joints and one prismatic joint.

0 commit comments

Comments
 (0)