From 875c91e2e9019ede92b33bf12497196802a0100c Mon Sep 17 00:00:00 2001 From: mohamedsayed18 Date: Thu, 2 Dec 2021 22:01:56 +0100 Subject: [PATCH] new constraint --- .../default_planner_namespaces.h | 1 + .../trajopt/CMakeLists.txt | 1 + .../profile/trajopt_constraint_plan_profile.h | 30 +++++++ .../trajopt_constraint_plan_profile.cpp | 82 +++++++++++++++++++ 4 files changed, 114 insertions(+) create mode 100644 tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_constraint_plan_profile.h create mode 100644 tesseract_motion_planners/trajopt/src/profile/trajopt_constraint_plan_profile.cpp diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/default_planner_namespaces.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/default_planner_namespaces.h index 3b2747c4ae0..49c885c0bcb 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/default_planner_namespaces.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/default_planner_namespaces.h @@ -34,6 +34,7 @@ static const std::string DESCARTES_DEFAULT_NAMESPACE = "DESCARTES"; static const std::string OMPL_DEFAULT_NAMESPACE = "OMPL"; static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TRAJOPT"; static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TRAJOPT_IFOPT"; +static const std::string TRAJOPT_CONSTRAINT_NAMESPACE = "TRAJOPT_CONSTRAINT"; // clang-format on } // namespace tesseract_planning::profile_ns #endif // TESSERACT_MOTION_PLANNERS_DEFAULT_PLANNER_NAMESPACES_H diff --git a/tesseract_motion_planners/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt index 6f11fe84343..3aa959b424a 100644 --- a/tesseract_motion_planners/trajopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt/CMakeLists.txt @@ -10,6 +10,7 @@ add_library( src/profile/trajopt_default_plan_profile.cpp src/profile/trajopt_default_composite_profile.cpp src/profile/trajopt_default_solver_profile.cpp + src/profile/trajopt_constraint_plan_profile.cpp src/serialize.cpp src/deserialize.cpp) target_link_libraries( diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_constraint_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_constraint_plan_profile.h new file mode 100644 index 00000000000..872800fa15d --- /dev/null +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_constraint_plan_profile.h @@ -0,0 +1,30 @@ +#ifndef TRAJOPT_CONSTRAINT_PLAN_PROFILE_H +#define TRAJOPT_CONSTRAINT_PLAN_PROFILE_H + +#include + +#ifdef SWIG +%shared_ptr(tesseract_planning::TrajOptConstraintPlanProfile) +#endif // SWIG + +namespace tesseract_planning +{ +class TrajOptConstraintPlanProfile : public TrajOptDefaultPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TrajOptConstraintPlanProfile() = default; + ~TrajOptConstraintPlanProfile() override = default; + + void apply(trajopt::ProblemConstructionInfo& pci, + const CartesianWaypoint& cartesian_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const override; +}; +} // namespace tesseract_planning + +#endif \ No newline at end of file diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_constraint_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_constraint_plan_profile.cpp new file mode 100644 index 00000000000..666bb5872c8 --- /dev/null +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_constraint_plan_profile.cpp @@ -0,0 +1,82 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +#include +#include +#include + +namespace tesseract_planning +{ +void TrajOptConstraintPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, + const CartesianWaypoint& cartesian_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const +{ + assert(isPlanInstruction(parent_instruction)); + const auto& base_instruction = parent_instruction.as(); + assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); + ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo()); + + // TODO rename these checks later + if (mi.manipulator.empty()) + throw std::runtime_error("TrajOptPlanProfile, manipulator is empty!"); + + if (mi.tcp_frame.empty()) + throw std::runtime_error("TrajOptPlanProfile, tcp_frame is empty!"); + + if (mi.working_frame.empty()) + throw std::runtime_error("TrajOptPlanProfile, working_frame is empty!"); + + Eigen::Isometry3d tcp_offset = pci.env->findTCPOffset(mi); + + trajopt::TermInfo::Ptr ti{ nullptr }; + + bool is_active_tcp_frame = (std::find(active_links.begin(), active_links.end(), mi.tcp_frame) != active_links.end()); + bool is_static_working_frame = + (std::find(active_links.begin(), active_links.end(), mi.working_frame) == active_links.end()); + + if (cartesian_waypoint.isToleranced()) + CONSOLE_BRIDGE_logWarn("Tolerance cartesian waypoints are not supported in this version of TrajOpt."); + + if ((is_static_working_frame && is_active_tcp_frame) || (!is_active_tcp_frame && !is_static_working_frame)) + { + ti = createCartesianWaypointTermInfo( + index, mi.working_frame, cartesian_waypoint, mi.tcp_frame, tcp_offset, cartesian_coeff, term_type); + } + else if (!is_static_working_frame && is_active_tcp_frame) + { + ti = createDynamicCartesianWaypointTermInfo( + index, mi.working_frame, cartesian_waypoint, mi.tcp_frame, tcp_offset, cartesian_coeff, term_type); + } + else + { + throw std::runtime_error("TrajOpt, both tcp_frame and working_frame are both static!"); + } + + if (term_type == trajopt::TermType::TT_CNT) + pci.cnt_infos.push_back(ti); + else + pci.cost_infos.push_back(ti); + + // Add constraints from error functions if available. + addConstraintErrorFunctions(pci, index); + + // Add new constraints + Eigen::VectorXd new_coeff{ Eigen::VectorXd::Constant(6, 1, 5) }; + + new_coeff(0) = 0; + new_coeff(1) = 0; + new_coeff(2) = 0; + + ti = createDynamicCartesianWaypointTermInfo( + index, "world", cartesian_waypoint, mi.tcp_frame, tcp_offset, new_coeff, term_type); + pci.cnt_infos.push_back(ti); +} +} // namespace tesseract_planning