From 09eb17e73de8ae35e4ba02fe6b23634fbeb9650a Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Tue, 7 Nov 2023 22:10:24 -0500 Subject: [PATCH 1/9] Change planning frame to base of active subgroup --- .../include/moveit_servo/servo.hpp | 14 ++-- .../include/moveit_servo/utils/command.hpp | 6 +- moveit_ros/moveit_servo/src/servo.cpp | 66 +++++++++++++------ moveit_ros/moveit_servo/src/utils/command.cpp | 18 +++-- 4 files changed, 67 insertions(+), 37 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index ec9ff168a2..ee24c3c60a 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -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, @@ -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. diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index 2446670dbf..26c6ae8bd4 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -67,11 +67,12 @@ 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); /** @@ -79,11 +80,12 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: * @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); /** diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 2df47d8eed..fd57325ca3 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -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(); @@ -369,10 +373,21 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo { try { - const TwistCommand command_in_planning_frame = toPlanningFrame(std::get(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(); + const TwistCommand command_in_planning_frame = + toPlanningFrame(std::get(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) { @@ -384,10 +399,20 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo { try { - const PoseCommand command_in_planning_frame = toPlanningFrame(std::get(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(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) { @@ -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) { @@ -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 diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 63b7dde8c5..d3d801d2f7 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -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)) @@ -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 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) { @@ -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; @@ -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) { @@ -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); } } return std::make_pair(status, joint_position_delta); From b74185321301febe0c6ad95b96b174308eda3f15 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Wed, 8 Nov 2023 09:45:18 -0500 Subject: [PATCH 2/9] Make function to get IK base frame --- .../include/moveit_servo/utils/common.hpp | 11 +++++-- moveit_ros/moveit_servo/src/servo.cpp | 29 +++++++++---------- moveit_ros/moveit_servo/src/utils/common.cpp | 15 ++++++++++ 3 files changed, 37 insertions(+), 18 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp index 165d1bae9d..8406ca8155 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -54,6 +54,14 @@ namespace moveit_servo { +/** + * \brief Get the base frame of the current active joint group or subgroup's IK solver. + * @param robot_state A pointer to the current robot state. + * @param group_name The currently active joint group name. + * @return The IK solver base frame, if one exists, otherwise std::nullopt. + */ +std::optional getIKSolverBaseFrame(const moveit::core::RobotStatePtr& robot_state, + const std::string& group_name); /** * \brief Checks if a given VectorXd is a valid command. @@ -112,8 +120,7 @@ std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& s /** * \brief Computes scaling factor for velocity when the robot is near a singularity. - * @param joint_model_group The joint model group of the robot, used for fetching the Jacobian. - * @param current_state The current state of the robot, used for singularity look ahead. + * @param robot_state A pointer to the current robot state. * @param target_delta_x The vector containing the required change in Cartesian position. * @param servo_params The servo parameters, contains the singularity thresholds. * @return The velocity scaling factor and the reason for scaling. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index fd57325ca3..570b2b24b5 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -344,20 +344,17 @@ KinematicState Servo::haltJoints(const std::vector& 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 auto& active_subgroup_name = + servo_params_.active_subgroup.empty() ? servo_params_.move_group_name : servo_params_.active_subgroup; + const auto& joint_name_group_index_map = (active_subgroup_name != 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); 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(); @@ -373,10 +370,10 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo { try { - const auto ik_solver = active_joint_model_group->getSolverInstance(); - if (ik_solver) + const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); + if (planning_frame_maybe.has_value()) { - const auto planning_frame = ik_solver->getBaseFrame(); + const auto& planning_frame = *planning_frame_maybe; const TwistCommand command_in_planning_frame = toPlanningFrame(std::get(command), planning_frame); delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, planning_frame, @@ -386,7 +383,7 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo else { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_group_name.c_str()); + RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str()); } } catch (tf2::TransformException& ex) @@ -399,10 +396,10 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo { try { - const auto ik_solver = active_joint_model_group->getSolverInstance(); - if (ik_solver) + const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); + if (planning_frame_maybe.has_value()) { - const auto planning_frame = ik_solver->getBaseFrame(); + const auto& planning_frame = *planning_frame_maybe; const PoseCommand command_in_planning_frame = toPlanningFrame(std::get(command), planning_frame); delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, planning_frame, joint_name_group_index_map); @@ -411,7 +408,7 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo else { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_group_name.c_str()); + RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str()); } } catch (tf2::TransformException& ex) diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index 3936199653..64273a55e6 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -48,6 +48,21 @@ const double SCALING_OVERRIDE_THRESHOLD = 0.01; namespace moveit_servo { +std::optional getIKSolverBaseFrame(const moveit::core::RobotStatePtr& robot_state, + const std::string& group_name) +{ + const auto ik_solver = robot_state->getJointModelGroup(group_name)->getSolverInstance(); + + if (ik_solver) + { + return ik_solver->getBaseFrame(); + } + else + { + return std::nullopt; + } +} + bool isValidCommand(const Eigen::VectorXd& command) { // returns true only if there are no nan values. From f003315e65e71136d6968c60f432a9497c618f07 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Wed, 8 Nov 2023 10:02:03 -0500 Subject: [PATCH 3/9] Catch tf2 exception nearer to actual code, use std::optionals --- .../include/moveit_servo/servo.hpp | 11 +-- moveit_ros/moveit_servo/src/servo.cpp | 77 +++++++++++-------- 2 files changed, 53 insertions(+), 35 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index ee24c3c60a..11689d0755 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -139,10 +139,11 @@ class Servo * 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. + * @return The transformation between planning frame and command frame, or std::nullopt if there was a failure looking + * up a transform. */ - Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame, - const std::string& planning_frame) const; + std::optional getPlanningToCommandFrameTransform(const std::string& command_frame, + const std::string& planning_frame) const; /** * \brief Convert a given twist command to planning frame, @@ -154,7 +155,7 @@ class Servo * @param planning_frame The name of the planning frame. * @return The transformed twist command. */ - TwistCommand toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const; + std::optional toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const; /** * \brief Convert a given pose command to planning frame @@ -162,7 +163,7 @@ class Servo * @param planning_frame The name of the planning frame. * @return The transformed pose command. */ - PoseCommand toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const; + std::optional toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const; /** * \brief Compute the change in joint position required to follow the received command. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 570b2b24b5..de03047ca1 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -368,53 +368,52 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo } else if (expected_type == CommandType::TWIST) { - try + const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); + if (planning_frame_maybe.has_value()) { - const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); - if (planning_frame_maybe.has_value()) + const auto& planning_frame = *planning_frame_maybe; + const auto command_in_planning_frame_maybe = toPlanningFrame(std::get(command), planning_frame); + if (command_in_planning_frame_maybe.has_value()) { - const auto& planning_frame = *planning_frame_maybe; - const TwistCommand command_in_planning_frame = - toPlanningFrame(std::get(command), planning_frame); - delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_, planning_frame, - joint_name_group_index_map); + delta_result = jointDeltaFromTwist(*command_in_planning_frame_maybe, 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_subgroup_name.c_str()); + RCLCPP_ERROR_STREAM(logger_, "Could not transform twist command to planning frame."); } } - catch (tf2::TransformException& ex) + else { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "Could not transform twist to planning frame."); + RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str()); } } else if (expected_type == CommandType::POSE) { - try + const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); + if (planning_frame_maybe.has_value()) { - const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); - if (planning_frame_maybe.has_value()) + const auto& planning_frame = *planning_frame_maybe; + const auto command_in_planning_frame_maybe = toPlanningFrame(std::get(command), planning_frame); + if (command_in_planning_frame_maybe.has_value()) { - const auto& planning_frame = *planning_frame_maybe; - const PoseCommand command_in_planning_frame = toPlanningFrame(std::get(command), planning_frame); - delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_, planning_frame, - joint_name_group_index_map); + delta_result = jointDeltaFromPose(*command_in_planning_frame_maybe, 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_subgroup_name.c_str()); + RCLCPP_ERROR_STREAM(logger_, "Could not transform pose command to planning frame."); } } - catch (tf2::TransformException& ex) + else { servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "Could not transform pose to planning frame."); + RCLCPP_ERROR(logger_, "No IK solver for planning group %s.", active_subgroup_name.c_str()); } } @@ -519,8 +518,8 @@ KinematicState Servo::getNextJointState(const ServoInput& command) return target_state; } -Eigen::Isometry3d Servo::getPlanningToCommandFrameTransform(const std::string& command_frame, - const std::string& planning_frame) const +std::optional 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) && (robot_state->knowsFrameTransform(planning_frame))) @@ -530,19 +529,31 @@ Eigen::Isometry3d Servo::getPlanningToCommandFrameTransform(const std::string& c } else { - return tf2::transformToEigen( - planning_scene_monitor_->getTFClient()->lookupTransform(planning_frame, command_frame, rclcpp::Time(0))); + try + { + return tf2::transformToEigen( + planning_scene_monitor_->getTFClient()->lookupTransform(planning_frame, command_frame, rclcpp::Time(0))); + } + catch (tf2::TransformException& ex) + { + return std::nullopt; + } } } -TwistCommand Servo::toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const +std::optional Servo::toPlanningFrame(const TwistCommand& command, const std::string& planning_frame) const { Eigen::VectorXd transformed_twist = command.velocities; 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, planning_frame); + const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame); + if (!planning_to_command_tf_maybe) + { + return std::nullopt; + } + const auto& planning_to_command_tf = *planning_to_command_tf_maybe; if (servo_params_.apply_twist_commands_about_ee_frame) { @@ -587,10 +598,16 @@ TwistCommand Servo::toPlanningFrame(const TwistCommand& command, const std::stri return TwistCommand{ planning_frame, transformed_twist }; } -PoseCommand Servo::toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const +std::optional Servo::toPlanningFrame(const PoseCommand& command, const std::string& planning_frame) const { - return PoseCommand{ planning_frame, - getPlanningToCommandFrameTransform(command.frame_id, planning_frame) * command.pose }; + const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame); + if (!planning_to_command_tf_maybe) + { + return std::nullopt; + } + + const auto& planning_to_command_tf = *planning_to_command_tf_maybe; + return PoseCommand{ planning_frame, planning_to_command_tf * command.pose }; } KinematicState Servo::getCurrentRobotState() const From 5612d88f7448e531e4ee03cf12867c92e4dde108 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Wed, 8 Nov 2023 11:45:00 -0500 Subject: [PATCH 4/9] Remove ee_frame and planning_frame params --- .../config/panda_simulated_config.yaml | 4 -- .../moveit_servo/config/servo_parameters.yaml | 15 ------ .../config/test_config_panda.yaml | 4 -- .../demos/cpp_interface/demo_pose.cpp | 6 +-- .../demos/cpp_interface/demo_twist.cpp | 2 +- .../include/moveit_servo/servo.hpp | 6 ++- .../include/moveit_servo/utils/command.hpp | 2 + .../include/moveit_servo/utils/common.hpp | 9 ++++ moveit_ros/moveit_servo/src/servo.cpp | 48 +++++++------------ moveit_ros/moveit_servo/src/utils/command.cpp | 3 +- moveit_ros/moveit_servo/src/utils/common.cpp | 17 ++++++- .../moveit_servo/tests/test_integration.cpp | 16 +++---- 12 files changed, 62 insertions(+), 70 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index d20a9e4570..9ff8ea16bc 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -37,10 +37,6 @@ is_primary_planning_scene_monitor: true ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' -planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' - -## Other frames -ee_frame: panda_link8 # The name of the IK tip link ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index f5dc614552..56696b1742 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -31,21 +31,6 @@ servo: must be passed to the node during launch time." } - planning_frame: { - type: string, - description: "The name of moveit planning frame.\ - This parameter does not have a default value and \ - must be passed to the node during launch time." - } - - ee_frame: { - type: string, - description: "The name of end effector frame of your robot, \ - this should also be the IK tip frame of your IK solver. \ - This parameter does not have a default value and \ - must be passed to the node during launch time." - } - active_subgroup: { type: string, default_value: "", diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 59dc407f61..987af440fe 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -38,10 +38,6 @@ is_primary_planning_scene_monitor: true ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' -planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' - -## Other frames -ee_frame: panda_link8 # The name of the IK tip link ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index 1b3a7ae097..dc88251b5f 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -88,9 +88,9 @@ int main(int argc, char* argv[]) // The dynamically updated target pose. PoseCommand target_pose; - target_pose.frame_id = servo_params.planning_frame; + target_pose.frame_id = "panda_link0"; // Initializing the target pose as end effector pose, this can be any pose. - target_pose.pose = servo.getEndEffectorPose(); + target_pose.pose = servo.getCurrentPose("panda_link8"); // The pose tracking lambda that will be run in a separate thread. auto pose_tracker = [&]() { @@ -130,7 +130,7 @@ int main(int argc, char* argv[]) { { std::lock_guard pguard(pose_guard); - target_pose.pose = servo.getEndEffectorPose(); + target_pose.pose = servo.getCurrentPose("panda_link8"); const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox( terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance); const bool satisfies_angular_tolerance = diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index 9a67ce7ed4..b3b6512bfc 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -83,7 +83,7 @@ int main(int argc, char* argv[]) // Move end effector in the +z direction at 10 cm/s // while turning around z axis in the +ve direction at 0.5 rad/s - TwistCommand target_twist{ servo_params.planning_frame, { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } }; + TwistCommand target_twist{ "panda_link0", { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } }; // Frequency at which commands will be sent to the robot controller. rclcpp::WallRate rate(1.0 / servo_params.publish_period); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 11689d0755..765aaa6ace 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -104,9 +104,11 @@ class Servo std::string getStatusMessage() const; /** - * \brief Returns the end effector pose in planning frame + * \brief Returns the current pose of a specified target frame. + * @param target_frame The name of the target frame. + * @return The global transform of the specified frame. */ - Eigen::Isometry3d getEndEffectorPose() const; + Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const; /** * \brief Start/Stop collision checking thread. diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp index 26c6ae8bd4..0e7f07d914 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -81,11 +81,13 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: * @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 ee_frame The end effector 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 std::string& planning_frame, + const std::string& ee_frame, const JointNameToMoveGroupIndexMap& joint_name_group_index_map); /** diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp index 8406ca8155..c897fe7106 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -63,6 +63,15 @@ namespace moveit_servo std::optional getIKSolverBaseFrame(const moveit::core::RobotStatePtr& robot_state, const std::string& group_name); +/** + * \brief Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver. + * @param robot_state A pointer to the current robot state. + * @param group_name The currently active joint group name. + * @return The IK solver tip frame, if one exists, otherwise std::nullopt. + */ +std::optional getIKSolverTipFrame(const moveit::core::RobotStatePtr& robot_state, + const std::string& group_name); + /** * \brief Checks if a given VectorXd is a valid command. * @param command The command to be checked. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index de03047ca1..689e72dbbf 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -88,36 +88,23 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptrgetStateMonitor()->getCurrentState(); - // Check if the transforms to planning frame and end effector frame exist. - if (!robot_state->knowsFrameTransform(servo_params_.planning_frame)) - { - servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "No transform available for planning frame " << servo_params_.planning_frame); - } - else if (!robot_state->knowsFrameTransform(servo_params_.ee_frame)) + + // Load the smoothing plugin + if (servo_params_.use_smoothing) { - servo_status_ = StatusCode::INVALID; - RCLCPP_ERROR_STREAM(logger_, "No transform available for end effector frame " << servo_params_.ee_frame); + setSmoothingPlugin(); } else { - // Load the smoothing plugin - if (servo_params_.use_smoothing) - { - setSmoothingPlugin(); - } - else - { - RCLCPP_WARN(logger_, "No smoothing plugin loaded"); - } + RCLCPP_WARN(logger_, "No smoothing plugin loaded"); + } - // Create the collision checker and start collision checking. - collision_monitor_ = - std::make_unique(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_)); - collision_monitor_->start(); + // Create the collision checker and start collision checking. + collision_monitor_ = + std::make_unique(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_)); + collision_monitor_->start(); - servo_status_ = StatusCode::NO_WARNING; - } + servo_status_ = StatusCode::NO_WARNING; const auto& move_group_joint_names = planning_scene_monitor_->getRobotModel() ->getJointModelGroup(servo_params_.move_group_name) @@ -299,9 +286,9 @@ void Servo::setCommandType(const CommandType& command_type) expected_command_type_ = command_type; } -Eigen::Isometry3d Servo::getEndEffectorPose() const +Eigen::Isometry3d Servo::getCurrentPose(const std::string& target_frame) const { - return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(servo_params_.ee_frame); + return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); } KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, @@ -394,14 +381,15 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo else if (expected_type == CommandType::POSE) { const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); - if (planning_frame_maybe.has_value()) + const auto ee_frame_maybe = getIKSolverTipFrame(robot_state, active_subgroup_name); + if (planning_frame_maybe.has_value() && ee_frame_maybe.has_value()) { const auto& planning_frame = *planning_frame_maybe; const auto command_in_planning_frame_maybe = toPlanningFrame(std::get(command), planning_frame); if (command_in_planning_frame_maybe.has_value()) { delta_result = jointDeltaFromPose(*command_in_planning_frame_maybe, robot_state, servo_params_, - planning_frame, joint_name_group_index_map); + planning_frame, *ee_frame_maybe, joint_name_group_index_map); servo_status_ = delta_result.first; } else @@ -549,7 +537,7 @@ std::optional Servo::toPlanningFrame(const TwistCommand& command, { // Look up the transform between the planning and command frames. const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame); - if (!planning_to_command_tf_maybe) + if (!planning_to_command_tf_maybe.has_value()) { return std::nullopt; } @@ -633,7 +621,7 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta const KinematicState current_state = getCurrentRobotState(); const size_t num_joints = current_state.joint_names.size(); - for (size_t i = 0; i < num_joints; i++) + for (size_t i = 0; i < num_joints; ++i) { const double vel = (target_state.positions[i] - current_state.positions[i]) / servo_params_.publish_period; target_state.velocities[i] = (vel > STOPPED_VELOCITY_EPS) ? vel : 0.0; diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index d3d801d2f7..91acd9607c 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -208,6 +208,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::core::RobotStatePtr& robot_state, const servo::Params& servo_params, const std::string& planning_frame, + const std::string& ee_frame, const JointNameToMoveGroupIndexMap& joint_name_group_index_map) { StatusCode status = StatusCode::NO_WARNING; @@ -223,7 +224,7 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co Eigen::Vector cartesian_position_delta; // Compute linear and angular change needed. - const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(servo_params.ee_frame) }; + const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(ee_frame) }; const Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation()); const Eigen::Quaterniond q_error = q_target * q_current.inverse(); const Eigen::AngleAxisd angle_axis_error(q_error); diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index 64273a55e6..8d76f0e509 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -63,6 +63,21 @@ std::optional getIKSolverBaseFrame(const moveit::core::RobotStatePt } } +std::optional getIKSolverTipFrame(const moveit::core::RobotStatePtr& robot_state, + const std::string& group_name) +{ + const auto ik_solver = robot_state->getJointModelGroup(group_name)->getSolverInstance(); + + if (ik_solver) + { + return ik_solver->getTipFrame(); + } + else + { + return std::nullopt; + } +} + bool isValidCommand(const Eigen::VectorXd& command) { // returns true only if there are no nan values. @@ -130,8 +145,6 @@ trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Para // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement trajectory_msgs::msg::JointTrajectory joint_trajectory; - joint_trajectory.header.stamp = rclcpp::Time(0); - joint_trajectory.header.frame_id = servo_params.planning_frame; joint_trajectory.joint_names = joint_state.joint_names; static trajectory_msgs::msg::JointTrajectoryPoint point; diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index 113031779d..a048119be7 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -71,8 +71,8 @@ TEST_F(ServoCppFixture, JointJogTest) TEST_F(ServoCppFixture, TwistTest) { moveit_servo::StatusCode status_curr, status_next, status_initial; - moveit_servo::TwistCommand twist_non_zero{ servo_params_.planning_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; - moveit_servo::TwistCommand twist_zero{ servo_params_.planning_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + moveit_servo::TwistCommand twist_non_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; + moveit_servo::TwistCommand twist_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); status_initial = servo_test_instance_->getStatus(); @@ -95,8 +95,8 @@ TEST_F(ServoCppFixture, TwistTest) TEST_F(ServoCppFixture, NonPlanningFrameTwistTest) { moveit_servo::StatusCode status_curr, status_next, status_initial; - moveit_servo::TwistCommand twist_non_zero{ servo_params_.ee_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; - moveit_servo::TwistCommand twist_zero{ servo_params_.ee_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + moveit_servo::TwistCommand twist_non_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; + moveit_servo::TwistCommand twist_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); status_initial = servo_test_instance_->getStatus(); @@ -120,11 +120,11 @@ TEST_F(ServoCppFixture, PoseTest) { moveit_servo::StatusCode status_curr, status_next, status_initial; moveit_servo::PoseCommand zero_pose, non_zero_pose; - zero_pose.frame_id = servo_params_.planning_frame; - zero_pose.pose = servo_test_instance_->getEndEffectorPose(); + zero_pose.frame_id = "panda_link0"; + zero_pose.pose = servo_test_instance_->getCurrentPose("panda_link8"); - non_zero_pose.frame_id = servo_params_.planning_frame; - non_zero_pose.pose = servo_test_instance_->getEndEffectorPose(); + non_zero_pose.frame_id = "panda_link0"; + non_zero_pose.pose = servo_test_instance_->getCurrentPose("panda_link8"); non_zero_pose.pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); servo_test_instance_->setCommandType(moveit_servo::CommandType::POSE); From 7601fc68f089bb9f930f86cd6e98902275ad7b25 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 10 Nov 2023 08:50:19 -0500 Subject: [PATCH 5/9] PR comments --- .../demos/cpp_interface/demo_pose.cpp | 17 ++++++++++++++--- .../moveit_servo/include/moveit_servo/servo.hpp | 7 ------- moveit_ros/moveit_servo/src/servo.cpp | 5 ----- .../moveit_servo/tests/servo_cpp_fixture.hpp | 7 +++++++ .../moveit_servo/tests/test_integration.cpp | 4 ++-- 5 files changed, 23 insertions(+), 17 deletions(-) diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index dc88251b5f..639a1c2637 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -51,6 +51,12 @@ using moveit::getLogger; using namespace moveit_servo; +namespace +{ +constexpr auto kBaseFrame = "panda_link0"; +constexpr auto kTipFrame = "panda_link8"; +} // namespace + int main(int argc, char* argv[]) { rclcpp::init(argc, argv); @@ -75,6 +81,11 @@ int main(int argc, char* argv[]) createPlanningSceneMonitor(demo_node, servo_params); Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + // Helper function to get the current pose of a specified frame. + const auto getCurrentPose = [planning_scene_monitor](const std::string& target_frame) { + return planning_scene_monitor->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); + }; + // Wait for some time, so that the planning scene is loaded in rviz. // This is just for convenience, should not be used for sync in real application. std::this_thread::sleep_for(std::chrono::seconds(3)); @@ -88,9 +99,9 @@ int main(int argc, char* argv[]) // The dynamically updated target pose. PoseCommand target_pose; - target_pose.frame_id = "panda_link0"; + target_pose.frame_id = kBaseFrame; // Initializing the target pose as end effector pose, this can be any pose. - target_pose.pose = servo.getCurrentPose("panda_link8"); + target_pose.pose = getCurrentPose(kTipFrame); // The pose tracking lambda that will be run in a separate thread. auto pose_tracker = [&]() { @@ -130,7 +141,7 @@ int main(int argc, char* argv[]) { { std::lock_guard pguard(pose_guard); - target_pose.pose = servo.getCurrentPose("panda_link8"); + target_pose.pose = getCurrentPose(kTipFrame); const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox( terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance); const bool satisfies_angular_tolerance = diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 765aaa6ace..4b01e8cd7c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -103,13 +103,6 @@ class Servo */ std::string getStatusMessage() const; - /** - * \brief Returns the current pose of a specified target frame. - * @param target_frame The name of the target frame. - * @return The global transform of the specified frame. - */ - Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const; - /** * \brief Start/Stop collision checking thread. * @param check_collision Stops collision checking thread if false, starts it if true. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 689e72dbbf..5883bf5d45 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -286,11 +286,6 @@ void Servo::setCommandType(const CommandType& command_type) expected_command_type_ = command_type; } -Eigen::Isometry3d Servo::getCurrentPose(const std::string& target_frame) const -{ - return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); -} - KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, const KinematicState& target_state) const { diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index 55fff52865..ae4971e535 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -49,6 +49,13 @@ class ServoCppFixture : public testing::Test { +public: + /// Helper function to get the current pose of a specified frame. + Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const + { + return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); + } + protected: ServoCppFixture() { diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index a048119be7..93177fed30 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -121,10 +121,10 @@ TEST_F(ServoCppFixture, PoseTest) moveit_servo::StatusCode status_curr, status_next, status_initial; moveit_servo::PoseCommand zero_pose, non_zero_pose; zero_pose.frame_id = "panda_link0"; - zero_pose.pose = servo_test_instance_->getCurrentPose("panda_link8"); + zero_pose.pose = getCurrentPose("panda_link8"); non_zero_pose.frame_id = "panda_link0"; - non_zero_pose.pose = servo_test_instance_->getCurrentPose("panda_link8"); + non_zero_pose.pose = getCurrentPose("panda_link8"); non_zero_pose.pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); servo_test_instance_->setCommandType(moveit_servo::CommandType::POSE); From b20b14eb351b7237a39732f7a2dcf4ee3cfc8d82 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 10 Nov 2023 08:52:48 -0500 Subject: [PATCH 6/9] Helper function doesn't need to be public actually --- moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index ae4971e535..a97ed79c9b 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -49,13 +49,6 @@ class ServoCppFixture : public testing::Test { -public: - /// Helper function to get the current pose of a specified frame. - Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const - { - return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); - } - protected: ServoCppFixture() { @@ -73,6 +66,12 @@ class ServoCppFixture : public testing::Test std::make_shared(servo_test_node_, servo_param_listener_, planning_scene_monitor_); } + /// Helper function to get the current pose of a specified frame. + Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const + { + return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); + } + std::shared_ptr servo_test_node_; std::shared_ptr servo_param_listener_; servo::Params servo_params_; From daf5f6f865d3e4181d4ddd2d37ab09e3e81a35a0 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 10 Nov 2023 10:15:43 -0500 Subject: [PATCH 7/9] Weird clang-tidy style changes --- .../moveit_servo/demos/cpp_interface/demo_pose.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index 639a1c2637..f1426f6e7c 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -53,8 +53,8 @@ using namespace moveit_servo; namespace { -constexpr auto kBaseFrame = "panda_link0"; -constexpr auto kTipFrame = "panda_link8"; +constexpr auto K_BASE_FRAME = "panda_link0"; +constexpr auto K_TIP_FRAME = "panda_link8"; } // namespace int main(int argc, char* argv[]) @@ -82,7 +82,7 @@ int main(int argc, char* argv[]) Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); // Helper function to get the current pose of a specified frame. - const auto getCurrentPose = [planning_scene_monitor](const std::string& target_frame) { + const auto get_current_pose = [planning_scene_monitor](const std::string& target_frame) { return planning_scene_monitor->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); }; @@ -99,9 +99,9 @@ int main(int argc, char* argv[]) // The dynamically updated target pose. PoseCommand target_pose; - target_pose.frame_id = kBaseFrame; + target_pose.frame_id = K_BASE_FRAME; // Initializing the target pose as end effector pose, this can be any pose. - target_pose.pose = getCurrentPose(kTipFrame); + target_pose.pose = get_current_pose(K_TIP_FRAME); // The pose tracking lambda that will be run in a separate thread. auto pose_tracker = [&]() { @@ -141,7 +141,7 @@ int main(int argc, char* argv[]) { { std::lock_guard pguard(pose_guard); - target_pose.pose = getCurrentPose(kTipFrame); + target_pose.pose = get_current_pose(K_TIP_FRAME); const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox( terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance); const bool satisfies_angular_tolerance = From 85a382f0f60100c82cc5f9863e913e7fed848b67 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 10 Nov 2023 10:58:40 -0500 Subject: [PATCH 8/9] Move test fixture code from constructors to setup --- .../moveit_servo/tests/servo_cpp_fixture.hpp | 2 +- .../moveit_servo/tests/servo_ros_fixture.hpp | 45 ++++++++++--------- 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index a97ed79c9b..7e98f8e680 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -50,7 +50,7 @@ class ServoCppFixture : public testing::Test { protected: - ServoCppFixture() + void SetUp() override { // Create a node to be given to Servo. servo_test_node_ = std::make_shared("moveit_servo_test"); diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp index 585c15eae4..c950240299 100644 --- a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -73,6 +73,29 @@ class ServoRosFixture : public testing::Test void SetUp() override { + // Create a node to be given to Servo. + servo_test_node_ = std::make_shared("moveit_servo_test"); + executor_ = std::make_shared(); + + status_ = moveit_servo::StatusCode::INVALID; + + status_subscriber_ = servo_test_node_->create_subscription( + "/servo_node/status", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::ServoStatus::ConstSharedPtr& msg) { return statusCallback(msg); }); + + joint_state_subscriber_ = servo_test_node_->create_subscription( + "/joint_states", rclcpp::SystemDefaultsQoS(), + [this](const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { return jointStateCallback(msg); }); + + trajectory_subscriber_ = servo_test_node_->create_subscription( + "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), + [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCallback(msg); }); + + switch_input_client_ = + servo_test_node_->create_client("/servo_node/switch_command_type"); + + waitForService(); + executor_->add_node(servo_test_node_); executor_thread_ = std::thread([this]() { executor_->spin(); }); } @@ -122,28 +145,6 @@ class ServoRosFixture : public testing::Test ServoRosFixture() : state_count_{ 0 }, traj_count_{ 0 } { - // Create a node to be given to Servo. - servo_test_node_ = std::make_shared("moveit_servo_test"); - executor_ = std::make_shared(); - - status_ = moveit_servo::StatusCode::INVALID; - - status_subscriber_ = servo_test_node_->create_subscription( - "/servo_node/status", rclcpp::SystemDefaultsQoS(), - [this](const moveit_msgs::msg::ServoStatus::ConstSharedPtr& msg) { return statusCallback(msg); }); - - joint_state_subscriber_ = servo_test_node_->create_subscription( - "/joint_states", rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { return jointStateCallback(msg); }); - - trajectory_subscriber_ = servo_test_node_->create_subscription( - "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), - [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCallback(msg); }); - - switch_input_client_ = - servo_test_node_->create_client("/servo_node/switch_command_type"); - - waitForService(); } std::shared_ptr servo_test_node_; From db9716513cc5e2da051d523f8027abec2e121186 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 13 Nov 2023 10:51:15 -0500 Subject: [PATCH 9/9] PR comments: errors and comments --- moveit_ros/moveit_servo/src/servo.cpp | 6 ++++++ moveit_ros/moveit_servo/src/utils/command.cpp | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 5883bf5d45..8022762eeb 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -350,6 +350,8 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo } else if (expected_type == CommandType::TWIST) { + // Transform the twist command to the planning frame, which is the base frame of the active subgroup's IK solver, + // before applying it. Additionally verify there is an IK solver, and that the transformation is successful. const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); if (planning_frame_maybe.has_value()) { @@ -375,6 +377,9 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo } else if (expected_type == CommandType::POSE) { + // Transform the twist command to the planning frame, which is the base frame of the active subgroup's IK solver, + // before applying it. The end effector frame is also extracted as the tip frame of the IK solver. + // Additionally verify there is an IK solver, and that the transformation is successful. const auto planning_frame_maybe = getIKSolverBaseFrame(robot_state, active_subgroup_name); const auto ee_frame_maybe = getIKSolverTipFrame(robot_state, active_subgroup_name); if (planning_frame_maybe.has_value() && ee_frame_maybe.has_value()) @@ -519,6 +524,7 @@ std::optional Servo::getPlanningToCommandFrameTransform(const } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(logger_, "Failed to get planning to command frame transform: %s", ex.what()); return std::nullopt; } } diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 91acd9607c..fe8f038ee7 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -196,11 +196,11 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: status = StatusCode::INVALID; if (!valid_command) { - RCLCPP_WARN_STREAM(getLogger(), "Invalid twist command."); + RCLCPP_ERROR_STREAM(getLogger(), "Invalid twist command."); } if (!is_planning_frame) { - RCLCPP_WARN_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame); + RCLCPP_ERROR_STREAM(getLogger(), "Command frame is: " << command.frame_id << ", expected: " << planning_frame); } } return std::make_pair(status, joint_position_delta);