diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h index f5f53402535..e02c5979c5a 100644 --- a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h +++ b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h @@ -264,6 +264,25 @@ class CartesianWaypoint /** @brief The Cartesian Waypoint */ Eigen::Isometry3d waypoint; + /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */ + Eigen::VectorXd lower_tolerance; + /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/ + Eigen::VectorXd upper_tolerance; + + bool isToleranced() const + { + // Check if they are empty + if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0) + return false; + + // Check if they are close to 0 + Eigen::VectorXd range = upper_tolerance - lower_tolerance; + double sum = range.sum(); + if (sum < 1e-5) + return false; + + return true; + } }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h index 3c280b5831f..96283c4f01e 100644 --- a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h +++ b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h @@ -265,13 +265,32 @@ class JointWaypoint inline operator Eigen::Ref() { return Eigen::Ref(waypoint); } ////////////////////////////////// - // Cartesian Waypoint Container // + // Joint Waypoint Container // ////////////////////////////////// #endif // SWIG Eigen::VectorXd waypoint; std::vector joint_names; + /** @brief Joint distance below waypoint that is allowed. Each element should be <= 0 */ + Eigen::VectorXd lower_tolerance; + /** @brief Joint distance above waypoint that is allowed. Each element should be >= 0 */ + Eigen::VectorXd upper_tolerance; + + bool isToleranced() const + { + // Check if they are empty + if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0) + return false; + + // Check if they are close to 0 + Eigen::VectorXd range = upper_tolerance - lower_tolerance; + double sum = range.sum(); + if (sum < 1e-5) + return false; + + return true; + } }; } // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index 317bf22c2fa..529bfff1c16 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h @@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include #ifdef SWIG @@ -76,14 +78,14 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile constraint_error_functions; void apply(trajopt::ProblemConstructionInfo& pci, - const Eigen::Isometry3d& cartesian_waypoint, + const CartesianWaypoint& cartesian_waypoint, const Instruction& parent_instruction, const ManipulatorInfo& manip_info, const std::vector& active_links, int index) const override; void apply(trajopt::ProblemConstructionInfo& pci, - const Eigen::VectorXd& joint_waypoint, + const JointWaypoint& joint_waypoint, const Instruction& parent_instruction, const ManipulatorInfo& manip_info, const std::vector& active_links, diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index 6bc79912558..aee044fd7c6 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -51,14 +51,14 @@ class TrajOptPlanProfile using ConstPtr = std::shared_ptr; virtual void apply(trajopt::ProblemConstructionInfo& pci, - const Eigen::Isometry3d& cartesian_waypoint, + const CartesianWaypoint& cartesian_waypoint, const Instruction& parent_instruction, const ManipulatorInfo& manip_info, const std::vector& active_links, int index) const = 0; virtual void apply(trajopt::ProblemConstructionInfo& pci, - const Eigen::VectorXd& joint_waypoint, + const JointWaypoint& joint_waypoint, const Instruction& parent_instruction, const ManipulatorInfo& manip_info, const std::vector& active_links, diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h index a392c1f18cf..a125b7fdd2d 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h @@ -63,6 +63,14 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp, const Eigen::VectorXd& coeffs, trajopt::TermType type); +trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp, + const Eigen::VectorXd& lower_tol, + const Eigen::VectorXd& upper_tol, + int index, + const Eigen::VectorXd& coeffs, + trajopt::TermType type); + + trajopt::TermInfo::Ptr createCollisionTermInfo( int start_index, int end_index, diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp index 71e6821cf6c..ddb8f9ec714 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp @@ -138,8 +138,9 @@ DefaultTrajoptProblemGenerator(const std::string& name, } else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) { - const Eigen::VectorXd& position = getJointPosition(start_waypoint); - start_plan_profile->apply(*pci, position, *start_instruction, composite_mi, active_links, index); + JointWaypoint position_wp; + position_wp.waypoint = getJointPosition(start_waypoint); + start_plan_profile->apply(*pci, position_wp, *start_instruction, composite_mi, active_links, index); } else { @@ -225,7 +226,8 @@ DefaultTrajoptProblemGenerator(const std::string& name, } else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint())) { - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint()); + JointWaypoint cur_position; + cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint()); Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity(); if (!pci->kin->calcFwdKin(cur_pose, cur_position)) throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!"); @@ -283,7 +285,8 @@ DefaultTrajoptProblemGenerator(const std::string& name, { if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint())) { - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint()); + JointWaypoint cur_position; + cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint()); // Add intermediate points with path costs and constraints for (std::size_t s = 0; s < seed_composite->size() - 1; ++s) diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp index 34fa0273fee..6cc4a498adc 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp @@ -99,7 +99,7 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& } } void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, - const Eigen::Isometry3d& cartesian_waypoint, + const CartesianWaypoint& cartesian_waypoint, const Instruction& parent_instruction, const ManipulatorInfo& manip_info, const std::vector& active_links, @@ -119,6 +119,9 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame); if (it != active_links.end()) { + if (cartesian_waypoint.isToleranced()) + CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt."); + if (mi.tcp.isExternal()) { // If external, the part is attached to the robot so working frame is passed as link instead of target frame @@ -147,13 +150,18 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, } void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, - const Eigen::VectorXd& joint_waypoint, + const JointWaypoint& joint_waypoint, const Instruction& /*parent_instruction*/, const ManipulatorInfo& /*manip_info*/, const std::vector& /*active_links*/, int index) const { - auto ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type); + trajopt::TermInfo::Ptr ti; + if (joint_waypoint.isToleranced()) + ti = createTolerancedJointWaypointTermInfo( + joint_waypoint, joint_waypoint.lower_tolerance, joint_waypoint.upper_tolerance, index, joint_coeff, term_type); + else + ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type); if (term_type == trajopt::TermType::TT_CNT) pci.cnt_infos.push_back(ti); diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp index ac80d41ef6d..dd8eda1d4d6 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp @@ -138,6 +138,31 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp, return joint_info; } +trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp, + const Eigen::VectorXd& lower_tol, + const Eigen::VectorXd& upper_tol, + int index, + const Eigen::VectorXd& coeffs, + trajopt::TermType type) +{ + auto joint_info = std::make_shared(); + if (coeffs.size() == 1) + joint_info->coeffs = std::vector(static_cast(j_wp.size()), coeffs(0)); + else if (coeffs.size() == j_wp.size()) + joint_info->coeffs = std::vector(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols()); + + joint_info->targets = std::vector(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols()); + joint_info->lower_tols = std::vector(lower_tol.data(), lower_tol.data() + lower_tol.rows() * lower_tol.cols()); + joint_info->upper_tols = std::vector(upper_tol.data(), upper_tol.data() + upper_tol.rows() * upper_tol.cols()); + joint_info->first_step = index; + joint_info->last_step = index; + joint_info->name = "joint_waypoint_" + std::to_string(index); + joint_info->term_type = type; + + return joint_info; +} + + trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index, int end_index, double collision_safety_margin,