DesignerTrajectoryManager.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXGuiPlugins::RobotTrajectoryDesigner::Manager
 * @author     Luca Quaer
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#ifndef DESIGNERTRAJECTORYMANAGER_H
#define DESIGNERTRAJECTORYMANAGER_H
 
#include <functional>
#include <set>
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/util/PropagateConst.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
 
#include <RobotComponents/interface/components/RobotIK.h>
 
#include "../Environment.h"
#include "../Interpolation/InterpolationSegmentFactory.h"
#include "../Interpolation/InterpolationType.h"
#include "../KinematicSolver.h"
#include "../Model/DesignerTrajectory.h"
#include "../TrajectoryCalculation/DesignerTrajectoryCalculator.h"
#include <MotionPlanning/CSpace/CSpaceSampled.h>
#include <MotionPlanning/Planner/BiRrt.h>
 
namespace armarx
{
    /**
     * @brief The DesignerTrajectoryManager class enables easy interaction with the model.
     *
     * Using this class, the model is always self-consistent.
     *
     * @see DesignerTrajectory
     */
    class DesignerTrajectoryManager
    {
    private:
        ////////////////////////////////////////////////////////////////////////////////////////////
        /// Structs
        ////////////////////////////////////////////////////////////////////////////////////////////
        ///
        struct ManipulationInterval
        {
        private:
            std::list<UserWaypointPtr> userWaypointsList;
            std::list<TransitionPtr> transitionsList;
 
            std::set<int> breakpointIndicesSet;
 
            // made public
            //std::vector<UserWaypointPtr> userWaypoints;
            //std::vector<TransitionPtr> transitions;
 
            // made public
            //std::vector<PoseBasePtr> interpolatedPoses; //including poses of UserWaypoints
 
            // this vector holds the new index (in interpolatedPoses) of the UserWaypoints
            //std::vector<int> newIndexOfUserWaypoint; // e.g.: uwp(0) -> new index 9
 
        public:
            unsigned int upperIntervalLimit;
            unsigned int lowerIntervalLimit;
 
            std::vector<UserWaypointPtr> userWaypoints;
            std::vector<TransitionPtr> transitions;
 
            /* absolute (absolute to all designer trajectory user waypoints)
             * breakpoint indices of breakpoints in the manipulation interval
             * BEFORE inserting interpolated poses...
             * points with index upperIntervalLimit and lowerIntervalLimit included !
             */
            std::vector<unsigned int> breakpointIndices;
 
            //std::vector<PoseBasePtr> interpolatedPoses; //including poses of UserWaypoints
            std::vector<std::vector<PoseBasePtr>> interpolatedTransitions;
 
            // this vector holds the new index (in interpolatedPoses) of the UserWaypoints
            std::vector<unsigned int> newIndexOfUserWaypoint; // e.g.: uwp(0) -> new index 9
 
            void addFirstUserWaypoint(UserWaypointPtr uw);
            void pushFront(UserWaypointPtr uw, TransitionPtr t);
            void pushBack(UserWaypointPtr uw, TransitionPtr t);
            void addBreakpointIndex(int index);
            void finishSearch();
 
            //int getFirstBreakpointIndex() const;
            //int getLastBreakpointIndex() const;
            UserWaypointPtr getUserWaypointByRealIndex(unsigned int i) const;
            UserWaypointPtr getUserWaypointByZeroBasedIndex(unsigned int i) const;
            TransitionPtr getTransitionByRealIndex(unsigned int i) const;
            TransitionPtr getTransitionByZeroBasedIndex(unsigned int i) const;
 
            std::vector<std::vector<double>>
            getUserWaypointsIKSolutions(std::vector<std::vector<double>>& ikSolutions,
                                        unsigned int intervalStart,
                                        unsigned int intervalEnd);
            void applyJointAnglesOfUserWaypoints(std::vector<std::vector<double>> ikSolution);
        };
 
        struct InsertTransition
        {
        public:
            unsigned int transitionIndex;
 
            // insert transition with transitionIndex between
            // insertionIndex and insertionIndex + 1
            unsigned int insertionIndex;
        };
 
        struct Mementos
        {
            std::list<DesignerTrajectoryPtr> mementoList;
            unsigned int currentMemento = 0;
        };
 
        ////////////////////////////////////////////////////////////////////////////////////////////
        /// Attributes
        ////////////////////////////////////////////////////////////////////////////////////////////
        VirtualRobot::RobotNodeSetPtr rns;
        DesignerTrajectoryPtr designerTrajectory;
        KinematicSolverPtr kinSolver;
        bool isInitialized = false;
        Mementos mementos;
        static const unsigned int MEMENTO_MAX = 20;
        static const unsigned int DEFAULT_FRAME_COUNT = 100;
        static constexpr double MAX_DEVIATION = 0.0001;
        EnvironmentPtr environment;
        //CollisionDetectionPtr collisionDetection;
        ////////////////////////////////////////////////////////////////////////////////////////////
 
        /**
         * @brief saveState
         */
        void saveState();
 
        /*struct ManipulationInterval
        {
            int upperIndex;
            int lowerIndex;
            std::vector<UserWaypointPtr> userWaypoints;
            std::vector<TransitionPtr> transitions;
 
            void push(UserWaypointPtr uw, TransitionPtr t);
 
            static ManipulationInterval mergeManipulationIntervalHalfs(
                ManipulationInterval lowerHalf,
                ManipulationInterval upperHalf);
        };
 
        struct InterpolatedPoints
        {
            std::vector<PoseBasePtr> poseBases;
            std::vector<int> userWaypoint; // e.g.: uwp(0) -> new index 9
        };*/
 
        /**
         * @brief Calculates one half of the manipulation interval
         * @param i index where the manipulaion takes place
         * @param f function specifying the search direction
         * @param fInv function specifying the inverse search direction
         * @return interval half which is affected by the manipulation as ManipulationInterval
         *
         * @see ManipulationInterval
         */
        //ManipulationInterval searchFrom(int i,
        //                                std::function<int (int)> f,
        //                                std::function<int (int)> fInv);
 
        /**
         * @brief Calculates the whole manipulation interval
         * @param manipulationIndex index where the manipulation takes place
         * @return interval which is affected by the manipulation
         */
        ManipulationInterval calculateManipulationInterval(unsigned int manipulationIndex);
 
        /**
         * @brief getInterpolationObjects calculates the interpolation object for each transition
         *        in the given manipulation interval
         * @param mi manipulation interval
         * @return interpolation objects for each transition
         *
         * @see InterpolationSegmentFactory
         */
        std::vector<AbstractInterpolationPtr> getInterpolationObjects(ManipulationInterval& mi);
 
        /**
         * @brief Calculates the interpolated points between two userwaypoints according to the
         *        userDuration of the transition between them and the desired frames per second.
         * @param interpolationObjects interpolation objects
         * @param fps frames per second
         * @param ManipulationInterval object to insert the interpolated points into
         */
        void calculateInterpolatedPoints(std::vector<AbstractInterpolationPtr> interpolationObjects,
                                         unsigned int fps,
                                         ManipulationInterval& mi);
 
        /**
         * @brief Checks if the transition with the given index is reachable
         * @param mi ManipulationInterval
         * @param transitionIndex transition index
         * @return true, if transition is reachable;
         *         false, if transition is not reachable
         */
        bool checkTransitionReachability(ManipulationInterval& mi, unsigned int transitionIndex);
 
        /**
         * @brief Calls motion planning with a start and end joint angle configuration
         * @param start joint angle configuration to begin with
         * @param end joint angle configuration to end with
         * @return vector of joint angle configurations
         */
        /*
        std::vector<std::vector<double>> motionPlanning(
                                          std::vector<double> start,
                                          std::vector<double> end);
        */
 
        /**
         * @brief Calculates the IKSelection of the given InterpolatedPoints.
         *        If transitions are not reachable or have collisions, these transitions get
         *        motion planned. The motion planned IK-solutions are inserted into the
         *        regularly calculated IK-solutions.
         *        The caller of this function passes the parameter 'motionPlanningCalled' to know
         *        if something was motion planned.
         * @param mi ManipulationInterval
         * @param motionPlanningCalled boolean flag which indicates if something was motion planned
         * @return IKSolutions of the given InterpolatedPoints
         */
        std::vector<std::vector<double>> calculateIKSolutions(
                                          ManipulationInterval& mi/*,
                                          bool& motionPlanningCalled*/);
 
        /**
         * @brief Calculates the TimedTrajectory of each breakpoint-interval
         * @param ikSolutions all ikSolutions of current calculation
         * @param mi ManipulationInterval
         * @return returns vector of TimedTrajectories
         */
        std::vector<TimedTrajectory>
        calculateTimeOptimalTrajectories(std::vector<std::vector<double>> ikSolutions,
                                         ManipulationInterval& mi);
 
        void theUniversalMethod(unsigned int index);
 
        std::vector<double> getNewIkSolutionOfFirstPoint(PoseBasePtr oldStart,
                                                         PoseBasePtr newStart,
                                                         std::vector<double> jointAnglesOldStart);
 
    public:
        ////////////////////////////////////////////////////////////////////////////////////////////
        /// Constructor(s)
        ////////////////////////////////////////////////////////////////////////////////////////////
        /**
         * @brief Constructor of this class.
         * @param rns RobotNodeSet
         */
        DesignerTrajectoryManager(std::string rnsName, EnvironmentPtr environment_);
 
        ////////////////////////////////////////////////////////////////////////////////////////////
        /// Public functions
        ////////////////////////////////////////////////////////////////////////////////////////////
        /**
         * @brief Initializes the DesignerTrajectory object of this manager instance.
         * @param jointAngles start jointangle-configuration of the trajectory
         */
        void initializeDesignerTrajectory(std::vector<double>& jointAngles);
 
        /**
         * @brief Adds a UserWaypoint to the end of this manager's DesignerTrajectory.
         *
         * To create this UserWaypoint the following parameters are needed:
         *
         * @param pose pose of the UserWaypoint
         */
        void addWaypoint(const PoseBasePtr pose);
 
        /**
         * @brief Inserts a UserWaypoint at the given position to this manager's DesignerTrajectory.
         *
         * To create this UserWaypoint the following parameters are needed:
         *
         * @param index position to insert the UserWaypoint into
         * @param pb pose of the UserWaypoint
         */
        void insertWaypoint(unsigned int index, const PoseBasePtr pose);
 
        /**
         * @brief Edits the pose of a UserWaypoint.
         * @param index index of the UserWaypoint to edit
         * @param pose new pose
         */
        void editWaypointPoseBase(unsigned int index, const PoseBasePtr pose);
 
        /**
         * @brief Edits the IKSelection of a UserWaypoint.
         * @param index index of the UserWaypoint to edit
         * @param ikSelection new IKSelection
         */
        void editWaypointIKSelection(unsigned int index,
                                     VirtualRobot::IKSolver::CartesianSelection ikSelection);
 
        /**
         * @brief Deletes a UserWaypoint from this manager's DesignerTrajectory.
         * @param index index of the UserWaypoint to delete
         */
        void deleteWaypoint(unsigned int index);
 
        /**
         * @brief Sets the InterpolationType of a Transition from this manager's DesignerTrajectory.
         * @param index index of the Transition to edit
         * @param it new InterpolationType
         */
        void setTransitionInterpolation(unsigned int index, InterpolationType it);
 
        /**
         * @brief Sets a UserWaypoint as (not-) breakpoint.
         * @param index index of the UserWaypoint to edit
         * @param b true when the selected UserWaypoint should be a breakpoint
         *          false when the selected UserWaypoint should not be a breakpoint
         */
        void setWaypointAsBreakpoint(unsigned int index, bool b);
 
        /**
         * @brief Sets the desired duration of a Transition
         * @param index index of the Transition to edit
         * @param duration desired duration
         */
        void setTransitionUserDuration(unsigned int index, double duration);
 
        // returns deep copy of DesignerTrajectory
        /**
         * @brief Returns a deep copy of this manager's DesignerTrajectory.
         * @return a deep copy of this manager's DesignerTrajectory
         */
        DesignerTrajectoryPtr getDesignerTrajectory() const;
 
        /**
         * @brief Overrides the current DesignerTrajectory with the given DesignerTrajectory.
         * @param designerTrajectory new DesignerTrajectory
         */
        bool import(DesignerTrajectoryPtr newDesignerTrajectory);
 
        /**
         * @brief undo
         * @return
         */
        bool undo();
 
        /**
         * @brief redo
         * @return
         */
        bool redo();
 
        /**
         * @brief True if the DesignerTrajectory is initialized
         * @return isInitialized member
         */
        bool getIsInitialized();
 
        /**
         * @brief setCollisionModels Sets the collision models on the collision detection attribute
         * @param activeCollisionModel The collision model of the RobotNodeSet which gets
         *        manipulated
         * @param bodyCollisionModels The collision models to check against
         */
        /*
        void setCollisionModels(VirtualRobot::RobotNodeSetPtr activeCollisionModel,
                                std::vector<VirtualRobot::RobotNodeSetPtr> bodyCollisionModels);
                                */
 
        bool undoPossible();
 
        bool redoPossible();
    };
 
    ////////////////////////////////////////////////////////////////////////////////////////////////
    /// Typedefinitions
    ////////////////////////////////////////////////////////////////////////////////////////////////
    using DesignerTrajectoryManagerPtr = std::shared_ptr<armarx::DesignerTrajectoryManager>;
 
    // old typedef (can be deleted, if it is not used anymore)
    using DesignerTrajectoryManagerStdPtr = std::shared_ptr<armarx::DesignerTrajectoryManager>;
} // namespace armarx
 
#endif // DESIGNERTRAJECTORYMANAGER_H