00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "frames.hpp"
00029
00030 #define _USE_MATH_DEFINES // For MSVC
00031 #include <math.h>
00032
00033 namespace KDL {
00034
00035 #ifndef KDL_INLINE
00036 #include "frames.inl"
00037 #endif
00038
00039 void Frame::Make4x4(double * d)
00040 {
00041 int i;
00042 int j;
00043 for (i=0;i<3;i++) {
00044 for (j=0;j<3;j++)
00045 d[i*4+j]=M(i,j);
00046 d[i*4+3] = p(i);
00047 }
00048 for (j=0;j<3;j++)
00049 d[12+j] = 0.;
00050 d[15] = 1;
00051 }
00052
00053 Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
00054
00055 {
00056 double ct,st,ca,sa;
00057 ct = cos(theta);
00058 st = sin(theta);
00059 sa = sin(alpha);
00060 ca = cos(alpha);
00061 return Frame(Rotation(
00062 ct, -st, 0,
00063 st*ca, ct*ca, -sa,
00064 st*sa, ct*sa, ca ),
00065 Vector(
00066 a, -sa*d, ca*d )
00067 );
00068 }
00069
00070 Frame Frame::DH(double a,double alpha,double d,double theta)
00071
00072 {
00073 double ct,st,ca,sa;
00074 ct = cos(theta);
00075 st = sin(theta);
00076 sa = sin(alpha);
00077 ca = cos(alpha);
00078 return Frame(Rotation(
00079 ct, -st*ca, st*sa,
00080 st, ct*ca, -ct*sa,
00081 0, sa, ca ),
00082 Vector(
00083 a*ct, a*st, d )
00084 );
00085 }
00086
00087 double Vector2::Norm() const
00088 {
00089 if (fabs(data[0]) > fabs(data[1]) ) {
00090 return data[0]*sqrt(1+sqr(data[1]/data[0]));
00091 } else {
00092 return data[1]*sqrt(1+sqr(data[0]/data[1]));
00093 }
00094 }
00095
00096
00097
00098 double Vector2::Normalize(double eps) {
00099 double v = this->Norm();
00100 if (v < eps) {
00101 *this = Vector2(1,0);
00102 return v;
00103 } else {
00104 *this = (*this)/v;
00105 return v;
00106 }
00107 }
00108
00109
00110
00111 double Vector::Norm() const
00112 {
00113 double tmp1;
00114 double tmp2;
00115 tmp1 = fabs(data[0]);
00116 tmp2 = fabs(data[1]);
00117 if (tmp1 >= tmp2) {
00118 tmp2=fabs(data[2]);
00119 if (tmp1 >= tmp2) {
00120 if (tmp1 == 0) {
00121
00122 return 0;
00123 }
00124 return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
00125 } else {
00126 return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
00127 }
00128 } else {
00129 tmp1=fabs(data[2]);
00130 if (tmp2 > tmp1) {
00131 return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
00132 } else {
00133 return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
00134 }
00135 }
00136 }
00137
00138
00139
00140
00141 double Vector::Normalize(double eps) {
00142 double v = this->Norm();
00143 if (v < eps) {
00144 *this = Vector(1,0,0);
00145 return v;
00146 } else {
00147 *this = (*this)/v;
00148 return v;
00149 }
00150 }
00151
00152
00153 bool Equal(const Rotation& a,const Rotation& b,double eps) {
00154 return (Equal(a.data[0],b.data[0],eps) &&
00155 Equal(a.data[1],b.data[1],eps) &&
00156 Equal(a.data[2],b.data[2],eps) &&
00157 Equal(a.data[3],b.data[3],eps) &&
00158 Equal(a.data[4],b.data[4],eps) &&
00159 Equal(a.data[5],b.data[5],eps) &&
00160 Equal(a.data[6],b.data[6],eps) &&
00161 Equal(a.data[7],b.data[7],eps) &&
00162 Equal(a.data[8],b.data[8],eps) );
00163 }
00164
00165
00166
00167 Rotation operator *(const Rotation& lhs,const Rotation& rhs)
00168
00169 {
00170 return Rotation(
00171 lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
00172 lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
00173 lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
00174 lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
00175 lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
00176 lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
00177 lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
00178 lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
00179 lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
00180 );
00181
00182 }
00183
00184 Rotation Rotation::Quaternion(double x,double y,double z, double w)
00185 {
00186 double x2, y2, z2, w2;
00187 x2 = x*x; y2 = y*y; z2 = z*z; w2 = w*w;
00188 return Rotation(w2+x2-y2-z2, 2*x*y-2*w*z, 2*x*z+2*w*y,
00189 2*x*y+2*w*z, w2-x2+y2-z2, 2*y*z-2*w*x,
00190 2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2);
00191 }
00192
00193
00194
00195
00196
00197
00198 void Rotation::GetQuaternion(double& x,double& y,double& z, double& w) const
00199 {
00200 double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2);
00201 double epsilon=1E-12;
00202 if( trace > epsilon ){
00203 double s = 0.5 / sqrt(trace + 1.0);
00204 w = 0.25 / s;
00205 x = ( (*this)(2,1) - (*this)(1,2) ) * s;
00206 y = ( (*this)(0,2) - (*this)(2,0) ) * s;
00207 z = ( (*this)(1,0) - (*this)(0,1) ) * s;
00208 }else{
00209 if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) ){
00210 double s = 2.0 * sqrt( 1.0 + (*this)(0,0) - (*this)(1,1) - (*this)(2,2));
00211 w = ((*this)(2,1) - (*this)(1,2) ) / s;
00212 x = 0.25 * s;
00213 y = ((*this)(0,1) + (*this)(1,0) ) / s;
00214 z = ((*this)(0,2) + (*this)(2,0) ) / s;
00215 } else if ((*this)(1,1) > (*this)(2,2)) {
00216 double s = 2.0 * sqrt( 1.0 + (*this)(1,1) - (*this)(0,0) - (*this)(2,2));
00217 w = ((*this)(0,2) - (*this)(2,0) ) / s;
00218 x = ((*this)(0,1) + (*this)(1,0) ) / s;
00219 y = 0.25 * s;
00220 z = ((*this)(1,2) + (*this)(2,1) ) / s;
00221 }else {
00222 double s = 2.0 * sqrt( 1.0 + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) );
00223 w = ((*this)(1,0) - (*this)(0,1) ) / s;
00224 x = ((*this)(0,2) + (*this)(2,0) ) / s;
00225 y = ((*this)(1,2) + (*this)(2,1) ) / s;
00226 z = 0.25 * s;
00227 }
00228 }
00229 }
00230
00231 Rotation Rotation::RPY(double roll,double pitch,double yaw)
00232 {
00233 double ca1,cb1,cc1,sa1,sb1,sc1;
00234 ca1 = cos(yaw); sa1 = sin(yaw);
00235 cb1 = cos(pitch);sb1 = sin(pitch);
00236 cc1 = cos(roll);sc1 = sin(roll);
00237 return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
00238 sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
00239 -sb1,cb1*sc1,cb1*cc1);
00240 }
00241
00242
00243 void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
00244 {
00245 double epsilon=1E-12;
00246 pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) );
00247 if ( fabs(pitch) > (M_PI/2.0-epsilon) ) {
00248 yaw = atan2( -data[1], data[4]);
00249 roll = 0.0 ;
00250 } else {
00251 roll = atan2(data[7], data[8]);
00252 yaw = atan2(data[3], data[0]);
00253 }
00254 }
00255
00256 Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
00257 double sa,ca,sb,cb,sg,cg;
00258 sa = sin(Alfa);ca = cos(Alfa);
00259 sb = sin(Beta);cb = cos(Beta);
00260 sg = sin(Gamma);cg = cos(Gamma);
00261 return Rotation( ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb,
00262 sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb,
00263 -sb*cg , sb*sg, cb
00264 );
00265
00266 }
00267
00268
00269 void Rotation::GetEulerZYZ(double& alpha,double& beta,double& gamma) const {
00270 double epsilon = 1E-12;
00271 if (fabs(data[8]) > 1-epsilon ) {
00272 gamma=0.0;
00273 if (data[8]>0) {
00274 beta = 0.0;
00275 alpha= atan2(data[3],data[0]);
00276 } else {
00277 beta = PI;
00278 alpha= atan2(-data[3],-data[0]);
00279 }
00280 } else {
00281 alpha=atan2(data[5], data[2]);
00282 beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
00283 gamma=atan2(data[7], -data[6]);
00284 }
00285 }
00286
00287 Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
00288
00289
00290
00291
00292 Vector rotvec = rotaxis;
00293 rotvec.Normalize();
00294 return Rotation::Rot2(rotvec,angle);
00295 }
00296
00297 Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
00298
00299
00300
00301
00302
00303 double ct = cos(angle);
00304 double st = sin(angle);
00305 double vt = 1-ct;
00306 double m_vt_0=vt*rotvec(0);
00307 double m_vt_1=vt*rotvec(1);
00308 double m_vt_2=vt*rotvec(2);
00309 double m_st_0=rotvec(0)*st;
00310 double m_st_1=rotvec(1)*st;
00311 double m_st_2=rotvec(2)*st;
00312 double m_vt_0_1=m_vt_0*rotvec(1);
00313 double m_vt_0_2=m_vt_0*rotvec(2);
00314 double m_vt_1_2=m_vt_1*rotvec(2);
00315 return Rotation(
00316 ct + m_vt_0*rotvec(0),
00317 -m_st_2 + m_vt_0_1,
00318 m_st_1 + m_vt_0_2,
00319 m_st_2 + m_vt_0_1,
00320 ct + m_vt_1*rotvec(1),
00321 -m_st_0 + m_vt_1_2,
00322 -m_st_1 + m_vt_0_2,
00323 m_st_0 + m_vt_1_2,
00324 ct + m_vt_2*rotvec(2)
00325 );
00326 }
00327
00328
00329
00330 Vector Rotation::GetRot() const
00331
00332
00333 {
00334 Vector axis;
00335 double angle;
00336 angle = Rotation::GetRotAngle(axis,epsilon);
00337 return axis * angle;
00338 }
00339
00340
00341
00352 double Rotation::GetRotAngle(Vector& axis,double eps) const {
00353 double ca = (data[0]+data[4]+data[8]-1)/2.0;
00354 double t= eps*eps/2.0;
00355 if (ca>1-t) {
00356
00357 axis = Vector(0,0,1);
00358 return 0;
00359 }
00360 if (ca < -1+t) {
00361
00362
00363 double x = sqrt( (data[0]+1.0)/2);
00364 double y = sqrt( (data[4]+1.0)/2);
00365 double z = sqrt( (data[8]+1.0)/2);
00366 if ( data[2] < 0) x=-x;
00367 if ( data[7] < 0) y=-y;
00368 if ( x*y*data[1] < 0) x=-x;
00369
00370
00371 axis = Vector( x,y,z );
00372 return PI;
00373 }
00374 double angle;
00375 double mod_axis;
00376 double axisx, axisy, axisz;
00377 axisx = data[7]-data[5];
00378 axisy = data[2]-data[6];
00379 axisz = data[3]-data[1];
00380 mod_axis = sqrt(axisx*axisx+axisy*axisy+axisz*axisz);
00381 axis = Vector(axisx/mod_axis,
00382 axisy/mod_axis,
00383 axisz/mod_axis );
00384 angle = atan2(mod_axis/2,ca);
00385 return angle;
00386 }
00387
00388 bool operator==(const Rotation& a,const Rotation& b) {
00389 #ifdef KDL_USE_EQUAL
00390 return Equal(a,b);
00391 #else
00392 return ( a.data[0]==b.data[0] &&
00393 a.data[1]==b.data[1] &&
00394 a.data[2]==b.data[2] &&
00395 a.data[3]==b.data[3] &&
00396 a.data[4]==b.data[4] &&
00397 a.data[5]==b.data[5] &&
00398 a.data[6]==b.data[6] &&
00399 a.data[7]==b.data[7] &&
00400 a.data[8]==b.data[8] );
00401 #endif
00402 }
00403 }