00001 00026 #ifndef GOAL_ORIENTATION_H 00027 #define GOAL_ORIENTATION_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 GoalOrientation : public Constraint 00044 { 00045 public: 00046 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00048 struct GoalOrientationData: public ConstraintData 00049 { 00050 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00051 double rot_err_; 00054 GoalOrientationData(const constrained_ik::SolverState &state); 00055 }; 00056 00057 GoalOrientation(); 00058 00060 constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; 00061 00063 void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override; 00064 00072 virtual Eigen::MatrixXd calcJacobian(const GoalOrientationData &cdata) const; 00080 virtual Eigen::VectorXd calcError(const GoalOrientationData &cdata) const; 00081 00089 static double calcAngle(const Eigen::Affine3d &p1, const Eigen::Affine3d &p2); 00090 00098 static Eigen::Vector3d calcAngleError(const Eigen::Affine3d &p1, const Eigen::Affine3d &p2); 00099 00106 virtual bool checkStatus(const GoalOrientationData &cdata) const; 00107 00112 virtual Eigen::Vector3d getWeight() const {return weight_;} 00113 00118 virtual void setWeight(const Eigen::Vector3d &weight) {weight_ = weight;} 00119 00124 virtual double getTolerance() const {return rot_err_tol_;} 00125 00130 virtual void setTolerance(double tol) {rot_err_tol_ = tol;} //TODO turn tolerance into Vector3d 00131 00132 protected: 00133 double rot_err_tol_; 00134 Eigen::Vector3d weight_; 00136 }; // class GoalOrientation 00137 00138 } // namespace constraints 00139 } // namespace constrained_ik 00140 00141 00142 #endif // GOAL_ORIENTATION_H 00143