00001 /* 00002 * Software License Agreement (Apache License) 00003 * 00004 * Copyright (c) 2013, Southwest Research Institute 00005 * 00006 * Licensed under the Apache License, Version 2.0 (the "License"); 00007 * you may not use this file except in compliance with the License. 00008 * You may obtain a copy of the License at 00009 * 00010 * http://www.apache.org/licenses/LICENSE-2.0 00011 * 00012 * Unless required by applicable law or agreed to in writing, software 00013 * distributed under the License is distributed on an "AS IS" BASIS, 00014 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00015 * See the License for the specific language governing permissions and 00016 * limitations under the License. 00017 */ 00018 00019 00020 #ifndef GOAL_POSITION_H 00021 #define GOAL_POSITION_H 00022 00023 #include "constrained_ik/constraint.h" 00024 00025 namespace constrained_ik 00026 { 00027 namespace constraints 00028 { 00029 00033 class GoalPosition : public Constraint 00034 { 00035 public: 00036 GoalPosition(); 00037 virtual ~GoalPosition() {}; 00038 00044 static double calcDistance(const Eigen::Affine3d &p1, const Eigen::Affine3d &p2); 00045 00051 virtual Eigen::MatrixXd calcJacobian(); 00052 00058 virtual Eigen::VectorXd calcError(); 00059 00064 virtual bool checkStatus() const; 00065 00069 Eigen::Vector3d getWeight() {return weight_;} 00070 00073 virtual void reset(); 00074 00079 virtual void update(const SolverState &state); 00080 00084 void setWeight(const Eigen::Vector3d &weight) {weight_ = weight;}; 00085 00086 protected: 00087 double pos_err_tol_; // termination criteria 00088 double pos_err_; // current solution error 00089 Eigen::Vector3d weight_; 00090 }; // class GoalPosition 00091 00092 } // namespace constraints 00093 } // namespace constrained_ik 00094 00095 00096 #endif // GOAL_POSITION_H 00097