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 62ce080a61..23e441539c 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 @@ -140,18 +140,16 @@ class CartesianInterpolator double meters; }; - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group. - - The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin - The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin - of a robot link (\e link). The direction is assumed to be either in a global reference frame or in the local - reference frame of the link. In the latter case (\e global_reference_frame is false) the \e direction is rotated - accordingly. The link needs to move in a straight line, following the specified direction, for the desired \e - distance. The resulting joint values are stored in the vector \e traj, one by one. The maximum distance in + /** \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 (\e global_reference_frame is false). + The resulting joint values are stored in the vector \e traj, one by one. The maximum 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 computed and for which corresponding states were added to the path. At the end of the + the distance that was achieved and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose. During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a @@ -173,20 +171,38 @@ class CartesianInterpolator Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */ static Distance + computeCartesianPath(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 JumpThreshold& jump_threshold, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + 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(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 JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn()); + kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn()) + { + return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + jump_threshold, validCallback, options, cost_function); + } - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group. + /** \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 the origin of a robot link (\e link). The target frame is assumed to be either in a global reference frame or - in the local reference frame of the link. In the latter case (\e global_reference_frame is false) the \e target is - rotated accordingly. All other comments from the previous function apply. */ + 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 + (\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(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, @@ -194,14 +210,15 @@ class CartesianInterpolator const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn()); + 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 for the origin of a robot link (\e link). The waypoints are transforms given either in a global reference - frame or in the local reference frame of the link at the immediately preceding waypoint. The link needs to move - in a straight line between two consecutive waypoints. All other comments apply. */ + 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(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, @@ -209,7 +226,8 @@ class CartesianInterpolator const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), - kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn()); + kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); /** \brief Tests joint space jumps of a trajectory. diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 3ece3590f8..4060968783 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -56,24 +56,20 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_state.cart CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath( 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 Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function) { - // this is the Cartesian pose we start from, and have to move in the direction indicated - // getGlobalLinkTransform() returns a valid isometry by contract - const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link); + 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 direction can be in the local reference frame (in which case we rotate it) - const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction; - - // The target pose is built by applying a translation to the start pose for the desired direction and distance - Eigen::Isometry3d target_pose = start_pose; // valid isometry - target_pose.translation() += rotated_direction * distance; + // 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, target_pose, - true, max_step, jump_threshold, validCallback, + return CartesianInterpolator::Distance(distance) * computeCartesianPath(start_state, group, traj, link, pose, true, + max_step, jump_threshold, validCallback, options, cost_function); } @@ -81,21 +77,24 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( 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 JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function) + const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function, + const Eigen::Isometry3d& link_offset) { + // check unsanitized inputs for non-isometry + ASSERT_ISOMETRY(target) + ASSERT_ISOMETRY(link_offset) + const std::vector& cjnt = group->getContinuousJointModels(); // make sure that continuous joints wrap for (const JointModel* joint : cjnt) start_state->enforceBounds(joint); - // this is the Cartesian pose we start from, and we move in the direction indicated - // getGlobalLinkTransform() returns a valid isometry by contract - Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); // valid isometry - - ASSERT_ISOMETRY(target) // unsanitized input, could contain a non-isometry + // Cartesian pose we start from + Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link) * link_offset; + Eigen::Isometry3d 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; // valid isometry + 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()); @@ -160,7 +159,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( // Explicitly use a single IK attempt only: We want a smooth trajectory. // Random seeding (of additional attempts) would probably create IK jumps. - if (start_state->setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options, + if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options, cost_function)) traj.push_back(std::make_shared(*start_state)); else @@ -178,7 +177,8 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( 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 JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function) + const kinematics::KinematicsQueryOptions& options, 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) @@ -188,7 +188,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( std::vector waypoint_traj; double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, - NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function); + NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset); if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = (double)(i + 1) / (double)waypoints.size();