Skip to content

Commit

Permalink
Merge pull request #89 from ipa-fxm/fix_collision_avoidance_dimensions
Browse files Browse the repository at this point in the history 
Fix collision avoidance dimensions
  • Loading branch information
Florian Weisshardt committed Feb 24, 2016
2 parents 82c3887 + 99e6e9c commit 3125bfe
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 32 deletions.
2 changes: 1 addition & 1 deletion cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ class CobTwistController

bool initialize();

void reinitServiceRegistration();
bool registerCollisionLinks();

void reconfigureCallback(cob_twist_controller::TwistControllerConfig& config, uint32_t level);
void checkSolverAndConstraints(cob_twist_controller::TwistControllerConfig& config);
Expand Down
18 changes: 11 additions & 7 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 @@ -267,12 +267,12 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
// ROS_INFO_STREAM("CollisionAvoidance::calcPartialValues:");
Eigen::VectorXd partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
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_;
int32_t size_of_joints = params.joints.size();
std::vector<Eigen::VectorXd> vec_partial_values;

// ROS_INFO_STREAM("this->jacobian_data_.cols: " << this->jacobian_data_.cols());
// ROS_INFO_STREAM("size_of_joints: " << size_of_joints);
// 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(),
Expand Down Expand Up @@ -307,14 +307,17 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
uint32_t idx = str_it - params.frame_names.begin();
uint32_t frame_number = idx + 1; // segment nr not index represents frame number

KDL::Jacobian new_jac_chain(size_of_joints);
KDL::JntArray ja = this->joint_states_.current_q_;
KDL::Jacobian new_jac_chain(this->joint_states_.current_q_.rows());

// ROS_INFO_STREAM("frame_number: " << frame_number);
// ROS_INFO_STREAM("ja.rows: " << ja.rows());
// ROS_INFO_STREAM("new_jac_chain: " << new_jac_chain.rows() << " x " << new_jac_chain.columns());

if (0 != this->jnt_to_jac_.JntToJac(ja, new_jac_chain, frame_number))
{
ROS_ERROR_STREAM("Failed to calculate JntToJac.");
ROS_ERROR_STREAM("Failed to calculate JntToJac. Error Code: " << this->jnt_to_jac_.getError() << " (" << this->jnt_to_jac_.strError(this->jnt_to_jac_.getError()) << ")");
ROS_ERROR_STREAM("This is likely due to using a KinematicExtension! The ChainJntToJac-Solver is configured for the main chain only!");
return;
}
// ROS_INFO_STREAM("new_jac_chain.columns: " << new_jac_chain.columns());
Expand Down Expand Up @@ -366,7 +369,6 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
// ROS_INFO_STREAM("this->task_jacobian_.rows:" << this->task_jacobian_.rows());
// ROS_INFO_STREAM("this->task_jacobian_.cols:" << this->task_jacobian_.cols());

this->partial_values_.resize(sum_partial_values.rows(), 1);
this->partial_values_ = sum_partial_values;
}

Expand Down Expand Up @@ -402,9 +404,11 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
// ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows());

// Calculate prediction for pos and vel
if (0 != this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number))
int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number);
if (error != 0)
{
ROS_ERROR_STREAM("Could not calculate twist for frame: " << frame_number);
ROS_ERROR_STREAM("Could not calculate twist for frame: " << frame_number << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")");
ROS_ERROR_STREAM("This is likely due to using a KinematicExtension! The ChainFkSolverVel is configured for the main chain only!");
return;
}
// ROS_INFO_STREAM("Calculated twist for frame: " << frame_number);
Expand Down
1 change: 1 addition & 0 deletions cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -543,6 +543,7 @@ void JointLimitAvoidanceIneq<T_PARAMS, PRIO>::calcPartialValues()
double limit_max = limiter_params.limits_max[joint_idx];
double joint_pos = this->joint_states_.current_q_(joint_idx);
Eigen::VectorXd partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());

partial_values(this->constraint_params_.joint_idx_) = -2.0 * joint_pos + limit_max + limit_min;
this->partial_values_ = partial_values;
}
Expand Down
52 changes: 28 additions & 24 deletions cob_twist_controller/src/cob_twist_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,7 @@ bool CobTwistController::initialize()
}
register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("obstacle_distance/registerLinkOfInterest");
register_link_client_.waitForExistence(ros::Duration(5.0));
twist_controller_params_.constraint_ca = CA_OFF;

/// initialize configuration control solver
p_inv_diff_kin_solver_.reset(new InverseDifferentialKinematicsSolver(twist_controller_params_, chain_, callback_data_mediator_));
Expand Down Expand Up @@ -179,7 +180,7 @@ bool CobTwistController::initialize()
return true;
}

void CobTwistController::reinitServiceRegistration()
bool CobTwistController::registerCollisionLinks()
{
ROS_WARN_COND(twist_controller_params_.collision_check_links.size() <= 0,
"No collision_check_links set for this chain. Nothing will be registered. Ensure parameters are set correctly.");
Expand All @@ -194,18 +195,25 @@ void CobTwistController::reinitServiceRegistration()
if (register_link_client_.call(r))
{
ROS_INFO_STREAM("Called registration service with success: " << int(r.response.success) << ". Got message: " << r.response.message);
if (!r.response.success)
{
return false;
}
}
else
{
ROS_WARN_STREAM("Failed to call registration service for namespace: " << nh_.getNamespace());
break;
return false;
}
}

return true;
}

void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControllerConfig& config, uint32_t level)
{
this->checkSolverAndConstraints(config);

twist_controller_params_.controller_interface = static_cast<ControllerInterfaceTypes>(config.controller_interface);
twist_controller_params_.integrator_smoothing = config.integrator_smoothing;

Expand Down Expand Up @@ -256,22 +264,14 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll
this->controller_interface_.reset(ControllerInterfaceBuilder::createControllerInterface(this->nh_, this->twist_controller_params_, this->joint_states_));

p_inv_diff_kin_solver_->resetAll(this->twist_controller_params_);

if (CA_OFF != twist_controller_params_.constraint_ca)
{
this->reinitServiceRegistration();
}
}

void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistControllerConfig& config)
{
bool warning = false;
SolverTypes solver = static_cast<SolverTypes>(config.solver);
ConstraintTypesJLA ct_jla = static_cast<ConstraintTypesJLA>(config.constraint_jla);
ConstraintTypesCA ct_ca = static_cast<ConstraintTypesCA>(config.constraint_ca);
KinematicExtensionTypes ket = static_cast<KinematicExtensionTypes>(config.kinematic_extension);

if (DEFAULT_SOLVER == solver && (JLA_OFF != ct_jla || CA_OFF != ct_ca))
if (DEFAULT_SOLVER == solver && (JLA_OFF != static_cast<ConstraintTypesJLA>(config.constraint_jla) || CA_OFF != static_cast<ConstraintTypesCA>(config.constraint_ca)))
{
ROS_ERROR("The selection of Default solver and a constraint doesn\'t make any sense. Switch settings back ...");
twist_controller_params_.constraint_jla = JLA_OFF;
Expand All @@ -281,21 +281,21 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo
warning = true;
}

if (WLN == solver && CA_OFF != ct_ca)
if (WLN == solver && CA_OFF != static_cast<ConstraintTypesCA>(config.constraint_ca))
{
ROS_ERROR("The WLN solution doesn\'t support collision avoidance. Currently WLN is only implemented for Identity and JLA ...");
twist_controller_params_.constraint_ca = CA_OFF;
config.constraint_ca = static_cast<int>(twist_controller_params_.constraint_ca);
warning = true;
}

if (GPM == solver && CA_OFF == ct_ca && JLA_OFF == ct_jla)
if (GPM == solver && CA_OFF == static_cast<ConstraintTypesCA>(config.constraint_ca) && JLA_OFF == static_cast<ConstraintTypesJLA>(config.constraint_jla))
{
ROS_ERROR("You have chosen GPM but without constraints! The behaviour without constraints will be the same like for DEFAULT_SOLVER.");
warning = true;
}

if (TASK_2ND_PRIO == solver && (JLA_ON == ct_jla || CA_OFF == ct_ca))
if (TASK_2ND_PRIO == solver && (JLA_ON == static_cast<ConstraintTypesJLA>(config.constraint_jla) || CA_OFF == static_cast<ConstraintTypesCA>(config.constraint_ca)))
{
ROS_ERROR("The projection of a task into the null space of the main EE task is currently only for the CA constraint supported!");
twist_controller_params_.constraint_jla = JLA_OFF;
Expand All @@ -305,13 +305,25 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo
warning = true;
}

if (CA_OFF != ct_ca)
if (CA_OFF != static_cast<ConstraintTypesCA>(config.constraint_ca))
{
if (!register_link_client_.exists())
{
ROS_WARN("ServiceServer 'obstacle_distance/registerLinkOfInterest' does not exist. CA not possible");
ROS_ERROR("ServiceServer 'obstacle_distance/registerLinkOfInterest' does not exist. CA not possible");
twist_controller_params_.constraint_ca = CA_OFF;
config.constraint_ca = static_cast<int>(twist_controller_params_.constraint_ca);
warning = true;
}
else if (twist_controller_params_.constraint_ca != static_cast<ConstraintTypesCA>(config.constraint_ca))
{
ROS_INFO("Collision Avoidance has been activated! Register links!");
if (!this->registerCollisionLinks())
{
ROS_ERROR("Registration of links failed. CA not possible");
twist_controller_params_.constraint_ca = CA_OFF;
config.constraint_ca = static_cast<int>(twist_controller_params_.constraint_ca);
warning = true;
}
}
}

Expand All @@ -323,14 +335,6 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo
twist_controller_params_.limiter_params.enforce_acc_limits = config.enforce_acc_limits = false;
}

if (CA_OFF != ct_ca && ket != NO_EXTENSION)
{
ROS_ERROR("ToDo: CollisionAvoidance currently cannot be used together with KinematicExtensions!");
// This is due to a dimension conflict in ConstraintsCA! ConstraintsCA needs to be refactored (i.e. KDL::ChainFkSolverVel_recursive and KDL::ChainJntToJacSolver jnt2jac_ in InverseDifferentialKinematicsSolver)
twist_controller_params_.constraint_ca = CA_OFF;
config.constraint_ca = static_cast<int>(twist_controller_params_.constraint_ca);
}

if (!warning)
{
ROS_INFO("Parameters seem to be ok.");
Expand Down

0 comments on commit 3125bfe

Please sign in to comment.