Skip to content

Commit

Permalink
Enable using a subgroup of the move group in servo (#2396)
Browse files Browse the repository at this point in the history 
* Enable using a subgroup of the move group in servo

* Remove unnecessary validations since the param is const

* Apply suggestions from code review

Co-authored-by: Sebastian Castro <[email protected]>

* Don't copy joints if subgroup == move group

* Re-add params_valid in validateParams

* Generalize active subgroup delta calculation

* Add more efficient move group joint position lookup

* Create subgroup map in the constructor

* Apply suggestions from code review

Co-authored-by: Sebastian Castro <[email protected]>

* Update moveit_ros/moveit_servo/src/servo.cpp

---------

Co-authored-by: Sebastian Castro <[email protected]>
  • Loading branch information
@sjahr @sea-bass
sjahr and sea-bass authored Oct 9, 2023
1 parent f949aa0 commit 01bae77
Show file tree
Hide file tree
Showing 6 changed files with 148 additions and 33 deletions.
8 changes: 7 additions & 1 deletion moveit_ros/moveit_servo/config/servo_parameters.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,14 @@ servo:
must be passed to the node during launch time."
}

############################# INCOMING COMMAND SETTINGS ########################
active_subgroup: {
type: string,
default_value: "",
description: "This parameter can be used to switch online to actuating a subgroup of the move group. \
If it is empty, the full move group is actuated."
}

############################# INCOMING COMMAND SETTINGS ########################
pose_command_in_topic: {
type: string,
read_only: true,
Expand Down
3 changes: 3 additions & 0 deletions moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,6 +214,9 @@ class Servo
std::unique_ptr<CollisionMonitor> collision_monitor_;

pluginlib::UniquePtr<online_signal_smoothing::SmoothingBaseClass> smoother_ = nullptr;

// Map between joint subgroup names and corresponding joint name - move group indices map
std::unordered_map<std::string, JointNameToMoveGroupIndexMap> joint_name_to_index_maps_;
};

} // namespace moveit_servo
16 changes: 12 additions & 4 deletions moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,39 +55,47 @@ namespace moveit_servo
* @param command The joint jog command.
* @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
* @param servo_params The servo parameters.
* @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 jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params);
const servo::Params& servo_params,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map);

/**
* \brief Compute the change in joint position for the given twist command.
* @param command The twist command.
* @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
* @param servo_params The servo parameters.
* @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 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 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 JointNameToMoveGroupIndexMap& joint_name_group_index_map);

/**
* \brief Computes the required change in joint angles for given Cartesian change, using the robot's IK solver.
* @param cartesian_position_delta The change in Cartesian position.
* @param robot_state_ The current robot state as obtained from PlanningSceneMonitor.
* @param servo_params The servo parameters.
* @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 jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta,
const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params);
const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map);

} // namespace moveit_servo
3 changes: 3 additions & 0 deletions moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,4 +129,7 @@ struct KinematicState
}
};

// Mapping joint names and their position in the move group vector
typedef std::unordered_map<std::string, std::size_t> JointNameToMoveGroupIndexMap;

} // namespace moveit_servo
71 changes: 55 additions & 16 deletions moveit_ros/moveit_servo/src/servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
{
servo_params_ = servo_param_listener_->get_params();

const bool params_valid = validateParams(servo_params_);
if (!params_valid)
if (!validateParams(servo_params_))
{
RCLCPP_ERROR_STREAM(LOGGER, "Got invalid parameters, exiting.");
std::exit(EXIT_FAILURE);
Expand Down Expand Up @@ -117,8 +116,38 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
collision_monitor_->start();

servo_status_ = StatusCode::NO_WARNING;
RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully");
}

const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel()
->getJointModelGroup(servo_params_.move_group_name)
->getActiveJointModelNames();
// Create subgroup map
for (const auto& sub_group_name : planning_scene_monitor_->getRobotModel()->getJointModelGroupNames())
{
// Skip move group
if (sub_group_name == servo_params_.move_group_name)
{
continue;
}
const auto& subgroup_joint_names =
planning_scene_monitor_->getRobotModel()->getJointModelGroup(sub_group_name)->getActiveJointModelNames();

JointNameToMoveGroupIndexMap new_map;
// For each joint name of the subgroup calculate the index in the move group joint vector
for (const auto& joint_name : subgroup_joint_names)
{
// Find subgroup joint name in move group joint names
const auto move_group_iterator =
std::find(move_group_joint_names.cbegin(), move_group_joint_names.cend(), joint_name);
// Calculate position and add a new mapping of joint name to move group joint vector position
new_map.insert(std::make_pair<std::string, std::size_t>(
std::string(joint_name), std::distance(move_group_joint_names.cbegin(), move_group_iterator)));
}
// Add new joint name to index map to existing maps
joint_name_to_index_maps_.insert(
std::make_pair<std::string, JointNameToMoveGroupIndexMap>(std::string(sub_group_name), std::move(new_map)));
}
RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully");
}

Servo::~Servo()
Expand Down Expand Up @@ -161,7 +190,6 @@ void Servo::setCollisionChecking(const bool check_collision)
bool Servo::validateParams(const servo::Params& servo_params) const
{
bool params_valid = true;

auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
if (joint_model_group == nullptr)
Expand Down Expand Up @@ -205,30 +233,32 @@ bool Servo::validateParams(const servo::Params& servo_params) const
params_valid = false;
}

if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name &&
!joint_model_group->isSubgroup(servo_params.active_subgroup))
{
RCLCPP_ERROR(LOGGER,
"The value '%s' Parameter 'active_subgroup' does not name a valid subgroup of joint group '%s'.",
servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str());
params_valid = false;
}

return params_valid;
}

bool Servo::updateParams()
{
bool params_updated = false;

if (servo_param_listener_->is_old(servo_params_))
{
auto params = servo_param_listener_->get_params();
const auto params = servo_param_listener_->get_params();

const bool params_valid = validateParams(params);
if (params_valid)
if (validateParams(params))
{
if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor)
{
RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : "
<< std::to_string(params.override_velocity_scaling_factor));
}
if (params.move_group_name != servo_params_.move_group_name)
{
RCLCPP_INFO_STREAM(LOGGER, "Move group changed from " << servo_params_.move_group_name << " to "
<< params.move_group_name);
}

servo_params_ = params;
params_updated = true;
Expand Down Expand Up @@ -310,6 +340,12 @@ KinematicState Servo::haltJoints(const std::vector<int>& joints_to_halt, const K

Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state)
{
// Determine joint_name_group_index_map, if no subgroup is active, the map is empty
const auto& joint_name_group_index_map =
(!servo_params_.active_subgroup.empty() && servo_params_.active_subgroup != servo_params_.move_group_name) ?
joint_name_to_index_maps_.at(servo_params_.active_subgroup) :
JointNameToMoveGroupIndexMap();

const int num_joints =
robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size();
Eigen::VectorXd joint_position_deltas(num_joints);
Expand All @@ -322,15 +358,17 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
{
if (expected_type == CommandType::JOINT_JOG)
{
delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_);
delta_result = jointDeltaFromJointJog(std::get<JointJogCommand>(command), robot_state, servo_params_,
joint_name_group_index_map);
servo_status_ = delta_result.first;
}
else if (expected_type == CommandType::TWIST)
{
try
{
const TwistCommand command_in_planning_frame = toPlanningFrame(std::get<TwistCommand>(command));
delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_);
delta_result =
jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map);
servo_status_ = delta_result.first;
}
catch (tf2::TransformException& ex)
Expand All @@ -344,7 +382,8 @@ 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_);
delta_result =
jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, joint_name_group_index_map);
servo_status_ = delta_result.first;
}
catch (tf2::TransformException& ex)
Expand Down
80 changes: 68 additions & 12 deletions moveit_ros/moveit_servo/src/utils/command.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,17 +42,49 @@
namespace
{
const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor");
}

/**
* @brief Helper function to create a move group deltas vector from a sub group deltas vector. A delta vector for the
* whole move group is created and all entries zeroed. The elements of the subgroup deltas vector are copied into the
* correct element of the bigger move group delta vector.
* @param sub_group_deltas Set of command deltas for a subgroup of the move group actuated by servo
* @param robot_state Current robot state
* @param servo_params Servo params
* @param joint_name_group_index_map Mapping between joint subgroup name and move group joint vector position.
* @return Delta vector for the whole move group. The elements that don't belong to the actuated subgroup are zero.
*/
const Eigen::VectorXd createMoveGroupDelta(const Eigen::VectorXd& sub_group_deltas,
const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params,
const moveit_servo::JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
const auto& subgroup_joint_names =
robot_state->getJointModelGroup(servo_params.active_subgroup)->getActiveJointModelNames();

// Create
Eigen::VectorXd move_group_delta_theta = Eigen::VectorXd::Zero(
robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size());
for (size_t index = 0; index < subgroup_joint_names.size(); index++)
{
move_group_delta_theta[joint_name_group_index_map.at(subgroup_joint_names.at(index))] = sub_group_deltas[index];
}
return move_group_delta_theta;
};
} // namespace

namespace moveit_servo
{

JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params)
const servo::Params& servo_params,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
// Find the target joint position based on the commanded joint velocity
StatusCode status = StatusCode::NO_WARNING;
const auto joint_names = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames();
const auto& group_name =
servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup;
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name);
const auto joint_names = joint_model_group->getActiveJointModelNames();
Eigen::VectorXd joint_position_delta(joint_names.size());
Eigen::VectorXd velocities(joint_names.size());

Expand All @@ -68,6 +100,8 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
}
else
{
RCLCPP_WARN_STREAM(LOGGER, "Invalid joint name: " << command.names[i]);

names_valid = false;
break;
}
Expand All @@ -86,18 +120,28 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
status = StatusCode::INVALID;
if (!names_valid)
{
RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command");
RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command. Either you're sending commands for a joint "
"that is not part of the move group or certain joints cannot be moved because a "
"subgroup is active and they are not part of it.");
}
if (!velocity_valid)
{
RCLCPP_WARN_STREAM(LOGGER, "Invalid velocity values in joint jog command");
}
}

if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name)
{
return std::make_pair(status, createMoveGroupDelta(joint_position_delta, robot_state, servo_params,
joint_name_group_index_map));
}

return std::make_pair(status, joint_position_delta);
}

JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params)
const servo::Params& servo_params,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
StatusCode status = StatusCode::NO_WARNING;
const int num_joints =
Expand All @@ -122,7 +166,8 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
}

// Compute the required change in joint angles.
const auto delta_result = jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params);
const auto delta_result =
jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
status = delta_result.first;
if (status != StatusCode::INVALID)
{
Expand Down Expand Up @@ -160,7 +205,8 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
}

JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state,
const servo::Params& servo_params)
const servo::Params& servo_params,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
StatusCode status = StatusCode::NO_WARNING;
const int num_joints =
Expand All @@ -184,7 +230,8 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();

// Compute the required change in joint angles.
const auto delta_result = jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params);
const auto delta_result =
jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
status = delta_result.first;
if (status != StatusCode::INVALID)
{
Expand All @@ -208,10 +255,12 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
}

JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta,
const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params)
const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params,
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
{
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params.move_group_name);
const auto& group_name =
servo_params.active_subgroup.empty() ? servo_params.move_group_name : servo_params.active_subgroup;
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(group_name);

std::vector<double> current_joint_positions;
robot_state->copyJointGroupPositions(joint_model_group, current_joint_positions);
Expand Down Expand Up @@ -242,6 +291,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt

// setup for IK call
std::vector<double> solution;
solution.reserve(current_joint_positions.size());
moveit_msgs::msg::MoveItErrorCodes err;
kinematics::KinematicsQueryOptions opts;
opts.return_approximate_solution = true;
Expand All @@ -251,7 +301,7 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt
// find the difference in joint positions that will get us to the desired pose
for (size_t i = 0; i < current_joint_positions.size(); ++i)
{
delta_theta[i] = solution[i] - current_joint_positions[i];
delta_theta[i] = solution.at(i) - current_joint_positions.at(i);
}
}
else
Expand All @@ -272,6 +322,12 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt
delta_theta = pseudo_inverse * cartesian_position_delta;
}

if (!servo_params.active_subgroup.empty() && servo_params.active_subgroup != servo_params.move_group_name)
{
return std::make_pair(status,
createMoveGroupDelta(delta_theta, robot_state, servo_params, joint_name_group_index_map));
}

return std::make_pair(status, delta_theta);
}

Expand Down

0 comments on commit 01bae77

Please sign in to comment.