Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[Servo] Change planning frame to base frame of active joint subgroup #2515

Merged
merged 12 commits into from
Nov 13, 2023
Merged

[Servo] Change planning frame to base frame of active joint subgroup #2515

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
09eb17e
Change planning frame to base of active subgroup
sea-bass Nov 8, 2023
b741853
Make function to get IK base frame
sea-bass Nov 8, 2023
f003315
Catch tf2 exception nearer to actual code, use std::optionals
sea-bass Nov 8, 2023
5612d88
Remove ee_frame and planning_frame params
sea-bass Nov 8, 2023
7601fc6
PR comments
sea-bass Nov 10, 2023
0c134b7
Merge remote-tracking branch 'origin/main' into fix-servo-subgroup-cmds
sea-bass Nov 10, 2023
b20b14e
Helper function doesn't need to be public actually
sea-bass Nov 10, 2023
daf5f6f
Weird clang-tidy style changes
sea-bass Nov 10, 2023
85a382f
Move test fixture code from constructors to setup
sea-bass Nov 10, 2023
b193aa4
Merge branch 'main' into fix-servo-subgroup-cmds
sea-bass Nov 10, 2023
db97165
PR comments: errors and comments
sea-bass Nov 13, 2023
0ad1d85
Merge branch 'main' into fix-servo-subgroup-cmds
sea-bass Nov 13, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,9 +138,11 @@ class Servo
* If the command frame is part of the robot model, directly look up the transform using the robot model.
* Else, fall back to using TF to look up the transform.
* @param command_frame The command frame name.
* @param planning_frame The planning frame name.
* @return The transformation between planning frame and command frame.
*/
Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame) const;
Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame,
const std::string& planning_frame) const;

/**
* \brief Convert a given twist command to planning frame,
Expand All @@ -149,16 +151,18 @@ class Servo
* https://core.ac.uk/download/pdf/154240607.pdf
* https://www.seas.upenn.edu/~meam520/notes02/Forces8.pdf
* @param command The twist command to be converted.
* @param planning_frame The name of the planning frame.
* @return The transformed twist command.
*/
TwistCommand toPlanningFrame(const TwistCommand& command) const;
TwistCommand toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const;

/**
* \brief Convert a given pose command to planning frame
* @param command The pose command to be converted
* @return The transformed pose command
* @param command The pose command to be converted.
* @param planning_frame The name of the planning frame.
* @return The transformed pose command.
*/
PoseCommand toPlanningFrame(const PoseCommand& command) const;
PoseCommand toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const;

/**
* \brief Compute the change in joint position required to follow the received command.
Expand Down
6 changes: 4 additions & 2 deletions moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,23 +67,25 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
* @param command The twist command.
* @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
* @param servo_params The servo parameters.
* @param planning_frame The planning frame name.
* @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position.
* @return The status and joint position change required (delta).
*/
JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params,
const servo::Params& servo_params, const std::string& planning_frame,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map);

/**
* \brief Compute the change in joint position for the given pose command.
* @param command The pose command.
* @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
* @param servo_params The servo parameters.
* @param planning_frame The planning frame name.
* @param joint_name_group_index_map Mapping between sub group joint name and move group joint vector position
* @return The status and joint position change required (delta).
*/
JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params,
const servo::Params& servo_params, const std::string& planning_frame,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map);

/**
Expand Down
66 changes: 46 additions & 20 deletions moveit_ros/moveit_servo/src/servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -354,6 +354,10 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
Eigen::VectorXd joint_position_deltas(num_joints);
joint_position_deltas.setZero();

const auto& active_group_name =
servo_params_.active_subgroup.empty() ? servo_params_.move_group_name : servo_params_.active_subgroup;
const auto active_joint_model_group = robot_state->getJointModelGroup(active_group_name);

JointDeltaResult delta_result;

const CommandType expected_type = getCommandType();
Expand All @@ -369,10 +373,21 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
{
try
{
const TwistCommand command_in_planning_frame = toPlanningFrame(std::get<TwistCommand>(command));
delta_result =
jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map);
servo_status_ = delta_result.first;
const auto ik_solver = active_joint_model_group->getSolverInstance();
if (ik_solver)
{
const auto planning_frame = ik_solver->getBaseFrame();
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a better way to get the base link of a joint model group without having to grab the IK solver like this?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can't you do this?

const JointModel* root_joint_model = group->getJointModels().front();
const LinkModel* root_link_model = root_joint_model->getParentLinkModel();

But now I wonder if you rather need the IK solver base frame (i.e. your code as is), since the underlying IK solver could be working on a different frame, not necessarily the group root?
If that's the case, maybe create a getIKSolverBaseFrame() function that can be called here and below?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks!

This raises a follow up question that, if these changes are the right thing for changing subgroups, can we actually get rid of the planning frame parameter and have servo always do its own internal conversions to active group's IK base frame?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm.. looking deeper into Servo I got more confused about what the 'planning frame' is, but also how this fix works, since the IK base frame for the manipulator group is the one tilted 45 deg. right? so I would expect this to still move the wrong way.
Another q: the command has a frame_id. Shouldn't that be all that is needed to move in the same frame (e.g. world), independently of the active group? Sorry I have more questions than answers!

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should have more questions than answers, I think there's a lot here that needs to be disentangled for clarity.

Basically, Servo has this rule that enforces that all commands passed into it are expressed w.r.t. a common frame -- this is the "planning frame" we speak of.

All we're trying to do, perhaps in an overly complex way, is the following:

  • Receive a Cartesian command w.r.t. some arbitrary frame ID
  • Convert it to a reference frame that agrees with the IK solver (this is our planning frame)
  • ???
  • Profit

Copy link
Contributor Author

@sea-bass sea-bass Nov 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmm... I just saw some other piece of code that reminded me of the fact that a planning group can have multiple tip frames. So can we really get rid of moveit_params.ee_frame after all?

The other alternative, which is way more destructive, would be to modify the command message definitions for twist and pose tracking to contain both base and tip frames... which would basically require making custom messages for both instead of relying on existing geometry_msgs definitions.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we can, at least the way it is now, because ee_frame doesn't seem to be used anywhere. The IK computation is done for ik_solver->getTipFrame(), so it will always use the first tip.
Longer term what about having Servo expose a /configure service, where the user could set the planning group and tip frame?

Copy link
Contributor Author

@sea-bass sea-bass Nov 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Given all the struggles we've had trying to make sure that parameters stay consistent w.r.t. each other, I quite like the idea of dedicated services that touch these interrelated parameters like planning group + ee frame.

Also, having this configure service / method means that configuring can implicitly do the task of pausing servo, changing values, and then unpausing it.

Also, I guess to be fair this code was calling getTipFrame() already, and also there is an error logged when you call getTipFrame() on a planning group with multiple tips.

So maybe this is just a known limitation for now and we proceed as is?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A service to update the parameters might be a good idea b/c it would fix this issue: #2412

i.e. do the parameter validation in a callback in a different thread

Hate to change the API so drastically, so soon, though. Have to think carefully about this.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think servo has changed enough with the refactor since we kept running into limitations trying to keep the API as stable as possible with the old version. It might be beneficial to do all the big changes as soon as possible in hopes that we get to a somewhat more stable API before people actually start using this new version.

That said, we should try do all this in a way that minimizes our need to keep twiddling the interfaces going forward.

const TwistCommand command_in_planning_frame =
toPlanningFrame(std::get<TwistCommand>(command), planning_frame);
delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, planning_frame,
joint_name_group_index_map);
servo_status_ = delta_result.first;
}
else
{
servo_status_ = StatusCode::INVALID;
RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_group_name.c_str());
}
}
catch (tf2::TransformException& ex)
{
Expand All @@ -384,10 +399,20 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
{
try
{
const PoseCommand command_in_planning_frame = toPlanningFrame(std::get<PoseCommand>(command));
delta_result =
jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map);
servo_status_ = delta_result.first;
const auto ik_solver = active_joint_model_group->getSolverInstance();
if (ik_solver)
{
const auto planning_frame = ik_solver->getBaseFrame();
const PoseCommand command_in_planning_frame = toPlanningFrame(std::get<PoseCommand>(command), planning_frame);
delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, planning_frame,
joint_name_group_index_map);
servo_status_ = delta_result.first;
}
else
{
servo_status_ = StatusCode::INVALID;
RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_group_name.c_str());
}
}
catch (tf2::TransformException& ex)
{
Expand Down Expand Up @@ -497,29 +522,30 @@ KinematicState Servo::getNextJointState(const ServoInput& command)
return target_state;
}

Eigen::Isometry3d Servo::getPlanningToCommandFrameTransform(const std::string& command_frame) const
Eigen::Isometry3d Servo::getPlanningToCommandFrameTransform(const std::string& command_frame,
const std::string& planning_frame) const
{
const moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
if (robot_state->knowsFrameTransform(command_frame))
if (robot_state->knowsFrameTransform(command_frame) && (robot_state->knowsFrameTransform(planning_frame)))
{
return robot_state->getGlobalLinkTransform(servo_params_.planning_frame).inverse() *
return robot_state->getGlobalLinkTransform(planning_frame).inverse() *
robot_state->getGlobalLinkTransform(command_frame);
}
else
{
return tf2::transformToEigen(planning_scene_monitor_->getTFClient()->lookupTransform(
servo_params_.planning_frame, command_frame, rclcpp::Time(0)));
return tf2::transformToEigen(
planning_scene_monitor_->getTFClient()->lookupTransform(planning_frame, command_frame, rclcpp::Time(0)));
}
}

TwistCommand Servo::toPlanningFrame(const TwistCommand& command) const
TwistCommand Servo::toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const
{
Eigen::VectorXd transformed_twist = command.velocities;

if (command.frame_id != servo_params_.planning_frame)
if (command.frame_id != planning_frame)
{
// Look up the transform between the planning and command frames.
const auto planning_to_command_tf = getPlanningToCommandFrameTransform(command.frame_id);
const auto planning_to_command_tf = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);

if (servo_params_.apply_twist_commands_about_ee_frame)
{
Expand Down Expand Up @@ -561,13 +587,13 @@ TwistCommand Servo::toPlanningFrame(const TwistCommand& command) const
}
}

return TwistCommand{ servo_params_.planning_frame, transformed_twist };
return TwistCommand{ planning_frame, transformed_twist };
}

PoseCommand Servo::toPlanningFrame(const PoseCommand& command) const
PoseCommand Servo::toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const
{
return PoseCommand{ servo_params_.planning_frame,
getPlanningToCommandFrameTransform(command.frame_id) * command.pose };
return PoseCommand{ planning_frame,
getPlanningToCommandFrameTransform(command.frame_id, planning_frame) * command.pose };
}

KinematicState Servo::getCurrentRobotState() const
Expand Down
18 changes: 8 additions & 10 deletions moveit_ros/moveit_servo/src/utils/command.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
velocities.setZero();
bool names_valid = true;

for (size_t i = 0; i < command.names.size(); i++)
for (size_t i = 0; i < command.names.size(); ++i)
{
auto it = std::find(joint_names.begin(), joint_names.end(), command.names[i]);
if (it != std::end(joint_names))
Expand Down Expand Up @@ -143,17 +143,17 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
}

JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params,
const servo::Params& servo_params, const std::string& planning_frame,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
StatusCode status = StatusCode::NO_WARNING;
const int num_joints =
robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size();
Eigen::VectorXd joint_position_delta(num_joints);
Eigen::VectorXd cartesian_position_delta;
Eigen::Vector<double, 6> cartesian_position_delta;

const bool valid_command = isValidCommand(command);
const bool is_planning_frame = (command.frame_id == servo_params.planning_frame);
const bool is_planning_frame = (command.frame_id == planning_frame);
const bool is_zero = command.velocities.isZero();
if (!is_zero && is_planning_frame && valid_command)
{
Expand Down Expand Up @@ -200,15 +200,14 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
}
if (!is_planning_frame)
{
RCLCPP_WARN_STREAM(getLogger(),
"Command frame is: " << command.frame_id << ", expected: " << servo_params.planning_frame);
RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame);
}
}
return std::make_pair(status, joint_position_delta);
}

JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params,
const servo::Params& servo_params, const std::string& planning_frame,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
StatusCode status = StatusCode::NO_WARNING;
Expand All @@ -217,7 +216,7 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
Eigen::VectorXd joint_position_delta(num_joints);

const bool valid_command = isValidCommand(command);
const bool is_planning_frame = command.frame_id == servo_params.planning_frame;
const bool is_planning_frame = (command.frame_id == planning_frame);

if (valid_command && is_planning_frame)
{
Expand Down Expand Up @@ -250,8 +249,7 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
}
if (!is_planning_frame)
{
RCLCPP_WARN_STREAM(getLogger(),
"Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame);
RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << " expected: " << planning_frame);
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
}
}
return std::make_pair(status, joint_position_delta);
Expand Down
Loading