00001 00026 #ifndef GOAL_POSITION_H 00027 #define GOAL_POSITION_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 GoalPosition : public Constraint 00044 { 00045 public: 00046 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00047 00049 struct GoalPositionData: public ConstraintData 00050 { 00051 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00052 double pos_err_; 00055 GoalPositionData(const constrained_ik::SolverState &state); 00056 }; 00057 00058 GoalPosition(); 00059 00061 constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; 00062 00064 void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override; 00065 00073 static double calcDistance(const Eigen::Affine3d &p1, const Eigen::Affine3d &p2); 00074 00082 virtual Eigen::MatrixXd calcJacobian(const GoalPositionData &cdata) const; 00083 00091 virtual Eigen::VectorXd calcError(const GoalPositionData &cdata) const; 00092 00099 virtual bool checkStatus(const GoalPositionData &cdata) const; 00100 00105 virtual Eigen::Vector3d getWeight() const {return weight_;} 00106 00111 virtual void setWeight(const Eigen::Vector3d &weight) {weight_ = weight;} 00112 00117 virtual double getTolerance() const {return pos_err_tol_;} 00118 00123 virtual void setTolerance(const double &tol) {pos_err_tol_ = tol;} 00124 00125 protected: 00126 double pos_err_tol_; 00127 Eigen::Vector3d weight_; 00129 }; // class GoalPosition 00130 00131 } // namespace constraints 00132 } // namespace constrained_ik 00133 00134 00135 #endif // GOAL_POSITION_H 00136