Skip to content

Commit

Permalink
Merge pull request #156 from ipa-fxm/simplify_parameter_structure_con…
Browse files Browse the repository at this point in the history 
…straints

simplify parameter structure ConstraintParams
  • Loading branch information
@fmessmer
fmessmer authored Apr 21, 2017
2 parents 4975916 + 8f2d298 commit a7f3aaf
Show file tree
Hide file tree
Showing 15 changed files with 223 additions and 495 deletions.
20 changes: 11 additions & 9 deletions cob_twist_controller/cfg/TwistController.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) =====================================================
Expand All @@ -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)
Expand All @@ -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)
Expand Down
97 changes: 54 additions & 43 deletions cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -199,6 +202,21 @@ struct LimiterParams
std::vector<double> 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() :
Expand All @@ -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<ConstraintTypes, ConstraintParams>(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<ConstraintTypes, ConstraintParams>(JLA, cp_jla));
}

uint8_t dof;
Expand All @@ -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<ConstraintTypes, ConstraintParams> constraint_params;

UJSSolverParams ujs_solver_params;
LimiterParams limiter_params;

KinematicExtensionTypes kinematic_extension;
Expand Down
4 changes: 2 additions & 2 deletions .../cob_twist_controller/constraint_solvers/solvers/unified_joint_limit_singularity_solver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{}

Expand Down
34 changes: 0 additions & 34 deletions cob_twist_controller/include/cob_twist_controller/constraints/constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
<typename T>
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.
Expand Down Expand Up @@ -100,7 +76,6 @@ class CollisionAvoidance : public ConstraintBase<T_PARAMS, PRIO>
virtual ~CollisionAvoidance()
{}

virtual Task_t createTask();
virtual std::string getTaskId() const;
virtual Eigen::MatrixXd getTaskJacobian() const;
virtual Eigen::VectorXd getTaskDerivatives() const;
Expand All @@ -115,7 +90,6 @@ class CollisionAvoidance : public ConstraintBase<T_PARAMS, PRIO>
double getSelfMotionMagnitude(double current_cost_func_value) const;

private:
virtual ConstraintTypes getType() const;
virtual double getCriticalValue() const;

void calcValue();
Expand Down Expand Up @@ -152,7 +126,6 @@ class JointLimitAvoidance : public ConstraintBase<T_PARAMS, PRIO>
virtual ~JointLimitAvoidance()
{}

virtual Task_t createTask();
virtual std::string getTaskId() const;

virtual void calculate();
Expand All @@ -163,8 +136,6 @@ class JointLimitAvoidance : public ConstraintBase<T_PARAMS, PRIO>
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();
Expand Down Expand Up @@ -199,8 +170,6 @@ class JointLimitAvoidanceMid : public ConstraintBase<T_PARAMS, PRIO>
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();
Expand All @@ -226,7 +195,6 @@ class JointLimitAvoidanceIneq : public ConstraintBase<T_PARAMS, PRIO>
virtual ~JointLimitAvoidanceIneq()
{}

virtual Task_t createTask();
virtual std::string getTaskId() const;
virtual Eigen::MatrixXd getTaskJacobian() const;
virtual Eigen::VectorXd getTaskDerivatives() const;
Expand All @@ -237,8 +205,6 @@ class JointLimitAvoidanceIneq : public ConstraintBase<T_PARAMS, PRIO>
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();
Expand Down
26 changes: 1 addition & 25 deletions cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};


Expand Down Expand Up @@ -153,8 +151,7 @@ class ConstraintBase : public PriorityBase<PRIO>
Task_t task(this->getPriority(),
this->getTaskId(),
this->getTaskJacobian(),
this->getTaskDerivatives(),
this->getType());
this->getTaskDerivatives());
return task;
}

Expand Down Expand Up @@ -234,31 +231,10 @@ class ConstraintBase : public PriorityBase<PRIO>
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 <typename T_PARAMS, typename PRIO>
Expand Down
Loading

0 comments on commit a7f3aaf

Please sign in to comment.