Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

simplify parameter structure ConstraintParams #156

Merged
merged 6 commits into from
Apr 21, 2017
Merged

simplify parameter structure ConstraintParams #156

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
b028245
own param struct for unified solver
fmessmer Apr 13, 2017
81290ce
set default values for damping methods
fmessmer Apr 13, 2017
7d6a54c
harmonize ConstraintParams
fmessmer Apr 14, 2017
1325dbf
remove ConstraintParamFactory
fmessmer Apr 14, 2017
7259c35
remove unused TaskDamping
fmessmer Apr 14, 2017
8f2d298
harmonize createTask
fmessmer Apr 14, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 11 additions & 9 deletions cob_twist_controller/cfg/TwistController.cfg
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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