00001 00026 #ifndef GOAL_TOOL_POINTING_H 00027 #define GOAL_TOOL_POINTING_H 00028 00029 #include "constrained_ik/constraint.h" 00030 #include <constrained_ik/solver_state.h> 00031 00032 namespace constrained_ik 00033 { 00034 namespace constraints 00035 { 00043 class GoalToolPointing : public Constraint 00044 { 00045 public: 00046 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00048 struct GoalToolPointingData: public ConstraintData 00049 { 00050 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00051 double pos_err_; 00052 double rot_err_; 00055 GoalToolPointingData(const constrained_ik::SolverState &state); 00056 }; 00057 00058 GoalToolPointing(); 00059 00061 constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; 00062 00064 void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override; 00065 00073 virtual Eigen::MatrixXd calcJacobian(const GoalToolPointingData &cdata) const; 00074 00082 virtual Eigen::VectorXd calcError(const GoalToolPointingData &cdata) const; 00083 00090 virtual bool checkStatus(const GoalToolPointingData &cdata) const; 00091 00096 virtual Eigen::Matrix<double, 5, 1> getWeight() const {return weight_.diagonal();} 00097 00102 virtual void setWeight(const Eigen::Matrix<double, 5, 1> &weight) {weight_ = weight.asDiagonal();} 00103 00108 virtual double getPositionTolerance() const {return pos_err_tol_;} 00109 00114 virtual void setPositionTolerance(const double &position_tolerance) {pos_err_tol_ = position_tolerance;} 00115 00120 virtual double getOrientationTolerance() const {return rot_err_tol_;} 00121 00126 virtual void setOrientationTolerance(const double &orientation_tolerance) {rot_err_tol_ = orientation_tolerance;} 00127 00128 protected: 00129 double pos_err_tol_; 00130 double rot_err_tol_; 00131 Eigen::Matrix<double, 5, 5> weight_; 00133 }; // class GoalToolPointing 00134 00135 } // namespace constraints 00136 } // namespace constrained_ik 00137 00138 #endif // GOAL_TOOL_POINTING_H 00139