diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index 5c04ff289b9..544d8764452 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -50,14 +50,10 @@ class CompositeInstruction; * * The filter should return true when the instruction passed should be included not throw. */ -using flattenFilterFn = - std::function; -using locateFilterFn = - std::function; +using flattenFilterFn = std::function; +using locateFilterFn = std::function; -bool moveFilter(const InstructionPoly& instruction, - const CompositeInstruction& composite, - bool parent_is_first_composite); +bool moveFilter(const InstructionPoly& instruction, const CompositeInstruction& composite); enum class CompositeInstructionOrder { @@ -73,10 +69,38 @@ class CompositeInstruction EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP + /** value_type */ + using value_type = InstructionPoly; + /** pointer */ + using pointer = typename std::vector::pointer; + /** const_pointer */ + using const_pointer = typename std::vector::const_pointer; + /** reference */ + using reference = typename std::vector::reference; + /** const_reference */ + using const_reference = typename std::vector::const_reference; + /** size_type */ + using size_type = typename std::vector::size_type; + /** difference_type */ + using difference_type = typename std::vector::difference_type; + /** iterator */ + using iterator = typename std::vector::iterator; + /** const_iterator */ + using const_iterator = typename std::vector::const_iterator; + /** reverse_iterator */ + using reverse_iterator = typename std::vector::reverse_iterator; + /** const_reverse_iterator */ + using const_reverse_iterator = typename std::vector::const_reverse_iterator; + CompositeInstruction(std::string profile = DEFAULT_PROFILE_KEY, CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED, tesseract_common::ManipulatorInfo manipulator_info = tesseract_common::ManipulatorInfo()); + template + CompositeInstruction(InputIt first, InputIt last) : container_(first, last) + { + } + CompositeInstructionOrder getOrder() const; const boost::uuids::uuid& getUUID() const; @@ -100,13 +124,6 @@ class CompositeInstruction const tesseract_common::ManipulatorInfo& getManipulatorInfo() const; tesseract_common::ManipulatorInfo& getManipulatorInfo(); - void setStartInstruction(MoveInstructionPoly instruction); - - void resetStartInstruction(); - const MoveInstructionPoly& getStartInstruction() const; - MoveInstructionPoly& getStartInstruction(); - bool hasStartInstruction() const; - void setInstructions(std::vector instructions); std::vector& getInstructions(); const std::vector& getInstructions() const; @@ -114,8 +131,8 @@ class CompositeInstruction void appendMoveInstruction(const MoveInstructionPoly& mi); void appendMoveInstruction(const MoveInstructionPoly&& mi); - void appendInstruction(const InstructionPoly& i); - void appendInstruction(const InstructionPoly&& i); + iterator insertMoveInstruction(const_iterator p, const MoveInstructionPoly& x); + iterator insertMoveInstruction(const_iterator p, MoveInstructionPoly&& x); /** * @brief Get the first Move Instruction in a Composite Instruction @@ -228,34 +245,6 @@ class CompositeInstruction // C++ container support - /** value_type */ - using value_type = InstructionPoly; - /** pointer */ - using pointer = typename std::vector::pointer; - /** const_pointer */ - using const_pointer = typename std::vector::const_pointer; - /** reference */ - using reference = typename std::vector::reference; - /** const_reference */ - using const_reference = typename std::vector::const_reference; - /** size_type */ - using size_type = typename std::vector::size_type; - /** difference_type */ - using difference_type = typename std::vector::difference_type; - /** iterator */ - using iterator = typename std::vector::iterator; - /** const_iterator */ - using const_iterator = typename std::vector::const_iterator; - /** reverse_iterator */ - using reverse_iterator = typename std::vector::reverse_iterator; - /** const_reverse_iterator */ - using const_reverse_iterator = typename std::vector::const_reverse_iterator; - - // template - // CompositeInstruction(InputIt first, InputIt last) : container_(first, last) - // { - // } - /////////////// // Iterators // /////////////// @@ -330,10 +319,42 @@ class CompositeInstruction /** @brief clears the contents */ void clear(); + /** @brief inserts element */ + iterator insert(const_iterator p, const value_type& x); + iterator insert(const_iterator p, value_type&& x); + iterator insert(const_iterator p, std::initializer_list l); + template + void insert(const_iterator pos, InputIt first, InputIt last) + { + container_.insert(pos, first, last); + } + + /** @brief constructs element in-place */ + template + iterator emplace(const_iterator pos, Args&&... args); + /** @brief erases element */ iterator erase(const_iterator p); iterator erase(const_iterator first, const_iterator last); + /** @brief adds an element to the end */ + void push_back(const value_type& x); + void push_back(const value_type&& x); + + /** @brief constructs an element in-place at the end */ + template +#if __cplusplus > 201402L + reference emplace_back(Args&&... args) + { + return container_.emplace_back(std::forward(args)...); + } +#else + void emplace_back(Args&&... args) + { + container_.emplace_back(std::forward(args)...); + } +#endif + /** @brief removes the last element */ void pop_back(); @@ -368,38 +389,25 @@ class CompositeInstruction /** @brief The order of the composite instruction */ CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED }; - /** - * @brief The start instruction to use for composite instruction. This should be of type - * MoveInstruction but is stored as type Instruction because it is not required - * - * If not provided, the planner should use the current state of the robot is used and defined as fixed. - */ - InstructionPoly start_instruction_; - const InstructionPoly* getFirstInstructionHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const; + bool process_child_composites) const; InstructionPoly* getFirstInstructionHelper(CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite); + bool process_child_composites); const InstructionPoly* getLastInstructionHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const; + bool process_child_composites) const; InstructionPoly* getLastInstructionHelper(CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite); + bool process_child_composites); long getInstructionCountHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const; + bool process_child_composites) const; /** * @brief Helper function used by Flatten. Not intended for direct use @@ -410,8 +418,7 @@ class CompositeInstruction */ void flattenHelper(std::vector>& flattened, CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite); + const flattenFilterFn& filter); /** * @brief Helper function used by Flatten. Not intended for direct use @@ -422,8 +429,7 @@ class CompositeInstruction */ void flattenHelper(std::vector>& flattened, const CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite) const; + const flattenFilterFn& filter) const; friend class boost::serialization::access; template diff --git a/tesseract_command_language/include/tesseract_command_language/move_instruction.h b/tesseract_command_language/include/tesseract_command_language/move_instruction.h index cfe8b286b28..11d4556368a 100644 --- a/tesseract_command_language/include/tesseract_command_language/move_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/move_instruction.h @@ -159,7 +159,7 @@ class MoveInstruction boost::uuids::uuid parent_uuid_{}; /** @brief The move instruction type */ - MoveInstructionType move_type_{ MoveInstructionType::START }; + MoveInstructionType move_type_{ MoveInstructionType::FREESPACE }; /** @brief The description of the instruction */ std::string description_{ "Tesseract Move Instruction" }; diff --git a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h index 1e73232f573..a1de2a2abf4 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h @@ -84,7 +84,6 @@ enum class MoveInstructionType : int LINEAR = 0, FREESPACE = 1, CIRCULAR = 2, - START = 3 /**< This indicates it is a start instruction. */ }; } // namespace tesseract_planning @@ -345,8 +344,6 @@ struct MoveInstructionPoly : MoveInstructionPolyBase bool isCircular() const; - bool isStart() const; - bool isChild() const; private: diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index e59ae672da1..a41fb84c87b 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -41,18 +41,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include /** @todo Remove after refactor is complete */ namespace tesseract_planning { -bool moveFilter(const InstructionPoly& instruction, - const CompositeInstruction& /*composite*/, - bool parent_is_first_composite) +bool moveFilter(const InstructionPoly& instruction, const CompositeInstruction& /*composite*/) { - if (instruction.isMoveInstruction()) - { - if (instruction.as().isStart()) - return (parent_is_first_composite); - - return true; - } - return false; + return instruction.isMoveInstruction(); } CompositeInstruction::CompositeInstruction(std::string profile, @@ -96,25 +87,6 @@ void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo const tesseract_common::ManipulatorInfo& CompositeInstruction::getManipulatorInfo() const { return manipulator_info_; } tesseract_common::ManipulatorInfo& CompositeInstruction::getManipulatorInfo() { return manipulator_info_; } -void CompositeInstruction::setStartInstruction(MoveInstructionPoly instruction) -{ - start_instruction_ = std::move(instruction); -} - -void CompositeInstruction::resetStartInstruction() { start_instruction_ = InstructionPoly(); } - -const MoveInstructionPoly& CompositeInstruction::getStartInstruction() const -{ - return start_instruction_.as(); -} - -MoveInstructionPoly& CompositeInstruction::getStartInstruction() -{ - return start_instruction_.as(); -} - -bool CompositeInstruction::hasStartInstruction() const { return (!start_instruction_.isNull()); } - void CompositeInstruction::setInstructions(std::vector instructions) { container_.swap(instructions); } std::vector& CompositeInstruction::getInstructions() { return container_; } @@ -125,9 +97,15 @@ void CompositeInstruction::appendMoveInstruction(const MoveInstructionPoly& mi) void CompositeInstruction::appendMoveInstruction(const MoveInstructionPoly&& mi) { container_.emplace_back(mi); } -void CompositeInstruction::appendInstruction(const InstructionPoly& i) { container_.push_back(i); } - -void CompositeInstruction::appendInstruction(const InstructionPoly&& i) { container_.push_back(i); } +CompositeInstruction::iterator CompositeInstruction::insertMoveInstruction(const_iterator p, + const MoveInstructionPoly& x) +{ + return container_.insert(p, x); +} +CompositeInstruction::iterator CompositeInstruction::insertMoveInstruction(const_iterator p, MoveInstructionPoly&& x) +{ + return container_.insert(p, x); +} MoveInstructionPoly* CompositeInstruction::getFirstMoveInstruction() { @@ -170,36 +148,36 @@ long CompositeInstruction::getMoveInstructionCount() const { return getInstructi const InstructionPoly* CompositeInstruction::getFirstInstruction(const locateFilterFn& locate_filter, bool process_child_composites) const { - return getFirstInstructionHelper(*this, locate_filter, process_child_composites, true); + return getFirstInstructionHelper(*this, locate_filter, process_child_composites); } InstructionPoly* CompositeInstruction::getFirstInstruction(const locateFilterFn& locate_filter, bool process_child_composites) { - return getFirstInstructionHelper(*this, locate_filter, process_child_composites, true); + return getFirstInstructionHelper(*this, locate_filter, process_child_composites); } const InstructionPoly* CompositeInstruction::getLastInstruction(const locateFilterFn& locate_filter, bool process_child_composites) const { - return getLastInstructionHelper(*this, locate_filter, process_child_composites, true); + return getLastInstructionHelper(*this, locate_filter, process_child_composites); } InstructionPoly* CompositeInstruction::getLastInstruction(const locateFilterFn& locate_filter, bool process_child_composites) { - return getLastInstructionHelper(*this, locate_filter, process_child_composites, true); + return getLastInstructionHelper(*this, locate_filter, process_child_composites); } long CompositeInstruction::getInstructionCount(const locateFilterFn& locate_filter, bool process_child_composites) const { - return getInstructionCountHelper(*this, locate_filter, process_child_composites, true); + return getInstructionCountHelper(*this, locate_filter, process_child_composites); } std::vector> CompositeInstruction::flatten(const flattenFilterFn& filter) { std::vector> flattened; - flattenHelper(flattened, *this, filter, true); + flattenHelper(flattened, *this, filter); return flattened; } @@ -207,17 +185,13 @@ std::vector> CompositeInstruction::flatten(const flattenFilterFn& filter) const { std::vector> flattened; - flattenHelper(flattened, *this, filter, true); + flattenHelper(flattened, *this, filter); return flattened; } void CompositeInstruction::print(const std::string& prefix) const { std::cout << prefix + "Composite Instruction, Description: " << getDescription() << std::endl; - if (!start_instruction_.isNull()) - std::cout << prefix + "--- Start Instruction, Description: " << start_instruction_.getDescription() << std::endl; - else - std::cout << prefix + "--- Start Instruction, Description: Null Instruction" << std::endl; std::cout << prefix + "{" << std::endl; for (const auto& i : container_) { @@ -235,7 +209,6 @@ bool CompositeInstruction::operator==(const CompositeInstruction& rhs) const equal &= (static_cast(order_) == static_cast(rhs.order_)); equal &= (profile_ == rhs.profile_); // NOLINT equal &= (manipulator_info_ == rhs.manipulator_info_); - equal &= (start_instruction_ == rhs.start_instruction_); equal &= (container_.size() == rhs.container_.size()); if (equal) { @@ -290,19 +263,41 @@ CompositeInstruction::const_reference CompositeInstruction::at(size_type n) cons CompositeInstruction::pointer CompositeInstruction::data() { return container_.data(); } CompositeInstruction::const_pointer CompositeInstruction::data() const { return container_.data(); } CompositeInstruction::reference CompositeInstruction::operator[](size_type pos) { return container_[pos]; } -CompositeInstruction::const_reference CompositeInstruction::operator[](size_type pos) const { return container_[pos]; }; +CompositeInstruction::const_reference CompositeInstruction::operator[](size_type pos) const { return container_[pos]; } /////////////// // Modifiers // /////////////// void CompositeInstruction::clear() { container_.clear(); } +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, const value_type& x) +{ + return container_.insert(p, x); +} +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, value_type&& x) +{ + return container_.insert(p, x); +} +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, std::initializer_list l) +{ + return container_.insert(p, l); +} + +template +CompositeInstruction::iterator CompositeInstruction::emplace(const_iterator pos, Args&&... args) +{ + return container_.emplace(pos, std::forward(args)...); +} + CompositeInstruction::iterator CompositeInstruction::erase(const_iterator p) { return container_.erase(p); } CompositeInstruction::iterator CompositeInstruction::erase(const_iterator first, const_iterator last) { return container_.erase(first, last); } +void CompositeInstruction::push_back(const value_type& x) { container_.push_back(x); } +void CompositeInstruction::push_back(const value_type&& x) { container_.push_back(x); } + void CompositeInstruction::pop_back() { container_.pop_back(); } void CompositeInstruction::swap(std::vector& other) { container_.swap(other); } @@ -313,25 +308,19 @@ void CompositeInstruction::swap(std::vector& other) { container_.swa const InstructionPoly* CompositeInstruction::getFirstInstructionHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const + bool process_child_composites) const { - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - if (process_child_composites) { for (const auto& instruction : composite_instruction.container_) { - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; if (instruction.isCompositeInstruction()) { - const InstructionPoly* result = getFirstInstructionHelper( - instruction.as(), locate_filter, process_child_composites, false); + const InstructionPoly* result = + getFirstInstructionHelper(instruction.as(), locate_filter, process_child_composites); if (result != nullptr) return result; } @@ -341,7 +330,7 @@ CompositeInstruction::getFirstInstructionHelper(const CompositeInstruction& comp } for (const auto& instruction : composite_instruction.container_) - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; return nullptr; @@ -349,25 +338,19 @@ CompositeInstruction::getFirstInstructionHelper(const CompositeInstruction& comp InstructionPoly* CompositeInstruction::getFirstInstructionHelper(CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) + bool process_child_composites) { - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - if (process_child_composites) { for (auto& instruction : composite_instruction.container_) { - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; if (instruction.isCompositeInstruction()) { - InstructionPoly* result = getFirstInstructionHelper( - instruction.as(), locate_filter, process_child_composites, false); + InstructionPoly* result = + getFirstInstructionHelper(instruction.as(), locate_filter, process_child_composites); if (result != nullptr) return result; } @@ -377,7 +360,7 @@ InstructionPoly* CompositeInstruction::getFirstInstructionHelper(CompositeInstru } for (auto& instruction : composite_instruction.container_) - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) return &instruction; return nullptr; @@ -385,128 +368,95 @@ InstructionPoly* CompositeInstruction::getFirstInstructionHelper(CompositeInstru const InstructionPoly* CompositeInstruction::getLastInstructionHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const + bool process_child_composites) const { if (process_child_composites) { for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) { - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); if (it->isCompositeInstruction()) { const InstructionPoly* result = - getLastInstructionHelper(it->as(), locate_filter, process_child_composites, false); + getLastInstructionHelper(it->as(), locate_filter, process_child_composites); if (result != nullptr) return result; } } - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } InstructionPoly* CompositeInstruction::getLastInstructionHelper(CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) + bool process_child_composites) { if (process_child_composites) { for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) { - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); if (it->isCompositeInstruction()) { InstructionPoly* result = - getLastInstructionHelper(it->as(), locate_filter, process_child_composites, false); + getLastInstructionHelper(it->as(), locate_filter, process_child_composites); if (result != nullptr) return result; } } - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } for (auto it = composite_instruction.container_.rbegin(); it != composite_instruction.container_.rend(); ++it) - if (!locate_filter || locate_filter(*it, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(*it, composite_instruction)) return &(*it); - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - return &(composite_instruction.start_instruction_); - return nullptr; } long CompositeInstruction::getInstructionCountHelper(const CompositeInstruction& composite_instruction, const locateFilterFn& locate_filter, - bool process_child_composites, - bool first_composite) const + bool process_child_composites) const { long cnt = 0; - if (composite_instruction.hasStartInstruction()) - if (!locate_filter || - locate_filter(composite_instruction.start_instruction_, composite_instruction, first_composite)) - ++cnt; if (process_child_composites) { for (const auto& instruction : composite_instruction.container_) { - if (!locate_filter || locate_filter(instruction, composite_instruction, first_composite)) + if (!locate_filter || locate_filter(instruction, composite_instruction)) ++cnt; if (instruction.isCompositeInstruction()) - cnt += getInstructionCountHelper( - instruction.as(), locate_filter, process_child_composites, false); + cnt += + getInstructionCountHelper(instruction.as(), locate_filter, process_child_composites); } return cnt; } - cnt += std::count_if( - composite_instruction.container_.begin(), composite_instruction.container_.end(), [=](const auto& i) { - return (!locate_filter || locate_filter(i, composite_instruction, first_composite)); - }); + cnt += std::count_if(composite_instruction.container_.begin(), + composite_instruction.container_.end(), + [=](const auto& i) { return (!locate_filter || locate_filter(i, composite_instruction)); }); return cnt; } void CompositeInstruction::flattenHelper(std::vector>& flattened, CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite) + const flattenFilterFn& filter) { - if (composite.hasStartInstruction()) - if (!filter || filter(composite.start_instruction_, composite, first_composite)) - flattened.emplace_back(composite.start_instruction_); - for (auto& i : composite.container_) { if (i.isCompositeInstruction()) @@ -514,12 +464,12 @@ void CompositeInstruction::flattenHelper(std::vector(), filter, false); + flattenHelper(flattened, i.as(), filter); } - else if (!filter || (filter && filter(i, composite, first_composite))) + else if (!filter || (filter && filter(i, composite))) { flattened.emplace_back(i); } @@ -528,13 +478,8 @@ void CompositeInstruction::flattenHelper(std::vector>& flattened, const CompositeInstruction& composite, - const flattenFilterFn& filter, - bool first_composite) const + const flattenFilterFn& filter) const { - if (composite.hasStartInstruction()) - if (!filter || filter(composite.start_instruction_, composite, first_composite)) - flattened.emplace_back(composite.start_instruction_); - for (const auto& i : composite.container_) { if (i.isCompositeInstruction()) @@ -542,12 +487,12 @@ void CompositeInstruction::flattenHelper(std::vector(), filter, false); + flattenHelper(flattened, i.as(), filter); } - else if (!filter || filter(i, composite, first_composite)) + else if (!filter || filter(i, composite)) { flattened.emplace_back(i); } @@ -563,7 +508,6 @@ void CompositeInstruction::serialize(Archive& ar, const unsigned int /*version*/ ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_); ar& boost::serialization::make_nvp("profile", profile_); ar& boost::serialization::make_nvp("order", order_); - ar& boost::serialization::make_nvp("start_instruction", start_instruction_); ar& boost::serialization::make_nvp("container", container_); } diff --git a/tesseract_command_language/src/poly/move_instruction_poly.cpp b/tesseract_command_language/src/poly/move_instruction_poly.cpp index 1d818930c63..b11cd0addda 100644 --- a/tesseract_command_language/src/poly/move_instruction_poly.cpp +++ b/tesseract_command_language/src/poly/move_instruction_poly.cpp @@ -154,11 +154,6 @@ bool tesseract_planning::MoveInstructionPoly::isCircular() const return (getInterface().getMoveType() == MoveInstructionType::CIRCULAR); } -bool tesseract_planning::MoveInstructionPoly::isStart() const -{ - return (getInterface().getMoveType() == MoveInstructionType::START); -} - bool tesseract_planning::MoveInstructionPoly::isChild() const { return (!getInterface().getParentUUID().is_nil()); } template diff --git a/tesseract_command_language/src/utils.cpp b/tesseract_command_language/src/utils.cpp index e591d4282a9..8cb341f6503 100644 --- a/tesseract_command_language/src/utils.cpp +++ b/tesseract_command_language/src/utils.cpp @@ -37,18 +37,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { static const tesseract_planning::locateFilterFn toJointTrajectoryInstructionFilter = - [](const tesseract_planning::InstructionPoly& i, - const tesseract_planning::CompositeInstruction& /*composite*/, - bool parent_is_first_composite) { - if (i.isMoveInstruction()) - { - if (i.as().isStart()) - return (parent_is_first_composite); - - return true; - } - - return false; + [](const tesseract_planning::InstructionPoly& i, const tesseract_planning::CompositeInstruction& /*composite*/) { + return i.isMoveInstruction(); }; tesseract_common::JointTrajectory toJointTrajectory(const InstructionPoly& instruction) diff --git a/tesseract_command_language/test/move_instruction_unit.cpp b/tesseract_command_language/test/move_instruction_unit.cpp index 567d017085a..967fec8762e 100644 --- a/tesseract_command_language/test/move_instruction_unit.cpp +++ b/tesseract_command_language/test/move_instruction_unit.cpp @@ -45,11 +45,11 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, constructor) // NOLINT // Minimum arguments { - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); - EXPECT_TRUE(instr.getPathProfile().empty()); + EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -79,11 +79,11 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, constructor) // NOLINT // With plan profile { - MoveInstruction instr(swp, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction instr(swp, MoveInstructionType::CIRCULAR, "TEST_PROFILE"); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); - EXPECT_TRUE(instr.getPathProfile().empty()); + EXPECT_EQ(instr.getPathProfile(), "TEST_PROFILE"); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -113,9 +113,9 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, constructor) // NOLINT // With plan and path profile { - MoveInstruction instr(swp, MoveInstructionType::START, "TEST_PROFILE", "TEST_PATH_PROFILE"); + MoveInstruction instr(swp, MoveInstructionType::CIRCULAR, "TEST_PROFILE", "TEST_PATH_PROFILE"); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); EXPECT_FALSE(instr.getDescription().empty()); @@ -152,9 +152,9 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, setters) // NOLINT std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_TRUE(instr.getPathProfile().empty()); EXPECT_FALSE(instr.getDescription().empty()); @@ -192,9 +192,9 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, UUID) // NOLINT std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getWaypoint(), swp); - EXPECT_EQ(instr.getMoveType(), MoveInstructionType::START); + EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_TRUE(instr.getPathProfile().empty()); EXPECT_FALSE(instr.getDescription().empty()); @@ -219,7 +219,7 @@ TEST(TesseractCommandLanguageMoveInstructionUnit, boostSerialization) // NOLINT std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); instr.setMoveType(MoveInstructionType::LINEAR); instr.setProfile("TEST_PROFILE"); instr.setPathProfile("TEST_PATH_PROFILE"); @@ -245,7 +245,7 @@ TEST(TesseractCommandLanguageMoveInstructionPolyUnit, boostSerialization) // NO std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; StateWaypointPoly swp(StateWaypoint(jn, jv)); - MoveInstruction instr(swp, MoveInstructionType::START); + MoveInstruction instr(swp, MoveInstructionType::FREESPACE); instr.setMoveType(MoveInstructionType::LINEAR); instr.setProfile("TEST_PROFILE"); instr.setPathProfile("TEST_PATH_PROFILE"); diff --git a/tesseract_command_language/test/serialize_test.cpp b/tesseract_command_language/test/serialize_test.cpp index dfdf1512357..310fceede1b 100644 --- a/tesseract_command_language/test/serialize_test.cpp +++ b/tesseract_command_language/test/serialize_test.cpp @@ -55,8 +55,8 @@ CompositeInstruction getProgram() // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly wp0{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile"); + start_instruction.setDescription("Start Instruction"); // Define raster poses CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * @@ -85,8 +85,9 @@ CompositeInstruction getProgram() plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); { CompositeInstruction raster_segment; @@ -97,7 +98,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -112,9 +113,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -126,7 +127,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -141,9 +142,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -155,7 +156,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); @@ -163,26 +164,26 @@ CompositeInstruction getProgram() CompositeInstruction to_end; to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); // Add a wait instruction WaitInstruction wait_instruction_time(1.5); - program.appendInstruction(wait_instruction_time); + program.push_back(wait_instruction_time); WaitInstruction wait_instruction_io(WaitInstructionType::DIGITAL_INPUT_LOW, 10); - program.appendInstruction(wait_instruction_io); + program.push_back(wait_instruction_io); // Add a timer instruction TimerInstruction timer_instruction(TimerInstructionType::DIGITAL_OUTPUT_LOW, 3.1, 5); - program.appendInstruction(timer_instruction); + program.push_back(timer_instruction); // Add a set tool instruction SetToolInstruction set_tool_instruction(5); - program.appendInstruction(set_tool_instruction); + program.push_back(set_tool_instruction); // Add a set tool instruction SetAnalogInstruction set_analog_instruction("R", 0, 1.5); - program.appendInstruction(set_analog_instruction); + program.push_back(set_analog_instruction); return program; } diff --git a/tesseract_command_language/test/type_erasure_benchmark.cpp b/tesseract_command_language/test/type_erasure_benchmark.cpp index 8d99079254f..0f4568ae206 100644 --- a/tesseract_command_language/test/type_erasure_benchmark.cpp +++ b/tesseract_command_language/test/type_erasure_benchmark.cpp @@ -57,8 +57,8 @@ CompositeInstruction getProgram() // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly wp0{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile"); + start_instruction.setDescription("Start Instruction"); // Define raster poses CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * @@ -87,8 +87,9 @@ CompositeInstruction getProgram() plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); { CompositeInstruction raster_segment; @@ -99,7 +100,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -114,9 +115,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -128,7 +129,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -143,9 +144,9 @@ CompositeInstruction getProgram() CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -157,7 +158,7 @@ CompositeInstruction getProgram() raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); @@ -165,26 +166,26 @@ CompositeInstruction getProgram() CompositeInstruction to_end; to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); // Add a wait instruction WaitInstruction wait_instruction_time(1.5); - program.appendInstruction(wait_instruction_time); + program.push_back(wait_instruction_time); WaitInstruction wait_instruction_io(WaitInstructionType::DIGITAL_INPUT_LOW, 10); - program.appendInstruction(wait_instruction_io); + program.push_back(wait_instruction_io); // Add a timer instruction TimerInstruction timer_instruction(TimerInstructionType::DIGITAL_OUTPUT_LOW, 3.1, 5); - program.appendInstruction(timer_instruction); + program.push_back(timer_instruction); // Add a set tool instruction SetToolInstruction set_tool_instruction(5); - program.appendInstruction(set_tool_instruction); + program.push_back(set_tool_instruction); // Add a set tool instruction SetAnalogInstruction set_analog_instruction("R", 0, 1.5); - program.appendInstruction(set_analog_instruction); + program.push_back(set_analog_instruction); return program; } @@ -236,7 +237,7 @@ static void BM_MoveInstructionCreation(benchmark::State& state) { CartesianWaypointPoly w{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; for (auto _ : state) - MoveInstruction i(w, MoveInstructionType::START); + MoveInstruction i(w, MoveInstructionType::FREESPACE); } BENCHMARK(BM_MoveInstructionCreation); diff --git a/tesseract_command_language/test/utils_test.cpp b/tesseract_command_language/test/utils_test.cpp index 67a5a7522c7..296a07fc73c 100644 --- a/tesseract_command_language/test/utils_test.cpp +++ b/tesseract_command_language/test/utils_test.cpp @@ -63,9 +63,9 @@ TEST(TesseractCommandLanguageUtilsUnit, flatten) // NOLINT std::to_string(k)); sub_sub_composite.appendMoveInstruction(instruction); } - sub_composite.appendInstruction(sub_sub_composite); + sub_composite.push_back(sub_sub_composite); } - composite.appendInstruction(sub_composite); + composite.push_back(sub_composite); } // flatten(composite); @@ -112,7 +112,7 @@ TEST(TesseractCommandLanguageUtilsUnit, flatten) // NOLINT composite.print(); // flatten the composite keeping the composite instructions - flattenFilterFn filter = [](const InstructionPoly&, const CompositeInstruction&, bool) { return true; }; + flattenFilterFn filter = [](const InstructionPoly&, const CompositeInstruction&) { return true; }; std::vector> flattened = composite.flatten(filter); EXPECT_EQ(flattened.size(), i_max * j_max * k_max + 16); // Add 16 for the composite instructions diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index c7675c2f0b6..b271c919811 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -157,8 +157,8 @@ bool BasicCartesianExample::run() // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile"); + start_instruction.setDescription("Start Instruction"); // Create cartesian waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.5, -0.2, 0.62) * @@ -179,6 +179,7 @@ bool BasicCartesianExample::run() plan_f1.setDescription("to_end_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); program.appendMoveInstruction(plan_c0); program.appendMoveInstruction(plan_f1); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 87b2736f5a7..16f5b0fe7c6 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -278,14 +278,15 @@ bool CarSeatExample::run() StateWaypointPoly wp1{ StateWaypoint(joint_group->getJointNames(), pick_pose) }; // Start Joint Position for the program - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics @@ -362,14 +363,15 @@ bool CarSeatExample::run() StateWaypointPoly wp1{ StateWaypoint(joint_group->getJointNames(), pick_pose) }; // Start Joint Position for the program - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 7d6908d6327..52de24e0638 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -137,14 +137,15 @@ bool FreespaceHybridExample::run() StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("freespace_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 0b7aebeb65b..446a3d13e26 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -137,14 +137,15 @@ bool FreespaceOMPLExample::run() StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); plan_f0.setDescription("freespace_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index a1caa91dc40..03483a27e60 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -151,8 +151,8 @@ bool GlassUprightExample::run() StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; StateWaypointPoly wp1{ StateWaypoint(joint_names, joint_end_pos) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::LINEAR, "UPRIGHT"); + start_instruction.setDescription("Start Instruction"); // Plan freespace from start // Assign a linear motion so cartesian is defined as the target @@ -160,6 +160,7 @@ bool GlassUprightExample::run() plan_f0.setDescription("freespace_plan"); // Add Instructions to program + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); // Print Diagnostics diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 3d4306f3795..330d0fa9eb7 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -152,8 +152,8 @@ bool PickAndPlaceExample::run() ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); StateWaypointPoly pick_swp{ StateWaypoint(joint_names, joint_pos) }; - MoveInstruction start_instruction(pick_swp, MoveInstructionType::START); - pick_program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(pick_swp, MoveInstructionType::FREESPACE, "FREESPACE"); + start_instruction.setDescription("Start Instruction"); // Define the final pose (on top of the box) Eigen::Isometry3d pick_final_pose; @@ -176,6 +176,7 @@ bool PickAndPlaceExample::run() pick_plan_a1.setDescription("Pick Approach"); // Add Instructions to program + pick_program.appendMoveInstruction(start_instruction); pick_program.appendMoveInstruction(pick_plan_a0); pick_program.appendMoveInstruction(pick_plan_a1); @@ -305,10 +306,6 @@ bool PickAndPlaceExample::run() CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", LINK_BASE_NAME, LINK_END_EFFECTOR_NAME)); - MoveInstructionPoly place_start_instruction(*pick_final_state); - place_start_instruction.setMoveType(MoveInstructionType::START); - place_program.setStartInstruction(place_start_instruction); - // Define the approach pose CartesianWaypointPoly place_wp0{ CartesianWaypoint(retreat_pose) }; @@ -331,6 +328,7 @@ bool PickAndPlaceExample::run() place_plan_a2.setDescription("Place approach"); // Add Instructions to program + place_program.appendMoveInstruction(*pick_final_state); place_program.appendMoveInstruction(place_plan_a0); place_program.appendMoveInstruction(place_plan_a1); place_program.appendMoveInstruction(place_plan_a2); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index d18c21af983..fc3101ad88c 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -172,12 +172,7 @@ bool PuzzlePieceAuxillaryAxesExample::run() CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi); // Create cartesian waypoint - CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[0]) }; - MoveInstruction plan_instruction(wp, MoveInstructionType::START, "CARTESIAN"); - plan_instruction.setDescription("from_start_plan"); - program.setStartInstruction(plan_instruction); - - for (std::size_t i = 1; i < tool_poses.size(); ++i) + for (std::size_t i = 0; i < tool_poses.size(); ++i) { CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[i]) }; MoveInstruction plan_instruction(wp, MoveInstructionType::LINEAR, "CARTESIAN"); diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 6b2dccaef82..941bac6c22f 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -167,12 +167,7 @@ bool PuzzlePieceExample::run() CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi); // Create cartesian waypoint - CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[0]) }; - MoveInstruction plan_instruction(wp, MoveInstructionType::START, "CARTESIAN"); - plan_instruction.setDescription("from_start_plan"); - program.setStartInstruction(plan_instruction); - - for (std::size_t i = 1; i < tool_poses.size(); ++i) + for (std::size_t i = 0; i < tool_poses.size(); ++i) { CartesianWaypointPoly wp{ CartesianWaypoint(tool_poses[i]) }; MoveInstruction plan_instruction(wp, MoveInstructionType::LINEAR, "CARTESIAN"); diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h index 9b31e7120a4..36a5a26bbe9 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h @@ -69,10 +69,6 @@ class SimpleMotionPlanner : public MotionPlanner MotionPlanner::Ptr clone() const override; protected: - static MoveInstructionPoly getStartInstruction(const PlannerRequest& request, - const tesseract_scene_graph::SceneState& current_state, - const tesseract_kinematics::JointGroup& manip); - CompositeInstruction processCompositeInstruction(const CompositeInstruction& instructions, MoveInstructionPoly& prev_instruction, MoveInstructionPoly& prev_seed, diff --git a/tesseract_motion_planners/core/src/core/utils.cpp b/tesseract_motion_planners/core/src/core/utils.cpp index 1a5f128fbe7..78a3c444a4e 100644 --- a/tesseract_motion_planners/core/src/core/utils.cpp +++ b/tesseract_motion_planners/core/src/core/utils.cpp @@ -164,9 +164,6 @@ tesseract_common::Toolpath toToolpath(const MoveInstructionPoly& mi, const tesse void assignCurrentStateAsSeed(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env) { - if (!composite_instructions.hasStartInstruction()) - throw std::runtime_error("Top most composite instruction is missing start instruction!"); - std::unordered_map manip_joint_state; tesseract_scene_graph::SceneState state = env.getState(); const tesseract_common::ManipulatorInfo& global_mi = composite_instructions.getManipulatorInfo(); @@ -240,42 +237,10 @@ bool formatProgramHelper(CompositeInstruction& composite_instructions, bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env) { - if (!composite_instructions.hasStartInstruction()) - throw std::runtime_error("Top most composite instruction is missing start instruction!"); - std::unordered_map> manip_joint_names; bool format_required = false; tesseract_common::ManipulatorInfo mi = composite_instructions.getManipulatorInfo(); - std::unordered_map manipulators; - - if (composite_instructions.hasStartInstruction()) - { - auto& pi = composite_instructions.getStartInstruction(); - - tesseract_common::ManipulatorInfo start_mi = mi.getCombined(pi.getManipulatorInfo()); - - std::vector joint_names; - auto it = manip_joint_names.find(start_mi.manipulator); - if (it == manip_joint_names.end()) - { - joint_names = env.getGroupJointNames(start_mi.manipulator); - manip_joint_names[start_mi.manipulator] = joint_names; - } - else - { - joint_names = it->second; - } - - if (pi.getWaypoint().isStateWaypoint() || pi.getWaypoint().isJointWaypoint()) - { - if (formatJointPosition(joint_names, pi.getWaypoint())) - format_required = true; - } - } - else - throw std::runtime_error("Top most composite instruction start instruction has invalid waypoint type!"); - if (formatProgramHelper(composite_instructions, env, mi, manip_joint_names)) format_required = true; diff --git a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp index 821404db2a6..a894e202a61 100644 --- a/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp +++ b/tesseract_motion_planners/core/src/simple/simple_motion_planner.cpp @@ -77,18 +77,14 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const // Create seed CompositeInstruction seed; - // Get the start waypoint/instruction - MoveInstructionPoly start_instruction = getStartInstruction(request, request.env_state, *manip); - - // Set start instruction - MoveInstructionPoly start_instruction_seed = start_instruction; - start_instruction_seed.setMoveType(MoveInstructionType::START); + // Place holder for start instruction + MoveInstructionPoly null_instruction; // Process the instructions into the seed try { - MoveInstructionPoly start_instruction_copy = start_instruction; - MoveInstructionPoly start_instruction_seed_copy = start_instruction_seed; + MoveInstructionPoly start_instruction_copy = null_instruction; + MoveInstructionPoly start_instruction_seed_copy = null_instruction; seed = processCompositeInstruction(request.instructions, start_instruction_copy, start_instruction_seed_copy, request); } @@ -100,9 +96,6 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const return response; } - // Set seed start state - seed.setStartInstruction(start_instruction_seed); - // Fill out the response response.results = seed; @@ -135,31 +128,6 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const return response; } -MoveInstructionPoly SimpleMotionPlanner::getStartInstruction(const PlannerRequest& request, - const tesseract_scene_graph::SceneState& current_state, - const tesseract_kinematics::JointGroup& manip) -{ - // Get start instruction - MoveInstructionPoly start_instruction = request.instructions.getStartInstruction(); - assert(start_instruction.isStart()); - auto& start_waypoint = start_instruction.getWaypoint(); - if (start_waypoint.isJointWaypoint() || start_waypoint.isStateWaypoint()) - { - assert(checkJointPositionFormat(manip.getJointNames(), start_waypoint)); - return start_instruction; - } - - if (start_waypoint.isCartesianWaypoint()) - { - /** @todo Update to run IK to find solution closest to start */ - start_waypoint.as().setSeed( - tesseract_common::JointState(manip.getJointNames(), current_state.getJointValues(manip.getJointNames()))); - return start_instruction; - } - - throw std::runtime_error("Unsupported waypoint type!"); -} - CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const CompositeInstruction& instructions, MoveInstructionPoly& prev_instruction, MoveInstructionPoly& prev_seed, @@ -174,12 +142,38 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp if (instruction.isCompositeInstruction()) { - seed.appendInstruction( + seed.push_back( processCompositeInstruction(instruction.as(), prev_instruction, prev_seed, request)); } else if (instruction.isMoveInstruction()) { const auto& base_instruction = instruction.as(); + if (prev_instruction.isNull()) + { + const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; + const std::string manipulator_ik_solver = request.instructions.getManipulatorInfo().manipulator_ik_solver; + tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); + + prev_instruction = base_instruction; + auto& start_waypoint = prev_instruction.getWaypoint(); + if (start_waypoint.isJointWaypoint() || start_waypoint.isStateWaypoint()) + { + assert(checkJointPositionFormat(manip->getJointNames(), start_waypoint)); + } + else if (start_waypoint.isCartesianWaypoint()) + { + /** @todo Update to run IK to find solution closest to start */ + start_waypoint.as().setSeed(tesseract_common::JointState( + manip->getJointNames(), request.env_state.getJointValues(manip->getJointNames()))); + } + else + { + throw std::runtime_error("Unsupported waypoint type!"); + } + prev_seed = prev_instruction; + seed.push_back(prev_instruction); + continue; + } // Get the next plan instruction if it exists InstructionPoly next_instruction; @@ -222,14 +216,14 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp request.instructions.getManipulatorInfo()); for (const auto& instr : instruction_seed) - seed.appendInstruction(instr); + seed.push_back(instr); prev_instruction = base_instruction; prev_seed = instruction_seed.back(); } else { - seed.appendInstruction(instruction); + seed.push_back(instruction); } } // for (const auto& instruction : instructions) return seed; diff --git a/tesseract_motion_planners/examples/chain_example.cpp b/tesseract_motion_planners/examples/chain_example.cpp index 14dfbac6784..a334068cce8 100644 --- a/tesseract_motion_planners/examples/chain_example.cpp +++ b/tesseract_motion_planners/examples/chain_example.cpp @@ -111,7 +111,7 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Plan Instructions - MoveInstruction start_instruction(wp0, MoveInstructionType::START); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "DEFAULT"); MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "DEFAULT"); MoveInstruction plan_c1(wp2, MoveInstructionType::LINEAR, "DEFAULT"); MoveInstruction plan_c2(wp3, MoveInstructionType::LINEAR, "DEFAULT"); @@ -122,8 +122,8 @@ int main(int /*argc*/, char** /*argv*/) // Create program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); program.appendMoveInstruction(plan_c1); program.appendMoveInstruction(plan_c2); diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 3ad79107278..fa47e8e4694 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -94,13 +94,13 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Plan Instructions - MoveInstruction start_instruction(wp0, MoveInstructionType::START); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "DEFAULT"); MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "DEFAULT"); // Create program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Plot Program diff --git a/tesseract_motion_planners/examples/raster_example.cpp b/tesseract_motion_planners/examples/raster_example.cpp index d802d3dff2d..f2999dbc589 100644 --- a/tesseract_motion_planners/examples/raster_example.cpp +++ b/tesseract_motion_planners/examples/raster_example.cpp @@ -92,8 +92,8 @@ int main(int /*argc*/, char** /*argv*/) // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(kin_group->getJointNames(), Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp0, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE); + start_instruction.setDescription("Start"); // Define raster poses CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * @@ -123,8 +123,9 @@ int main(int /*argc*/, char** /*argv*/) plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); { CompositeInstruction raster_segment; @@ -135,7 +136,7 @@ int main(int /*argc*/, char** /*argv*/) raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -150,9 +151,9 @@ int main(int /*argc*/, char** /*argv*/) CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -164,7 +165,7 @@ int main(int /*argc*/, char** /*argv*/) raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } { @@ -179,9 +180,9 @@ int main(int /*argc*/, char** /*argv*/) CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); - transitions.appendInstruction(transition_from_start); - transitions.appendInstruction(transition_from_end); - program.appendInstruction(transitions); + transitions.push_back(transition_from_start); + transitions.push_back(transition_from_end); + program.push_back(transitions); } { @@ -193,7 +194,7 @@ int main(int /*argc*/, char** /*argv*/) raster_segment.appendMoveInstruction(plan_c3); raster_segment.appendMoveInstruction(plan_c4); raster_segment.appendMoveInstruction(plan_c5); - program.appendInstruction(raster_segment); + program.push_back(raster_segment); } MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); @@ -201,7 +202,7 @@ int main(int /*argc*/, char** /*argv*/) CompositeInstruction to_end; to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); // Plot Program auto state_solver = env->getStateSolver(); diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index b858fdf51de..e76cf679eed 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -248,16 +248,6 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const bool found{ false }; Eigen::Index row{ 0 }; - if (start_index == 0) - { - MoveInstructionPoly& mi = response.results.getStartInstruction(); - if (mi.getUUID() == pc.start_uuid) - { - assignSolution(mi, joint_names, traj.row(row++), request.format_result_as_input); - found = true; - } - } - auto& ci = response.results.getInstructions(); for (auto it = ci.begin() + static_cast(start_index); it != ci.end(); ++it) { diff --git a/tesseract_motion_planners/test/descartes_planner_tests.cpp b/tesseract_motion_planners/test/descartes_planner_tests.cpp index 2062db24095..c0785857b75 100644 --- a/tesseract_motion_planners/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/test/descartes_planner_tests.cpp @@ -99,15 +99,15 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE", manip); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -211,15 +211,15 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE", manip); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -314,15 +314,15 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) Eigen::Quaterniond(0, 0, -1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE", manip); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 260beb09d78..c7ce701a0c7 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -161,7 +161,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT Eigen::Map(end_state.data(), static_cast(end_state.size()))) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); @@ -169,8 +169,8 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); program.appendMoveInstruction(plan_f2); @@ -211,9 +211,8 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT } EXPECT_TRUE(&planner_response); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 21); // 10 per segment + start a instruction - EXPECT_EQ(planner_response.results.size(), 20); + EXPECT_EQ(planner_response.results.size(), 21); EXPECT_TRUE(wp1.getPosition().isApprox( getJointPosition(planner_response.results.getFirstMoveInstruction()->getWaypoint()), 1e-5)); @@ -237,12 +236,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT wp1.setPosition(Eigen::Map(swp.data(), static_cast(swp.size()))); wp1.setNames(joint_group->getJointNames()); - start_instruction = MoveInstruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + start_instruction = MoveInstruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a new program program = CompositeInstruction(); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a new seed @@ -267,15 +266,15 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT wp2.setNames(joint_group->getJointNames()); // Define Start Instruction - start_instruction = MoveInstruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + start_instruction = MoveInstruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions plan_f1 = MoveInstruction(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a new program program = CompositeInstruction(); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a new seed @@ -326,15 +325,15 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT CartesianWaypointPoly wp2{ CartesianWaypoint(goal) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -373,7 +372,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT CONSOLE_BRIDGE_logError("CI Error: %s", planner_response.message.c_str()); } EXPECT_TRUE(&planner_response); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 11); EXPECT_TRUE(wp1.getPosition().isApprox( getJointPosition(planner_response.results.getFirstMoveInstruction()->getWaypoint()), 1e-5)); @@ -420,15 +418,15 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT Eigen::Map(end_state.data(), static_cast(end_state.size()))) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program; - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -468,7 +466,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT } EXPECT_TRUE(&planner_response); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); EXPECT_EQ(planner_response.results.getMoveInstructionCount(), 11); EXPECT_TRUE(wp2.getPosition().isApprox( getJointPosition(planner_response.results.getLastMoveInstruction()->getWaypoint()), 1e-5)); diff --git a/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp index d7d01de3c47..93162603c98 100644 --- a/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/test/simple_planner_fixed_size_assign_position.cpp @@ -69,7 +69,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian request.env = env_; request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -111,7 +111,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint request.env = env_; request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -152,7 +152,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte request.env = env_; request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); diff --git a/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp index 244d11a04d0..47853570df1 100644 --- a/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/test/simple_planner_fixed_size_interpolation.cpp @@ -69,7 +69,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join request.env = env_; request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -111,7 +111,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint request.env = env_; request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -158,7 +158,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -201,7 +201,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointI request.env_state = env_->getState(); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); diff --git a/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp index 7fd88aa4a7e..5d4a0af16af 100644 --- a/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp @@ -70,7 +70,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -128,7 +128,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -197,7 +197,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -254,7 +254,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo auto joint_group = request.env->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -325,7 +325,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -382,7 +382,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -451,7 +451,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); @@ -510,7 +510,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo CartesianWaypointPoly wp1{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; - MoveInstruction instr1(wp1, MoveInstructionType::START, "TEST_PROFILE", manip_info_); + MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); diff --git a/tesseract_motion_planners/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/test/trajopt_planner_tests.cpp index 5e1bfed2722..05e03259462 100644 --- a/tesseract_motion_planners/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/test/trajopt_planner_tests.cpp @@ -117,15 +117,15 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N wp2.getPosition() << 0, 0, 0, 1.57, 0, 0, 0; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -192,15 +192,15 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT wp2.getPosition() << 0, 0, 0, 1.57, 0, 0, 0; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -272,15 +272,15 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT Eigen::Quaterniond(0, 0, 1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Define Plan Instructions MoveInstruction plan_f1(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE"); // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -355,7 +355,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT wp2.getPosition() << 0, 0, 0, -1.57, 0, 0, 0; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); start_instruction.getManipulatorInfo().working_frame = "base_link"; // Define Plan Instructions @@ -363,8 +363,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -438,7 +438,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT Eigen::Quaterniond(0, 0, 1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE"); start_instruction.getManipulatorInfo().working_frame = "base_link"; // Define Plan Instructions @@ -447,8 +447,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -522,7 +522,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL Eigen::Quaterniond(0, 0, 1.0, 0)) }; // Define Start Instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START, "TEST_PROFILE"); + MoveInstruction start_instruction(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE"); start_instruction.getManipulatorInfo().working_frame = "base_link"; // Define Plan Instructions @@ -531,8 +531,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Create a program CompositeInstruction program("TEST_PROFILE"); - program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f1); // Create a seed @@ -613,18 +613,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // Specify a Joint Waypoint as the finish JointWaypointPoly wp{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; wp.getPosition() << 0, 0, 0, -1.57 + ind * 0.1, 0, 0, 0; - if (ind == 0) - { - // Define Start Instruction - MoveInstruction start_instruction(wp, MoveInstructionType::START, "TEST_PROFILE"); - program.setStartInstruction(start_instruction); - } - else - { - wp.setNames(joint_names); - MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); - program.appendMoveInstruction(plan_f); - } + MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); + program.appendMoveInstruction(plan_f); } // Create a seed @@ -683,17 +673,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT // Specify a Joint Waypoint as the finish JointWaypointPoly wp{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; wp.getPosition() << 0, 0, 0, -1.57 + ind * 0.1, 0, 0, 0; - if (ind == 0) - { - // Define Start Instruction - MoveInstruction start_instruction(wp, MoveInstructionType::START, "TEST_PROFILE"); - program.setStartInstruction(start_instruction); - } - else - { - MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); - program.appendMoveInstruction(plan_f); - } + MoveInstruction plan_f(wp, MoveInstructionType::FREESPACE, "TEST_PROFILE"); + program.appendMoveInstruction(plan_f); } // Create a seed diff --git a/tesseract_task_composer/examples/freespace_example_program.h b/tesseract_task_composer/examples/freespace_example_program.h index a275b3e3879..9d61ba02318 100644 --- a/tesseract_task_composer/examples/freespace_example_program.h +++ b/tesseract_task_composer/examples/freespace_example_program.h @@ -27,13 +27,14 @@ inline CompositeInstruction freespaceExampleProgramIIWA( std::vector joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6", "joint_a7" }; StateWaypointPoly wp1{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, freespace_profile); + start_instruction.setDescription("Start Instruction"); // Define target pose CartesianWaypointPoly wp2{ CartesianWaypoint(goal) }; MoveInstruction plan_f0(wp2, MoveInstructionType::FREESPACE, freespace_profile); plan_f0.setDescription("freespace_motion"); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); JointWaypointPoly wp3{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -54,13 +55,14 @@ inline CompositeInstruction freespaceExampleProgramABB( // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly wp1{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE, freespace_profile); + start_instruction.setDescription("Start Instruction"); // Define target pose CartesianWaypointPoly wp2{ CartesianWaypoint(goal) }; MoveInstruction plan_f0(wp2, MoveInstructionType::FREESPACE, freespace_profile); plan_f0.setDescription("freespace_motion"); + program.appendMoveInstruction(start_instruction); program.appendMoveInstruction(plan_f0); JointWaypointPoly wp3{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; diff --git a/tesseract_task_composer/examples/raster_example_program.h b/tesseract_task_composer/examples/raster_example_program.h index 0535e21aabf..8a800a837e3 100644 --- a/tesseract_task_composer/examples/raster_example_program.h +++ b/tesseract_task_composer/examples/raster_example_program.h @@ -47,8 +47,8 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypointPoly swp1{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; - MoveInstruction start_instruction(swp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(swp1, MoveInstructionType::FREESPACE, freespace_profile); + start_instruction.setDescription("Start"); CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -58,8 +58,9 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start(freespace_profile); from_start.setDescription("from_start"); + from_start.appendMoveInstruction(start_instruction); from_start.appendMoveInstruction(plan_f0); - program.appendInstruction(from_start); + program.push_back(from_start); // for (int i = 0; i < 4; ++i) @@ -100,7 +101,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr raster_segment.appendMoveInstruction(MoveInstruction(wp2, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp1, MoveInstructionType::LINEAR, process_profile)); } - program.appendInstruction(raster_segment); + program.push_back(raster_segment); // Add transition if (i == 0 || i == 2) @@ -115,7 +116,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } else if (i == 1) { @@ -129,7 +130,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } } @@ -138,7 +139,7 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CompositeInstruction to_end(freespace_profile); to_end.setDescription("to_end"); to_end.appendMoveInstruction(plan_f2); - program.appendInstruction(to_end); + program.push_back(to_end); return program; } @@ -149,14 +150,6 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CompositeInstruction program( DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); - CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * - Eigen::Quaterniond(0, 0, -1.0, 0)); - - // Define start instruction - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - start_instruction.setDescription("Start Instruction"); - program.setStartInstruction(start_instruction); - for (int i = 0; i < 4; ++i) { double x = 0.8 + (i * 0.1); @@ -179,6 +172,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { + raster_segment.appendMoveInstruction(MoveInstruction(wp1, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp2, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp3, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp4, MoveInstructionType::LINEAR, process_profile)); @@ -195,7 +189,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac raster_segment.appendMoveInstruction(MoveInstruction(wp2, MoveInstructionType::LINEAR, process_profile)); raster_segment.appendMoveInstruction(MoveInstruction(wp1, MoveInstructionType::LINEAR, process_profile)); } - program.appendInstruction(raster_segment); + program.push_back(raster_segment); // Add transition if (i == 0 || i == 2) @@ -210,7 +204,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } else if (i == 1) { @@ -224,7 +218,7 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CompositeInstruction transition(freespace_profile); transition.setDescription("Transition #" + std::to_string(i + 1)); transition.appendMoveInstruction(plan_f1); - program.appendInstruction(transition); + program.push_back(transition); } } diff --git a/tesseract_task_composer/src/nodes/motion_planner_task.cpp b/tesseract_task_composer/src/nodes/motion_planner_task.cpp index 5d027ed2c0a..76b14474409 100644 --- a/tesseract_task_composer/src/nodes/motion_planner_task.cpp +++ b/tesseract_task_composer/src/nodes/motion_planner_task.cpp @@ -81,9 +81,6 @@ TaskComposerNodeInfo::UPtr MotionPlannerTask::runImpl(TaskComposerInput& input, assert(!(input.problem.manip_info.empty() && instructions.getManipulatorInfo().empty())); instructions.setManipulatorInfo(instructions.getManipulatorInfo().getCombined(input.problem.manip_info)); - // It should always have a start instruction which required by the motion planners - assert(instructions.hasStartInstruction()); - // -------------------- // Fill out request // -------------------- diff --git a/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp index 242fbcf5605..e1cbeb89952 100644 --- a/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ct_motion_task.cpp @@ -109,29 +109,12 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 1) - { - const InstructionPoly& from_start_input_instruction = program[0]; - assert(from_start_input_instruction.isCompositeInstruction()); - const auto& ci = from_start_input_instruction.as(); - const auto* li = ci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - else - { - const InstructionPoly& pre_input_instruction = program[idx - 1]; - assert(pre_input_instruction.isCompositeInstruction()); - const auto& tci = pre_input_instruction.as(); - const auto* li = tci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + raster_input.insertMoveInstruction(raster_input.begin(), *li); auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); @@ -157,6 +140,14 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -181,8 +172,6 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, // Plan from_start - preceded by the first raster auto from_start_input = program[0].as(); - - from_start_input.setStartInstruction(program.getStartInstruction()); from_start_input.setManipulatorInfo(from_start_input.getManipulatorInfo().getCombined(program_manip_info)); auto from_start_pipeline_task = @@ -205,6 +194,14 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, to_end_input.setManipulatorInfo(to_end_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[program.size() - 2]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + to_end_input.insertMoveInstruction(to_end_input.begin(), *li); + auto to_end_pipeline_task = std::make_unique("To End: " + to_end_input.getDescription()); std::string to_end_pipeline_key = to_end_pipeline_task->getUUIDString(); auto to_end_pipeline_uuid = task_graph.addNode(std::move(to_end_pipeline_task)); @@ -237,14 +234,23 @@ TaskComposerNodeInfo::UPtr RasterCtMotionTask::runImpl(TaskComposerInput& input, } program.clear(); - program.appendInstruction(input.data_storage.getData(from_start_pipeline_key).as()); + program.emplace_back(input.data_storage.getData(from_start_pipeline_key).as()); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + segment.erase(segment.begin()); + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } - program.appendInstruction(input.data_storage.getData(to_end_pipeline_key).as()); + CompositeInstruction to_end = input.data_storage.getData(to_end_pipeline_key).as(); + to_end.erase(to_end.begin()); + program.emplace_back(to_end); input.data_storage.setData(output_keys_[0], program); @@ -267,10 +273,6 @@ void RasterCtMotionTask::checkTaskInput(const tesseract_common::AnyPoly& input) const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterCtMotionTask, input should have a start instruction"); - // Check from_start if (!composite.at(0).isCompositeInstruction()) throw std::runtime_error("RasterCtMotionTask, from_start should be a composite"); diff --git a/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp index 1609fb3a39d..1600a84dbb7 100644 --- a/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ct_only_motion_task.cpp @@ -107,26 +107,17 @@ TaskComposerNodeInfo::UPtr RasterCtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); - // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 0) - { - start_instruction = program.getStartInstruction(); - } - else + // Insert Start Plan Instruction + if (idx > 0) { const InstructionPoly& pre_input_instruction = program[idx - 1]; assert(pre_input_instruction.isCompositeInstruction()); const auto& tci = pre_input_instruction.as(); const auto* li = tci.getLastMoveInstruction(); assert(li != nullptr); - start_instruction = *li; + raster_input.insertMoveInstruction(raster_input.begin(), *li); } - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); - auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); std::string raster_pipeline_key = raster_pipeline_task->getUUIDString(); @@ -151,6 +142,13 @@ TaskComposerNodeInfo::UPtr RasterCtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -193,9 +191,18 @@ TaskComposerNodeInfo::UPtr RasterCtOnlyMotionTask::runImpl(TaskComposerInput& in program.clear(); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + if (i != 0) + segment.erase(segment.begin()); + + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } input.data_storage.setData(output_keys_[0], program); @@ -219,10 +226,6 @@ void RasterCtOnlyMotionTask::checkTaskInput(const tesseract_common::AnyPoly& inp const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterCtOnlyMotionTask, input should have a start instruction"); - // Check rasters and transitions for (const auto& i : composite) { diff --git a/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp index 6b39340d0f9..c03f93432a7 100644 --- a/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ft_motion_task.cpp @@ -109,29 +109,12 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 1) - { - const InstructionPoly& from_start_input_instruction = program[0]; - assert(from_start_input_instruction.isCompositeInstruction()); - const auto& ci = from_start_input_instruction.as(); - const auto* li = ci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - else - { - const InstructionPoly& pre_input_instruction = program[idx - 1]; - assert(pre_input_instruction.isCompositeInstruction()); - const auto& tci = pre_input_instruction.as(); - const auto* li = tci.getLastMoveInstruction(); - assert(li != nullptr); - start_instruction = *li; - } - - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + raster_input.insertMoveInstruction(raster_input.begin(), *li); auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); @@ -157,6 +140,14 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -181,8 +172,6 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, // Plan from_start - preceded by the first raster auto from_start_input = program[0].as(); - - from_start_input.setStartInstruction(program.getStartInstruction()); from_start_input.setManipulatorInfo(from_start_input.getManipulatorInfo().getCombined(program_manip_info)); auto from_start_pipeline_task = @@ -205,6 +194,14 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, to_end_input.setManipulatorInfo(to_end_input.getManipulatorInfo().getCombined(program_manip_info)); + // Get Start Plan Instruction + const InstructionPoly& pre_input_instruction = program[program.size() - 2]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + to_end_input.insertMoveInstruction(to_end_input.begin(), *li); + auto to_end_pipeline_task = std::make_unique("To End: " + to_end_input.getDescription()); std::string to_end_pipeline_key = to_end_pipeline_task->getUUIDString(); auto to_end_pipeline_uuid = task_graph.addNode(std::move(to_end_pipeline_task)); @@ -237,14 +234,23 @@ TaskComposerNodeInfo::UPtr RasterFtMotionTask::runImpl(TaskComposerInput& input, } program.clear(); - program.appendInstruction(input.data_storage.getData(from_start_pipeline_key).as()); + program.emplace_back(input.data_storage.getData(from_start_pipeline_key).as()); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + segment.erase(segment.begin()); + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } - program.appendInstruction(input.data_storage.getData(to_end_pipeline_key).as()); + CompositeInstruction to_end = input.data_storage.getData(to_end_pipeline_key).as(); + to_end.erase(to_end.begin()); + program.emplace_back(to_end); input.data_storage.setData(output_keys_[0], program); @@ -267,10 +273,6 @@ void RasterFtMotionTask::checkTaskInput(const tesseract_common::AnyPoly& input) const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterFtMotionTask, input should have a start instruction"); - // Check from_start if (!composite.at(0).isCompositeInstruction()) throw std::runtime_error("RasterFtMotionTask, from_start should be a composite"); diff --git a/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp b/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp index 7f5c2b46bda..7ffb17ffbf5 100644 --- a/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp +++ b/tesseract_task_composer/src/nodes/raster_ft_only_motion_task.cpp @@ -108,26 +108,17 @@ TaskComposerNodeInfo::UPtr RasterFtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info raster_input.setManipulatorInfo(raster_input.getManipulatorInfo().getCombined(program_manip_info)); - // Get Start Plan Instruction - MoveInstructionPoly start_instruction; - if (idx == 0) - { - start_instruction = program.getStartInstruction(); - } - else + // Insert Start Plan Instruction + if (idx > 0) { const InstructionPoly& pre_input_instruction = program[idx - 1]; assert(pre_input_instruction.isCompositeInstruction()); const auto& tci = pre_input_instruction.as(); const auto* li = tci.getLastMoveInstruction(); assert(li != nullptr); - start_instruction = *li; + raster_input.insertMoveInstruction(raster_input.begin(), *li); } - // Update start state - start_instruction.setMoveType(MoveInstructionType::START); - raster_input.setStartInstruction(start_instruction); - auto raster_pipeline_task = std::make_unique( "Raster #" + std::to_string(raster_idx + 1) + ": " + raster_input.getDescription()); std::string raster_pipeline_key = raster_pipeline_task->getUUIDString(); @@ -152,6 +143,13 @@ TaskComposerNodeInfo::UPtr RasterFtOnlyMotionTask::runImpl(TaskComposerInput& in // Set the manipulator info transition_input.setManipulatorInfo(transition_input.getManipulatorInfo().getCombined(program_manip_info)); + const InstructionPoly& pre_input_instruction = program[idx - 1]; + assert(pre_input_instruction.isCompositeInstruction()); + const auto& tci = pre_input_instruction.as(); + const auto* li = tci.getLastMoveInstruction(); + assert(li != nullptr); + transition_input.insertMoveInstruction(transition_input.begin(), *li); + auto transition_pipeline_task = std::make_unique( "Transition #" + std::to_string(transition_idx + 1) + ": " + transition_input.getDescription()); std::string transition_pipeline_key = transition_pipeline_task->getUUIDString(); @@ -194,9 +192,18 @@ TaskComposerNodeInfo::UPtr RasterFtOnlyMotionTask::runImpl(TaskComposerInput& in program.clear(); for (std::size_t i = 0; i < raster_tasks.size(); ++i) { - program.appendInstruction(input.data_storage.getData(raster_tasks[i].second).as()); + CompositeInstruction segment = input.data_storage.getData(raster_tasks[i].second).as(); + if (i != 0) + segment.erase(segment.begin()); + + program.emplace_back(segment); + if (i < raster_tasks.size() - 1) - program.appendInstruction(input.data_storage.getData(transition_keys[i]).as()); + { + CompositeInstruction transition = input.data_storage.getData(transition_keys[i]).as(); + transition.erase(transition.begin()); + program.emplace_back(transition); + } } input.data_storage.setData(output_keys_[0], program); @@ -220,10 +227,6 @@ void RasterFtOnlyMotionTask::checkTaskInput(const tesseract_common::AnyPoly& inp const auto& composite = input.as(); - // Check that it has a start instruction - if (!composite.hasStartInstruction()) - throw std::runtime_error("RasterFtOnlyMotionTask, input should have a start instruction"); - // Check rasters and transitions for (const auto& i : composite) { diff --git a/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp index c73d69b0aeb..2897b0d2aab 100644 --- a/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/src/nodes/time_optimal_parameterization_task.cpp @@ -166,7 +166,6 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer RescaleTimings(unflattened, scaling_factors); } - ci.setStartInstruction(unflattened.getStartInstruction()); for (std::size_t idx = 0; idx < ci.size(); idx++) ci[idx] = unflattened[idx]; } @@ -176,7 +175,6 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer CONSOLE_BRIDGE_logWarn("TOTG Move Profile specified but unflatten is not set in the composite profile. Move " "Profile will be ignored"); - ci.setStartInstruction(resampled.getStartInstruction()); for (std::size_t idx = 0; idx < ci.size(); idx++) ci[idx] = resampled[idx]; } @@ -194,7 +192,6 @@ CompositeInstruction TimeOptimalParameterizationTask::unflatten(const CompositeI double tolerance) { CompositeInstruction unflattened(pattern); - unflattened.setStartInstruction(flattened_input.getStartInstruction()); for (auto& instr : unflattened) instr.as().clear(); @@ -242,7 +239,7 @@ CompositeInstruction TimeOptimalParameterizationTask::unflatten(const CompositeI } // Add flattened point to the subcomposite - unflattened[original_idx].as().appendInstruction(flattened_input.at(resample_idx)); + unflattened[original_idx].as().push_back(flattened_input.at(resample_idx)); // Correct the meta information, taking information from the last element of each composite in the original if (unflattened[original_idx].as().back().isMoveInstruction()) diff --git a/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp b/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp index dd8a545bb4a..fc8b51c800f 100644 --- a/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp +++ b/tesseract_task_composer/src/nodes/update_start_and_end_state_task.cpp @@ -118,8 +118,7 @@ TaskComposerNodeInfo::UPtr UpdateStartAndEndStateTask::runImpl(TaskComposerInput const auto* next_start_move = input_next_data_poly.as().getFirstMoveInstruction(); // Update start instruction - instructions.setStartInstruction(*prev_last_move); - instructions.getStartInstruction().setMoveType(MoveInstructionType::START); + instructions.at(0) = (*prev_last_move); // Update end instruction if (next_start_move->getWaypoint().isCartesianWaypoint()) diff --git a/tesseract_task_composer/src/nodes/update_start_state_task.cpp b/tesseract_task_composer/src/nodes/update_start_state_task.cpp index ada18f45d81..c3296fdbba5 100644 --- a/tesseract_task_composer/src/nodes/update_start_state_task.cpp +++ b/tesseract_task_composer/src/nodes/update_start_state_task.cpp @@ -101,8 +101,7 @@ TaskComposerNodeInfo::UPtr UpdateStartStateTask::runImpl(TaskComposerInput& inpu const auto* prev_last_move = input_prev_data_poly.as().getLastMoveInstruction(); // Update start instruction - instructions.setStartInstruction(*prev_last_move); - instructions.getStartInstruction().setMoveType(MoveInstructionType::START); + instructions.at(0) = (*prev_last_move); // Store results input.data_storage.setData(output_keys_[0], input_data_poly); diff --git a/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp index cb4b605c74f..f6a239bdd35 100644 --- a/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/src/nodes/upsample_trajectory_task.cpp @@ -85,7 +85,7 @@ TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& in cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); assert(cur_composite_profile->longest_valid_segment_length > 0); - InstructionPoly start_instruction = ci.getStartInstruction(); + InstructionPoly start_instruction; CompositeInstruction new_results{ ci }; new_results.clear(); @@ -113,10 +113,16 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, new_cc.clear(); upsample(new_cc, cc, start_instruction, longest_valid_segment_length); - composite.appendInstruction(new_cc); + composite.push_back(new_cc); } else if (i.isMoveInstruction()) { + if (start_instruction.isNull()) + { + start_instruction = i.as(); + continue; + } + assert(start_instruction.isMoveInstruction()); const auto& mi0 = start_instruction.as(); const auto& mi1 = i.as(); @@ -145,7 +151,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, } else { - composite.appendInstruction(i); + composite.push_back(i); } start_instruction = i; @@ -153,7 +159,7 @@ void UpsampleTrajectoryTask::upsample(CompositeInstruction& composite, else { assert(!i.isMoveInstruction()); - composite.appendInstruction(i); + composite.push_back(i); } } } diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index 2511324700e..115d980d4a3 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -49,8 +49,9 @@ CompositeInstruction createProgram(const Eigen::VectorXd& start_state, std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; JointWaypointPoly wp1{ JointWaypoint(joint_names, start_state) }; - MoveInstruction start_instruction(wp1, MoveInstructionType::START); - program.setStartInstruction(start_instruction); + MoveInstruction start_instruction(wp1, MoveInstructionType::FREESPACE); + start_instruction.setDescription("Start Instruction"); + program.appendMoveInstruction(start_instruction); JointWaypointPoly wp2{ JointWaypoint(joint_names, start_state + ((goal_state - start_state) / 2)) }; MoveInstruction plan_f0(wp2, MoveInstructionType::FREESPACE); diff --git a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp index 3884c1f390b..40f3dbc4cfe 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp @@ -75,7 +75,6 @@ TEST_F(TesseractTaskComposerUnit, MinLengthTaskTest) // NOLINT EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Define the Process Input @@ -129,7 +128,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlanPr EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -157,7 +155,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlanPr // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(((pcnt - 1) * 10) + 1, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -170,7 +167,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -197,7 +193,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(98, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -210,7 +205,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanPro EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -237,7 +231,6 @@ TEST_F(TesseractTaskComposerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanPro // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(83, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -247,7 +240,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerFixedSizeAssignPla EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); auto interpolator = std::make_shared(); @@ -273,7 +265,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerFixedSizeAssignPla // The first plan instruction is the start instruction and every other plan instruction should be converted into // ten move instruction. EXPECT_EQ(((pcnt - 1) * 10) + 1, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } @@ -283,7 +274,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerDefaultLVSPlanProf EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); - EXPECT_TRUE(program.hasStartInstruction()); EXPECT_FALSE(program.getManipulatorInfo().empty()); // Profile Dictionary @@ -308,7 +298,6 @@ TEST_F(TesseractTaskComposerUnit, FreespaceSimpleMotionPlannerDefaultLVSPlanProf // The first plan instruction is the start instruction and every other plan instruction should be converted into // 32 move instruction. EXPECT_EQ(37, mcnt); - EXPECT_TRUE(response.results.hasStartInstruction()); EXPECT_FALSE(response.results.getManipulatorInfo().empty()); } diff --git a/tesseract_time_parameterization/src/instructions_trajectory.cpp b/tesseract_time_parameterization/src/instructions_trajectory.cpp index b2cc657c343..49f7445479b 100644 --- a/tesseract_time_parameterization/src/instructions_trajectory.cpp +++ b/tesseract_time_parameterization/src/instructions_trajectory.cpp @@ -31,17 +31,7 @@ namespace tesseract_planning { static const flattenFilterFn programFlattenMoveInstructionFilter = - [](const InstructionPoly& i, const CompositeInstruction& /*composite*/, bool parent_is_first_composite) { - if (i.isMoveInstruction()) - { - if (i.as().isStart()) - return (parent_is_first_composite); - - return true; - } - - return false; - }; + [](const InstructionPoly& i, const CompositeInstruction& /*composite*/) { return i.isMoveInstruction(); }; InstructionsTrajectory::InstructionsTrajectory(std::vector> trajectory) : trajectory_(std::move(trajectory)) diff --git a/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp b/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp index 20d98f9659e..a92a6c262c3 100644 --- a/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp +++ b/tesseract_time_parameterization/src/time_optimal_trajectory_generation.cpp @@ -55,17 +55,7 @@ constexpr double EPS = 0.000001; namespace tesseract_planning { static const flattenFilterFn programFlattenMoveInstructionFilter = - [](const InstructionPoly& i, const CompositeInstruction& /*composite*/, bool parent_is_first_composite) { - if (i.isMoveInstruction()) - { - if (i.as().isStart()) - return (parent_is_first_composite); - - return true; - } - - return false; - }; + [](const InstructionPoly& i, const CompositeInstruction& /*composite*/) { return i.isMoveInstruction(); }; TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(double path_tolerance, double resample_dt, @@ -199,16 +189,6 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(CompositeInstruction& pr CompositeInstruction new_program(program); new_program.clear(); - if (new_program.hasStartInstruction()) - { - if (new_program.getStartInstruction().getWaypoint().isStateWaypoint()) - { - auto& waypoint = new_program.getStartInstruction().getWaypoint().as(); - waypoint.setVelocity(Eigen::VectorXd::Zero(num_joints)); - waypoint.setAcceleration(Eigen::VectorXd::Zero(num_joints)); - } - } - /// @todo Figure out how to maintain the original seed structure of subcomposites for (size_t sample = 0; sample <= sample_count; ++sample) { diff --git a/tesseract_time_parameterization/test/iterative_spline_tests.cpp b/tesseract_time_parameterization/test/iterative_spline_tests.cpp index 79a63ec5e16..e6218d0b158 100644 --- a/tesseract_time_parameterization/test/iterative_spline_tests.cpp +++ b/tesseract_time_parameterization/test/iterative_spline_tests.cpp @@ -58,10 +58,7 @@ CompositeInstruction createRepeatedPointTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = 1; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } return program; @@ -80,10 +77,7 @@ CompositeInstruction createStraightTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = i * max / num; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } // leave final velocity/acceleration unset @@ -129,11 +123,10 @@ TEST(TestTimeParameterization, TestIterativeSplineDynamicParams) // NOLINT max_velocity << 2.088, 2.082, 3.27, 3.6, 3.3, 3.078; Eigen::VectorXd max_acceleration(6); max_acceleration << 1, 1, 1, 1, 1, 1; - Eigen::VectorXd max_velocity_scaling_factors = Eigen::VectorXd::Ones(static_cast(program.size() + 1)); + Eigen::VectorXd max_velocity_scaling_factors = Eigen::VectorXd::Ones(static_cast(program.size())); // +1 for start instruction - Eigen::VectorXd max_acceleration_scaling_factors = - Eigen::VectorXd::Ones(static_cast(program.size() + 1)); + Eigen::VectorXd max_acceleration_scaling_factors = Eigen::VectorXd::Ones(static_cast(program.size())); TrajectoryContainer::Ptr trajectory = std::make_shared(program); EXPECT_TRUE(time_parameterization.compute( diff --git a/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp b/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp index 1f09789baf1..af4d80bde34 100644 --- a/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp +++ b/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp @@ -27,10 +27,7 @@ CompositeInstruction createRepeatedPointTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = 1; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } return program; @@ -49,10 +46,7 @@ CompositeInstruction createStraightTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = i * max / num; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } // leave final velocity/acceleration unset diff --git a/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp b/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp index 8d5395f0612..bc1e56cb648 100644 --- a/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp +++ b/tesseract_time_parameterization/test/time_optimal_trajectory_generation_tests.cpp @@ -327,10 +327,7 @@ CompositeInstruction createStraightTrajectory() { StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; swp.getPosition()[0] = i * max / num; - if (i == 0) - program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); - else - program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); } // leave final velocity/acceleration unset