Skip to content

Commit

Permalink
harmonize ConstraintParams
Browse files Browse the repository at this point in the history
  • Loading branch information
@fmessmer
fmessmer committed Apr 14, 2017
1 parent 81290ce commit a6923f2
Show file tree
Hide file tree
Showing 10 changed files with 139 additions and 362 deletions.
64 changes: 32 additions & 32 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 @@ -165,6 +156,20 @@ struct ConstraintThresholds
double critical;
};

struct ConstraintParams
{
uint32_t priority;
double k_H;
double damping;
ConstraintThresholds thresholds;
};

enum ConstraintTypes
{
CA,
JLA,
};

struct LimiterParams
{
LimiterParams() :
Expand Down Expand Up @@ -234,25 +239,28 @@ struct TwistControllerParams
k_H(1.0),

constraint_jla(JLA_ON),
priority_jla(50),
k_H_jla(-10.0),
damping_jla(0.000001),

constraint_ca(CA_ON),
priority_ca(100),
k_H_ca(2.0),
damping_ca(0.000001),

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
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 @@ -277,16 +285,8 @@ 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;
ConstraintThresholds thresholds_jla;
std::map<ConstraintTypes, ConstraintParams> constraint_params;

UJSSolverParams ujs_solver_params;
LimiterParams limiter_params;
Expand Down
8 changes: 0 additions & 8 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,7 +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.
Expand Down Expand Up @@ -115,7 +114,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 @@ -163,8 +161,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 +195,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 Down Expand Up @@ -237,8 +231,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
7 changes: 1 addition & 6 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,7 +108,6 @@ 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 +152,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,8 +232,6 @@ 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;
Expand All @@ -251,7 +247,6 @@ class ConstraintBase : public PriorityBase<PRIO>
*/
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;
Expand Down
72 changes: 28 additions & 44 deletions cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,10 +56,9 @@ Task_t CollisionAvoidance<T_PARAMS, PRIO>::createTask()
Task_t task(this->getPriority(),
this->getTaskId(),
this->getTaskJacobian(),
this->getTaskDerivatives(),
this->getType());
this->getTaskDerivatives());

task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.tc_params_.damping_ca);
task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.params_.damping);
task.db_.reset(DampingBuilder::createDamping(task.tcp_));

return task;
Expand Down Expand Up @@ -104,17 +103,17 @@ Eigen::VectorXd CollisionAvoidance<T_PARAMS, PRIO>::getTaskDerivatives() const
template <typename T_PARAMS, typename PRIO>
void CollisionAvoidance<T_PARAMS, PRIO>::calculate()
{
const TwistControllerParams& params = this->constraint_params_.tc_params_;
const ConstraintParams& params = this->constraint_params_.params_;

this->calcValue();
this->calcDerivativeValue();
this->calcPartialValues();
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)
Expand All @@ -138,10 +137,10 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calculate()
template <typename T_PARAMS, typename PRIO>
double CollisionAvoidance<T_PARAMS, PRIO>::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
{
Expand Down Expand Up @@ -169,8 +168,8 @@ double CollisionAvoidance<T_PARAMS, PRIO>::getActivationGain() const
template <typename T_PARAMS, typename PRIO>
double CollisionAvoidance<T_PARAMS, PRIO>::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)
Expand All @@ -185,7 +184,7 @@ double CollisionAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(double current
}
}

double k_H = params.k_H_ca;
double k_H = params.k_H;
return k_H * magnitude;
}

Expand All @@ -196,12 +195,6 @@ double CollisionAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(const Eigen::M
return 1.0;
}

template <typename T_PARAMS, typename PRIO>
ConstraintTypes CollisionAvoidance<T_PARAMS, PRIO>::getType() const
{
return CA;
}

template <typename T_PARAMS, typename PRIO>
double CollisionAvoidance<T_PARAMS, PRIO>::getCriticalValue() const
{
Expand All @@ -222,17 +215,17 @@ double CollisionAvoidance<T_PARAMS, PRIO>::getCriticalValue() const
template <typename T_PARAMS, typename PRIO>
void CollisionAvoidance<T_PARAMS, PRIO>::calcValue()
{
const TwistControllerParams& params = this->constraint_params_.tc_params_;
const ConstraintParams& params = this->constraint_params_.params_;
std::vector<double> relevant_values;
for (std::vector<ObstacleDistanceData>::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);
}
}
Expand Down Expand Up @@ -269,23 +262,23 @@ void CollisionAvoidance<T_PARAMS, PRIO>::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<Eigen::VectorXd> 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<std::string>::const_iterator str_it = std::find(params.frame_names.begin(),
params.frame_names.end(),
std::vector<std::string>::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<ObstacleDistanceData>::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;
Expand All @@ -304,7 +297,7 @@ void CollisionAvoidance<T_PARAMS, PRIO>::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_;
Expand Down Expand Up @@ -339,7 +332,7 @@ void CollisionAvoidance<T_PARAMS, PRIO>::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);
Expand All @@ -351,7 +344,7 @@ void CollisionAvoidance<T_PARAMS, PRIO>::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");
Expand All @@ -375,34 +368,25 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
template <typename T_PARAMS, typename PRIO>
void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
{
const TwistControllerParams& params = this->constraint_params_.tc_params_;
const ConstraintParams& params = this->constraint_params_.params_;
this->prediction_value_ = std::numeric_limits<double>::max();

ros::Time now = ros::Time::now();
double cycle = (now - this->last_pred_time_).toSec();
this->last_pred_time_ = now;

std::vector<std::string>::const_iterator str_it = std::find(params.frame_names.begin(),
params.frame_names.end(),
std::vector<std::string>::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)
Expand Down
Loading

0 comments on commit a6923f2

Please sign in to comment.