00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_H
00019 #define COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_H
00020
00021 #include <set>
00022 #include <string>
00023 #include <limits>
00024 #include <kdl/chainjnttojacsolver.hpp>
00025 #include <kdl/chainfksolvervel_recursive.hpp>
00026
00027 #include "cob_twist_controller/cob_twist_controller_data_types.h"
00028 #include "cob_twist_controller/constraints/constraint_base.h"
00029 #include "cob_twist_controller/callback_data_mediator.h"
00030
00031
00033 template <typename PRIO = uint32_t>
00034 class ConstraintsBuilder
00035 {
00036 public:
00037 static std::set<ConstraintBase_t> createConstraints(const TwistControllerParams& params,
00038 const LimiterParams& limiter_params,
00039 KDL::ChainJntToJacSolver& jnt_to_jac_,
00040 KDL::ChainFkSolverVel_recursive& fk_solver_vel,
00041 CallbackDataMediator& data_mediator);
00042
00043 private:
00044 ConstraintsBuilder() {}
00045 ~ConstraintsBuilder() {}
00046 };
00047
00048
00049
00051 template <typename T_PARAMS, typename PRIO = uint32_t>
00052 class CollisionAvoidance : public ConstraintBase<T_PARAMS, PRIO>
00053 {
00054 public:
00055 CollisionAvoidance(PRIO prio,
00056 T_PARAMS constraint_params,
00057 CallbackDataMediator& cbdm,
00058 KDL::ChainJntToJacSolver& jnt_to_jac,
00059 KDL::ChainFkSolverVel_recursive& fk_solver_vel) :
00060 ConstraintBase<T_PARAMS, PRIO>(prio, constraint_params, cbdm),
00061 jnt_to_jac_(jnt_to_jac),
00062 fk_solver_vel_(fk_solver_vel)
00063 {}
00064
00065 virtual ~CollisionAvoidance()
00066 {}
00067
00068 virtual std::string getTaskId() const;
00069 virtual Eigen::MatrixXd getTaskJacobian() const;
00070 virtual Eigen::VectorXd getTaskDerivatives() const;
00071
00072 virtual void calculate();
00073
00074 virtual double getActivationGain() const;
00075 virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution,
00076 const Eigen::MatrixXd& homogeneous_solution) const;
00077
00078 double getActivationGain(double current_cost_func_value) const;
00079 double getSelfMotionMagnitude(double current_cost_func_value) const;
00080
00081 private:
00082 virtual double getCriticalValue() const;
00083
00084 void calcValue();
00085 void calcDerivativeValue();
00086 void calcPartialValues();
00087 void calcPredictionValue();
00088 double getActivationThresholdWithBuffer() const;
00089
00090 KDL::ChainJntToJacSolver& jnt_to_jac_;
00091 KDL::ChainFkSolverVel_recursive& fk_solver_vel_;
00092
00093 Eigen::VectorXd values_;
00094 Eigen::VectorXd derivative_values_;
00095 Eigen::MatrixXd task_jacobian_;
00096 };
00097
00098
00099
00101 template <typename T_PARAMS, typename PRIO = uint32_t>
00102 class JointLimitAvoidance : public ConstraintBase<T_PARAMS, PRIO>
00103 {
00104 public:
00105 JointLimitAvoidance(PRIO prio,
00106 T_PARAMS constraint_params,
00107 CallbackDataMediator& cbdm)
00108 : ConstraintBase<T_PARAMS, PRIO>(prio, constraint_params, cbdm),
00109 abs_delta_max_(std::numeric_limits<double>::max()),
00110 abs_delta_min_(std::numeric_limits<double>::max()),
00111 rel_max_(1.0),
00112 rel_min_(1.0)
00113 {}
00114
00115 virtual ~JointLimitAvoidance()
00116 {}
00117
00118 virtual std::string getTaskId() const;
00119
00120 virtual void calculate();
00121 virtual Eigen::MatrixXd getTaskJacobian() const;
00122 virtual Eigen::VectorXd getTaskDerivatives() const;
00123
00124 virtual double getActivationGain() const;
00125 virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const;
00126
00127 private:
00128 void calcValue();
00129 void calcDerivativeValue();
00130 void calcPartialValues();
00131
00132 double abs_delta_max_;
00133 double abs_delta_min_;
00134 double rel_max_;
00135 double rel_min_;
00136 };
00137
00138
00139
00141 template <typename T_PARAMS, typename PRIO = uint32_t>
00142 class JointLimitAvoidanceMid : public ConstraintBase<T_PARAMS, PRIO>
00143 {
00144 public:
00145 JointLimitAvoidanceMid(PRIO prio,
00146 T_PARAMS constraint_params,
00147 CallbackDataMediator& cbdm)
00148 : ConstraintBase<T_PARAMS, PRIO>(prio, constraint_params, cbdm)
00149 {}
00150
00151 virtual ~JointLimitAvoidanceMid()
00152 {}
00153
00154 virtual std::string getTaskId() const;
00155
00156 virtual void calculate();
00157
00158 virtual double getActivationGain() const;
00159 virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const;
00160
00161 private:
00162 void calcValue();
00163 void calcDerivativeValue();
00164 void calcPartialValues();
00165 };
00166
00167
00168
00170 template <typename T_PARAMS, typename PRIO = uint32_t>
00171 class JointLimitAvoidanceIneq : public ConstraintBase<T_PARAMS, PRIO>
00172 {
00173 public:
00174 JointLimitAvoidanceIneq(PRIO prio,
00175 T_PARAMS constraint_params,
00176 CallbackDataMediator& cbdm)
00177 : ConstraintBase<T_PARAMS, PRIO>(prio, constraint_params, cbdm),
00178 abs_delta_max_(std::numeric_limits<double>::max()),
00179 abs_delta_min_(std::numeric_limits<double>::max()),
00180 rel_max_(1.0),
00181 rel_min_(1.0)
00182 {}
00183
00184 virtual ~JointLimitAvoidanceIneq()
00185 {}
00186
00187 virtual std::string getTaskId() const;
00188 virtual Eigen::MatrixXd getTaskJacobian() const;
00189 virtual Eigen::VectorXd getTaskDerivatives() const;
00190
00191 virtual void calculate();
00192
00193 virtual double getActivationGain() const;
00194 virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const;
00195
00196 private:
00197 void calcValue();
00198 void calcDerivativeValue();
00199 void calcPartialValues();
00200
00201 double abs_delta_max_;
00202 double abs_delta_min_;
00203 double rel_max_;
00204 double rel_min_;
00205 };
00206
00207
00208 typedef ConstraintsBuilder<uint32_t> ConstraintsBuilder_t;
00209
00210 #include "cob_twist_controller/constraints/constraint_impl.h"
00211
00212 #endif // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_H