kinematics6M90T.h

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2008 by Neuronics AG                                    *
 *   support@neuronics.ch                                                  *
 ***************************************************************************/

#ifndef _KINEMATICS6M90T_H_
#define _KINEMATICS6M90T_H_
//std:
#include <string>
#include <queue>
#include <vector>
#include <map>
//kinematics:
#include "kinematics.h"
#include "KatanaKinematicsDecisionAlgorithms.h"
#include "MathHelperFunctions.h"


namespace AnaGuess {


class Kinematics6M90T : public Kinematics {
private:
        int mNumberOfMotors;
        int mNumberOfSegments;
        std::vector<double> mSegmentLength;
        std::vector<double> mAngleOffset;
        std::vector<double> mAngleStop;
        std::vector<int> mEncodersPerCycle;
        std::vector<int> mEncoderOffset;
        std::vector<int> mRotationDirection;
        bool mIsInitialized;

        struct position {
                double x;
                double y;
                double z;
        };
        struct angles_calc {
                double theta1;
                double theta2;
                double theta3;
                double theta4;
                double theta5;
                double theta6;
                double theta234;
                double b1;
                double b2;
                double costh3;
        };
        typedef std::vector<angles_calc> angles_container;
        static const int cNrOfPossibleSolutions = 8;

        bool initialize();

        void IK_theta234theta5(angles_calc& angle, const position &p_gr) const;
        bool GripperTest(const position &p_gr, const angles_calc &angle) const;
        void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const;
        double findFirstEqualAngle(const std::vector<double>& v1, const std::vector<double>& v2) const;
        void thetacomp(angles_calc &angle, const position &p_m, const std::vector<double>& pose) const;
        bool PositionTest6MS(const double& theta1, const double& theta2, const double& theta3,
                const double& theta234, const position &p) const;
        bool angledef(angles_calc &a) const;
        bool AnglePositionTest(const angles_calc &a) const;

protected:

public:
        Kinematics6M90T();

        ~Kinematics6M90T();

        std::vector<double> getLinkLength();
        std::vector<int> getEpc();
        std::vector<int> getEncOff();
        std::vector<int> getDir();
        std::vector<double> getAngOff();
        std::vector<double> getAngStop();
        std::vector<double> getAngRange();
        std::vector<double> getAngMin();
        std::vector<double> getAngMax();
        bool setLinkLength(const std::vector<double> aLengths);
        bool setAngOff(const std::vector<double> aAngOff);
        bool setAngStop(const std::vector<double> aAngStop);

        bool enc2rad(std::vector<double>& aAngles, const std::vector<int> aEncoders);

        bool rad2enc(std::vector<int>& aEncoders, const std::vector<double> aAngles);

        bool directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles);

        bool inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition, const std::vector<double> aStartingAngles);
};

} // namespace

#endif //_KINEMATICS6M90T_H_