stiffness.hpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #ifndef KDL_STIFFNESS_H
00023 #define KDL_STIFFNESS_H
00024 #include "frames.hpp"
00025  
00026 
00027 namespace KDL {
00036 class Stiffness {
00037     double data[6];
00038 public:
00039         Stiffness() {
00040                 data[0]=0;
00041                 data[1]=0;
00042                 data[2]=0;
00043                 data[3]=0;
00044                 data[4]=0;
00045                 data[5]=0;
00046         }
00047     Stiffness(double* d) {
00048         data[0]=d[0];
00049         data[1]=d[1];
00050         data[2]=d[2];
00051         data[3]=d[3];
00052         data[4]=d[4];
00053         data[5]=d[5];
00054     }
00055     Stiffness(double x,double y,double z,double rx,double ry,double rz) {
00056         data[0]=x;
00057         data[1]=y;
00058         data[2]=z;
00059         data[3]=rx;
00060         data[4]=ry;
00061         data[5]=rz;
00062     }
00063     double& operator[](int i) {
00064         return data[i];
00065     }
00066     double operator[](int i) const {
00067         return data[i];
00068     }
00069         Twist Inverse(const Wrench& w) const{
00070                 Twist t;
00071                 t[0]=w[0]/data[0];
00072                 t[1]=w[1]/data[1];
00073                 t[2]=w[2]/data[2];
00074                 t[3]=w[3]/data[3];
00075                 t[4]=w[4]/data[4];
00076                 t[5]=w[5]/data[5];
00077                 return t;
00078         }
00079 };
00080 
00081 inline Wrench operator * (const Stiffness& s, const Twist& t) {
00082     Wrench w;
00083     w[0]=s[0]*t[0];
00084     w[1]=s[1]*t[1];
00085     w[2]=s[2]*t[2];
00086     w[3]=s[3]*t[3];
00087     w[4]=s[4]*t[4];
00088     w[5]=s[5]*t[5];
00089     return w;
00090 }
00091 
00092 inline Stiffness operator+(const Stiffness& s1, const Stiffness& s2) {
00093         Stiffness s;
00094         s[0]=s1[0]+s2[0];
00095         s[1]=s1[1]+s2[1];
00096         s[2]=s1[2]+s2[2];
00097         s[3]=s1[3]+s2[3];
00098         s[4]=s1[4]+s2[4];
00099         s[5]=s1[5]+s2[5];
00100         return s;
00101 }
00102 inline void posrandom(Stiffness& F) {
00103         posrandom(F[0]);
00104         posrandom(F[1]);
00105         posrandom(F[2]);
00106         posrandom(F[3]);
00107         posrandom(F[4]);
00108         posrandom(F[5]);
00109 }
00110 
00111 inline void random(Stiffness& F) {
00112         posrandom(F);
00113 }
00114 
00115 
00116 } 
00117 #endif
00118 
00119 
00120 


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:17