diff --git a/cob_twist_controller/cfg/TwistController.cfg b/cob_twist_controller/cfg/TwistController.cfg index 33f53f88..e10022c6 100755 --- a/cob_twist_controller/cfg/TwistController.cfg +++ b/cob_twist_controller/cfg/TwistController.cfg @@ -48,14 +48,13 @@ kinematic_extension_enum = gen.enum([ # ==================================== Damping and truncation (singular value adaption) ==================================================== damp_trunc = gen.add_group("Damping and Truncation", "damping_truncation") damp_trunc.add("numerical_filtering", bool_t, 0, "Numerical Filtering yes/no", False) -damp_trunc.add("damping_method", int_t, 0, "The damping method to use.", 2, None, None, edit_method=damping_method_enum) +damp_trunc.add("damping_method", int_t, 0, "The damping method to use.", 4, None, None, edit_method=damping_method_enum) damp_trunc.add("damping_factor", double_t, 0, "The constant damping_factor (used in CONSTANT)", 0.01, 0, 1) -damp_trunc.add("lambda_max", double_t, 0, "Value for maximum damping_factor (used in MANIPULABILITY/LSV/SIGMOID)", 0.001, 0.0001, 1.0) -damp_trunc.add("w_threshold", double_t, 0, "Value for manipulability threshold (used in MANIPULABILITY/SIGMOID)", 0.001, 0.0001, 0.1) +damp_trunc.add("lambda_max", double_t, 0, "Value for maximum damping_factor (used in MANIPULABILITY/LSV/SIGMOID)", 0.001, 0, 10) +damp_trunc.add("w_threshold", double_t, 0, "Value for manipulability threshold (used in MANIPULABILITY/SIGMOID)", 0.001, 0, 0.1) damp_trunc.add("beta", double_t, 0, "Beta for Low Isotropic Damping", 0.005, 0, 1) damp_trunc.add("slope_damping", double_t, 0, "Value for the activation gradient (used in SIGMOID)", 0.05, 0.001, 0.1) damp_trunc.add("eps_damping", double_t, 0, "Value for least singular value damping", 0.003, 0, 1) - damp_trunc.add("eps_truncation", double_t, 0, "Value for singular value threshold (used for truncation: sing. value < eps)", 0.001, 0, 1) # ==================================== Parameters for the solver (e.g. WLN, CA and JLA) ===================================================== @@ -68,13 +67,10 @@ jla = solv_constr.add_group("Joint Limit Avoidance", "jla") jla.add("constraint_jla", int_t, 0, "The JLA constraint to use (edited via an enum)", 1, None, None, edit_method=jla_constraints_enum) jla.add("priority_jla", int_t, 0, "Priority for the joint limit avoidance constraint (important for task processing; 0 = highest prio)", 50, 0, 1000) jla.add("k_H_jla", double_t, 0, "Self-motion factor for GPM. Special weighting for JLA constraint", -1.0, -1000, 1000) -jla.add("activation_threshold_jla", double_t, 0, "In [%]. Tolerance from min and max joint positition limit. (used in WLN and WLN SIGMOID)", 10.0, 0.0, 100.0) -jla.add("activation_position_threshold_jla", double_t, 0, "In [rad]. Tolerance from min and max joint positition limit.(used in WLN SIGMOID)", 0.5, 0.001, 1.0) -jla.add("activation_speed_threshold_jla", double_t, 0, "In [rad/s]. Tolerance from min and max joint positition limit.(used in WLN SIGMOID)", 1.0, 0.001, 10.0) +jla.add("activation_threshold_jla", double_t, 0, "In [%]. Tolerance from min and max joint positition limit.", 10.0, 0.0, 100.0) jla.add("activation_buffer_jla", double_t, 0, "In [%]. For smooth transition an additional buffer to activation threshold can be given. Smoothing is started prior to activation threshold. (0 % = no smoothing)", 300.0, 0.0, 500.0) jla.add("critical_threshold_jla", double_t, 0, "In [%]. Tolerance when critical region becomes active. JLA becomes task. Should be less than activation threshold (best experienced: 1/2 of activation threshold).", 5.0, 0.0, 100.0) -jla.add("damping_jla", double_t, 0, "Const. damping factor for the inv. of the JLA task jacobian", 0.01, 0.01, 1.0) -jla.add("damping_speed_jla", double_t, 0, "Const. damping factor for the inv. of the JLA task jacobian", 5.0, 0.01, 10.0) +jla.add("damping_jla", double_t, 0, "Const. damping factor for the inv. of the JLA task jacobian", 0.000001, 0.0, 1.0) ca = solv_constr.add_group("Collision Avoidance", "ca") ca.add("constraint_ca", int_t, 0, "The CA constraint to use (edited via an enum)", 0, None, None, edit_method=ca_constraints_enum) @@ -85,6 +81,12 @@ ca.add("activation_buffer_ca", double_t, 0, "In [%]. For smooth transition an ca.add("critical_threshold_ca", double_t, 0, "Tolerance when critical region becomes active. CA becomes task. Should be less than activation threshold (best experienced: 1/4 of activation threshold).", 0.025, 0.0, 1.0) ca.add("damping_ca", double_t, 0, "Const. damping factor for the inv. of the CA task jacobian", 0.000001, 0.0, 1.0) +ujs = solv_constr.add_group("Unified JLA SA", "ujs") +ujs.add("sigma", double_t, 0, "Sigma", 0.01, 0.01, 1.0) +ujs.add("sigma_speed", double_t, 0, "Sigma Speed", 5.0, 0.01, 10.0) +ujs.add("delta_pos", double_t, 0, "Delta Pos", 0.5, 0.001, 1.0) +ujs.add("delta_speed", double_t, 0, "Delta Speed", 1.0, 0.001, 10.0) + # ==================================== Parameters for limits enforcement ===================================================== limits = gen.add_group("Limits", "limits") limits.add("keep_direction", bool_t, 0, "With keep_direction the whole joint positions and velocities vector is affected by a scaling factor. Else only individual components of the vectors are affected -> direction will be changed.", True) diff --git a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h index 1eadb6cf..a89d9cc8 100644 --- a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h +++ b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h @@ -87,15 +87,6 @@ enum ConstraintTypesJLA JLA_INEQ_ON = cob_twist_controller::TwistController_JLA_INEQ, }; -enum ConstraintTypes -{ - None, - CA, - JLA, - JLA_MID, - JLA_INEQ, -}; - enum LookatAxisTypes { X_POSITIVE, @@ -163,8 +154,20 @@ struct ConstraintThresholds double activation; double activation_with_buffer; double critical; - double activation_position_threshold_jla; - double activation_speed_threshold_jla; +}; + +struct ConstraintParams +{ + uint32_t priority; + double k_H; + double damping; + ConstraintThresholds thresholds; +}; + +enum ConstraintTypes +{ + CA, + JLA, }; struct LimiterParams @@ -199,6 +202,21 @@ struct LimiterParams std::vector limits_acc; }; +struct UJSSolverParams +{ + UJSSolverParams() : + sigma(0.05), + sigma_speed(0.005), + delta_pos(0.5), + delta_speed(1.0) + {} + + double sigma; + double sigma_speed; + double delta_pos; + double delta_speed; +}; + struct TwistControllerParams { TwistControllerParams() : @@ -208,40 +226,41 @@ struct TwistControllerParams numerical_filtering(false), damping_method(SIGMOID), - damping_factor(0.2), - lambda_max(0.01), - w_threshold(0.01), + damping_factor(0.01), + lambda_max(0.001), + w_threshold(0.001), beta(0.005), slope_damping(0.05), eps_damping(0.003), eps_truncation(0.001), + solver(GPM), priority_main(500), k_H(1.0), - constraint_jla(JLA_OFF), - priority_jla(50), - k_H_jla(-10.0), - damping_jla(0.05), - damping_speed_jla(0.005), - - constraint_ca(CA_OFF), - priority_ca(100), - damping_ca(0.000001), - k_H_ca(2.0), + constraint_jla(JLA_ON), + constraint_ca(CA_ON), kinematic_extension(NO_EXTENSION), extension_ratio(0.0) { - this->thresholds_ca.activation = 0.1; - this->thresholds_ca.critical = 0.025; - this->thresholds_ca.activation_with_buffer = this->thresholds_ca.activation * 1.5; // best experienced value - - this->thresholds_jla.activation = 0.1; - this->thresholds_jla.critical = 0.05; - this->thresholds_jla.activation_with_buffer = this->thresholds_jla.activation * 4.0; // best experienced value - this->thresholds_jla.activation_position_threshold_jla = 0.5; - this->thresholds_jla.activation_speed_threshold_jla = 1.0; + ConstraintParams cp_ca; + cp_ca.priority = 100; + cp_ca.k_H = 2.0; + cp_ca.damping = 0.000001; + cp_ca.thresholds.activation = 0.1; + cp_ca.thresholds.critical = 0.025; + cp_ca.thresholds.activation_with_buffer = cp_ca.thresholds.activation * 1.5; // best experienced value + constraint_params.insert(std::pair(CA, cp_ca)); + + ConstraintParams cp_jla; + cp_jla.priority = 50; + cp_jla.k_H = -10.0; + cp_jla.damping = 0.000001; + cp_jla.thresholds.activation = 0.1; + cp_jla.thresholds.critical = 0.05; + cp_jla.thresholds.activation_with_buffer = cp_jla.thresholds.activation * 4.0; // best experienced value + constraint_params.insert(std::pair(JLA, cp_jla)); } uint8_t dof; @@ -266,18 +285,10 @@ struct TwistControllerParams double k_H; ConstraintTypesCA constraint_ca; - uint32_t priority_ca; - double k_H_ca; - double damping_ca; - ConstraintThresholds thresholds_ca; - ConstraintTypesJLA constraint_jla; - uint32_t priority_jla; - double k_H_jla; - double damping_jla; - double damping_speed_jla; - ConstraintThresholds thresholds_jla; + std::map constraint_params; + UJSSolverParams ujs_solver_params; LimiterParams limiter_params; KinematicExtensionTypes kinematic_extension; diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unified_joint_limit_singularity_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unified_joint_limit_singularity_solver.h index 828b595e..5dea082c 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unified_joint_limit_singularity_solver.h +++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unified_joint_limit_singularity_solver.h @@ -38,8 +38,8 @@ class UnifiedJointLimitSingularitySolver : public ConstraintSolver<> { public: UnifiedJointLimitSingularitySolver(const TwistControllerParams& params, - const LimiterParams& limiter_params, - TaskStackController_t& task_stack_controller) : + const LimiterParams& limiter_params, + TaskStackController_t& task_stack_controller) : ConstraintSolver(params, limiter_params, task_stack_controller) {} diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h index 863ba9d0..c85e6b76 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h @@ -38,30 +38,6 @@ #include "cob_twist_controller/cob_twist_controller_data_types.h" #include "cob_twist_controller/constraints/constraint_base.h" #include "cob_twist_controller/callback_data_mediator.h" -#include "cob_twist_controller/utils/moving_average.h" - -/* BEGIN ConstraintParamFactory *********************************************************************************/ -/// Creates constraint parameters and fills them with the values provided by CallbackDataMediator. -template - -class ConstraintParamFactory -{ - public: - static T createConstraintParams(const TwistControllerParams& twist_controller_params, - const LimiterParams& limiter_params, - CallbackDataMediator& data_mediator, - const std::string& id = std::string()) - { - T params(twist_controller_params, limiter_params, id); - data_mediator.fill(params); - return params; - } - - private: - ConstraintParamFactory() - {} -}; -/* END ConstraintParamFactory ***********************************************************************************/ /* BEGIN ConstraintsBuilder *************************************************************************************/ /// Class providing a static method to create constraints. @@ -100,7 +76,6 @@ class CollisionAvoidance : public ConstraintBase virtual ~CollisionAvoidance() {} - virtual Task_t createTask(); virtual std::string getTaskId() const; virtual Eigen::MatrixXd getTaskJacobian() const; virtual Eigen::VectorXd getTaskDerivatives() const; @@ -115,7 +90,6 @@ class CollisionAvoidance : public ConstraintBase double getSelfMotionMagnitude(double current_cost_func_value) const; private: - virtual ConstraintTypes getType() const; virtual double getCriticalValue() const; void calcValue(); @@ -152,7 +126,6 @@ class JointLimitAvoidance : public ConstraintBase virtual ~JointLimitAvoidance() {} - virtual Task_t createTask(); virtual std::string getTaskId() const; virtual void calculate(); @@ -163,8 +136,6 @@ class JointLimitAvoidance : public ConstraintBase virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const; private: - virtual ConstraintTypes getType() const; - void calcValue(); void calcDerivativeValue(); void calcPartialValues(); @@ -199,8 +170,6 @@ class JointLimitAvoidanceMid : public ConstraintBase virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const; private: - virtual ConstraintTypes getType() const; - void calcValue(); void calcDerivativeValue(); void calcPartialValues(); @@ -226,7 +195,6 @@ class JointLimitAvoidanceIneq : public ConstraintBase virtual ~JointLimitAvoidanceIneq() {} - virtual Task_t createTask(); virtual std::string getTaskId() const; virtual Eigen::MatrixXd getTaskJacobian() const; virtual Eigen::VectorXd getTaskDerivatives() const; @@ -237,8 +205,6 @@ class JointLimitAvoidanceIneq : public ConstraintBase virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const; private: - virtual ConstraintTypes getType() const; - void calcValue(); void calcDerivativeValue(); void calcPartialValues(); diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h index fb9519b2..dbc349d0 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h @@ -108,9 +108,7 @@ class PriorityBase protected: PRIO priority_; - virtual ConstraintTypes getType() const = 0; virtual double getCriticalValue() const = 0; - virtual TwistControllerParams adaptDampingParamsForTask(double const_damping_factor) = 0; }; @@ -153,8 +151,7 @@ class ConstraintBase : public PriorityBase Task_t task(this->getPriority(), this->getTaskId(), this->getTaskJacobian(), - this->getTaskDerivatives(), - this->getType()); + this->getTaskDerivatives()); return task; } @@ -234,31 +231,10 @@ class ConstraintBase : public PriorityBase uint32_t member_inst_cnt_; static uint32_t instance_ctr_; - virtual ConstraintTypes getType() const = 0; - virtual double getCriticalValue() const { return 0.0; } - - /** - * Copy the parameters and adapt them for the task damping. - * Currently only constant damping is supported without numerical filtering. - * (Tasks sometimes consist of a row "vector" Jacobian. The inverse is a - * column vector with the reciprocal compontents. Another damping method might not be sufficient here!) - * @param const_damping_factor The constant damping factor, usually from parameter server. - * @return Adapted twist controller params struct. - */ - virtual TwistControllerParams adaptDampingParamsForTask(double const_damping_factor) - { - const TwistControllerParams& params = this->constraint_params_.tc_params_; - TwistControllerParams adapted_params; - adapted_params.damping_method = CONSTANT; - adapted_params.damping_factor = const_damping_factor; - adapted_params.eps_truncation = 0.0; - adapted_params.numerical_filtering = false; - return adapted_params; - } }; template diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h index 16eca092..b40109a5 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h @@ -50,21 +50,6 @@ #include "cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h" /* BEGIN CollisionAvoidance *************************************************************************************/ -template -Task_t CollisionAvoidance::createTask() -{ - Task_t task(this->getPriority(), - this->getTaskId(), - this->getTaskJacobian(), - this->getTaskDerivatives(), - this->getType()); - - task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.tc_params_.damping_ca); - task.db_.reset(DampingBuilder::createDamping(task.tcp_)); - - return task; -} - template std::string CollisionAvoidance::getTaskId() const { @@ -104,7 +89,7 @@ Eigen::VectorXd CollisionAvoidance::getTaskDerivatives() const template void CollisionAvoidance::calculate() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; this->calcValue(); this->calcDerivativeValue(); @@ -112,9 +97,9 @@ void CollisionAvoidance::calculate() this->calcPredictionValue(); const double pred_min_dist = this->getPredictionValue(); - const double activation = params.thresholds_ca.activation; - const double critical = params.thresholds_ca.critical; - const double activation_buffer = params.thresholds_ca.activation_with_buffer; + const double activation = params.thresholds.activation; + const double critical = params.thresholds.critical; + const double activation_buffer = params.thresholds.activation_with_buffer; const double crit_min_distance = this->getCriticalValue(); if (this->state_.getCurrent() == CRITICAL && pred_min_dist < crit_min_distance) @@ -138,10 +123,10 @@ void CollisionAvoidance::calculate() template double CollisionAvoidance::getActivationGain(double current_cost_func_value) const { - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; double activation_gain; - const double activation = params.thresholds_ca.activation; - const double activation_buffer_region = params.thresholds_ca.activation_with_buffer; + const double activation = params.thresholds.activation; + const double activation_buffer_region = params.thresholds.activation_with_buffer; if (current_cost_func_value < activation) // activation == d_m { @@ -169,8 +154,8 @@ double CollisionAvoidance::getActivationGain() const template double CollisionAvoidance::getSelfMotionMagnitude(double current_distance_value) const { - const TwistControllerParams& params = this->constraint_params_.tc_params_; - const double activation_with_buffer = params.thresholds_ca.activation_with_buffer; + const ConstraintParams& params = this->constraint_params_.params_; + const double activation_with_buffer = params.thresholds.activation_with_buffer; double magnitude = 0.0; if (current_distance_value < activation_with_buffer) @@ -185,7 +170,7 @@ double CollisionAvoidance::getSelfMotionMagnitude(double current } } - double k_H = params.k_H_ca; + double k_H = params.k_H; return k_H * magnitude; } @@ -196,12 +181,6 @@ double CollisionAvoidance::getSelfMotionMagnitude(const Eigen::M return 1.0; } -template -ConstraintTypes CollisionAvoidance::getType() const -{ - return CA; -} - template double CollisionAvoidance::getCriticalValue() const { @@ -222,17 +201,17 @@ double CollisionAvoidance::getCriticalValue() const template void CollisionAvoidance::calcValue() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; std::vector relevant_values; for (std::vector::const_iterator it = this->constraint_params_.current_distances_.begin(); it != this->constraint_params_.current_distances_.end(); ++it) { - if (params.thresholds_ca.activation_with_buffer > it->min_distance) + if (params.thresholds.activation_with_buffer > it->min_distance) { const double activation_gain = this->getActivationGain(it->min_distance); const double magnitude = std::abs(this->getSelfMotionMagnitude(it->min_distance)); // important only for task! - double value = activation_gain * magnitude * pow(it->min_distance - params.thresholds_ca.activation_with_buffer, 2.0); + double value = activation_gain * magnitude * pow(it->min_distance - params.thresholds.activation_with_buffer, 2.0); relevant_values.push_back(value); } } @@ -269,23 +248,23 @@ void CollisionAvoidance::calcPartialValues() Eigen::VectorXd sum_partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols()); this->partial_values_ = Eigen::VectorXd::Zero(this->jacobian_data_.cols()); - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; std::vector vec_partial_values; // ROS_INFO_STREAM("this->jacobian_data_.cols: " << this->jacobian_data_.cols()); // ROS_INFO_STREAM("this->joint_states_.current_q_.rows: " << this->joint_states_.current_q_.rows()); - std::vector::const_iterator str_it = std::find(params.frame_names.begin(), - params.frame_names.end(), + std::vector::const_iterator str_it = std::find(this->constraint_params_.frame_names_.begin(), + this->constraint_params_.frame_names_.end(), this->constraint_params_.id_); for (std::vector::const_iterator it = this->constraint_params_.current_distances_.begin(); it != this->constraint_params_.current_distances_.end(); ++it) { - if (params.thresholds_ca.activation_with_buffer > it->min_distance) + if (params.thresholds.activation_with_buffer > it->min_distance) { - if (params.frame_names.end() != str_it) + if (this->constraint_params_.frame_names_.end() != str_it) { Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector; Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector; @@ -304,7 +283,7 @@ void CollisionAvoidance::calcPartialValues() T.block(3, 3, 3, 3) << ident; // **************************************************************************************************** - uint32_t idx = str_it - params.frame_names.begin(); + uint32_t idx = str_it - this->constraint_params_.frame_names_.begin(); uint32_t frame_number = idx + 1; // segment nr not index represents frame number KDL::JntArray ja = this->joint_states_.current_q_; @@ -339,7 +318,7 @@ void CollisionAvoidance::calcPartialValues() const double denom = it->min_distance > 0.0 ? it->min_distance : DIV0_SAFE; const double activation_gain = this->getActivationGain(it->min_distance); const double magnitude = this->getSelfMotionMagnitude(it->min_distance); - partial_values = (2.0 * ((it->min_distance - params.thresholds_ca.activation_with_buffer) / denom) * term_2nd); + partial_values = (2.0 * ((it->min_distance - params.thresholds.activation_with_buffer) / denom) * term_2nd); // only consider the gain for the partial values, because of GPM, not for the task jacobian! sum_partial_values += (activation_gain * magnitude * partial_values); vec_partial_values.push_back(partial_values); @@ -351,7 +330,7 @@ void CollisionAvoidance::calcPartialValues() } else { - // ROS_INFO_STREAM("min_dist not within activation_buffer: " << params.thresholds_ca.activation_with_buffer << " <= " << it->min_distance); + // ROS_INFO_STREAM("min_dist not within activation_buffer: " << params.thresholds.activation_with_buffer << " <= " << it->min_distance); } } // ROS_INFO_STREAM("Done all distances"); @@ -375,34 +354,25 @@ void CollisionAvoidance::calcPartialValues() template void CollisionAvoidance::calcPredictionValue() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; this->prediction_value_ = std::numeric_limits::max(); ros::Time now = ros::Time::now(); double cycle = (now - this->last_pred_time_).toSec(); this->last_pred_time_ = now; - std::vector::const_iterator str_it = std::find(params.frame_names.begin(), - params.frame_names.end(), + std::vector::const_iterator str_it = std::find(this->constraint_params_.frame_names_.begin(), + this->constraint_params_.frame_names_.end(), this->constraint_params_.id_); // ROS_INFO_STREAM("constraint_params_.id_: " << this->constraint_params_.id_); - if (params.frame_names.end() != str_it) + if (this->constraint_params_.frame_names_.end() != str_it) { if (this->constraint_params_.current_distances_.size() > 0) { - uint32_t frame_number = (str_it - params.frame_names.begin()) + 1; // segment nr not index represents frame number + uint32_t frame_number = (str_it - this->constraint_params_.frame_names_.begin()) + 1; // segment nr not index represents frame number KDL::FrameVel frame_vel; - // ToDo: the fk_solver_vel_ is only initialized for the primary chain - kinematic extensions cannot be considered yet! - KDL::JntArrayVel jnts_prediction_chain(params.dof); - for (unsigned int i = 0; i < params.dof; i++) - { - jnts_prediction_chain.q(i) = this->jnts_prediction_.q(i); - jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i); - } - // ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows()); - // Calculate prediction for pos and vel int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number); if (error != 0) diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h index 876deca5..c1aa54db 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h @@ -60,38 +60,37 @@ std::set ConstraintsBuilder::createConstraints(const Twi { typedef JointLimitAvoidance Jla_t; - ConstraintParamsJLA params = ConstraintParamFactory::createConstraintParams(tc_params, limiter_params, data_mediator); - uint32_t startPrio = tc_params.priority_jla; + ConstraintParamsJLA params = ConstraintParamsJLA(tc_params.constraint_params.at(JLA), limiter_params); + uint32_t startPrio = params.params_.priority; for (uint32_t i = 0; i < tc_params.joints.size(); ++i) { // TODO: take care PRIO could be of different type than UINT32 params.joint_ = tc_params.joints[i]; params.joint_idx_ = static_cast(i); - // copy of params will be created; priority increased with each joint. boost::shared_ptr jla(new Jla_t(startPrio++, params, data_mediator)); constraints.insert(boost::static_pointer_cast >(jla)); } } else if (JLA_MID_ON == tc_params.constraint_jla) { - // same params as for normal JLA typedef JointLimitAvoidanceMid JlaMid_t; - ConstraintParamsJLA params = ConstraintParamFactory::createConstraintParams(tc_params, limiter_params, data_mediator); + + ConstraintParamsJLA params = ConstraintParamsJLA(tc_params.constraint_params.at(JLA), limiter_params); // TODO: take care PRIO could be of different type than UINT32 - boost::shared_ptr jla(new JlaMid_t(tc_params.priority_jla, params, data_mediator)); + boost::shared_ptr jla(new JlaMid_t(params.params_.priority, params, data_mediator)); constraints.insert(boost::static_pointer_cast >(jla)); } else if (JLA_INEQ_ON == tc_params.constraint_jla) { typedef JointLimitAvoidanceIneq Jla_t; - ConstraintParamsJLA params = ConstraintParamFactory::createConstraintParams(tc_params, limiter_params, data_mediator); - uint32_t startPrio = tc_params.priority_jla; + + ConstraintParamsJLA params = ConstraintParamsJLA(tc_params.constraint_params.at(JLA), limiter_params); + uint32_t startPrio = params.params_.priority; for (uint32_t i = 0; i < tc_params.joints.size(); ++i) { // TODO: take care PRIO could be of different type than UINT32 params.joint_ = tc_params.joints[i]; params.joint_idx_ = static_cast(i); - // copy of params will be created; priority increased with each joint. boost::shared_ptr jla(new Jla_t(startPrio++, params, data_mediator)); constraints.insert(boost::static_pointer_cast >(jla)); } @@ -105,12 +104,13 @@ std::set ConstraintsBuilder::createConstraints(const Twi if (CA_ON == tc_params.constraint_ca) { typedef CollisionAvoidance CollisionAvoidance_t; - uint32_t startPrio = tc_params.priority_ca; + uint32_t startPrio = tc_params.constraint_params.at(CA).priority; for (std::vector::const_iterator it = tc_params.collision_check_links.begin(); it != tc_params.collision_check_links.end(); it++) { - ConstraintParamsCA params = ConstraintParamFactory::createConstraintParams(tc_params, limiter_params, data_mediator, *it); + ConstraintParamsCA params = ConstraintParamsCA(tc_params.constraint_params.at(CA),tc_params.frame_names, *it); + data_mediator.fill(params); // TODO: take care PRIO could be of different type than UINT32 boost::shared_ptr ca(new CollisionAvoidance_t(startPrio--, params, data_mediator, jnt_to_jac, fk_solver_vel)); constraints.insert(boost::static_pointer_cast >(ca)); @@ -119,9 +119,6 @@ std::set ConstraintsBuilder::createConstraints(const Twi else { // CA_OFF selected. - // Nothing to do here! - // Create constraints will be called also in case of an unconstraint solver etc. - // So the log would be filled unnecessarily. } return constraints; diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h index 48104d5c..b848131b 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h @@ -50,22 +50,6 @@ #include /* BEGIN JointLimitAvoidance ************************************************************************************/ -template -Task_t JointLimitAvoidance::createTask() -{ - Eigen::MatrixXd cost_func_jac = this->getTaskJacobian(); - Eigen::VectorXd derivs = this->getTaskDerivatives(); - Task_t task(this->getPriority(), - this->getTaskId(), - cost_func_jac, - derivs, - this->getType()); - - task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.tc_params_.damping_jla); - task.db_ = boost::shared_ptr(DampingBuilder::createDamping(task.tcp_)); - return task; -} - template std::string JointLimitAvoidance::getTaskId() const { @@ -94,7 +78,7 @@ Eigen::VectorXd JointLimitAvoidance::getTaskDerivatives() const template void JointLimitAvoidance::calculate() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; const int32_t joint_idx = this->constraint_params_.joint_idx_; const double limit_min = limiter_params.limits_min[joint_idx]; @@ -120,8 +104,8 @@ void JointLimitAvoidance::calculate() const double pred_rel_min = std::abs(pred_delta_min / limit_min); const double pred_rel_val = pred_rel_max < pred_rel_min ? pred_rel_max : pred_rel_min; - const double activation = params.thresholds_jla.activation; - const double critical = params.thresholds_jla.critical; + const double activation = params.thresholds.activation; + const double critical = params.thresholds.critical; if (this->state_.getCurrent() == CRITICAL && pred_rel_val < rel_val) { @@ -140,9 +124,9 @@ void JointLimitAvoidance::calculate() template double JointLimitAvoidance::getActivationGain() const { - const TwistControllerParams& params = this->constraint_params_.tc_params_; - const double activation_threshold = params.thresholds_jla.activation; - const double activation_buffer_region = params.thresholds_jla.activation_with_buffer; // [%] + const ConstraintParams& params = this->constraint_params_.params_; + const double activation_threshold = params.thresholds.activation; + const double activation_buffer_region = params.thresholds.activation_with_buffer; // [%] double activation_gain; const double rel_delta = this->rel_min_ < this->rel_max_ ? this->rel_min_ : this->rel_max_; @@ -172,22 +156,13 @@ double JointLimitAvoidance::getActivationGain() const template double JointLimitAvoidance::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const { - const TwistControllerParams& params = this->constraint_params_.tc_params_; - double k_H = params.k_H_jla; - return k_H; -} - -template -ConstraintTypes JointLimitAvoidance::getType() const -{ - return JLA; + return this->constraint_params_.params_.k_H; } /// Calculate values of the JLA cost function. template void JointLimitAvoidance::calcValue() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; const int32_t joint_idx = this->constraint_params_.joint_idx_; std::vector limits_min = limiter_params.limits_min; @@ -212,7 +187,6 @@ void JointLimitAvoidance::calcDerivativeValue() template void JointLimitAvoidance::calcPartialValues() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; const double joint_pos = this->joint_states_.current_q_(this->constraint_params_.joint_idx_); const double joint_vel = this->joint_states_.current_q_dot_(this->constraint_params_.joint_idx_); @@ -260,22 +234,13 @@ double JointLimitAvoidanceMid::getActivationGain() const template double JointLimitAvoidanceMid::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const { - const TwistControllerParams& params = this->constraint_params_.tc_params_; - double k_H = params.k_H_jla; - return k_H; -} - -template -ConstraintTypes JointLimitAvoidanceMid::getType() const -{ - return JLA_MID; + return this->constraint_params_.params_.k_H; } /// Calculate values of the JLA cost function. template void JointLimitAvoidanceMid::calcValue() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; std::vector limits_min = limiter_params.limits_min; std::vector limits_max = limiter_params.limits_max; @@ -314,7 +279,6 @@ void JointLimitAvoidanceMid::calcDerivativeValue() template void JointLimitAvoidanceMid::calcPartialValues() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; const KDL::JntArray joint_pos = this->joint_states_.current_q_; std::vector limits_min = limiter_params.limits_min; @@ -345,22 +309,6 @@ void JointLimitAvoidanceMid::calcPartialValues() /* END JointLimitAvoidanceMid ************************************************************************************/ /* BEGIN JointLimitAvoidanceIneq ************************************************************************************/ -template -Task_t JointLimitAvoidanceIneq::createTask() -{ - Eigen::MatrixXd cost_func_jac = this->getTaskJacobian(); - Eigen::VectorXd derivs = this->getTaskDerivatives(); - Task_t task(this->getPriority(), - this->getTaskId(), - cost_func_jac, - derivs, - this->getType()); - - task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.tc_params_.damping_jla); - task.db_ = boost::shared_ptr(DampingBuilder::createDamping(task.tcp_)); - return task; -} - template std::string JointLimitAvoidanceIneq::getTaskId() const { @@ -387,7 +335,7 @@ Eigen::VectorXd JointLimitAvoidanceIneq::getTaskDerivatives() co template void JointLimitAvoidanceIneq::calculate() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; const int32_t joint_idx = this->constraint_params_.joint_idx_; const double limit_min = limiter_params.limits_min[joint_idx]; @@ -415,8 +363,8 @@ void JointLimitAvoidanceIneq::calculate() this->prediction_value_ = pred_rel_max < pred_rel_min ? pred_rel_max : pred_rel_min; - double activation = params.thresholds_jla.activation; - double critical = params.thresholds_jla.critical; + double activation = params.thresholds.activation; + double critical = params.thresholds.critical; if (this->state_.getCurrent() == CRITICAL && this->prediction_value_ < rel_val) { @@ -440,9 +388,9 @@ void JointLimitAvoidanceIneq::calculate() template double JointLimitAvoidanceIneq::getActivationGain() const { - const TwistControllerParams& params = this->constraint_params_.tc_params_; - const double activation_threshold = params.thresholds_jla.activation; // [%] - const double activation_buffer_region = params.thresholds_jla.activation_with_buffer; // [%] + const ConstraintParams& params = this->constraint_params_.params_; + const double activation_threshold = params.thresholds.activation; // [%] + const double activation_buffer_region = params.thresholds.activation_with_buffer; // [%] double activation_gain; double rel_delta; @@ -482,16 +430,16 @@ template double JointLimitAvoidanceIneq::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const { double factor; - const TwistControllerParams& params = this->constraint_params_.tc_params_; + const ConstraintParams& params = this->constraint_params_.params_; if (this->abs_delta_max_ > this->abs_delta_min_ && this->rel_min_ > 0.0) { - factor = (params.thresholds_jla.activation * 1.1 / this->rel_min_) - 1.0; + factor = (params.thresholds.activation * 1.1 / this->rel_min_) - 1.0; } else { if (this->rel_max_ > 0.0) { - factor = (params.thresholds_jla.activation * 1.1 / this->rel_max_) - 1.0; + factor = (params.thresholds.activation * 1.1 / this->rel_max_) - 1.0; } else { @@ -499,21 +447,14 @@ double JointLimitAvoidanceIneq::getSelfMotionMagnitude(const Eig } } - double k_H = factor * params.k_H_jla; + double k_H = factor * params.k_H; return k_H; } -template -ConstraintTypes JointLimitAvoidanceIneq::getType() const -{ - return JLA_INEQ; -} - /// Calculate values of the JLA cost function. template void JointLimitAvoidanceIneq::calcValue() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; int32_t joint_idx = this->constraint_params_.joint_idx_; double limit_min = limiter_params.limits_min[joint_idx]; @@ -536,7 +477,6 @@ void JointLimitAvoidanceIneq::calcDerivativeValue() template void JointLimitAvoidanceIneq::calcPartialValues() { - const TwistControllerParams& params = this->constraint_params_.tc_params_; const LimiterParams& limiter_params = this->constraint_params_.limiter_params_; int32_t joint_idx = this->constraint_params_.joint_idx_; double limit_min = limiter_params.limits_min[joint_idx]; diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h index 65838249..d9dc2551 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h @@ -42,11 +42,9 @@ class ConstraintParamsBase { public: - ConstraintParamsBase(const TwistControllerParams& params, - const LimiterParams& limiter_params, + ConstraintParamsBase(const ConstraintParams& params, const std::string& id = std::string()) : - tc_params_(params), - limiter_params_(limiter_params), + params_(params), id_(id) {} @@ -54,8 +52,7 @@ class ConstraintParamsBase {} const std::string id_; - const TwistControllerParams& tc_params_; - const LimiterParams& limiter_params_; + const ConstraintParams params_; }; /* END ConstraintParamsBase *************************************************************************************/ @@ -64,21 +61,23 @@ class ConstraintParamsBase class ConstraintParamsCA : public ConstraintParamsBase { public: - ConstraintParamsCA(const TwistControllerParams& params, - const LimiterParams& limiter_params, + ConstraintParamsCA(const ConstraintParams& params, + const std::vector& frame_names = std::vector(), const std::string& id = std::string()) : - ConstraintParamsBase(params, limiter_params, id) + ConstraintParamsBase(params, id), + frame_names_(frame_names) {} ConstraintParamsCA(const ConstraintParamsCA& cpca) : - ConstraintParamsBase(cpca.tc_params_, cpca.limiter_params_, cpca.id_) - { - current_distances_ = cpca.current_distances_; - } + ConstraintParamsBase(cpca.params_, cpca.id_), + frame_names_(cpca.frame_names_), + current_distances_(cpca.current_distances_) + {} virtual ~ConstraintParamsCA() {} + std::vector frame_names_; std::vector current_distances_; }; /* END ConstraintParamsCA ***************************************************************************************/ @@ -88,17 +87,19 @@ class ConstraintParamsCA : public ConstraintParamsBase class ConstraintParamsJLA : public ConstraintParamsBase { public: - ConstraintParamsJLA(const TwistControllerParams& params, - const LimiterParams& limiter_params, + ConstraintParamsJLA(const ConstraintParams& params, + const LimiterParams& limiter_params = LimiterParams(), const std::string& id = std::string()) : - ConstraintParamsBase(params, limiter_params, id), - joint_idx_(-1) + ConstraintParamsBase(params, id), + joint_idx_(-1), + limiter_params_(limiter_params) {} ConstraintParamsJLA(const ConstraintParamsJLA& cpjla) : - ConstraintParamsBase(cpjla.tc_params_, cpjla.limiter_params_, cpjla.id_), + ConstraintParamsBase(cpjla.params_, cpjla.id_), joint_(cpjla.joint_), - joint_idx_(cpjla.joint_idx_) + joint_idx_(cpjla.joint_idx_), + limiter_params_(cpjla.limiter_params_) {} virtual ~ConstraintParamsJLA() @@ -106,6 +107,7 @@ class ConstraintParamsJLA : public ConstraintParamsBase std::string joint_; int32_t joint_idx_; + const LimiterParams& limiter_params_; }; /* END ConstraintParamsJLA **************************************************************************************/ diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/self_motion_magnitude.h b/cob_twist_controller/include/cob_twist_controller/constraints/self_motion_magnitude.h deleted file mode 100644 index ee1e6820..00000000 --- a/cob_twist_controller/include/cob_twist_controller/constraints/self_motion_magnitude.h +++ /dev/null @@ -1,162 +0,0 @@ -/*! - ***************************************************************** - * \file - * - * \note - * Copyright (c) 2015 \n - * Fraunhofer Institute for Manufacturing Engineering - * and Automation (IPA) \n\n - * - ***************************************************************** - * - * \note - * Project name: care-o-bot - * \note - * ROS stack name: cob_control - * \note - * ROS package name: cob_twist_controller - * - * \author - * Author: Marco Bezzon, email: Marco.Bezzon@ipa.fraunhofer.de - * - * \date Date of creation: June, 2015 - * - * \brief - * This header contains the template implementations for - * self motion magnitude determination (factor k_H for GPM) - * One proposal according to: - * Euler J.A., Dubey R.V., Babcock S.M. (1989), - * "Self motion determination based on actuator velocity bounds - * for redundant manipulators". J. Robotic Syst., 6: 417-425 - * - * Other: - * Parameter from dynamic_reconfigure. - * - ****************************************************************/ - -#ifndef COB_TWIST_CONTROLLER_CONSTRAINTS_SELF_MOTION_MAGNITUDE_H -#define COB_TWIST_CONTROLLER_CONSTRAINTS_SELF_MOTION_MAGNITUDE_H - -#include -#include -#include -#include -#include - -#include "cob_twist_controller/cob_twist_controller_data_types.h" - -class SelfMotionMagnitudeDeterminatorBase -{ - public: - SelfMotionMagnitudeDeterminatorBase() - {} - - virtual ~SelfMotionMagnitudeDeterminatorBase() - {} - - virtual double calculate(const LimiterParams& params, - const Eigen::MatrixXd& particular_solution, - const Eigen::MatrixXd& homogeneous_solution) const = 0; -}; - -template - -class SmmDeterminatorVelocityBounds : public SelfMotionMagnitudeDeterminatorBase -{ - public: - SmmDeterminatorVelocityBounds() - {} - - virtual ~SmmDeterminatorVelocityBounds() - {} - - /// Implementation of SMM. Formula: See header comment! - virtual double calculate(const LimiterParams& params, const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const - { - if (params.limits_vel.size() != homogeneous_solution.rows() || params.limits_vel.size() != particular_solution.rows()) - { - ROS_ERROR("Count of rows do not match for particular solution, homogeneous solution and vector limits."); - ROS_ERROR_STREAM("Part.Solution: " << particular_solution.rows()); - ROS_ERROR_STREAM("Hom.Solution: " << homogeneous_solution.rows()); - ROS_ERROR_STREAM("Vel.Lim: " << params.limits_vel.size()); - return 0.0; - } - - if (homogeneous_solution.norm() <= ZERO_THRESHOLD) - { - return 0.0; - } - - double kMax = std::numeric_limits::max(); - double kMin = -std::numeric_limits::max(); - double kResult = 0.0; - for (uint16_t i = 0; i < cntRows; ++i) - { - double upper = 0.0; - double lower = 0.0; - if (std::fabs(static_cast(homogeneous_solution(i))) > ZERO_THRESHOLD) - { - upper = (params.limits_vel[i] - particular_solution(i)) / homogeneous_solution(i); - lower = (-params.limits_vel[i] - particular_solution(i)) / homogeneous_solution(i); - } - - kMax = std::min(std::max(upper, lower), kMax); - kMin = std::max(std::min(upper, lower), kMin); - } - - if (kMax > kMin) - { - if (MAXIMIZE) - { - kResult = kMax; - ROS_INFO_STREAM("Calculated MAX k = " << kResult); - } - else - { - kResult = kMin; - ROS_INFO_STREAM("Calculated MIN k = " << kResult); - } - } - else - { - ROS_ERROR("The requested end-effector velocity is too high. A proper k cannot be found! Assuming MIN: -1.0 or MAX 1.0. "); - kResult = MAXIMIZE ? 1.0 : -1.0; - } - - return kResult; - } -}; - -class SmmDeterminatorConstant : public SelfMotionMagnitudeDeterminatorBase -{ - public: - SmmDeterminatorConstant() - {} - - virtual ~SmmDeterminatorConstant() - {} - - virtual double calculate(const LimiterParams& params, - const Eigen::MatrixXd& particular_solution, - const Eigen::MatrixXd& homogeneous_solution) const - { - // not really used anymore - return 1.0; - } -}; - -/// Factory to create an SMM type object and call calculate method on it. -template -class SelfMotionMagnitudeFactory -{ - public: - static double calculate(const LimiterParams& params, - const Eigen::MatrixXd& particular_solution, - const Eigen::MatrixXd& homogeneous_solution) - { - T smm_determinator; - return smm_determinator.calculate(params, particular_solution, homogeneous_solution); - } -}; - -#endif // COB_TWIST_CONTROLLER_CONSTRAINTS_SELF_MOTION_MAGNITUDE_H diff --git a/cob_twist_controller/include/cob_twist_controller/task_stack/task_stack_controller.h b/cob_twist_controller/include/cob_twist_controller/task_stack/task_stack_controller.h index bb86bcc3..b9752794 100644 --- a/cob_twist_controller/include/cob_twist_controller/task_stack/task_stack_controller.h +++ b/cob_twist_controller/include/cob_twist_controller/task_stack/task_stack_controller.h @@ -50,15 +50,13 @@ struct Task Eigen::VectorXd task_; std::string id_; bool is_active_; - ConstraintTypes constraint_type_; - boost::shared_ptr db_; TwistControllerParams tcp_; - Task(PRIO prio, std::string id) : prio_(prio), id_(id), is_active_(true), constraint_type_(None) + Task(PRIO prio, std::string id) : prio_(prio), id_(id), is_active_(true) {} - Task(PRIO prio, std::string id, Eigen::MatrixXd task_jacobian, Eigen::VectorXd task, ConstraintTypes ct = None) - : prio_(prio), id_(id), task_jacobian_(task_jacobian), task_(task), is_active_(true), constraint_type_(ct) + Task(PRIO prio, std::string id, Eigen::MatrixXd task_jacobian, Eigen::VectorXd task) + : prio_(prio), id_(id), task_jacobian_(task_jacobian), task_(task), is_active_(true) {} Task(const Task& task) @@ -67,15 +65,11 @@ struct Task task_jacobian_(task.task_jacobian_), task_(task.task_), is_active_(task.is_active_), - constraint_type_(task.constraint_type_), - tcp_(task.tcp_), - db_(task.db_) + tcp_(task.tcp_) {} ~Task() - { - this->db_.reset(); - } + {} inline void setPriority(PRIO prio) { @@ -176,7 +170,6 @@ void TaskStackController::addTask(Task t) it->task_jacobian_ = t.task_jacobian_; it->task_ = t.task_; it->tcp_ = t.tcp_; - it->db_ = t.db_; break; } } diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp index f4171c96..51ccd249 100644 --- a/cob_twist_controller/src/cob_twist_controller.cpp +++ b/cob_twist_controller/src/cob_twist_controller.cpp @@ -239,36 +239,41 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll twist_controller_params_.slope_damping = config.slope_damping; twist_controller_params_.beta = config.beta; twist_controller_params_.eps_damping = config.eps_damping; + twist_controller_params_.eps_truncation = config.eps_truncation; twist_controller_params_.solver = static_cast(config.solver); twist_controller_params_.priority_main = config.priority; + twist_controller_params_.k_H = config.k_H; twist_controller_params_.constraint_jla = static_cast(config.constraint_jla); - twist_controller_params_.priority_jla = config.priority_jla; - twist_controller_params_.k_H_jla = config.k_H_jla; + twist_controller_params_.constraint_ca = static_cast(config.constraint_ca); + + ConstraintParams cp_jla; + cp_jla.priority = config.priority_jla; + cp_jla.k_H = config.k_H_jla; + cp_jla.damping = config.damping_jla; const double activation_jla_in_percent = config.activation_threshold_jla; const double activation_buffer_jla_in_percent = config.activation_buffer_jla; const double critical_jla_in_percent = config.critical_threshold_jla; - twist_controller_params_.thresholds_jla.activation = activation_jla_in_percent / 100.0; - twist_controller_params_.thresholds_jla.activation_position_threshold_jla=config.activation_position_threshold_jla; - twist_controller_params_.thresholds_jla.activation_speed_threshold_jla=config.activation_speed_threshold_jla; - twist_controller_params_.thresholds_jla.activation_with_buffer = twist_controller_params_.thresholds_jla.activation * (1.0 + activation_buffer_jla_in_percent / 100.0); - twist_controller_params_.thresholds_jla.critical = critical_jla_in_percent / 100.0; - twist_controller_params_.damping_jla = config.damping_jla; - twist_controller_params_.damping_speed_jla = config.damping_speed_jla; - - twist_controller_params_.constraint_ca = static_cast(config.constraint_ca); - twist_controller_params_.priority_ca = config.priority_ca; - twist_controller_params_.k_H_ca = config.k_H_ca; + cp_jla.thresholds.activation = activation_jla_in_percent / 100.0; + cp_jla.thresholds.critical = critical_jla_in_percent / 100.0; + cp_jla.thresholds.activation_with_buffer = cp_jla.thresholds.activation * (1.0 + activation_buffer_jla_in_percent / 100.0); + twist_controller_params_.constraint_params[JLA] = cp_jla; + + ConstraintParams cp_ca; + cp_ca.priority = config.priority_ca; + cp_ca.k_H = config.k_H_ca; + cp_ca.damping = config.damping_ca; const double activaton_buffer_ca_in_percent = config.activation_buffer_ca; - twist_controller_params_.thresholds_ca.activation = config.activation_threshold_ca; // in [m] - twist_controller_params_.thresholds_ca.activation_with_buffer = twist_controller_params_.thresholds_ca.activation * (1.0 + activaton_buffer_ca_in_percent / 100.0); - twist_controller_params_.thresholds_ca.critical = config.critical_threshold_ca; // in [m] - twist_controller_params_.damping_ca = config.damping_ca; + cp_ca.thresholds.activation = config.activation_threshold_ca; // in [m] + cp_ca.thresholds.critical = config.critical_threshold_ca; // in [m] + cp_ca.thresholds.activation_with_buffer = cp_ca.thresholds.activation * (1.0 + activaton_buffer_ca_in_percent / 100.0); + twist_controller_params_.constraint_params[JLA] = cp_ca; - twist_controller_params_.k_H = config.k_H; - - twist_controller_params_.eps_truncation = config.eps_truncation; + twist_controller_params_.ujs_solver_params.sigma = config.sigma; + twist_controller_params_.ujs_solver_params.sigma_speed = config.sigma_speed; + twist_controller_params_.ujs_solver_params.delta_pos = config.delta_pos; + twist_controller_params_.ujs_solver_params.delta_speed = config.delta_speed; twist_controller_params_.limiter_params.keep_direction = config.keep_direction; twist_controller_params_.limiter_params.enforce_input_limits = config.enforce_input_limits; @@ -290,6 +295,32 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistControllerConfig& config) { bool warning = false; + + DampingMethodTypes damping = static_cast(config.damping_method); + if (damping != twist_controller_params_.damping_method) + { + //damping method has changed - setting back to proper default values + if (damping == CONSTANT) + { + config.damping_factor = 0.01; + } + else if (damping == MANIPULABILITY) + { + config.lambda_max = 0.1; + config.w_threshold = 0.005; + } + else if (damping == LEAST_SINGULAR_VALUE) + { + config.lambda_max = 0.1; + config.w_threshold = 0.005; + } + else if (damping == SIGMOID) + { + config.lambda_max = 0.001; + config.w_threshold = 0.001; + } + } + SolverTypes solver = static_cast(config.solver); if (DEFAULT_SOLVER == solver && (JLA_OFF != static_cast(config.constraint_jla) || CA_OFF != static_cast(config.constraint_ca))) @@ -302,14 +333,6 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo warning = true; } - if (UNIFIED_JLA_SA == solver && CA_OFF != static_cast(config.constraint_ca)) - { - ROS_ERROR("The Unified JLA and SA solution doesn\'t support collision avoidance. Currently UNIFIED_JLA_SA is only implemented for SA and JLA ..."); - twist_controller_params_.constraint_ca = CA_OFF; - config.constraint_ca = static_cast(twist_controller_params_.constraint_ca); - warning = true; - } - if (WLN == solver && CA_OFF != static_cast(config.constraint_ca)) { ROS_ERROR("The WLN solution doesn\'t support collision avoidance. Currently WLN is only implemented for Identity and JLA ..."); @@ -334,6 +357,14 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo warning = true; } + if (UNIFIED_JLA_SA == solver && CA_OFF != static_cast(config.constraint_ca)) + { + ROS_ERROR("The Unified JLA and SA solution doesn\'t support collision avoidance. Currently UNIFIED_JLA_SA is only implemented for SA and JLA ..."); + twist_controller_params_.constraint_ca = CA_OFF; + config.constraint_ca = static_cast(twist_controller_params_.constraint_ca); + warning = true; + } + if (CA_OFF != static_cast(config.constraint_ca)) { if (!register_link_client_.exists()) diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp index 69b5fce1..591da32c 100644 --- a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp +++ b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp @@ -95,7 +95,6 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities, const Vector6d_t scaled_in_cart_velocities = (1.0 / pow(this->in_cart_vel_damping_, 2.0)) * in_cart_velocities; Task_t t(this->params_.priority_main, "Main task", this->jacobian_data_, scaled_in_cart_velocities); t.tcp_ = this->params_; - t.db_ = this->damping_; this->task_stack_controller_.addTask(t); // ROS_INFO_STREAM("============== Task output ============= with main task damping: " << this->in_cart_vel_damping_); @@ -105,7 +104,7 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities, Eigen::MatrixXd J_task = it->task_jacobian_; Eigen::MatrixXd J_temp = J_task * projector_i; Eigen::VectorXd v_task = it->task_; - Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp); + Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp); //ToDo: Do we need damping here? q_i = q_i + J_temp_inv * (v_task - J_task * q_i); projector_i = projector_i - J_temp_inv * J_temp; } diff --git a/cob_twist_controller/src/constraint_solvers/solvers/unified_joint_limit_singularity_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/unified_joint_limit_singularity_solver.cpp index 6426b241..122b1f0d 100644 --- a/cob_twist_controller/src/constraint_solvers/solvers/unified_joint_limit_singularity_solver.cpp +++ b/cob_twist_controller/src/constraint_solvers/solvers/unified_joint_limit_singularity_solver.cpp @@ -93,19 +93,19 @@ Eigen::MatrixXd UnifiedJointLimitSingularitySolver::calculateWeighting(const Vec KDL::JntArray q = joint_states.current_q_; - double sigma = this->params_.damping_jla; - double sigma_speed = this->params_.damping_speed_jla; - double delta_pos = this->params_.thresholds_jla.activation_position_threshold_jla; - double delta_speed = this->params_.thresholds_jla.activation_speed_threshold_jla; + double sigma = this->params_.ujs_solver_params.sigma; + double sigma_speed = this->params_.ujs_solver_params.sigma_speed; + double delta_pos = this->params_.ujs_solver_params.delta_pos; + double delta_speed = this->params_.ujs_solver_params.delta_speed; for (uint32_t i = 0; i < cols ; ++i) { weighting(i) = (1.0/(1.0+exp(-(q(i)-limits_min[i]-delta_pos)/sigma)))*(1.0/(1.0+exp((q(i)-limits_max[i]+delta_pos)/sigma)))+(1.0/(1.0+exp((q(i)*q_dot(i)+delta_speed)*sigma_speed))); - if( (fabs(q(i)-limits_min[i])1.0) - weighting(i)=1.0; + if(weighting(i)>1.0){ + weighting(i)=1.0; + } } return weighting.asDiagonal(); diff --git a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py index 6eb646a0..f16dff1b 100644 --- a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py +++ b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py @@ -51,11 +51,8 @@ K_H_JLA = 'k_H_jla' ACTIV_THRESH_JLA = 'activation_threshold_jla' ACTIV_BUF_JLA = 'activation_buffer_jla' -ACTIV_POS_THRESH_JLA = 'activation_position_threshold_jla' -ACTIV_SPEED_THRESH_JLA = 'activation_speed_threshold_jla' CRIT_THRESH_JLA = 'critical_threshold_jla' DAMP_JLA = 'damping_jla' -DAMP_SPEED_JLA = 'damping_speed_jla' CONSTR_CA = 'constraint_ca' PRIO_CA = 'priority_ca' @@ -65,6 +62,11 @@ CRIT_THRESH_CA = 'critical_threshold_ca' DAMP_CA = 'damping_ca' +SIGMA_UJS = 'sigma_ujs' +SIGMA_SPEED_UJS = 'sigma_speed_ujs' +DELTA_POS_UJS = 'delta_pos_ujs' +SIGMA_SPEED_UJS = 'delta_speed_ujs' + KEEP_DIR = 'keep_direction' ENF_POS_LIM = 'enforce_pos_limits' ENF_VEL_LIM = 'enforce_vel_limits'