33 double a, b, c, alpha1, alpha2;
38 for(
unsigned int z = 0; z < current_encoders.size(); ++z) {
44 c = sqrt(
pow2(a)+
pow2(b)-2*a*b*cos(current_angles[2]));
45 alpha1 = asin(a*sin(current_angles[2])/c);
46 alpha2 = current_angles[1]-alpha1;
48 pose[0] = c*cos(alpha2)*cos(current_angles[0]);
49 pose[1] = c*cos(alpha2)*sin(current_angles[0]);
50 pose[2] = c*sin(alpha2);
57 assert( (length.size() == 3) &&
"You have to provide the metrics for exactly 3 links" );
58 assert( (parameters.size() == 5) &&
"You have to provide exactly 5 motor parameters" );
76 double alpha1Px, alpha2Px;
84 alpha2Px = asin(p_m.
z/dist);
86 angle[0].theta1 =
atan1(p_m.
x,p_m.
y);
88 angle[0].theta1 -= 2*
M_PI;
92 angle[0].theta3 = acos((
pow2(a)+
pow2(b)-
pow2(dist))/(2*a*b));
94 angle[0].theta3 -= 2*
M_PI;
96 alpha1Px = asin(a*sin(angle[0].theta3)/dist);
97 angle[0].theta2 = alpha1Px+alpha2Px;
99 angle[0].theta2 -= 2*
M_PI;
101 std::vector<int> temp_solution(5);
107 temp_solution[3] = current_encoders[3];
108 temp_solution[4] = current_encoders[4];
110 std::copy(temp_solution.begin(), temp_solution.end(), solution);
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
static const double _tolerance
std::vector< double > angles
Being used to store angles (in radian).
static const int _nrOfPossibleSolutions
void _setParameters(parameter_container const ¶meters)
void swap(Matrix &A, Matrix &B)
std::vector< KinematicParameters > parameter_container
FloatVector FloatVector * a
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
void IK(encoders::iterator solution, coordinates const &pose, encoders const &cur_angles) const
void DK(coordinates &solution, encoders const ¤t_encoders) const
void _setLength(metrics const &length)
std::vector< double > coordinates
To store coordinates.
std::vector< int > encoders
To store encoders.
void init(metrics const &length, parameter_container const ¶meters)
std::vector< double > metrics
To store metrics, 'aka' the length's of the different segments of the robot.
std::vector< angles_calc > angles_container
parameter_container _parameters