50 std::vector<double> result;
52 for (
int i = 0; i < 6; i++) {
55 result.push_back(-diff);
57 result.push_back(diff);
64 std::vector<double> result;
65 for (
int i = 0; i < 6; i++) {
76 std::vector<double> result;
77 for (
int i = 0; i < 6; i++) {
126 for(
int i = 0; i < 6; ++i) {
133 for(
int i = 0; i < 6; ++i ) {
147 double x0, x1, x2, x3;
148 double y0, y1, y2, y3;
149 double z0, z1, z2, z3;
151 std::vector<double> current_angles(6);
152 for(
int i = 0; i < 6; ++i) {
153 current_angles[i] = aAngles[i];
157 current_angles[1] = current_angles[1] -
MHF_PI/2.0;
158 current_angles[2] = current_angles[2] -
MHF_PI;
159 current_angles[3] = MHF_PI - current_angles[3];
160 current_angles[5] = -current_angles[5];
162 std::vector<double>
pose(6);
164 std::vector<double> cx(current_angles.size()), sx(current_angles.size());
165 std::vector<double>::iterator cx_iter, sx_iter;
167 std::vector<double>
angle = current_angles;
169 angle[2] = angle[1]+angle[2];
170 angle[3] = angle[2]+angle[3];
172 cx_iter = cx.begin();
173 sx_iter = sx.begin();
182 x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
189 y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
190 pose[1] = y0*mSegmentLength[0]+y1*mSegmentLength[1]+y2*mSegmentLength[2]+y3*mSegmentLength[3];
197 pose[2] = z0*mSegmentLength[0]+z1*mSegmentLength[1]+z2*mSegmentLength[2]+z3*mSegmentLength[3];
201 pose[4] = acos(cx[4]*sx[3]);
206 const double theta1 = angle[0];
207 const double theta5 = angle[4];
208 const double theta6 = angle[5];
209 const double theta234 = angle[3];
213 std::vector<double> v1(2), v2(2);
215 double R11 = -sin(theta1)*cos(theta5) *sin(theta6) + cos(theta1)*(sin(theta234)*cos(theta6)+cos(theta234)*sin(theta5)*sin(theta6));
216 double R21 = sin(theta1)*sin(theta234)*cos(theta6) + sin(theta6)*(cos(theta1) *cos(theta5)+cos(theta234)*sin(theta1)*sin(theta5));
221 v2[1] = MHF_PI - v2[0];
229 const double R13 = -cos(theta1)*cos(theta234)*cos(theta5) - sin(theta1)*sin(theta5);
230 const double R23 = -sin(theta1)*cos(theta234)*cos(theta5) + cos(theta1)*sin(theta5);
231 pose[3] = atan2(R13, -R23);
234 const double R31 = cos(theta234)*cos(theta6) - sin(theta234)*sin(theta5)*sin(theta6);
235 const double R32 = -cos(theta234)*sin(theta6) - sin(theta234)*sin(theta5)*cos(theta6);
236 pose[5] = atan2(R31, R32);
244 const std::vector<double> aStartingAngles) {
263 p_m.
x = aPosition[0]-p_gr.
x;
264 p_m.
y = aPosition[1]-p_gr.
y;
265 p_m.
z = aPosition[2]-p_gr.
z;
269 angle[4].theta1 = angle[0].theta1+
MHF_PI;
274 angle[0].theta1=angle[0].theta1-2.0*
MHF_PI;
277 angle[0].theta1=angle[0].theta1+2.0*
MHF_PI;
280 angle[4].theta1=angle[4].theta1-2.0*
MHF_PI;
283 angle[4].theta1=angle[4].theta1+2.0*
MHF_PI;
292 angle[0].theta3 = acos(angle[0].costh3)-
MHF_PI;
294 angle[1].theta3 = -acos(angle[1].costh3)+
MHF_PI;
298 angle[2].theta1=angle[0].theta1;
299 angle[2].theta234=angle[0].theta234-
MHF_PI;
300 angle[2].theta5=
MHF_PI-angle[0].theta5;
304 angle[2].theta3 = acos(angle[2].costh3)-
MHF_PI;
306 angle[3].theta3 = -acos(angle[3].costh3)+
MHF_PI;
316 angle[4].theta3 = acos(angle[4].costh3)-
MHF_PI;
318 angle[5].theta3 = -acos(angle[5].costh3)+
MHF_PI;
322 angle[6].theta1=angle[4].theta1;
323 angle[6].theta234=angle[4].theta234-
MHF_PI;
324 angle[6].theta5=
MHF_PI-angle[4].theta5;
327 angle[6].theta3 = acos(angle[6].costh3)-
MHF_PI;
329 angle[7].theta3 = -acos(angle[7].costh3)+
MHF_PI;
333 for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end();) {
336 iter = angle.erase(iter);
341 iter = angle.erase(iter);
346 if(angle.size() == 0) {
351 std::vector< std::vector<double> > PossibleTargets;
352 for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
353 std::vector<double> possangles(6);
355 possangles[0] = i->theta1;
356 possangles[1] = i->theta2;
357 possangles[2] = i->theta3;
358 possangles[3] = i->theta4;
359 possangles[4] = i->theta5;
360 possangles[5] = i->theta6;
362 PossibleTargets.push_back(possangles);
366 std::vector< std::vector<double> >::const_iterator sol =
KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
368 if(sol == PossibleTargets.end()) {
373 for (
int i = aAngles.size(); i < 6; ++i)
374 aAngles.push_back(0.0);
375 std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
479 double xgr2, ygr2, zgr2;
494 xg = p.
x + ( mSegmentLength[3] * cos(angle.
theta1) * sin(angle.
theta234) );
495 yg = p.
y + ( mSegmentLength[3] * sin(angle.
theta1) * sin(angle.
theta234) );
496 zg = p.
z + ( mSegmentLength[3] * cos(angle.
theta234) );
506 for(std::vector<double>::const_iterator i = v1.begin(); i != v1.end(); ++i) {
507 for(std::vector<double>::const_iterator j = v2.begin(); j != v2.end(); ++j) {
512 throw Exception(
"precondition for findFirstEqualAngle failed -> no equal angles found", -2);
518 const double theta1 = angle.
theta1;
520 const double theta3 = angle.
theta3;
522 const double theta5 = angle.
theta5;
524 const double theta234 = angle.
theta234;
525 const double b1 = angle.
b1;
526 const double b2 = angle.
b2;
528 const double phi = pose[3];
529 const double theta = pose[4];
530 const double psi = pose[5];
534 theta4 = theta234 - theta2 - theta3;
538 theta4 = theta234 - theta2 - theta3;
541 const double R11 = cos(phi)*cos(psi) - sin(phi)*cos(theta)*sin(psi);
542 const double R21 = sin(phi)*cos(psi) + cos(phi)*cos(theta)*sin(psi);
544 std::vector<double> theta16c(2), theta16s(2);
548 theta16c[0] = acos(-R11);
549 theta16c[1] = -theta16c[0];
550 theta16s[0] = asin(-R21);
551 theta16s[1] =
MHF_PI - theta16s[0];
556 theta16c[0] = acos(-R11);
557 theta16c[1] = -theta16c[0];
558 theta16s[0] = asin(-R21);
559 theta16s[1] =
MHF_PI - theta16s[0];
564 throw Exception(
"Special case \"|theta234+(1/2)*pi| = 0\" detected, but no solution found", -1);
569 theta16c[0] = acos(R11);
570 theta16c[1] = -theta16c[0];
571 theta16s[0] = asin(R21);
572 theta16s[1] =
MHF_PI - theta16s[0];
577 theta16c[0] = acos(R11);
578 theta16c[1] = -theta16c[0];
579 theta16s[0] = asin(R21);
580 theta16s[1] =
MHF_PI -theta16s[0];
584 throw Exception(
"Special case \"|theta234+(3/2)*pi| = 0\" detected, but no solution found", -1);
589 const double R31 = sin(theta)*sin(psi);
590 const double R32 = sin(theta)*cos(psi);
592 const double temp1 = cos(theta234);
593 const double temp2 = -sin(theta234)*sin(theta5);
598 theta16c[0] = acos(c);
599 theta16c[1] = -theta16c[0];
600 theta16s[0] = asin(s);
601 theta16s[1] =
MHF_PI - theta16s[0];
613 double temp, xm2, ym2, zm2;
616 xm2 = cos(theta1)*temp;
617 ym2 = sin(theta1)*temp;
std::vector< double > mAngleStop
Angle stop vector [rad].
bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)
std::vector< double > getAngMin()
get angle min
std::vector< int > getDir()
get direction
void thetacomp(angles_calc &angle, const position &p_m, const std::vector< double > &pose) const
bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)
std::vector< double > getAngStop()
get angle stop
_T anglereduce(const _T a)
std::vector< double > getLinkLength()
get link length
void swap(Matrix &A, Matrix &B)
bool PositionTest6MS(const double &theta1, const double &theta2, const double &theta3, const double &theta234, const position &p) const
void IK_theta234theta5(angles_calc &angle, const position &p_gr) const
helper functions
bool mIsInitialized
Initialized flag.
bool AnglePositionTest(const angles_calc &a) const
bool setAngOff(const std::vector< double > aAngOff)
set angle offset
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
Kinematics6M90T()
Constructor.
std::vector< int > mRotationDirection
Rotation direction vector [1|-1].
bool setLinkLength(const std::vector< double > aLengths)
set link length
~Kinematics6M90T()
Destructor.
int mNumberOfMotors
Number of motors of the robot.
bool rad2enc(std::vector< int > &aEncoders, const std::vector< double > aAngles)
std::vector< angles_calc > angles_container
structs, type and constants used in inverse kinematics calculation
int mNumberOfSegments
Number of segments of the robot.
std::vector< double > getAngMax()
get angle max
FloatVector FloatVector * a
bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)
std::vector< double > mSegmentLength
Effector segment lengths vector [m].
std::vector< double > getAngRange()
get angle range
_T atan0(const _T in1, const _T in2)
void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const
std::vector< int > mEncodersPerCycle
Encoders per cycle vector.
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< double > getAngOff()
get angle offset
double findFirstEqualAngle(const std::vector< double > &v1, const std::vector< double > &v2) const
bool setAngStop(const std::vector< double > aAngStop)
set angle stop
bool initialize()
initialization routine
std::vector< int > getEncOff()
get encoder offset
std::vector< int > mEncoderOffset
Encoder offset vector.
std::vector< int > getEpc()
get encoders per cycle
bool GripperTest(const position &p_gr, const angles_calc &angle) const
static const int cNrOfPossibleSolutions
std::vector< double > mAngleOffset
Angle offset vector [rad].
bool angledef(angles_calc &a) const