36 double R13, R23, R31, R32;
39 for(
int z = 0; z < 6; ++z) {
44 current_angles[1] = current_angles[1] -
M_PI/2.0;
45 current_angles[2] = current_angles[2] -
M_PI;
46 current_angles[3] = M_PI - current_angles[3];
47 current_angles[4] = -current_angles[4];
51 angles cx(current_angles.size()), sx(current_angles.size());
52 angles::iterator cx_iter, sx_iter;
56 angle[2] = angle[1]+angle[2];
57 angle[3] = angle[2]+angle[3];
66 pose[0] = cx[0]*factor;
69 pose[1] = sx[0]*factor;
77 pose[3] = atan2(R13,-R23);
80 pose[4] = acos(cx[3]);
85 pose[5] = atan2(R31,R32);
93 assert( (length.size() == 4) &&
"You have to provide the metrics for exactly 4 links" );
94 assert( (parameters.size() == 6) &&
"You have to provide exactly 5 motor parameters" );
128 double temp, xm2, ym2, zm2;
201 double coeff1, coeff2, theta234;
202 double costh5, sinth5, theta5[2];
203 double R11, R21, R31, R32;
204 double phi, theta, psi;
212 angle[0].theta1 =
atan1(pose[0],pose[1]);
213 if (angle[0].theta1 >
M_PI) {
214 angle[0].theta1 = angle[0].theta1 -
M_PI;
215 if (angle[0].theta1 > (179.91/180*
M_PI)) {
216 angle[0].theta1 = angle[0].theta1 -
M_PI;
219 angle[4].theta1 = angle[0].theta1+
M_PI;
226 R11 = cos(phi)*cos(psi)-sin(phi)*cos(theta)*sin(psi);
227 R21 = sin(phi)*cos(psi)+cos(phi)*cos(theta)*sin(psi);
228 R31 = sin(theta)*sin(psi);
229 R32 = sin(theta)*cos(psi);
234 for (
int i=0; i<2; ++i) {
235 coeff1 = -sin(angle[i*4].theta1);
236 coeff2 = -cos(angle[i*4].theta1);
237 costh5 = coeff1*R11-coeff2*R21;
238 sinth5 = coeff1*R21+coeff2*R11;
243 angle[i].theta5 = theta5[0];
245 angle[i].theta5 = theta5[1];
247 }
else if(theta234==
M_PI) {
249 for (
int i=0; i<2; ++i) {
250 coeff1 = -sin(angle[i*4].theta1);
251 coeff2 = cos(angle[i*4].theta1);
252 costh5 = coeff1*R11+coeff2*R21;
253 sinth5 = -coeff1*R21+coeff2*R11;
258 angle[i].theta5 = theta5[0];
260 angle[i].theta5 = theta5[1];
263 theta5[0] = -atan2(R31/sin(theta234),R32/sin(theta234));
264 theta5[1] = -atan2(R31/sin(-theta234),R32/sin(-theta234));
267 angle[i].theta5 = theta5[0];
269 angle[i].theta5 = theta5[1];
275 angle[0].theta234 = pose[4];
280 angle[0].theta3 = acos(angle[0].costh3)-
M_PI;
282 angle[1].theta3 = -acos(angle[1].costh3)+
M_PI;
286 angle[2].theta1 = angle[0].theta1;
287 angle[2].theta234 = -angle[0].theta234;
292 angle[2].theta3 = acos(angle[2].costh3)-
M_PI;
294 angle[3].theta3 = -acos(angle[3].costh3)+
M_PI;
300 angle[4].theta234 = pose[4];
305 angle[4].theta3 = acos(angle[4].costh3)-
M_PI;
307 angle[5].theta3 = -acos(angle[5].costh3)+
M_PI;
311 angle[6].theta1 = angle[4].theta1;
312 angle[6].theta234 = -angle[4].theta234;
317 angle[6].theta3 = acos(angle[6].costh3)-
M_PI;
319 angle[7].theta3 = -acos(angle[7].costh3)+
M_PI;
322 for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); ) {
323 if(
pow2(iter->costh3) <= 1.0) {
325 iter = angle.erase(iter);
330 iter = angle.erase(iter);
334 if(angle.size() == 0) {
338 std::vector< std::vector<int> > PossibleTargetsInEncoders;
339 for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
340 std::vector<int> solution(5);
348 PossibleTargetsInEncoders.push_back(solution);
352 std::vector< std::vector<int> >::const_iterator sol =
KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
354 assert( sol != PossibleTargetsInEncoders.end() &&
"All solutions are out of range");
357 encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
358 *gripper_encoder_iter = current_encoders[5];
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
parameter_container _parameters
static const double _tolerance
std::vector< double > angles
Being used to store angles (in radian).
bool angledef(angles_calc &a) const
bool AnglePositionTest(const angles_calc &a) const
std::vector< angles_calc > angles_container
void thetacomp(angles_calc &a, const position &p_m) const
_T anglereduce(const _T a)
void swap(Matrix &A, Matrix &B)
void DK(coordinates &solution, encoders const ¤t_encoders) const
void init(metrics const &length, parameter_container const ¶meters)
std::vector< KinematicParameters > parameter_container
void _setLength(metrics const &length)
FloatVector FloatVector * a
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
void IK_b1b2costh3_6M180(angles_calc &a, const position &p) const
std::vector< double > coordinates
To store coordinates.
bool PositionTest6M180(const angles_calc &a, const position &p) const
void _setParameters(parameter_container const ¶meters)
_T atan0(const _T in1, const _T in2)
std::vector< int > encoders
To store encoders.
static const int _nrOfPossibleSolutions
void IK(encoders::iterator solution, coordinates const &pose, encoders const &cur_angles) const
std::vector< double > metrics
To store metrics, 'aka' the length's of the different segments of the robot.