28 return (value > 0) - (value < 0);
31 return (value > 0) - (value < 0);
34 double KinematicsLib::round(
double x)
43 __int64 xint = (__int64) (x+0.5);
46 double diff = x - (double)xint;
48 return double(xint-1);
52 __int64 xint = (__int64) (x-0.5);
55 double diff = x - (double)xint;
57 return double(xint+1);
73 for (
int i = 0; i < 4; ++i) {
80 for (
int i = 0; i <
_dof; i++) {
108 for (
int i = 0; i <
_dom; ++i) {
109 angleMDH.push_back(arr[i]);
119 std::vector<double> angOff;
120 double angStopArr[
MaxDof];
121 std::vector<double> angStop;
122 std::vector<double> lengths;
128 for (
int i = 0; i <
_dof; i++) {
132 for (
int i = 0; i <
_dof; i++) {
142 for (
int i = 0; i < 4; i++) {
149 for (
int i = 0; i <
_dom; ++i) {
155 for (
int i = 0; i < 4; ++i) {
164 for (
int i = 0; i <
_dof; i++) {
171 for (
int i = 0; i <
_dof; i++) {
181 for (
int i = 0; i < 4; i++) {
187 angOff.push_back(-2.150246);
189 for (
int i = 0; i <
_dom; ++i) {
194 angStop.push_back(3.731514);
196 for (
int i = 0; i < 4; ++i) {
205 for (
int i = 0; i <
_dof; i++) {
209 for (
int i = 0; i <
_dof; i++) {
219 for (
int i = 0; i < 4; i++) {
225 angOff.push_back(-2.150246);
227 for (
int i = 0; i <
_dom; ++i) {
232 angStop.push_back(3.731514);
234 for (
int i = 0; i < 4; ++i) {
243 for (
int i = 0; i <
_dof; i++) {
247 for (
int i = 0; i <
_dof; i++) {
257 for (
int i = 0; i < 4; i++) {
264 for (
int i = 0; i <
_dom; ++i) {
270 for (
int i = 0; i < 4; ++i) {
279 for (
int i = 0; i <
_dof; i++) {
286 for (
int i = 0; i <
_dof; i++) {
296 for (
int i = 0; i < 4; i++) {
302 angOff.push_back(-2.150246);
304 for (
int i = 0; i <
_dom; ++i) {
309 angStop.push_back(3.731514);
311 for (
int i = 0; i < 4; ++i) {
324 std::vector<double>
a, std::vector<double>
alpha,
int typeNr) {
327 if ((
int)theta.size() >
MaxDof)
332 if ((
int)theta.size() !=
_dof || (int)d.size() !=
_dof ||
333 (int)a.size() !=
_dof || (int)alpha.size() !=
_dof) {
340 for (
int i = 0; i <
_dof; ++i) {
341 _data(i + 1, 2) = theta.at(i);
344 _data(i + 1, 5) = alpha.at(i);
345 _data(i + 1, 23) = 0;
356 if ((
_dof == -1) || ((
int)links.size() != 4))
379 for (
int i = 0; i < 4; ++i) {
384 std::vector<double> lengths;
385 for (
int i = 0; i < 4; ++i) {
394 if (
_dof == -1 || immob < 0 || immob > 1) {
411 if ((
int)epc.size() <
_dom) {
415 for (
int i = 0; i <
_dom; ++i) {
423 if ((
int)encOffset.size() <
_dom) {
427 for (
int i = 0; i <
_dom; ++i) {
435 if ((
int)rotDir.size() <
_dom) {
439 for (
int i = 0; i <
_dom; ++i) {
440 if (rotDir.at(i) < 0) {
451 if ((
int)angleOffset.size() <
_dom) {
455 for (
int i = 0; i <
_dom; ++i) {
463 std::vector<double> angOff;
464 double angStopArr[
MaxDof];
465 std::vector<double> angStop;
471 for (
int i = 0; i <
_dom; ++i) {
481 angOff.push_back(-2.150246);
483 for (
int i = 0; i <
_dom; ++i) {
488 angStop.push_back(3.731514);
493 angOff.push_back(-2.150246);
495 for (
int i = 0; i <
_dom; ++i) {
500 angStop.push_back(3.731514);
511 if ((
int)angleRange.size() <
_dom) {
515 for (
int i = 0; i <
_dom; ++i) {
523 double angStopArr[
MaxDof];
524 std::vector<double> angStop;
528 for (
int i = 0; i <
_dom; ++i) {
537 for (
int i = 0; i <
_dom; ++i) {
542 angStop.push_back(3.731514);
546 for (
int i = 0; i <
_dom; ++i) {
551 angStop.push_back(3.731514);
562 if ((
int)tcpOffset.size() < 4) {
566 for (
int i = 0; i < 4; ++i) {
590 std::vector<double>&
a, std::vector<double>&
alpha) {
597 for (
int i = 0; i <
_dof; ++i) {
598 theta.push_back(
_data(i + 1, 2));
601 alpha.push_back(
_data(i + 1, 5));
615 for (
int i = 0; i <
_dom; ++i) {
616 epc.push_back(
_epc[i]);
626 for (
int i = 0; i <
_dom; ++i) {
637 for (
int i = 0; i <
_dom; ++i) {
648 for (
int i = 0; i <
_dom; ++i) {
659 for (
int i = 0; i <
_dom; ++i) {
667 std::vector<double> angoff;
669 std::vector<int> encoff;
671 std::vector<int> rotdir;
673 std::vector<double> angran;
676 for (
int i = 0; i <
_dom; i++) {
677 angleStop.push_back(angoff.at(i) - (
sign(encoff.at(i)) * rotdir.at(i))
680 int ok = (okcount == 4) ? 1 : 0;
685 std::vector<double> angoff;
687 std::vector<double> angstop;
690 for (
int i = 0; i <
_dom; ++i) {
691 angleMin.push_back(min(angoff.at(i), angstop.at(i)));
693 int ok = (okcount == 2) ? 1 : 0;
698 std::vector<double> angoff;
700 std::vector<double> angstop;
703 for (
int i = 0; i <
_dom; ++i) {
704 angleMax.push_back(
max(angoff.at(i), angstop.at(i)));
706 int ok = (okcount == 2) ? 1 : 0;
714 for (
int i = 0; i < 4; ++i) {
744 if ((
int)angleK4D.size() <
_dom)
750 angleMDH.push_back(angleK4D.at(0) -
mPi);
751 angleMDH.push_back(angleK4D.at(1));
752 angleMDH.push_back(angleK4D.at(2) -
mPi);
753 angleMDH.push_back(
mPi / 2.0 - angleK4D.at(3));
759 angleMDH.push_back(
mPi / 2.0 - angleK4D.at(4));
760 angleMDH.push_back(
mPi / 2.0 - angleK4D.at(5));
764 angleMDH.push_back(
mPi / 2.0 - angleK4D.at(4));
767 angleMDH.push_back(-1.0 * angleK4D.at(4) + 3.0 *
mPi / 2.0);
780 if ((
int)angleMDH.size() <
_dom)
786 angleK4D.push_back(angleMDH.at(0) +
mPi);
787 angleK4D.push_back(angleMDH.at(1));
788 angleK4D.push_back(angleMDH.at(2) +
mPi);
789 angleK4D.push_back(
mPi / 2.0 - angleMDH.at(3));
795 angleK4D.push_back(
mPi / 2.0 - angleMDH.at(4));
796 angleK4D.push_back(
mPi / 2.0 - angleMDH.at(5));
800 angleK4D.push_back(
mPi / 2.0 - angleMDH.at(4));
803 angleK4D.push_back(-1.0 * angleMDH.at(4) + 3.0 *
mPi / 2.0);
813 std::vector<double>& angles) {
814 if ((
int)encoders.size() <
_dom)
818 for(
int i = 0; i <
_dom; ++i) {
828 if ((
int)angles.size() <
_dom)
832 for(
int i = 0; i <
_dom; ++i){
841 std::vector<double>&
pose) {
847 for(
int k = 0; k <
_dof; ++k){
852 qr(k + 1) = angles.at(k);
869 TCP_Offset.
row(4) << 0.0 << 0.0 << 0.0 << 1.0;
870 TCP_Position = TCP_Position * TCP_Offset;
875 pose.push_back(TCP_Position(1,4));
877 pose.push_back(TCP_Position(2,4));
879 pose.push_back(TCP_Position(3,4));
885 pose.push_back(eul(1));
887 pose.push_back(eul(2));
889 pose.push_back(eul(3));
898 std::vector<double>&
angle) {
899 if ((
int)pose.size() < 6 || (int)prev.size() <
_dof)
903 const int ikalgo = 0;
918 for (
int j = 0; j <
_dof; ++j) {
919 qs(j+1) = prev.at(j);
923 bool converge =
false;
927 for (
int j = 0; j <
_dom; ++j) {
928 angle.push_back(
q0(j + 1));
942 if ((
int)pose.size() < 6 || (int)prev.size() <
_dof || maxBisection < 0)
945 int ok =
invKin(pose, prev, conf);
951 if (ok < 0 && maxBisection > 0) {
953 std::vector<double> prev1pose;
957 std::vector<double> prev2pose;
958 for (
int i = 0; i < 6; ++i)
959 prev2pose.push_back(prev1pose.at(i) + pose.at(i) / 2.0);
962 std::vector<double> prev2conf;
976 std::vector<double>&
angle) {
977 if (
_type < 0 || (
int)pose.size() < 6 || (int)prev.size() <
_dof)
980 std::vector<double> positions;
981 for (
int i = 0; i < 6; ++i) {
983 positions.push_back(pose.at(i) * 1000);
985 positions.push_back(pose.at(i));
988 std::vector<double> previous_angles;
989 std::vector<double> prevK4D;
991 for (
int i = 0; i <
_dom; ++i) {
992 previous_angles.push_back(prevK4D.at(i));
996 previous_angles.push_back(0.0);
999 std::vector<double> anglesK4D;
1004 previous_angles)) - 1;
1016 for (
int i = 0; i <
_dof; ++i)
1017 angle.push_back(prev.at(i));
1020 int ok = (error == 0) ? 1 : -1;
1026 std::vector<double> configpose;
1028 double posdiff = 0.0;
1029 for (
int i = 0; i < 6; ++i) {
1030 posdiff += abs(pose.at(i) - configpose.at(i));
1050 double tol = 0.0001;
1059 Pos(1,4) = pose.at(0);
1060 Pos(2,4) = pose.at(1);
1061 Pos(3,4) = pose.at(2);
1067 TCP_Offset.
row(4) << 0.0 << 0.0 << 0.0 << 1.0;
1068 Pos = Pos * TCP_Offset;
1070 std::vector<double> flangepose;
1071 flangepose.push_back(Pos(1,4));
1072 flangepose.push_back(Pos(2,4));
1073 flangepose.push_back(Pos(3,4));
1076 flangepose.push_back(eul(1));
1080 double phi_calc =
atan1(Pos(1,4),Pos(2,4))+
mPi/2.0;
1081 if (Pos(1,4)*eul(1) < 0) {
1086 phi_calc = fmod(phi_calc+
mPi,2.0*
mPi)-
mPi;
1088 if(phi_calc==-
mPi) phi_calc+=2.0*
mPi;
1090 flangepose.push_back(phi_calc);
1092 flangepose.push_back(eul(2));
1093 flangepose.push_back(eul(3));
1096 std::vector<double> prev1conf;
1097 for(
int i = 0; i <
_dof; ++i){
1102 prev1conf.push_back(prev.at(i));
1106 std::vector<double> conf;
1109 int ok =
invKin_bisec(flangepose, prev1conf, conf, maxBisection);
1113 if (flangepose[3] > 0)
1114 flangepose[3] -=
mPi;
1116 flangepose[3] +=
mPi;
1117 ok =
invKin_bisec(flangepose, prev1conf, conf, maxBisection);
1121 if (ok < 0 && _type >= 0) {
1122 std::vector<double> guess;
1123 ok =
anaGuess(flangepose, prev1conf, guess);
1127 ok =
invKin_bisec(flangepose, guess, conf, maxBisection);
1135 std::vector<double> nearpose;
1136 nearpose.push_back(flangepose.at(0) -
sign(flangepose.at(0)) * 0.05);
1137 nearpose.push_back(flangepose.at(1) -
sign(flangepose.at(1)) * 0.05);
1138 nearpose.push_back(flangepose.at(2) -
sign(flangepose.at(2)) * 0.05);
1139 nearpose.push_back(flangepose.at(3) -
sign(flangepose.at(3)) * 0.2);
1140 nearpose.push_back(flangepose.at(4) -
sign(flangepose.at(4) -
mPi / 2.0) * 0.2);
1141 nearpose.push_back(flangepose.at(5) -
sign(flangepose.at(5)) * 0.2);
1142 ok =
anaGuess(nearpose, prev1conf, guess);
1145 ok =
invKin_bisec(flangepose, guess, conf, maxBisection);
1156 for (
int i = 0; i <
_dom; ++i)
1157 angles.push_back(conf.at(i));
1160 for (
int i = 0; i <
_dom; ++i)
1161 angles.push_back(prev.at(i));
int _rotDir[MaxDof]
Rotation direction.
const double Link_length_90A_G[]
virtual bool setLinkLength(const std::vector< double > aLengths)=0
set link length
const double Link_length_180[]
const double Angle_range_90A_F[]
virtual bool setAngStop(const std::vector< double > aAngStop)=0
set angle stop
FloatVector FloatVector FloatVector int typeNr
const double Link_length_90B_G[]
const double Angle_range_180[]
int setEncOff(std::vector< int > encOffset)
This sets the encoder offsets.
int getImmob()
Get the immobile flag of the last joint.
const int Rotation_direction[]
Rotation direction.
virtual bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)=0
const Real Katana6M90A_G_data[]
int setAngRan(std::vector< double > angleRange)
This sets the angle range.
double _tcpOffset[4]
TCP offset.
const int MaxDof
Maximum degree of freedom.
int getAngStop(std::vector< double > &angleStop)
Get angle stop.
int rad2enc(std::vector< double > angles, std::vector< int > &encoders)
Converts angles to encoders.
const int Encoder_offset[]
Encoder offset.
int setRotDir(std::vector< int > rotDir)
This sets the rotation direction.
const double Angle_offset_90A_F[]
Implements the kinematics for the Katana6M180.
int getAngRan(std::vector< double > &angleRange)
Get the angle range.
const Real Katana6M90A_F_data[]
~KinematicsLib()
Destructor.
const double Angle_range_90B_G[]
const Real Katana6M90B_G_data[]
const double Link_length_90B_F[]
int getEncOff(std::vector< int > &encOffset)
Get the encoder offsets.
Implements the kinematics for the Katana6M90T.
const int Encoder_per_cycle[]
Encoder per cycle.
int _dom
Degree of mobility.
double _angleMin[MaxDof]
Angle min.
bool _angRanInit
Angle range.
Base Class for the kinematics implementations.
int setMDH(std::vector< double > theta, std::vector< double > d, std::vector< double > a, std::vector< double > alpha, int typeNr=-1)
This sets the modified Denavit-Hartenberg parameters.
ReturnMatrix inv_kin(const Matrix &Tobj, const int mj=0)
Overload inv_kin function.
int invKin(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angle)
int getAngOff(std::vector< double > &angleOffset)
Get the angle offsets.
int _encoderOffset[MaxDof]
Encoder offset.
void set_q(const ColumnVector &q)
Set the joint position vector.
int getDOF()
Get the degree of freedom.
#define KINLIB_VERSION_MINOR
const Real Katana6M90B_F_data[]
Modified DH notation robot class.
FloatVector FloatVector int maxBisection
int getDOM()
Get the degree of mobility.
ReturnMatrix ieulzxz(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
const double Angle_range_90B_F[]
#define KINLIB_VERSION_MAJOR
int anaGuess(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angle)
int _epc[MaxDof]
Encoder per cycle.
int init()
This initializes the kinematics.
int getAngMin(std::vector< double > &angleMin)
Get angle min.
int invKin_bisec(std::vector< double > pose, std::vector< double > prev, std::vector< double > &conf, int maxBisection)
The usual rectangular matrix.
int setAngOff(std::vector< double > angleOffset)
This sets the angle offsets.
bool _matrixInit
Matrices for mDH, angle range and immobile data.
mRobot _robot
Roboop robot class.
const double Angle_offset_180[]
int _dof
Degree of freedom.
int mDH2K4DAng(std::vector< double > angleMDH, std::vector< double > &angleK4D)
This converts angles from mDH to Katana4D convention.
int getType()
Get the robot type.
FloatVector FloatVector * a
double _angleOffset[MaxDof]
const double Angle_offset_90B_F[]
bool checkConfig(std::vector< double > config, std::vector< double > pose, double tol=0.00001)
int setImmob(int immobile)
This sets the immobile flag of the last joint.
const double Angle_range_90A_G[]
bool _angOffInit
Angle offset.
double _angleRange[MaxDof]
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
const double LENGTH_MULTIPLIER
int getTcpOff(std::vector< double > &tcpOffset)
Get the tcp offset.
FloatVector FloatVector FloatVector * alpha
KinematicsLib()
Constructor.
int inverseKinematics(std::vector< double > pose, std::vector< double > prev, std::vector< double > &angles, int maxBisection=0)
Calculates the inverse kinematics.
Implements the kinematics for the Katana6M90G.
int setLinkLen(std::vector< double > links)
This sets the link length parameters (part of mDH-parameters)
AnaGuess::Kinematics * _anaGuess
Analytical guess.
int getVersion(std::vector< int > &version)
Get the version number of the library.
bool _initialized
Kinematic initialized.
int setTcpOff(std::vector< double > tcpOffset)
This sets the tcp offset.
const Real Katana6M180_data[]
int getEPC(std::vector< int > &epc)
Get the encoders per cycle.
int K4D2mDHAng(std::vector< double > angleK4D, std::vector< double > &angleMDH)
This converts angles from Katana4D to mDH convention.
ReturnMatrix eulzxz(const ColumnVector &a)
Euler ZXZ rotation.
int directKinematics(std::vector< double > angles, std::vector< double > &pose)
Calculates the direct kinematics.
int getRotDir(std::vector< int > &rotDir)
Get the rotation direction.
int enc2rad(std::vector< int > encoders, std::vector< double > &angles)
Converts encoders to angles.
int angleArrMDH2vecK4D(const double arr[], std::vector< double > *vec)
#define KINLIB_VERSION_REVISION
int setType(int type)
This sets the robot type.
int _immobile
Last joint is immobile (eg. K_6M90A_G, K_6M90B_G), _dom = _dof - 1.
const double Angle_offset_90B_G[]
const int errorNumber() const
double _angleMax[MaxDof]
Angle max.
int getMaxDOF()
Get the maximum degree of freedom.
int getAngMax(std::vector< double > &angleMax)
Get angle max.
int setEPC(std::vector< int > epc)
This sets the encoders per cycle.
std::vector< int > encoders
GetSubMatrix row(int) const
double _linkLength[4]
Link length (for defined types)
virtual bool setAngOff(const std::vector< double > aAngOff)=0
set angle offset
const double Link_length_90A_F[]
const double Angle_offset_90A_G[]
int getMDH(std::vector< double > &theta, std::vector< double > &d, std::vector< double > &a, std::vector< double > &alpha)
Get the modified Denavit-Hartenberg parameters.