00001 00030 #ifndef AVOID_JOINT_LIMITS_H_ 00031 #define AVOID_JOINT_LIMITS_H_ 00032 00033 #include "constrained_ik/constraint.h" 00034 #include <vector> 00035 00036 namespace constrained_ik 00037 { 00038 namespace constraints 00039 { 00040 00052 class AvoidJointLimits: public Constraint 00053 { 00054 protected: 00058 struct LimitsT 00059 { 00060 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00061 double min_pos; 00062 double max_pos; 00063 double lower_thresh; 00064 double upper_thresh; 00065 double e; 00066 double k3; 00074 LimitsT(double minPos, double maxPos, double threshold); 00075 00083 double cubicVelRamp(double angle, double limit) const; 00084 }; 00085 00086 std::vector<LimitsT> limits_; 00087 double weight_; 00088 double threshold_; 00090 public: 00091 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00093 struct AvoidJointLimitsData: public ConstraintData 00094 { 00095 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00096 std::vector<int> limited_joints_; 00097 const constraints::AvoidJointLimits* parent_; 00100 AvoidJointLimitsData(const constrained_ik::SolverState &state, const constraints::AvoidJointLimits* parent); 00101 00107 bool nearLowerLimit(size_t idx) const; 00108 00114 bool nearUpperLimit(size_t idx) const; 00115 }; 00116 00117 AvoidJointLimits(); 00118 00125 void init(const Constrained_IK* ik) override; 00126 00128 constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; 00129 00131 void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override; 00132 00139 virtual Eigen::MatrixXd calcJacobian(const AvoidJointLimitsData &cdata) const; 00140 00147 virtual Eigen::VectorXd calcError(const AvoidJointLimitsData &cdata) const; 00148 00155 virtual bool checkStatus(const AvoidJointLimitsData &cdata) const {return true;} 00156 00161 virtual double getWeight() const {return weight_;} 00162 00167 virtual void setWeight(const double &weight) {weight_ = weight;} 00168 00173 virtual double getThreshold() const {return threshold_;} 00174 00179 virtual void setThreshold(const double &threshold) {threshold_ = threshold;} 00180 }; 00181 00182 } /* namespace constraints */ 00183 } /* namespace constrained_ik */ 00184 #endif /* AVOID_JOINT_LIMITS_H_ */