00001 00029 #ifndef GOAL_AVOID_SINGULARITIES_H 00030 #define GOAL_AVOID_SINGULARITIES_H 00031 00032 #include "constrained_ik/constraint.h" 00033 00034 namespace constrained_ik 00035 { 00036 namespace constraints 00037 { 00048 class AvoidSingularities: public Constraint 00049 { 00050 public: 00051 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00053 struct AvoidSingularitiesData: public ConstraintData 00054 { 00055 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00056 const constraints::AvoidSingularities* parent_; 00057 bool avoidance_enabled_; 00058 double smallest_sv_; 00059 Eigen::VectorXd Ui_; 00060 Eigen::VectorXd Vi_; 00061 Eigen::MatrixXd jacobian_orig_; 00064 AvoidSingularitiesData(const constrained_ik::SolverState &state, const constraints::AvoidSingularities* parent); 00065 }; 00066 00067 AvoidSingularities(); 00068 00070 constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; 00071 00073 void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override; 00074 00080 virtual Eigen::MatrixXd calcJacobian(const AvoidSingularitiesData &cdata) const; 00081 00088 virtual Eigen::VectorXd calcError(const AvoidSingularitiesData &cdata) const; 00089 00095 virtual bool checkStatus(const AvoidSingularitiesData &cdata) const { return true;} //always return true 00096 00101 virtual double getWeight() const {return weight_;} 00102 00107 virtual void setWeight(double weight) {weight_ = weight;} 00108 00113 virtual double getEnableThreshold() const {return enable_threshold_;} 00114 00119 virtual void setEnableThreshold(double enable_threshold) {enable_threshold_ = enable_threshold;} 00120 00125 virtual double getIgnoreThreshold() const {return ignore_threshold_;} 00126 00131 virtual void setIgnoreThreshold(double ignore_threshold) {ignore_threshold_ = ignore_threshold;} 00132 00133 protected: 00134 double weight_; 00135 double enable_threshold_; 00136 double ignore_threshold_; 00148 Eigen::MatrixXd jacobianPartialDerivative(const AvoidSingularitiesData &cdata, size_t jntIdx, double eps=1e-6) const; 00149 00150 }; // class AvoidSingularities 00151 00152 } // namespace constraints 00153 } // namespace constrained_ik 00154 00155 00156 #endif // GOAL_AVOID_SINGULARITIES_H 00157