KinematicSolver.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
 * @author     Timo Birr
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#ifndef KINEMATICSOLVER_H
#define KINEMATICSOLVER_H
 
///ARMARX-INCLUDES
#include "RobotAPI/libraries/core/FramedPose.h"
 
///EIGEN-INCLUDES
#include <Eigen/Core>
#include <Eigen/Geometry>
 
///VIRTUAL-ROBOT-INCLUDES
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/GenericIKSolver.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include "MotionPlanning/CSpace/CSpacePath.h"
#include "VirtualRobot/IK/AdvancedIKSolver.h"
 
///STANDARD-INCLUDES
#include <map>
#include <memory>
 
namespace armarx
{
    /**
     * \class KinematicSolver
     * \brief Realizes the Singleton-Pattern, Provides Methods to solve Kinematic Problems (forward and inverse)
     *
     */
    class KinematicSolver
    {
 
        /**
         * @brief The IKCalibration struct
         * Represents the settings for the Inverse-Jacobi-IK-Approach(IKIterations, stepSize) for three different IK-Solvers.
         * Small: Recursive IK Multiple steps
         * Medium: Recursive IK single step
         * Large: solveIK
         */
        struct IKCalibration
        {
        public:
            int smallIterations;
            double smallStepSize;
            int mediumIterations;
            double mediumStepSize;
            int largeIterations;
            double largeStepSize;
            static IKCalibration createIKCalibration(int smallIterations,
                                                     double smallStepSize,
                                                     int mediumIterations,
                                                     double mediumStepSize,
                                                     int largeIterations,
                                                     double largeStepSize);
        };
 
    public:
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///SINGLETON-FEATURES////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /**
         * @brief getInstance returns an instance of a new KinematicSolver with the attributes scenario and robot if no KinematicSolver is initialized yet
         *          if a KinematicSolver has already been initialized the method returns that one (see Singleton-Pattern)
         * @param scenario the objects surronding the robot that have to be regarded when calculating an IKSolution
         * @param robot the robot for which the Kinematic Solution is calculated
         * @return the single instance of KinematicSolver
         */
        static std::shared_ptr<KinematicSolver> getInstance(VirtualRobot::ScenePtr scenario,
                                                            VirtualRobot::RobotPtr robot);
 
        /**
         * @brief reset
         * Destroys the current instance of KinematicSolver that a new one can be created via call of getInstance
         */
        static void reset();
 
        /**
         * @brief syncRobotPrx updates the robotProxy with the jointAngles of the "real" when the rns is different to the current rns
         * @param rns the newly selected rns
         */
        void syncRobotPrx(VirtualRobot::RobotNodeSetPtr rns);
        /**
         * @brief syncRobotPrx always updates the robotProxy with the jointAngles of the "real" robot
         */
        void syncRobotPrx();
 
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///IK////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /**
         * @brief solveIK returns a random solution for an inverse Kinematic problem with no consideration of the current pose of the robot
         * @param kc the kinematic chain for which the IK solution is calculated
         * @param pose the GLOBAL pose that should be reached by the tcp of the kinematic chain kc when the joint values of the solution are applied to the robot
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain
         */
        std::vector<double> solveIK(VirtualRobot::RobotNodeSetPtr kc,
                                    armarx::PoseBasePtr pose,
                                    VirtualRobot::IKSolver::CartesianSelection selection,
                                    int iterations);
 
        /**
         * @brief solveIK returns a random solution for an inverse Kinematic problem with no consideration of the current pose of the robot
         * @param kc the kinematic chain for which the IK solution is calculated
         * @param pose the FRAMED POSE RELATIVE TO THE ROBOTS KINEMATIC ROOT that should be reached by the tcp of the kinematic chain kc when the joint values of the solution are applied to the robot
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain RELATIVE TO THE ROBOTS KINEMATIC ROOT
         */
        std::vector<double> solveIKRelative(VirtualRobot::RobotNodeSetPtr kc,
                                            armarx::PoseBasePtr framedPose,
                                            VirtualRobot::IKSolver::CartesianSelection selection);
 
        /**
         * @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
         * @param kc the selected kinematic chain, for which the IK Solution should be found
         * @param startingPoint the jointAngle values of the starting Pose of the robot
         * @param destinations the GLOBAL Pose that should be reached
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
         */
        std::vector<float> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
                                            std::vector<float> startingPoint,
                                            PoseBasePtr globalDestination,
                                            VirtualRobot::IKSolver::CartesianSelection selection);
 
        /**
         * @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
         * @param kc the selected kinematic chain, for which the IK Solution should be found
         * @param startingPoint the jointAngle values of the starting Pose of the robot
         * @param destinations the FRAMED POSE RELATIVE TO THE ROBOTS KINEMATIC ROOT that should be reached
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
         */
        std::vector<float>
        solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc,
                                 std::vector<float> startingPoint,
                                 PoseBasePtr framedDestination,
                                 VirtualRobot::IKSolver::CartesianSelection selection);
 
 
        /**
         * @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
         * @param kc the selected kinematic chain, for which the IK Solution should be found
         * @param startingPoint the jointAngle values of the starting Pose of the robot
         * @param destinations the GLOBAL Pose that should be reached
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
         */
        std::vector<double> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
                                             std::vector<double> startingPoint,
                                             PoseBasePtr globalDestination,
                                             VirtualRobot::IKSolver::CartesianSelection selection);
 
        /**
         * @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
         * @param kc the selected kinematic chain, for which the IK Solution should be found
         * @param startingPoint the jointAngle values of the starting Pose of the robot
         * @param destinations the FRAMED POSE RELATIVE TO THE ROBOTS KINEMATIC ROOT that should be reached
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
         */
        std::vector<double>
        solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc,
                                 std::vector<double> startingPoint,
                                 PoseBasePtr framedDestination,
                                 VirtualRobot::IKSolver::CartesianSelection selection);
 
        /**
         * @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
         * @param kc the selected kinematic chain, for which the IK Solution should be found
         * @param startingPoint the jointAngle values of the starting Pose of the robot
         * @param destinations the GLOBAL Pose that should be reached
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
         */
        std::vector<std::vector<double>>
        solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
                         std::vector<double> startingPoint,
                         std::vector<PoseBasePtr> globalDestinations,
                         std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
 
        std::vector<std::vector<double>> solveRecursiveIKNoMotionPlanning(
            VirtualRobot::RobotNodeSetPtr kc,
            std::vector<double> startingPoint,
            std::vector<PoseBasePtr> globalDestinations,
            std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
 
        std::vector<std::vector<double>>
        solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
                         std::vector<double> startingPoint,
                         std::vector<PoseBasePtr> globalDestinations,
                         VirtualRobot::IKSolver::CartesianSelection selection);
 
        /**
         * @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
         * @param kc the selected kinematic chain, for which the IK Solution should be found
         * @param startingPoint the jointAngle values of the starting Pose of the robot
         * @param destinations the FRAMED POSE RELATIVE TO THE ROBOTS KINEMATIC ROOT that should be reached
         * @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
         * @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
         */
        std::vector<std::vector<double>> solveRecursiveIKRelative(
            VirtualRobot::RobotNodeSetPtr kc,
            std::vector<double> startingPoint,
            std::vector<PoseBasePtr> framedDestinations,
            std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
 
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///FORWARD KINEMATIC & REACHABILITY///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /**
         * @brief doForwardKinematic executes the given jointAngles on the loaded Robot
         * @param kc the kinematic Chain on which the joint angles are executed on
         * @param jointAngles the joint angles which the robot should have
         * @return a vector of joint angle configurations to reach destination 1,..., n in exactly the same order as the destinations
         */
        PoseBasePtr doForwardKinematic(VirtualRobot::RobotNodeSetPtr kc,
                                       std::vector<double> jointAngles);
 
        /**
         * @brief doForwardKinematic executes the given jointAngles on the loaded Robot
         * @param kc the kinematic Chain on which the joint angles are executed on
         * @param jointAngles the joint angles which the robot should have
         * @return a vector of joint angle configurations to reach destination 1,..., n in exactly the same order as the destinations
         */
        PoseBasePtr doForwardKinematicRelative(VirtualRobot::RobotNodeSetPtr kc,
                                               std::vector<double> jointAngles);
 
        /**
         * @brief isReachable calculates whether the PoseBase pose is reachable by the loaded robot or not
         * @param pose the pose that should be reachable by the robot
         * @return returns true if the pose is reachable, false otherwise
         */
        bool isReachable(VirtualRobot::RobotNodeSetPtr rns,
                         PoseBasePtr globalPose,
                         VirtualRobot::IKSolver::CartesianSelection cs);
 
 
    private:
        static std::shared_ptr<KinematicSolver> singleton;
        VirtualRobot::RobotPtr robot;
        VirtualRobot::RobotPtr robotProxy;
        std::map<std::string, IKCalibration> IKCalibrationMap;
        //std::map<std::string, VirtualRobot::ReachabilityPtr> reachabilityMap;
        KinematicSolver(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot);
        VirtualRobot::GenericIKSolverPtr IKSetup(double jacStepSize,
                                                 int IKIterations,
                                                 double vectorError,
                                                 double orientationError,
                                                 VirtualRobot::RobotNodeSetPtr kc);
        VirtualRobot::RobotNodeSetPtr setupProxy(VirtualRobot::RobotNodeSetPtr kc,
                                                 std::vector<double> jointAngles);
        std::string currentRns;
        IKCalibration calibrate();
        double getMaxDistance(Eigen::Vector3f destination,
                              std::vector<std::vector<double>> jointAngles,
                              VirtualRobot::RobotNodeSetPtr rns);
        static int getFurthestNode(VirtualRobot::RobotNodeSetPtr rns,
                                   Eigen::VectorXf startConfig,
                                   Eigen::VectorXf endConfig);
 
        //Saba::RrtPtr getMotionPlanning(Eigen::VectorXf start, Eigen::VectorXf end, VirtualRobot::RobotNodeSetPtr rns);
    };
 
    using KinematicSolverPtr = std::shared_ptr<KinematicSolver>;
} // namespace armarx
 
 
#endif