merge from 23153 to 23595soc-2009-imbusy

1 files changed, 389 insertions, 0 deletions

diff --git a/intern/itasc/kdl/frames.cpp b/intern/itasc/kdl/frames.cpp
new file mode 100644
index 00000000000..7dcc39f2cd4
--- /dev/null
+++ b/intern/itasc/kdl/frames.cpp
@@ -0,0 +1,389 @@
+/***************************************************************************
+                        frames.cxx -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ ***************************************************************************
+ *   This library is free software; you can redistribute it and/or         *
+ *   modify it under the terms of the GNU Lesser General Public            *
+ *   License as published by the Free Software Foundation; either          *
+ *   version 2.1 of the License, or (at your option) any later version.    *
+ *                                                                         *
+ *   This library is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
+ *   Lesser General Public License for more details.                       *
+ *                                                                         *
+ *   You should have received a copy of the GNU Lesser General Public      *
+ *   License along with this library; if not, write to the Free Software   *
+ *   Foundation, Inc., 59 Temple Place,                                    *
+ *   Suite 330, Boston, MA  02111-1307  USA                                *
+ *                                                                         *
+ ***************************************************************************/
+
+#include "frames.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+#include "frames.inl"
+#endif
+
+void Frame::Make4x4(double * d)
+{
+    int i;
+    int j;
+    for (i=0;i<3;i++) {
+        for (j=0;j<3;j++)
+            d[i*4+j]=M(i,j);
+        d[i*4+3] = p(i)/1000;
+    }
+    for (j=0;j<3;j++)
+        d[12+j] = 0.;
+    d[15] = 1;
+}
+
+Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
+// returns Modified Denavit-Hartenberg parameters (According to Craig)
+{
+    double ct,st,ca,sa;
+    ct = cos(theta);
+    st = sin(theta);
+    sa = sin(alpha);
+    ca = cos(alpha);
+    return Frame(Rotation(
+                    ct,       -st,     0,
+                    st*ca,  ct*ca,   -sa,
+                    st*sa,  ct*sa,    ca   ),
+                 Vector(
+                    a,      -sa*d,  ca*d   )
+            );
+}
+
+Frame Frame::DH(double a,double alpha,double d,double theta)
+// returns Denavit-Hartenberg parameters (Non-Modified DH)
+{
+    double ct,st,ca,sa;
+    ct = cos(theta);
+    st = sin(theta);
+    sa = sin(alpha);
+    ca = cos(alpha);
+    return Frame(Rotation(
+                    ct,    -st*ca,   st*sa,
+                    st,     ct*ca,  -ct*sa,
+                     0,        sa,      ca   ),
+                 Vector(
+                    a*ct,      a*st,  d   )
+            );
+}
+
+double Vector2::Norm() const
+{
+    double tmp0 = fabs(data[0]);
+    double tmp1 = fabs(data[1]);
+    if (tmp0 >= tmp1) {
+		if (tmp1 == 0)
+			return 0;
+        return tmp0*sqrt(1+sqr(tmp1/tmp0));
+    } else {
+        return tmp1*sqrt(1+sqr(tmp0/tmp1));
+    }
+}
+// makes v a unitvector and returns the norm of v.
+// if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+// if this is not good, check the return value of this method.
+double Vector2::Normalize(double eps) {
+	double v = this->Norm();
+	if (v < eps) {
+		*this = Vector2(1,0);
+		return v;
+	} else {
+		*this = (*this)/v;
+		return v;
+	}
+}
+
+
+// do some effort not to lose precision
+double Vector::Norm() const
+{
+    double tmp1;
+    double tmp2;
+    tmp1 = fabs(data[0]);
+    tmp2 = fabs(data[1]);
+    if (tmp1 >= tmp2) {
+        tmp2=fabs(data[2]);
+        if (tmp1 >= tmp2) {
+        	if (tmp1 == 0) {
+        		// only to everything exactly zero case, all other are handled correctly
+        		return 0;
+        	}
+            return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
+        } else {
+            return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
+        }
+    } else {
+        tmp1=fabs(data[2]);
+        if (tmp2 > tmp1) {
+            return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
+        } else {
+            return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
+        }
+    }
+}
+
+// makes v a unitvector and returns the norm of v.
+// if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+// if this is not good, check the return value of this method.
+double Vector::Normalize(double eps) {
+	double v = this->Norm();
+	if (v < eps) {
+		*this = Vector(1,0,0);
+		return v;
+	} else {
+		*this = (*this)/v;
+		return v;
+	}
+}
+
+
+bool Equal(const Rotation& a,const Rotation& b,double eps) {
+    return (Equal(a.data[0],b.data[0],eps) &&
+            Equal(a.data[1],b.data[1],eps) &&
+            Equal(a.data[2],b.data[2],eps) &&
+            Equal(a.data[3],b.data[3],eps) &&
+            Equal(a.data[4],b.data[4],eps) &&
+            Equal(a.data[5],b.data[5],eps) &&
+            Equal(a.data[6],b.data[6],eps) &&
+            Equal(a.data[7],b.data[7],eps) &&
+            Equal(a.data[8],b.data[8],eps)    );
+}
+
+void Rotation::Ortho()
+{
+	double n;
+	n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n; 
+	n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n; 
+	n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n; 
+}
+
+Rotation operator *(const Rotation& lhs,const Rotation& rhs)
+// Complexity : 27M+27A
+{
+    return Rotation(
+        lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
+        lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
+        lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
+        lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
+        lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
+        lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
+        lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
+        lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
+        lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
+    );
+
+}
+
+
+Rotation Rotation::RPY(double roll,double pitch,double yaw)
+    {
+        double ca1,cb1,cc1,sa1,sb1,sc1;
+        ca1 = cos(yaw); sa1 = sin(yaw);
+        cb1 = cos(pitch);sb1 = sin(pitch);
+        cc1 = cos(roll);sc1 = sin(roll);
+        return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
+                   sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
+                   -sb1,cb1*sc1,cb1*cc1);
+    }
+
+// Gives back a rotation matrix specified with RPY convention
+void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
+    {
+        if (fabs(data[6]) > 1.0 - epsilon ) {
+            roll = -sign(data[6]) * atan2(data[1], data[4]);
+            pitch= -sign(data[6]) * PI / 2;
+            yaw  = 0.0 ;
+        } else {
+            roll  = atan2(data[7], data[8]);
+            pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
+            yaw   = atan2(data[3], data[0]);
+        }
+    }
+
+Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
+        double sa,ca,sb,cb,sg,cg;
+        sa  = sin(Alfa);ca = cos(Alfa);
+        sb  = sin(Beta);cb = cos(Beta);
+        sg  = sin(Gamma);cg = cos(Gamma);
+        return Rotation( ca*cb*cg-sa*sg,     -ca*cb*sg-sa*cg,        ca*sb,
+                 sa*cb*cg+ca*sg,     -sa*cb*sg+ca*cg,        sa*sb,
+                 -sb*cg ,                sb*sg,              cb
+                );
+
+     }
+
+
+void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const {
+        if (fabs(data[6]) < epsilon ) {
+            alfa=0.0;
+            if (data[8]>0) {
+                beta = 0.0;
+                gamma= atan2(-data[1],data[0]);
+            } else {
+                beta = PI;
+                gamma= atan2(data[1],-data[0]);
+            }
+        } else {
+            alfa=atan2(data[5], data[2]);
+            beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
+            gamma=atan2(data[7], -data[6]);
+        }
+ }
+
+Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
+    // The formula is
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+    double ct = cos(angle);
+    double st = sin(angle);
+    double vt = 1-ct;
+    Vector rotvec = rotaxis;
+	rotvec.Normalize();
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0),
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+    }
+
+Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
+    // rotvec should be normalized !
+    // The formula is
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+    double ct = cos(angle);
+    double st = sin(angle);
+    double vt = 1-ct;
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0),
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+}
+
+
+
+Vector Rotation::GetRot() const
+         // Returns a vector with the direction of the equiv. axis
+         // and its norm is angle
+     {
+       Vector axis  = Vector((data[7]-data[5]),
+			     (data[2]-data[6]),
+			     (data[3]-data[1]) )/2;
+
+       double sa    = axis.Norm();
+       double ca    = (data[0]+data[4]+data[8]-1)/2.0;
+       double alfa;
+       if (sa > epsilon)
+           alfa = ::atan2(sa,ca)/sa;
+	   else {
+		   if (ca < 0.0) {
+			   alfa = KDL::PI;
+			   axis.data[0] = 0.0;
+			   axis.data[1] = 0.0;
+			   axis.data[2] = 0.0;
+			   if (data[0] > 0.0) {
+				   axis.data[0] = 1.0;
+			   } else if (data[4] > 0.0) {
+				   axis.data[1] = 1.0;
+			   } else {
+				   axis.data[2] = 1.0;
+			   }
+		   } else {
+			   alfa = 0.0;
+		   }
+	   }
+       return axis * alfa;
+     }
+
+Vector2 Rotation::GetXZRot() const
+{
+	// [0,1,0] x Y
+	Vector2 axis(data[7], -data[1]);
+	double norm = axis.Normalize();
+	if (norm < epsilon) {
+		norm = (data[4] < 0.0) ? PI : 0.0;
+	} else {
+		norm = acos(data[4]);
+	}
+	return axis*norm;
+}
+
+
+/** Returns the rotation angle around the equiv. axis
+ * @param axis the rotation axis is returned in this variable
+ * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
+ *                                         to be +/- Z-axis
+ *               in the case of angle == PI : 2 solutions, positive Z-component
+ *                                            of the axis is choosen.
+ * @result returns the rotation angle (between [0..PI] )
+ * /todo :
+ *   Check corresponding routines in rframes and rrframes
+ */
+double Rotation::GetRotAngle(Vector& axis,double eps) const {
+	double ca    = (data[0]+data[4]+data[8]-1)/2.0;
+	if (ca>1-eps) {
+		// undefined choose the Z-axis, and angle 0
+		axis = Vector(0,0,1);
+		return 0;
+	}
+	if (ca < -1+eps) {
+		// two solutions, choose a positive Z-component of the axis
+		double z = sqrt( (data[8]+1)/2 );
+		double x = (data[2])/2/z;
+		double y = (data[5])/2/z;
+		axis = Vector( x,y,z  );
+		return PI;
+	}
+	double angle = acos(ca);
+	double sa    = sin(angle);
+	axis  = Vector((data[7]-data[5])/2/sa,
+                       (data[2]-data[6])/2/sa,
+                       (data[3]-data[1])/2/sa  );
+	return angle;
+}
+
+bool operator==(const Rotation& a,const Rotation& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+    return ( a.data[0]==b.data[0] &&
+             a.data[1]==b.data[1] &&
+             a.data[2]==b.data[2] &&
+             a.data[3]==b.data[3] &&
+             a.data[4]==b.data[4] &&
+             a.data[5]==b.data[5] &&
+             a.data[6]==b.data[6] &&
+             a.data[7]==b.data[7] &&
+             a.data[8]==b.data[8]  );
+#endif
+}
+}