51 std::vector<double> result;
53 for (
int i = 0; i < 6; i++) {
56 result.push_back(-diff);
58 result.push_back(diff);
65 std::vector<double> result;
66 for (
int i = 0; i < 6; i++) {
77 std::vector<double> result;
78 for (
int i = 0; i < 6; i++) {
127 for(
int i = 0; i < 6; ++i) {
134 for(
int i = 0; i < 6; ++i ) {
148 double x0, x1, x2, x3;
149 double y0, y1, y2, y3;
150 double z0, z1, z2, z3;
151 double R13, R23, R33, R31, R32;
153 std::vector<double> current_angles(6);
154 for(
int i = 0; i < 6; ++i) {
155 current_angles[i] = aAngles[i];
159 current_angles[1] = current_angles[1] -
MHF_PI/2.0;
160 current_angles[2] = current_angles[2] -
MHF_PI;
161 current_angles[3] = MHF_PI - current_angles[3];
163 std::vector<double>
pose(6);
165 std::vector<double> cx(current_angles.size()), sx(current_angles.size());
166 std::vector<double>::iterator cx_iter, sx_iter;
168 std::vector<double>
angle = current_angles;
170 angle[2] = angle[1]+angle[2];
171 angle[3] = angle[2]+angle[3];
173 cx_iter = cx.begin();
174 sx_iter = sx.begin();
178 R13 = (-1*cx[0]*cx[3]*cx[4])-(sx[0]*sx[4]);
179 R23 = (-1*sx[0]*cx[3]*cx[4])+(cx[0]*sx[4]);
182 R32 =(-1)*sx[3]*sx[4];
188 x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
195 y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
196 pose[1] = y0*mSegmentLength[0]+y1*mSegmentLength[1]+y2*mSegmentLength[2]+y3*mSegmentLength[3];
203 pose[2] = z0*mSegmentLength[0]+z1*mSegmentLength[1]+z2*mSegmentLength[2]+z3*mSegmentLength[3];
208 pose[3] = atan2(pose[1],pose[0]);
210 }
else if(pose[4]==MHF_PI) {
211 pose[3] = atan2(pose[1],pose[0])+MHF_PI/2;
214 pose[3] = atan2(R13,-R23);
215 pose[5] = atan2(R31,R32);
224 const std::vector<double> aStartingAngles) {
244 p_m.
x = aPosition[0]-p_gr.
x;
245 p_m.
y = aPosition[1]-p_gr.
y;
246 p_m.
z = aPosition[2]-p_gr.
z;
250 angle[4].theta1 = angle[0].theta1+
MHF_PI;
255 angle[0].theta1=angle[0].theta1-2.0*
MHF_PI;
258 angle[0].theta1=angle[0].theta1+2.0*
MHF_PI;
261 angle[4].theta1=angle[4].theta1-2.0*
MHF_PI;
264 angle[4].theta1=angle[4].theta1+2.0*
MHF_PI;
273 angle[0].theta3 = acos(angle[0].costh3)-
MHF_PI;
275 angle[1].theta3 = -acos(angle[1].costh3)+
MHF_PI;
279 angle[2].theta1=angle[0].theta1;
280 angle[2].theta234=angle[0].theta234-
MHF_PI;
281 angle[2].theta5=
MHF_PI-angle[0].theta5;
285 angle[2].theta3 = acos(angle[2].costh3)-
MHF_PI;
287 angle[3].theta3 = -acos(angle[3].costh3)+
MHF_PI;
297 angle[4].theta3 = acos(angle[4].costh3)-
MHF_PI;
299 angle[5].theta3 = -acos(angle[5].costh3)+
MHF_PI;
303 angle[6].theta1=angle[4].theta1;
304 angle[6].theta234=angle[4].theta234-
MHF_PI;
305 angle[6].theta5=
MHF_PI-angle[4].theta5;
308 angle[6].theta3 = acos(angle[6].costh3)-
MHF_PI;
310 angle[7].theta3 = -acos(angle[7].costh3)+
MHF_PI;
314 for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); ) {
317 iter = angle.erase(iter);
322 iter = angle.erase(iter);
327 if(angle.size() == 0) {
332 std::vector< std::vector<double> > PossibleTargets;
333 for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
334 std::vector<double> possangles(5);
336 possangles[0] = i->theta1;
337 possangles[1] = i->theta2;
338 possangles[2] = i->theta3;
339 possangles[3] = i->theta4;
340 possangles[4] = i->theta5;
342 PossibleTargets.push_back(possangles);
346 std::vector< std::vector<double> >::const_iterator sol =
KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
348 if(sol == PossibleTargets.end()) {
353 for (
int i = aAngles.size(); i < 6; ++i)
354 aAngles.push_back(0.0);
355 std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
356 *gripper_iter = aStartingAngles[5];
461 double xgr2, ygr2, zgr2;
476 xg = p.
x + ( mSegmentLength[3] * cos(angle.
theta1) * sin(angle.
theta234) );
477 yg = p.
y + ( mSegmentLength[3] * sin(angle.
theta1) * sin(angle.
theta234) );
478 zg = p.
z + ( mSegmentLength[3] * cos(angle.
theta234) );
499 double temp, xm2, ym2, zm2;
std::vector< angles_calc > angles_container
std::vector< int > mEncodersPerCycle
Encoders per cycle vector.
void IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const
std::vector< int > mEncoderOffset
Encoder offset vector.
bool initialize()
initialization routine
std::vector< int > getDir()
get direction
std::vector< double > getAngStop()
get angle stop
void thetacomp(angles_calc &angle, const position &p_m) const
std::vector< double > getAngOff()
get angle offset
std::vector< double > getAngMax()
get angle max
_T anglereduce(const _T a)
bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)
bool rad2enc(std::vector< int > &aEncoders, const std::vector< double > aAngles)
void swap(Matrix &A, Matrix &B)
bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)
std::vector< double > getAngRange()
get angle range
void IK_theta234theta5(angles_calc &angle, const position &p_gr) const
helper functions
bool AnglePositionTest(const angles_calc &a) const
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
structs, type and constants used in inverse kinematics calculation
std::vector< double > mAngleStop
Angle stop vector [rad].
bool setAngOff(const std::vector< double > aAngOff)
set angle offset
std::vector< int > mRotationDirection
Rotation direction vector [1|-1].
bool GripperTest(const position &p_gr, const angles_calc &angle) const
std::vector< int > getEpc()
get encoders per cycle
std::vector< double > mAngleOffset
Angle offset vector [rad].
bool setLinkLength(const std::vector< double > aLengths)
set link length
bool setAngStop(const std::vector< double > aAngStop)
set angle offset
bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)
FloatVector FloatVector * a
_T atan0(const _T in1, const _T in2)
bool mIsInitialized
Initialized flag.
std::vector< int > getEncOff()
get encoder offset
int mNumberOfMotors
Number of motors of the robot.
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
bool angledef(angles_calc &a) const
Kinematics6M90G()
Constructor.
bool PositionTest6MS(const angles_calc &a, const position &p) const
static const int cNrOfPossibleSolutions
std::vector< double > getLinkLength()
get link length
std::vector< double > getAngMin()
get angle min
int mNumberOfSegments
Number of segments of the robot.
~Kinematics6M90G()
Destructor.
std::vector< double > mSegmentLength
Effector segment lengths vector [m].