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

Fix collision avoidance dimensions #89

Merged
merged 2 commits into from
Feb 24, 2016
+41 −32
Merged

Fix collision avoidance dimensions #89

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5af820e
fix collision avoidance dimension segfault
fmessmer Feb 21, 2016
99e6e9c
avoid unecessary service calls to obstacle_distance
fmessmer Feb 21, 2016
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
2 changes: 1 addition & 1 deletion cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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