Skip to content

Commit

Permalink
Merge branch 'main' into ch3/trajectory-cache-refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@methylDragon
methylDragon authored Nov 14, 2024
2 parents 8575486 + ab34495 commit f456a5c
Show file tree
Hide file tree
Showing 7 changed files with 401 additions and 79 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/tutorial_docker.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ jobs:
strategy:
fail-fast: false
matrix:
ROS_DISTRO: [rolling, humble, iron, jazzy]
ROS_DISTRO: [rolling, jazzy]
runs-on: ubuntu-latest
permissions:
packages: write
Expand Down
67 changes: 33 additions & 34 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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();
}

Expand Down Expand Up @@ -1870,42 +1877,34 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is

if (pose_frame != solver_tip_frame)
{
if (hasAttachedBody(pose_frame))
auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame);
if (!pose_parent)
{
const AttachedBody* body = getAttachedBody(pose_frame);
pose_frame = body->getAttachedLinkName();
pose = pose * body->getPose().inverse();
RCLCPP_ERROR_STREAM(getLogger(), "The following Pose Frame does not exist: " << pose_frame);
return false;
}
if (pose_frame != solver_tip_frame)
Eigen::Isometry3d pose_parent_to_frame = getFrameTransform(pose_frame);
auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame);
if (!tip_parent)
{
const LinkModel* link_model = getLinkModel(pose_frame);
if (!link_model)
{
RCLCPP_ERROR(getLogger(), "The following Pose Frame does not exist: %s", pose_frame.c_str());
return false;
}
const LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms();
for (const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
{
if (Transforms::sameFrame(fixed_link.first->getName(), solver_tip_frame))
{
pose_frame = solver_tip_frame;
pose = pose * fixed_link.second;
break;
}
}
RCLCPP_ERROR_STREAM(getLogger(), "The following Solver Tip Frame does not exist: " << solver_tip_frame);
return false;
}

} // end if pose_frame

// Check if this pose frame works
if (pose_frame == solver_tip_frame)
Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame);
if (pose_parent == tip_parent)
{
// transform goal pose as target for solver_tip_frame (instead of pose_frame)
pose = pose * pose_parent_to_frame.inverse() * tip_parent_to_tip;
found_valid_frame = true;
break;
}
}
else
{
found_valid_frame = true;
break;
}

} // end for solver_tip_frames
} // end if pose_frame
} // end for solver_tip_frames

// Make sure one of the tip frames worked
if (!found_valid_frame)
Expand Down
109 changes: 103 additions & 6 deletions moveit_core/robot_state/test/robot_state_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand Down
64 changes: 42 additions & 22 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");

info.group_name = req.group_name;
std::string frame_id{ robot_model_->getModelFrame() };
moveit::core::RobotState robot_state = scene->getCurrentState();

// goal given in joint space
Expand Down Expand Up @@ -130,6 +129,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
// goal given in Cartesian space
else
{
std::string frame_id;
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
Expand All @@ -142,39 +142,59 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
{
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
}
info.goal_pose = getConstraintPose(req.goal_constraints.front());

// goal pose with optional offset wrt. the planning frame
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
frame_id = robot_model_->getModelFrame();

// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
{
// LCOV_EXCL_START
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw CircInverseForGoalIncalculable(os.str());
// LCOV_EXCL_STOP // not able to trigger here since lots of checks before
// are in place
}
}

computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);

// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
// center point with wrt. the planning frame
std::string center_point_frame_id;

info.circ_path_point.first = req.path_constraints.name;
if (req.path_constraints.position_constraints.front().header.frame_id.empty())
{
// LCOV_EXCL_START
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw CircInverseForGoalIncalculable(os.str());
// LCOV_EXCL_STOP // not able to trigger here since lots of checks before
// are in place
RCLCPP_WARN(getLogger(), "Frame id is not set in position constraints of "
"path. Use model frame as default");
center_point_frame_id = robot_model_->getModelFrame();
}
info.circ_path_point.first = req.path_constraints.name;
else
{
center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id;
}

Eigen::Isometry3d center_point_pose;
tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(),
center_point_pose);

center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose;

if (!req.goal_constraints.front().position_constraints.empty())
{
const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front();
info.circ_path_point.second =
getConstraintPose(
req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset)
.translation();
geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation()));
info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation,
goal.position_constraints.front().target_point_offset)
.translation();
}
else
{
Eigen::Vector3d circ_path_point;
tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
circ_path_point);
info.circ_path_point.second = circ_path_point;
info.circ_path_point.second = center_point_pose.translation();
}
}

Expand Down
30 changes: 18 additions & 12 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");

info.group_name = req.group_name;
std::string frame_id{ robot_model_->getModelFrame() };
moveit::core::RobotState robot_state = scene->getCurrentState();

// goal given in joint space
Expand Down Expand Up @@ -103,6 +102,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
// goal given in Cartesian space
else
{
std::string frame_id;

info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
Expand All @@ -115,20 +116,25 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
{
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
}
info.goal_pose = getConstraintPose(req.goal_constraints.front());
}

computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
// goal pose with optional offset wrt. the planning frame
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
frame_id = robot_model_->getModelFrame();

// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
{
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw LinInverseForGoalIncalculable(os.str());
// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
{
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw LinInverseForGoalIncalculable(os.str());
}
}

// Ignored return value because at this point the function should always
// return 'true'.
computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
}

void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,
Expand Down
Loading

0 comments on commit f456a5c

Please sign in to comment.