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 ) {
146 std::vector<double> currentAngles(6);
147 for(
int i = 0; i < 6; ++i) {
148 currentAngles[i] = aAngles[i];
152 currentAngles[1] = currentAngles[1] -
MHF_PI/2.0;
153 currentAngles[2] = currentAngles[2] -
MHF_PI;
154 currentAngles[3] = MHF_PI - currentAngles[3];
155 currentAngles[4] = -currentAngles[4];
158 double r13, r23, r31, r32;
160 std::vector<double>
pose(6);
162 std::vector<double> cx(currentAngles.size()), sx(currentAngles.size());
163 std::vector<double>::iterator cx_iter, sx_iter;
165 std::vector<double>
angle = currentAngles;
167 angle[2] = angle[1]+angle[2];
168 angle[3] = angle[2]+angle[3];
170 cx_iter = cx.begin();
171 sx_iter = sx.begin();
177 pose[0] = cx[0]*factor;
180 pose[1] = sx[0]*factor;
188 pose[3] = atan2(r13,-r23);
191 pose[4] = acos(cx[3]);
196 pose[5] = atan2(r31,r32);
203 const std::vector<double> aStartingAngles) {
215 double coeff1, coeff2, theta234;
216 double costh5, sinth5, theta5[2];
217 double R11, R21, R31, R32;
218 double phi, theta, psi;
221 p_m.
x = aPosition[0];
222 p_m.
y = aPosition[1];
223 p_m.
z = aPosition[2];
226 angle[0].theta1 =
MHF::atan1(aPosition[0],aPosition[1]);
227 if (angle[0].theta1 >
MHF_PI) {
228 angle[0].theta1 = angle[0].theta1 -
MHF_PI;
229 if (angle[0].theta1 > (179.91/180*
MHF_PI)) {
230 angle[0].theta1 = angle[0].theta1 -
MHF_PI;
233 angle[4].theta1 = angle[0].theta1+
MHF_PI;
235 theta = aPosition[4];
238 theta234 = aPosition[4];
240 R11 = cos(phi)*cos(psi)-sin(phi)*cos(theta)*sin(psi);
241 R21 = sin(phi)*cos(psi)+cos(phi)*cos(theta)*sin(psi);
242 R31 = sin(theta)*sin(psi);
243 R32 = sin(theta)*cos(psi);
248 for (
int i=0; i<2; ++i) {
249 coeff1 = -sin(angle[i*4].theta1);
250 coeff2 = -cos(angle[i*4].theta1);
251 costh5 = coeff1*R11-coeff2*R21;
252 sinth5 = coeff1*R21+coeff2*R11;
257 angle[i].theta5 = theta5[0];
259 angle[i].theta5 = theta5[1];
261 }
else if(theta234==
MHF_PI) {
263 for (
int i=0; i<2; ++i) {
264 coeff1 = -sin(angle[i*4].theta1);
265 coeff2 = cos(angle[i*4].theta1);
266 costh5 = coeff1*R11+coeff2*R21;
267 sinth5 = -coeff1*R21+coeff2*R11;
272 angle[i].theta5 = theta5[0];
274 angle[i].theta5 = theta5[1];
277 theta5[0] = -atan2(R31/sin(theta234),R32/sin(theta234));
278 theta5[1] = -atan2(R31/sin(-theta234),R32/sin(-theta234));
281 angle[i].theta5 = theta5[0];
283 angle[i].theta5 = theta5[1];
289 angle[0].theta234 = aPosition[4];
294 angle[0].theta3 = acos(angle[0].costh3)-
MHF_PI;
296 angle[1].theta3 = -acos(angle[1].costh3)+
MHF_PI;
300 angle[2].theta1 = angle[0].theta1;
301 angle[2].theta234 = -angle[0].theta234;
306 angle[2].theta3 = acos(angle[2].costh3)-
MHF_PI;
308 angle[3].theta3 = -acos(angle[3].costh3)+
MHF_PI;
313 angle[4].theta234 = aPosition[4];
318 angle[4].theta3 = acos(angle[4].costh3)-
MHF_PI;
320 angle[5].theta3 = -acos(angle[5].costh3)+
MHF_PI;
324 angle[6].theta1 = angle[4].theta1;
325 angle[6].theta234 = -angle[4].theta234;
330 angle[6].theta3 = acos(angle[6].costh3)-
MHF_PI;
332 angle[7].theta3 = -acos(angle[7].costh3)+
MHF_PI;
336 for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end();) {
339 iter = angle.erase(iter);
344 iter = angle.erase(iter);
348 if(angle.size() == 0) {
353 std::vector< std::vector<double> > PossibleTargets;
354 for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
355 std::vector<double> possangles(5);
357 possangles[0] = i->theta1;
358 possangles[1] = i->theta2;
359 possangles[2] = i->theta3;
360 possangles[3] = i->theta4;
361 possangles[4] = i->theta5;
363 PossibleTargets.push_back(possangles);
367 std::vector< std::vector<double> >::const_iterator sol =
KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
369 if(sol == PossibleTargets.end()) {
374 for (
int i = aAngles.size(); i < 6; ++i)
375 aAngles.push_back(0.0);
376 std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
377 *gripper_iter = aStartingAngles[5];
460 double temp, xm2, ym2, zm2;
bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)
std::vector< int > mEncodersPerCycle
Encoders per cycle vector.
int mNumberOfSegments
Number of segments of the robot.
void IK_b1b2costh3_6M180(angles_calc &a, const position &p) const
helper functions
bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)
std::vector< int > getDir()
get direction
std::vector< int > mRotationDirection
Rotation direction vector [1|-1].
_T anglereduce(const _T a)
void swap(Matrix &A, Matrix &B)
bool setAngOff(const std::vector< double > aAngOff)
set angle offset
double findFirstEqualAngle(double cosValue, double sinValue, double tolerance)
std::vector< double > mSegmentLength
Effector segment lengths vector [m].
std::vector< int > getEncOff()
get encoder offset
static const int cNrOfPossibleSolutions
std::vector< int > getEpc()
get encoders per cycle
std::vector< angles_calc > angles_container
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< double > getLinkLength()
get link length
std::vector< double > getAngRange()
get angle range
~Kinematics6M180()
Destructor.
void thetacomp(angles_calc &a, const position &p_m) const
std::vector< double > mAngleStop
Angle stop vector [rad].
bool PositionTest6M180(const angles_calc &a, const position &p) const
bool setAngStop(const std::vector< double > aAngStop)
set angle stop
std::vector< double > getAngMin()
get angle min
std::vector< double > getAngMax()
get angle max
bool rad2enc(std::vector< int > &aEncoders, const std::vector< double > aAngles)
FloatVector FloatVector * a
std::vector< double > getAngStop()
get angle stop
bool setLinkLength(const std::vector< double > aLengths)
set link length
std::vector< int > mEncoderOffset
Encoder offset vector.
_T atan0(const _T in1, const _T in2)
int mNumberOfMotors
Number of motors of the robot.
std::vector< double > mAngleOffset
Angle offset vector [rad].
bool AnglePositionTest(const angles_calc &a) const
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
structs, type and constants used in inverse kinematics calculation
bool initialize()
initialization routine
bool mIsInitialized
Initialized flag.
Kinematics6M180()
Constructor.
std::vector< double > getAngOff()
get angle offset
bool angledef(angles_calc &a) const
bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)