10 #ifndef _KINEMATICS6M90G_H_ 11 #define _KINEMATICS6M90G_H_ 113 bool setAngOff(
const std::vector<double> aAngOff);
115 bool setAngStop(
const std::vector<double> aAngStop);
121 bool enc2rad(std::vector<double>& aAngles,
const std::vector<int> aEncoders);
127 bool rad2enc(std::vector<int>& aEncoders,
const std::vector<double> aAngles);
133 bool directKinematics(std::vector<double>& aPosition,
const std::vector<double> aAngles);
141 bool inverseKinematics(std::vector<double>& aAngles,
const std::vector<double> aPosition,
const std::vector<double> aStartingAngles);
147 #endif //_KINEMATICS6M90G_H_ std::vector< angles_calc > angles_container
std::vector< int > mEncodersPerCycle
Encoders per cycle vector.
void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const
std::vector< int > mEncoderOffset
Encoder offset vector.
bool initialize()
initialization routine
std::vector< int > getDir()
get direction
std::vector< double > getAngStop()
get angle stop
void thetacomp(angles_calc &angle, const position &p_m) const
std::vector< double > getAngOff()
get angle offset
std::vector< double > getAngMax()
get angle max
bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)
bool rad2enc(std::vector< int > &aEncoders, const std::vector< double > aAngles)
bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)
std::vector< double > getAngRange()
get angle range
void IK_theta234theta5(angles_calc &angle, const position &p_gr) const
helper functions
bool AnglePositionTest(const angles_calc &a) const
structs, type and constants used in inverse kinematics calculation
std::vector< double > mAngleStop
Angle stop vector [rad].
bool setAngOff(const std::vector< double > aAngOff)
set angle offset
Base Class for the kinematics implementations.
std::vector< int > mRotationDirection
Rotation direction vector [1|-1].
bool GripperTest(const position &p_gr, const angles_calc &angle) const
std::vector< int > getEpc()
get encoders per cycle
std::vector< double > mAngleOffset
Angle offset vector [rad].
bool setLinkLength(const std::vector< double > aLengths)
set link length
bool setAngStop(const std::vector< double > aAngStop)
set angle offset
bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)
FloatVector FloatVector * a
Implements the kinematics for the Katana6M90G.
bool mIsInitialized
Initialized flag.
std::vector< int > getEncOff()
get encoder offset
int mNumberOfMotors
Number of motors of the robot.
bool angledef(angles_calc &a) const
Kinematics6M90G()
Constructor.
bool PositionTest6MS(const angles_calc &a, const position &p) const
static const int cNrOfPossibleSolutions
std::vector< double > getLinkLength()
get link length
std::vector< double > getAngMin()
get angle min
int mNumberOfSegments
Number of segments of the robot.
~Kinematics6M90G()
Destructor.
std::vector< double > mSegmentLength
Effector segment lengths vector [m].