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;
00022 double* jsweight;
00023 PseudoInverse pinvtmps;
00024
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
