orocos_kdl: constraints.hpp Source File

Go to the documentation of this file.
00001 #ifndef KDL_CONSTRAINTS_HPP
00002 #define KDL_CONSTRAINTS_HPP
00003 
00004 #include <vector>
00005 #include <kdl/jacobianframe.hpp>
00006 #include <kdl/pinv.hpp>
00007 #include <kdl/frames.hpp>
00008 #include <fstream>
00009 
00010 
00011 namespace KDL {
00012 
00016 class ConstraintMatrix {
00017           int maxrows,maxcols;
00018     double* cmat;    
00019     int cmatstride;
00020     double* desval;   
00021     double* weight;   // square root of the constraint space weights ...
00022     double* jsweight; // square root of the joint space weights ...
00023     PseudoInverse pinvtmps;
00024     //PseudoInverse pinvtmps;
00025     double* cmati;
00026     int cmatistride;
00027 
00028     int nrOfConstraints;    
00029     double* output2;
00030     int nrofjoints;
00031 public:
00032     double eps;
00033     int    nrofits;
00034 protected:
00035 
00045     void reduce(const std::vector<bool>& list,const std::vector<double> value);
00046 
00054     void reducejv(const std::vector<bool>& list, std::vector<double>& vec);
00055 
00056     
00066     void expandjv(int nrofalljoints,const std::vector<bool>& list,const std::vector<double>& values,std::vector<double>& vec);
00067 
00068 
00069 public:
00077     ConstraintMatrix(int _maxrows,int _maxcols);
00078 
00087     void resetConstraints(int nrofjoints);
00088 
00099     void addConstraintJoint(int jointnr,double value,double K,double _desval,double _desvald,double _weight);
00100 
00111     void addConstraint(const Jacobian<double>& c,double K,double _desval,double _desvald,double _weight);
00112 
00116         void addConstraint(const Jacobian<Vector>& c,double K,const Vector& _desval,const Vector& _desvald,double _weight);
00117 
00121         void addConstraint(
00122                 const Jacobian<Frame>& c,
00123                 double Kpos,
00124                 double Krot,
00125                 const Frame& _desval,
00126                 const Twist& _desvald,
00127                 double _weightpos,
00128                 double _weightrot
00129         );
00130 
00135         void addConstraint(
00136                 const Jacobian<Frame>& c,
00137                 const std::vector<double>& K,
00138                 const Frame& _desval,
00139                 const Twist& _desvald,
00140                 const std::vector<double>& _weight
00141         );
00142 
00148         void addConstraintWithoutControl(
00149                 const Jacobian<Frame>& c,
00150                 const Twist& desired,
00151                 const std::vector<double>& _weight
00152         );
00153 
00157         void addConstraint(
00158                 const Jacobian<Twist>& c,
00159                 double Kpos,
00160                 double Krot,
00161                 const Twist& _desval,
00162                 const Twist& _desvald,
00163                 double _weightpos,
00164                 double _weightrot
00165         );
00166 
00167 
00171         void addConstraint(
00172                 const Jacobian<Wrench>& c,
00173                 double Kpos,
00174                 double Krot,
00175                 const Wrench& _desval,
00176                 const Wrench& _desvald,
00177                 double _weightpos,
00178                 double _weightrot
00179         );
00180 
00184         void setJointSpaceWeights(const std::vector<double>& jsw);
00185 
00186 
00187 
00194     void calculateOutput(std::vector<double>& output);
00195 
00202         void calculateOutput(std::vector<double>& output,const std::vector<double>& nullspace);
00203 
00204 
00213     void calculateOutput(const std::vector<bool>& list, const std::vector<double>& values, std::vector<double>& output);
00214 
00226     void calculateOutput(const std::vector<bool>& list, 
00227                                                 const std::vector<double>& values, 
00228                                                 std::vector<double>& output,
00229                                                 std::vector<double>& nullspace);
00230 
00234     void dump(std::ostream& os);
00235         
00239     void dumpsvd(std::ostream& os);
00240 
00241     ~ConstraintMatrix();
00242 };
00243 
00244 }
00245 
00246 #endif
00247