diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index d1af7d41bf..089eba6b8f 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -44,6 +44,14 @@ namespace moveit { namespace core { +/** Struct defining linear and rotational precision */ +struct CartesianPrecision +{ + double translational = 0.001; //< max deviation in translation (meters) + double rotational = 0.01; //< max deviation in rotation (radians) + double max_resolution = 1e-5; //< max resolution for waypoints (fraction of total path) +}; + /** \brief Struct with options for defining joint-space jump thresholds. */ struct JumpThreshold { @@ -144,6 +152,75 @@ class CartesianInterpolator double meters; }; + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. + + The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. + This vector is assumed to be specified either in the global reference frame or in the local + reference frame of the link. + The resulting joint values are stored in the vector \e traj, one by one. The interpolation distance in + Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which + provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal + call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to + the distance that was achieved and for which corresponding states were added to the path. + + The struct CartesianPrecision specifies the precision to which the path should follow the Cartesian straight line. + If the deviation at the mid point of two consecutive waypoints is larger than the specified precision, another waypoint + will be inserted at that mid point. The precision is specified separately for translation and rotation. + The maximal resolution to consider (as fraction of the total path length) is specified by max_resolution. + */ + static Distance computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()); + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. + + In contrast to the previous function, the translation vector is specified as a (unit) direction vector and + a distance. */ + static Distance computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) + { + return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + precision, validCallback, options, cost_function); + } + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. + + In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) + for a virtual frame attached to the robot \e link with the given \e link_offset. + The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame. + This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */ + static Percentage computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Compute the sequence of joint values that perform a general Cartesian path. + + In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially + reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global + reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs + to move in a straight line between two consecutive waypoints. All other comments apply. */ + static Percentage computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector>& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. @@ -158,12 +235,19 @@ class CartesianInterpolator During the computation of the path, it is usually preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold argument is provided. If a jump detection is enabled and a jump is - found, the path is truncated up to just before the jump. + To account for this, the \e jump_threshold struct is provided, which comprises three fields: + \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. + If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. + If \e jump_threshold_factor is non-zero, we test for relative jumps. To this end, the average joint-space distance + between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger than + this average distance multiplied by \e jump_threshold_factor, this step is considered a jump. + + Otherwise (if all params are zero), jump detection is disabled. + If a jump is detected, the path is truncated up to just before the jump. Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */ - static Distance computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, @@ -175,7 +259,7 @@ class CartesianInterpolator In contrast to the previous function, the translation vector is specified as a (unit) direction vector and a distance. */ - static Distance computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, @@ -183,8 +267,11 @@ class CartesianInterpolator const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn()) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step, jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop } /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. @@ -194,7 +281,7 @@ class CartesianInterpolator The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame (\e global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved. All other comments from the previous function apply. */ - static Percentage computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, @@ -209,7 +296,7 @@ class CartesianInterpolator reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move in a straight line between two consecutive waypoints. All other comments apply. */ - static Percentage computeCartesianPath( + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath( RobotState* start_state, const JointModelGroup* group, std::vector>& path, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 02707c3481..995338badd 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ +/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman, Robert Haschke */ #include #include @@ -60,6 +60,54 @@ rclcpp::Logger getLogger() return moveit::getLogger("moveit.core.cartesian_interpolator"); } +bool validateAndImproveInterval(const RobotState& start_state, const RobotState& end_state, + const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose, + std::vector& traj, double& percentage, const double width, + const JointModelGroup* group, const LinkModel* link, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const kinematics::KinematicsBase::IKCostFn& cost_function, + const Eigen::Isometry3d& link_offset) +{ + // compute pose at joint-space midpoint between start_state and end_state + RobotState mid_state(start_state.getRobotModel()); + start_state.interpolate(end_state, 0.5, mid_state); + Eigen::Isometry3d fk_pose = mid_state.getGlobalLinkTransform(link) * link_offset; + + // compute pose at Cartesian-space midpoint between start_pose and end_pose + Eigen::Isometry3d mid_pose(Eigen::Quaterniond(start_pose.linear()).slerp(0.5, Eigen::Quaterniond(end_pose.linear()))); + mid_pose.translation() = 0.5 * (start_pose.translation() + end_pose.translation()); + + // if deviation between both poses, fk_pose and mid_pose is within precision, we are satisfied + double linear_distance = (mid_pose.translation() - fk_pose.translation()).norm(); + double angular_distance = Eigen::Quaterniond(mid_pose.linear()).angularDistance(Eigen::Quaterniond(fk_pose.linear())); + if (linear_distance <= precision.translational && angular_distance <= precision.rotational) + { + traj.push_back(std::make_shared(end_state)); + return true; + } + + if (width < precision.max_resolution) + return false; // failed to find linear interpolation within max_resolution + + // otherwise subdivide interval further, computing IK for mid_pose + if (!mid_state.setFromIK(group, mid_pose * link_offset.inverse(), link->getName(), 0.0, validCallback, options, + cost_function)) + return false; + + // and recursively processing the two sub-intervals + const auto half_width = width / 2.0; + const auto old_percentage = percentage; + percentage = percentage - half_width; + if (!validateAndImproveInterval(start_state, mid_state, start_pose, mid_pose, traj, percentage, half_width, group, + link, precision, validCallback, options, cost_function, link_offset)) + return false; + + percentage = old_percentage; + return validateAndImproveInterval(mid_state, end_state, mid_pose, end_pose, traj, percentage, half_width, group, link, + precision, validCallback, options, cost_function, link_offset); +} + std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, const moveit::core::JointModelGroup& group, double jump_threshold_factor) { @@ -136,6 +184,129 @@ std::optional hasAbsoluteJointSpaceJump(const std::vector& traj, + const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function) +{ + const double distance = translation.norm(); + // The target pose is obtained by adding the translation vector to the link's current pose + Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link); + + // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) + pose.translation() += global_reference_frame ? translation : pose.linear() * translation; + + // call computeCartesianPath for the computed target pose in the global reference frame + return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, pose, true, + max_step, precision, validCallback, options, + cost_function); +} + +CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function, + const Eigen::Isometry3d& link_offset) +{ + // check unsanitized inputs for non-isometry + ASSERT_ISOMETRY(target) + ASSERT_ISOMETRY(link_offset) + + RobotState state(*start_state); + + const std::vector& cjnt = group->getContinuousJointModels(); + // make sure that continuous joints wrap + for (const JointModel* joint : cjnt) + state.enforceBounds(joint); + + // Cartesian pose we start from + Eigen::Isometry3d start_pose = state.getGlobalLinkTransform(link) * link_offset; + Eigen::Isometry3d inv_offset = link_offset.inverse(); + + // the target can be in the local reference frame (in which case we rotate it) + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); + + double rotation_distance = start_quaternion.angularDistance(target_quaternion); + double translation_distance = (rotated_target.translation() - start_pose.translation()).norm(); + + // decide how many steps we will need for this trajectory + std::size_t translation_steps = 0; + if (max_step.translation > 0.0) + translation_steps = floor(translation_distance / max_step.translation); + + std::size_t rotation_steps = 0; + if (max_step.rotation > 0.0) + rotation_steps = floor(rotation_distance / max_step.rotation); + std::size_t steps = std::max(translation_steps, rotation_steps) + 1; + + traj.clear(); + traj.push_back(std::make_shared(*start_state)); + + double last_valid_percentage = 0.0; + Eigen::Isometry3d prev_pose = start_pose; + RobotState prev_state(state); + for (std::size_t i = 1; i <= steps; ++i) + { + double percentage = static_cast(i) / static_cast(steps); + + Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion)); + pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation(); + + if (!state.setFromIK(group, pose * inv_offset, link->getName(), 0.0, validCallback, options, cost_function) || + !validateAndImproveInterval(prev_state, state, prev_pose, pose, traj, percentage, + 1.0 / static_cast(steps), group, link, precision, validCallback, options, + cost_function, link_offset)) + break; + + prev_pose = pose; + prev_state = state; + last_valid_percentage = percentage; + } + + return last_valid_percentage; +} + +CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function, + const Eigen::Isometry3d& link_offset) +{ + double percentage_solved = 0.0; + for (std::size_t i = 0; i < waypoints.size(); ++i) + { + std::vector waypoint_traj; + double wp_percentage_solved = + computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, + precision, validCallback, options, cost_function, link_offset); + if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) + { + percentage_solved = static_cast(i + 1) / static_cast(waypoints.size()); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + } + else + { + percentage_solved += wp_percentage_solved / static_cast(waypoints.size()); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + break; + } + } + + return percentage_solved; +} + JumpThreshold JumpThreshold::disabled() { return JumpThreshold(); @@ -184,10 +355,13 @@ CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) pose.translation() += global_reference_frame ? translation : pose.linear() * translation; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" // call computeCartesianPath for the computed target pose in the global reference frame return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, path, link, pose, true, max_step, jump_threshold, validCallback, options, cost_function); +#pragma GCC diagnostic pop } CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( @@ -310,9 +484,12 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // Don't test joint space jumps for every waypoint, test them later on the whole path. static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST = JumpThreshold::disabled(); std::vector waypoint_path; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset); +#pragma GCC diagnostic pop if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = static_cast((i + 1)) / static_cast(waypoints.size()); diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index c868454fed..34af2a8094 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -185,7 +185,7 @@ bool MoveGroupCartesianPathService::computeService( std::vector traj; res->fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, - moveit::core::MaxEEFStep(req->max_step), jump_threshold, constraint_fn); + moveit::core::MaxEEFStep(req->max_step), moveit::core::CartesianPrecision{}, constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name); diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp index 0ea73ec9cd..0d1315f6dc 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -95,7 +95,7 @@ int main(int argc, char* argv[]) // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); 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 aa71bda8b1..4c713fd8b6 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -119,7 +119,7 @@ int main(int argc, char* argv[]) // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); bool satisfies_linear_tolerance = false; 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 cd310b596e..eb816754eb 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -98,7 +98,7 @@ int main(int argc, char* argv[]) // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage()); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 7ab87d9cf5..663e4f7d2f 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -118,9 +118,10 @@ class Servo /** * \brief Get the current state of the robot as given by planning scene monitor. + * @param block_for_current_state If true, we explicitly wait for a new robot state * @return The current state of the robot. */ - KinematicState getCurrentRobotState() const; + KinematicState getCurrentRobotState(bool block_for_current_state) const; /** * \brief Smoothly halt at a commanded state when command goes stale. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 280baa3a8f..c8e27ee17c 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -158,7 +158,6 @@ void Servo::setSmoothingPlugin() RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized"); std::exit(EXIT_FAILURE); } - resetSmoothing(getCurrentRobotState()); } void Servo::doSmoothing(KinematicState& state) @@ -523,9 +522,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Adjust joint position based on scaled down velocity target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period); - // Apply smoothing to the positions if a smoother was provided. - doSmoothing(target_state); - // Apply collision scaling to the joint position delta target_state.positions = current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions); @@ -545,8 +541,8 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot } } - // Update internal state of filter with final calculated command. - resetSmoothing(target_state); + // Apply smoothing to the positions if a smoother was provided. + doSmoothing(target_state); return target_state; } @@ -644,8 +640,21 @@ std::optional Servo::toPlanningFrame(const PoseCommand& command, co return PoseCommand{ planning_frame, planning_to_command_tf * command.pose }; } -KinematicState Servo::getCurrentRobotState() const +KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const { + if (block_for_current_state) + { + bool have_current_state = false; + while (rclcpp::ok() && !have_current_state) + { + have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState( + rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */); + if (!have_current_state) + { + RCLCPP_WARN(logger_, "Waiting for the current state"); + } + } + } moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); return extractRobotState(robot_state, servo_params_.move_group_name); } @@ -662,9 +671,6 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta // set target velocity target_state.velocities *= 0.0; - // apply smoothing: this will change target position/velocity to make slow down gradual - doSmoothing(target_state); - // scale velocity in case of obstacle target_state.velocities *= collision_velocity_scale_; @@ -678,7 +684,9 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta } } - resetSmoothing(target_state); + // apply smoothing: this will change target position/velocity to make slow down gradual + doSmoothing(target_state); + return std::make_pair(stopped, target_state); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index deb8b02d01..9eae434869 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -152,12 +152,11 @@ void ServoNode::pauseServo(const std::shared_ptrgetCurrentRobotState(); + last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); servo_->resetSmoothing(last_commanded_state_); // clear out the command rolling window and reset last commanded state to be the current state joint_cmd_rolling_window_.clear(); - last_commanded_state_ = servo_->getCurrentRobotState(); // reactivate collision checking servo_->setCollisionChecking(true); @@ -301,8 +300,10 @@ void ServoNode::servoLoop() RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update."); rclcpp::sleep_for(std::chrono::seconds(1)); } - KinematicState current_state = servo_->getCurrentRobotState(); + KinematicState current_state = servo_->getCurrentRobotState(true /* block for current robot state */); last_commanded_state_ = current_state; + // Ensure the filter is up to date + servo_->resetSmoothing(current_state); // Get the robot state and joint model group info. moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); @@ -314,6 +315,7 @@ void ServoNode::servoLoop() // Skip processing if servoing is disabled. if (servo_paused_) { + servo_->resetSmoothing(current_state); servo_frequency.sleep(); continue; } @@ -329,7 +331,7 @@ void ServoNode::servoLoop() { // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state joint_cmd_rolling_window_.clear(); - current_state = servo_->getCurrentRobotState(); + current_state = servo_->getCurrentRobotState(false /* block for current robot state */); current_state.velocities *= 0.0; } diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 5e2fceb8a0..b9cd185d67 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -438,7 +438,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; debug_msg.step = w * sizeof(float); debug_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); + mesh_filter_->getModelDepth(reinterpret_cast(&debug_msg.data[0])); pub_model_depth_image_.publish(debug_msg, *info_msg); sensor_msgs::msg::Image filtered_depth_msg; @@ -449,7 +449,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; filtered_depth_msg.step = w * sizeof(float); filtered_depth_msg.data.resize(img_size * sizeof(float)); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_depth_msg.data[0])); pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); sensor_msgs::msg::Image label_msg; @@ -481,7 +481,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: if (filtered_data.size() < img_size) filtered_data.resize(img_size); - mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); unsigned short* msg_data = reinterpret_cast(&filtered_msg.data[0]); for (std::size_t i = 0; i < img_size; ++i) { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 3532389869..64b8dadfca 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -106,7 +106,7 @@ class GLRenderer * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] buffer pointer to memory where the depth values need to be stored */ - void getDepthBuffer(double* buffer) const; + void getDepthBuffer(float* buffer) const; /** * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context. diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 646c3ca569..879a9d87bc 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -131,7 +131,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] depth pointer to buffer to be filled with depth values. */ - void getFilteredDepth(double* depth) const; + void getFilteredDepth(float* depth) const; /** * \brief retrieves the labels of the rendered model @@ -149,7 +149,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \param[out] depth pointer to buffer to be filled with depth values. */ - void getModelDepth(double* depth) const; + void getModelDepth(float* depth) const; /** * \brief set the shadow threshold. points that are further away than the rendered model are filtered out. diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 0ba4405281..607fb663c0 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h @@ -103,13 +103,13 @@ class SensorModel * \brief transforms depth values from rendered model to metric depth values * \param[in,out] depth pointer to floating point depth buffer */ - virtual void transformModelDepthToMetricDepth(double* depth) const; + virtual void transformModelDepthToMetricDepth(float* depth) const; /** * \brief transforms depth values from filtered depth to metric depth values * \param[in,out] depth pointer to floating point depth buffer */ - virtual void transformFilteredDepthToMetricDepth(double* depth) const; + virtual void transformFilteredDepthToMetricDepth(float* depth) const; /** * \brief sets the image size diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 3c2d7d29e3..0dea00e55c 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -213,7 +213,7 @@ void mesh_filter::GLRenderer::getColorBuffer(unsigned char* buffer) const glBindFramebuffer(GL_FRAMEBUFFER, 0); } -void mesh_filter::GLRenderer::getDepthBuffer(double* buffer) const +void mesh_filter::GLRenderer::getDepthBuffer(float* buffer) const { glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_); glBindTexture(GL_TEXTURE_2D, depth_id_); diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 793058297e..6b4b551b9f 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp @@ -227,7 +227,7 @@ void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const job->wait(); } -void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const +void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const { JobPtr job1 = std::make_shared>([&renderer = *mesh_renderer_, depth] { renderer.getDepthBuffer(depth); }); @@ -243,7 +243,7 @@ void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const job2->wait(); } -void mesh_filter::MeshFilterBase::getFilteredDepth(double* depth) const +void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const { JobPtr job1 = std::make_shared>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); }); JobPtr job2 = std::make_shared>( diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 562e7b4359..9ff5cdbf0c 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp @@ -102,7 +102,7 @@ inline bool isAligned16(const void* pointer) #endif } // namespace -void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(double* depth) const +void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const { #if HAVE_SSE_EXTENSIONS const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_); @@ -161,7 +161,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub const float nf = near * far; const float f_n = far - near; - const double* depth_end = depth + width_ * height_; + const float* depth_end = depth + width_ * height_; while (depth < depth_end) { if (*depth != 0 && *depth != 1) @@ -178,7 +178,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub #endif } -void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(double* depth) const +void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(float* depth) const { #if HAVE_SSE_EXTENSIONS //* SSE version @@ -223,7 +223,7 @@ void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(d ++mmDepth; } #else - const double* depth_end = depth + width_ * height_; + const float* depth_end = depth + width_ * height_; const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_; const float offset = near_clipping_plane_distance_; while (depth < depth_end) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 4f82643017..be02e8c6d2 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -765,9 +765,16 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); + } double computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the @@ -781,8 +788,16 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, + double /*jump_threshold*/, moveit_msgs::msg::RobotTrajectory& trajectory, + const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, + moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, error_code); + } double computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, + moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 3fbf9e3c63..84a46005ce 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -860,7 +860,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } double computeCartesianPath(const std::vector& waypoints, double step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg, + moveit_msgs::msg::RobotTrajectory& msg, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes& error_code) { @@ -874,7 +874,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req->header.stamp = getClock()->now(); req->waypoints = waypoints; req->max_step = step; - req->jump_threshold = jump_threshold; req->path_constraints = path_constraints; req->avoid_collisions = avoid_collisions; req->link_name = getEndEffectorLink(); @@ -1553,31 +1552,29 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan) //} double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, - bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) + moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions, + moveit_msgs::msg::MoveItErrorCodes* error_code) { moveit_msgs::msg::Constraints path_constraints_tmp; - return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::msg::Constraints(), - avoid_collisions, error_code); + return computeCartesianPath(waypoints, eef_step, trajectory, moveit_msgs::msg::Constraints(), avoid_collisions, + error_code); } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory, + moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code) { if (error_code) { - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, *error_code); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, *error_code); } else { moveit_msgs::msg::MoveItErrorCodes err_tmp; err_tmp.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; moveit_msgs::msg::MoveItErrorCodes& err = error_code ? *error_code : err_tmp; - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, err); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, err); } } diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 370d2f72df..2b0595005e 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -238,6 +238,10 @@ TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest) // clear path constraints move_group_->clearPathConstraints(); + + // move back to ready pose + move_group_->setNamedTarget("ready"); + planAndMove(); } TEST_F(MoveGroupTestFixture, ModifyPlanningSceneAsyncInterfaces) @@ -311,11 +315,10 @@ TEST_F(MoveGroupTestFixture, CartPathTest) waypoints.push_back(target_waypoint); // up and left moveit_msgs::RobotTrajectory trajectory; - const auto jump_threshold = 0.0; const auto eef_step = 0.01; // test below is meaningless if Cartesian planning did not succeed - ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0); + ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, trajectory), 1.0); // Execute trajectory EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index e79a026806..746685ed43 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -135,13 +135,11 @@ bool MotionPlanningFrame::computeCartesianPlan() // setup default params double cart_step_size = 0.01; - double cart_jump_thresh = 0.0; bool avoid_collisions = true; // compute trajectory moveit_msgs::msg::RobotTrajectory trajectory; - double fraction = - move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions); + double fraction = move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions); if (fraction >= 1.0) {