InterpolationSegmentFactory.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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::Interpolation
 * @author     Timo Birr
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#ifndef INTERPOLATIONSEGMENTFACTORY_H
#define INTERPOLATIONSEGMENTFACTORY_H
 
#include <VirtualRobot/IK/AdvancedIKSolver.h>
 
#include "AbstractInterpolation.h"
#include "InterpolationType.h"
 
namespace armarx
{
    /**
    * @brief The InterpolationSegmentFactory class
    * Utility Class used to easily create InterpolationSegments, so that every Interpolation can be threated the same
    * An Interpolation produced by this factory always has exactly two Waypoints and is defined between 0 and 1
    * With 0 being the starting pose and 1 being the end pose
    */
    class InterpolationSegmentFactory
    {
    public:
        /**
         * @brief produceInterpolationSegments constructs a vector of AbstractInterpolation the concrete Interpolationtype of which is defined by interpolations
         * @param controlPoints the Poses the interpolation curves go through; segment i starts at controlPoint i and ends at control point i + 1
         * @param interpolations the InterpolationTypes of the Interpolation segments (warning: at least 2 segments in a row have to have spline intepolation)
         * @return a vector of |controlPoints| - 1 AbstractInterpolations between the control points
         */
        static std::vector<AbstractInterpolationPtr>
        produceInterpolationSegments(std::vector<PoseBasePtr> controlPoints,
                                     std::vector<InterpolationType> interpolations);
 
        /**
         * @brief produceInterpolationSegments constructs a vector of LinearInterpolations
         * @param controlPoints the Poses the interpolation curves go through; segment i starts at controlPoint i and ends at control point i + 1
         * @return a vector of |controlPoints| - 1 LinearInterpolations between the control points
         */
        static std::vector<AbstractInterpolationPtr>
        produceLinearInterpolationSegments(std::vector<PoseBasePtr> controlPoints);
 
        /**
         * @brief produceInterpolationSegments constructs a vector of SplineInterpolations
         * @param controlPoints the Poses the interpolation curves go through; segment i starts at controlPoint i and ends at control point i + 1
         * @return a vector of |controlPoints| - 1 SplineInterpolations between the control points
         */
        static std::vector<AbstractInterpolationPtr>
        produceSplineInterpolationSegments(std::vector<PoseBasePtr> controlPoints);
 
        /**
         * @brief produceSplineInterpolationSegment constructs the splineInterpolationSegment of a splineInterpolation between given controlPoints
         * @param controlPoints the controlPoints of the parent SplineInterpolatiom
         * @param segmentStart the first Point in the Interval
         * @return a SplineInterpolationSegment between segmentStart and the following control point
         */
        static AbstractInterpolationPtr
        produceSplineInterpolationSegment(std::vector<PoseBasePtr> controlPoints,
                                          PoseBasePtr segmentStart);
 
        /**
         * @brief optimizeControlPoints changes the cartian selections and control points so that the IKSolving produces a smooth Trajectory
         * @param controlPoints the controlPoints to change to make the Trajectory smooth
         * @param selections the cartesian selections to optimize
         */
        static void
        optimizeControlPoints(std::vector<PoseBasePtr>& controlPoints,
                              std::vector<VirtualRobot::IKSolver::CartesianSelection>& selections);
        /**
         * @brief needsOptimizing returns true if there is a CartesianSelection at i that dominates a CartesianSelection at i - 1
         * @param selections the cartesian selection that should be reached
         * @return true if the cartesian Selection need optimizing for a smooth trajectory
         */
        static bool
        needsOptimizing(std::vector<VirtualRobot::IKSolver::CartesianSelection>& selections);
        /**
         * @brief isDominantOver returns true when current selection dominates other selection
         * Definition: A CartesianSelection dominates another CartesianSelection when additional Information is needed to solve curren after solving other
         * Examples: - X dominates Y ; Y dominates X ; Pos dominates X,Y ans Z but X,Y and Z dont dominate Pos; Pos dominates Ori ; Ori dominates Pos
         * @param current the later cartesian selection
         * @param other the prior cartesian selection
         * @return true if current dominates other
         */
        static bool isDominantOver(VirtualRobot::IKSolver::CartesianSelection current,
                                   VirtualRobot::IKSolver::CartesianSelection other);
        /**
         * @brief getSmallestDominating returns the cartesian selection that dominates all other cartesian selections in selections
         * Examples: (X,Y,Y,Y,Z) -> Pos ; (X,Y, Ori) -> All; (Ori,Ori) -> Ori ; (X,Z,Pos) -> Pos
         * @param selections the selections to get the smallest dominating selection from
         * @return the smmallestDominating Selection
         */
        static VirtualRobot::IKSolver::CartesianSelection
        getSmallestDominating(std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
        /**
         * @brief optimizePose changes the original pose so that the interpolation is smove when it changes from selectionOriginal to selectionCorrected
         * @param original the original Pose
         * @param corrected the absolute corrected Pose
         * @param selectionOriginal the cartesian selection associated with the original Pose
         * @param selectionCorrected the cartesian selection associated with the corrected Pose
         * @return
         */
        static PoseBasePtr
        optimizePose(armarx::PoseBasePtr original,
                     armarx::PoseBasePtr corrected,
                     VirtualRobot::IKSolver::CartesianSelection selectionOriginal,
                     VirtualRobot::IKSolver::CartesianSelection selectionCorrected);
    };
} // namespace armarx
 
#endif