constraint_ca_impl.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #ifndef COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_CA_IMPL_H
00019 #define COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_CA_IMPL_H
00020 
00021 #include <vector>
00022 #include <string>
00023 #include <limits>
00024 #include <sstream>
00025 #include <ros/ros.h>
00026 
00027 #include <boost/shared_ptr.hpp>
00028 #include <boost/pointer_cast.hpp>
00029 
00030 #include <kdl/chainjnttojacsolver.hpp>
00031 #include <kdl/jntarray.hpp>
00032 
00033 #include <eigen_conversions/eigen_kdl.h>
00034 
00035 #include "cob_twist_controller/constraints/constraint.h"
00036 #include "cob_twist_controller/constraints/constraint_params.h"
00037 
00038 #include "cob_twist_controller/damping_methods/damping.h"
00039 #include "cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h"
00040 
00041 /* BEGIN CollisionAvoidance *************************************************************************************/
00042 template <typename T_PARAMS, typename PRIO>
00043 std::string CollisionAvoidance<T_PARAMS, PRIO>::getTaskId() const
00044 {
00045     const std::string frame_id = this->constraint_params_.id_;
00046     std::ostringstream oss;
00047     oss << this->member_inst_cnt_;
00048     oss << "_";
00049     oss << frame_id;
00050     oss << "_";
00051     oss << this->priority_;
00052     std::string taskid = "CollisionAvoidance_" + oss.str();
00053     return taskid;
00054 }
00055 
00061 template <typename T_PARAMS, typename PRIO>
00062 Eigen::MatrixXd CollisionAvoidance<T_PARAMS, PRIO>::getTaskJacobian() const
00063 {
00064     return this->task_jacobian_;
00065 }
00066 
00072 template <typename T_PARAMS, typename PRIO>
00073 Eigen::VectorXd CollisionAvoidance<T_PARAMS, PRIO>::getTaskDerivatives() const
00074 {
00075     return this->derivative_values_;
00076 }
00077 
00078 template <typename T_PARAMS, typename PRIO>
00079 void CollisionAvoidance<T_PARAMS, PRIO>::calculate()
00080 {
00081     const ConstraintParams& params = this->constraint_params_.params_;
00082 
00083     this->calcValue();
00084     this->calcDerivativeValue();
00085     this->calcPartialValues();
00086     this->calcPredictionValue();
00087 
00088     const double pred_min_dist = this->getPredictionValue();
00089     const double activation = params.thresholds.activation;
00090     const double critical = params.thresholds.critical;
00091     const double activation_buffer = params.thresholds.activation_with_buffer;
00092     const double crit_min_distance = this->getCriticalValue();
00093 
00094     if (this->state_.getCurrent() == CRITICAL && pred_min_dist < crit_min_distance)
00095     {
00096         ROS_WARN_STREAM(this->getTaskId() << ": Current state is CRITICAL but prediction " << pred_min_dist << " is smaller than current dist " << crit_min_distance << " -> Stay in CRIT.");
00097     }
00098     else if (crit_min_distance < critical || pred_min_dist < critical)
00099     {
00100         this->state_.setState(CRITICAL);
00101     }
00102     else if (crit_min_distance < activation_buffer)
00103     {
00104         this->state_.setState(DANGER);
00105     }
00106     else
00107     {
00108         this->state_.setState(NORMAL);
00109     }
00110 }
00111 
00112 template <typename T_PARAMS, typename PRIO>
00113 double CollisionAvoidance<T_PARAMS, PRIO>::getActivationGain(double current_cost_func_value) const
00114 {
00115     const ConstraintParams& params = this->constraint_params_.params_;
00116     double activation_gain;
00117     const double activation = params.thresholds.activation;
00118     const double activation_buffer_region = params.thresholds.activation_with_buffer;
00119 
00120     if (current_cost_func_value < activation)  // activation == d_m
00121     {
00122         activation_gain = 1.0;
00123     }
00124     else if (current_cost_func_value < activation_buffer_region)  // activation_buffer_region == d_i
00125     {
00126         activation_gain = 0.5 * (1.0 + cos(M_PI * (current_cost_func_value - activation) / (activation_buffer_region - activation)));
00127     }
00128     else
00129     {
00130         activation_gain = 0.0;
00131     }
00132 
00133     return activation_gain;
00134 }
00135 
00136 template <typename T_PARAMS, typename PRIO>
00137 double CollisionAvoidance<T_PARAMS, PRIO>::getActivationGain() const
00138 {
00139     return 1.0;
00140 }
00141 
00143 template <typename T_PARAMS, typename PRIO>
00144 double CollisionAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(double current_distance_value) const
00145 {
00146     const ConstraintParams& params = this->constraint_params_.params_;
00147     const double activation_with_buffer = params.thresholds.activation_with_buffer;
00148     double magnitude = 0.0;
00149 
00150     if (current_distance_value < activation_with_buffer)
00151     {
00152         if (current_distance_value > 0.0)
00153         {
00154             magnitude = pow(activation_with_buffer / current_distance_value, 2.0) - 1.0;
00155         }
00156         else
00157         {
00158             magnitude = activation_with_buffer / 0.000001;  // strong magnitude
00159         }
00160     }
00161 
00162     double k_H = params.k_H;
00163     return k_H * magnitude;
00164 }
00165 
00167 template <typename T_PARAMS, typename PRIO>
00168 double CollisionAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const
00169 {
00170     return 1.0;
00171 }
00172 
00173 template <typename T_PARAMS, typename PRIO>
00174 double CollisionAvoidance<T_PARAMS, PRIO>::getCriticalValue() const
00175 {
00176     double min_distance = std::numeric_limits<double>::max();
00177     for (std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
00178          it != this->constraint_params_.current_distances_.end();
00179          ++it)
00180     {
00181         if (it->min_distance < min_distance)
00182         {
00183             min_distance = it->min_distance;
00184         }
00185     }
00186 
00187     return min_distance;
00188 }
00189 
00190 template <typename T_PARAMS, typename PRIO>
00191 void CollisionAvoidance<T_PARAMS, PRIO>::calcValue()
00192 {
00193     const ConstraintParams& params = this->constraint_params_.params_;
00194     std::vector<double> relevant_values;
00195     for (std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
00196          it != this->constraint_params_.current_distances_.end();
00197          ++it)
00198     {
00199         if (params.thresholds.activation_with_buffer > it->min_distance)
00200         {
00201             const double activation_gain = this->getActivationGain(it->min_distance);
00202             const double magnitude = std::abs(this->getSelfMotionMagnitude(it->min_distance));  // important only for task!
00203             double value = activation_gain * magnitude * pow(it->min_distance - params.thresholds.activation_with_buffer, 2.0);
00204             relevant_values.push_back(value);
00205         }
00206     }
00207 
00208     if (relevant_values.size() > 0)
00209     {
00210         this->values_ = Eigen::VectorXd::Zero(relevant_values.size());
00211     }
00212 
00213     for (uint32_t idx = 0; idx < relevant_values.size(); ++idx)
00214     {
00215         this->values_(idx) = relevant_values.at(idx);
00216     }
00217 }
00218 
00219 template <typename T_PARAMS, typename PRIO>
00220 void CollisionAvoidance<T_PARAMS, PRIO>::calcDerivativeValue()
00221 {
00222     this->derivative_value_ = -0.1 * this->value_;  // exponential decay experimentally chosen -0.1
00223     this->derivative_values_ = -0.1 * this->values_;
00224 }
00225 
00232 template <typename T_PARAMS, typename PRIO>
00233 void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
00234 {
00235     // ROS_INFO_STREAM("CollisionAvoidance::calcPartialValues:");
00236     Eigen::VectorXd partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
00237     Eigen::VectorXd sum_partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
00238     this->partial_values_ = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
00239 
00240     const ConstraintParams& params = this->constraint_params_.params_;
00241     std::vector<Eigen::VectorXd> vec_partial_values;
00242 
00243     // ROS_INFO_STREAM("this->jacobian_data_.cols: " << this->jacobian_data_.cols());
00244     // ROS_INFO_STREAM("this->joint_states_.current_q_.rows: " << this->joint_states_.current_q_.rows());
00245 
00246     std::vector<std::string>::const_iterator str_it = std::find(this->constraint_params_.frame_names_.begin(),
00247                                                                 this->constraint_params_.frame_names_.end(),
00248                                                                 this->constraint_params_.id_);
00249 
00250     for (std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
00251          it != this->constraint_params_.current_distances_.end();
00252          ++it)
00253     {
00254         if (params.thresholds.activation_with_buffer > it->min_distance)
00255         {
00256             if (this->constraint_params_.frame_names_.end() != str_it)
00257             {
00258                 Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector;
00259                 Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector;
00260 
00261                 // Create a skew-symm matrix as transformation between the segment root and the critical point.
00262                 Eigen::Matrix3d skew_symm;
00263                 skew_symm <<     0.0,                       collision_pnt_vector.z(), -collision_pnt_vector.y(),
00264                                 -collision_pnt_vector.z(),  0.0,                       collision_pnt_vector.x(),
00265                                  collision_pnt_vector.y(), -collision_pnt_vector.x(),  0.0;
00266 
00267                 Eigen::Matrix3d ident = Eigen::Matrix3d::Identity();
00268                 Eigen::Matrix<double, 6, 6> T;
00269                 T.block(0, 0, 3, 3) << ident;
00270                 T.block(0, 3, 3, 3) << skew_symm;
00271                 T.block(3, 0, 3, 3) << Eigen::Matrix3d::Zero();
00272                 T.block(3, 3, 3, 3) << ident;
00273                 // ****************************************************************************************************
00274 
00275                 uint32_t idx = str_it - this->constraint_params_.frame_names_.begin();
00276                 uint32_t frame_number = idx + 1;  // segment nr not index represents frame number
00277 
00278                 KDL::JntArray ja = this->joint_states_.current_q_;
00279                 KDL::Jacobian new_jac_chain(this->joint_states_.current_q_.rows());
00280 
00281                 // ROS_INFO_STREAM("frame_number: " << frame_number);
00282                 // ROS_INFO_STREAM("ja.rows: " << ja.rows());
00283                 // ROS_INFO_STREAM("new_jac_chain: " << new_jac_chain.rows() << " x " << new_jac_chain.columns());
00284 
00285                 if (0 != this->jnt_to_jac_.JntToJac(ja, new_jac_chain, frame_number))
00286                 {
00287                     ROS_ERROR_STREAM("Failed to calculate JntToJac. Error Code: " << this->jnt_to_jac_.getError() << " (" << this->jnt_to_jac_.strError(this->jnt_to_jac_.getError()) << ")");
00288                     ROS_ERROR_STREAM("This is likely due to using a KinematicExtension! The ChainJntToJac-Solver is configured for the main chain only!");
00289                     return;
00290                 }
00291                 // ROS_INFO_STREAM("new_jac_chain.columns: " << new_jac_chain.columns());
00292 
00293                 Matrix6Xd_t jac_extension = this->jacobian_data_;
00294                 jac_extension.block(0, 0, new_jac_chain.data.rows(), new_jac_chain.data.cols()) = new_jac_chain.data;
00295                 Matrix6Xd_t crit_pnt_jac = T * jac_extension;
00296                 Eigen::Matrix3Xd m_transl = Eigen::Matrix3Xd::Zero(3, crit_pnt_jac.cols());
00297                 m_transl << crit_pnt_jac.row(0),
00298                             crit_pnt_jac.row(1),
00299                             crit_pnt_jac.row(2);
00300 
00301                 double vec_norm = distance_vec.norm();
00302                 vec_norm = (vec_norm > 0.0) ? vec_norm : DIV0_SAFE;
00303                 Eigen::VectorXd term_2nd = (m_transl.transpose()) * (distance_vec / vec_norm);  // use the unit vector only for direction!
00304 
00305                 // Gradient of the cost function from: Strasse O., Escande A., Mansard N. et al.
00306                 // "Real-Time (Self)-Collision Avoidance Task on a HRP-2 Humanoid Robot", 2008 IEEE International Conference
00307                 const double denom = it->min_distance > 0.0 ? it->min_distance : DIV0_SAFE;
00308                 const double activation_gain = this->getActivationGain(it->min_distance);
00309                 const double magnitude = this->getSelfMotionMagnitude(it->min_distance);
00310                 partial_values = (2.0 * ((it->min_distance - params.thresholds.activation_with_buffer) / denom) * term_2nd);
00311                 // only consider the gain for the partial values, because of GPM, not for the task jacobian!
00312                 sum_partial_values += (activation_gain * magnitude * partial_values);
00313                 vec_partial_values.push_back(partial_values);
00314             }
00315             else
00316             {
00317                 ROS_ERROR_STREAM("Frame id not found: " << this->constraint_params_.id_);
00318             }
00319         }
00320         else
00321         {
00322             // ROS_INFO_STREAM("min_dist not within activation_buffer: " << params.thresholds.activation_with_buffer << " <= " << it->min_distance);
00323         }
00324     }
00325     // ROS_INFO_STREAM("Done all distances");
00326     // ROS_INFO_STREAM("vec_partial_values.size:" << vec_partial_values.size());
00327 
00328     if (vec_partial_values.size() > 0)
00329     {
00330         this->task_jacobian_.resize(vec_partial_values.size(), this->jacobian_data_.cols());
00331     }
00332 
00333     for (uint32_t idx = 0; idx < vec_partial_values.size(); ++idx)
00334     {
00335         this->task_jacobian_.block(idx, 0, 1, this->jacobian_data_.cols()) = vec_partial_values.at(idx).transpose();
00336     }
00337     // ROS_INFO_STREAM("this->task_jacobian_.rows:" << this->task_jacobian_.rows());
00338     // ROS_INFO_STREAM("this->task_jacobian_.cols:" << this->task_jacobian_.cols());
00339 
00340     this->partial_values_ = sum_partial_values;
00341 }
00342 
00343 template <typename T_PARAMS, typename PRIO>
00344 void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
00345 {
00346     const ConstraintParams& params = this->constraint_params_.params_;
00347     this->prediction_value_ = std::numeric_limits<double>::max();
00348 
00349     ros::Time now = ros::Time::now();
00350     double cycle = (now - this->last_pred_time_).toSec();
00351     this->last_pred_time_ = now;
00352 
00353     std::vector<std::string>::const_iterator str_it = std::find(this->constraint_params_.frame_names_.begin(),
00354                                                                 this->constraint_params_.frame_names_.end(),
00355                                                                 this->constraint_params_.id_);
00356     // ROS_INFO_STREAM("constraint_params_.id_: " << this->constraint_params_.id_);
00357 
00358     if (this->constraint_params_.frame_names_.end() != str_it)
00359     {
00360         if (this->constraint_params_.current_distances_.size() > 0)
00361         {
00362             uint32_t frame_number = (str_it - this->constraint_params_.frame_names_.begin()) + 1;  // segment nr not index represents frame number
00363             KDL::FrameVel frame_vel;
00364 
00365             // Calculate prediction for pos and vel
00366             int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number);
00367             if (error != 0)
00368             {
00369                 ROS_ERROR_STREAM("Could not calculate twist for frame: " << frame_number << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")");
00370                 ROS_ERROR_STREAM("This is likely due to using a KinematicExtension! The ChainFkSolverVel is configured for the main chain only!");
00371                 return;
00372             }
00373             // ROS_INFO_STREAM("Calculated twist for frame: " << frame_number);
00374 
00375             KDL::Twist twist = frame_vel.GetTwist();  // predicted frame twist
00376 
00377             Eigen::Vector3d pred_twist_vel;
00378             tf::vectorKDLToEigen(twist.vel, pred_twist_vel);
00379 
00380             Eigen::Vector3d pred_twist_rot;
00381             tf::vectorKDLToEigen(twist.rot, pred_twist_rot);
00382 
00383             std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
00384             ObstacleDistanceData critical_data = *it;
00385             for ( ; it != this->constraint_params_.current_distances_.end(); ++it)
00386             {
00387                 if (it->min_distance < critical_data.min_distance)
00388                 {
00389                     critical_data = *it;
00390                 }
00391             }
00392 
00393             Eigen::Vector3d delta_pred_vel = pred_twist_vel + pred_twist_rot.cross(critical_data.nearest_point_frame_vector);
00394             Eigen::Vector3d pred_pos = critical_data.nearest_point_frame_vector + delta_pred_vel * cycle;
00395             this->prediction_value_ = (critical_data.nearest_point_obstacle_vector - pred_pos).norm();
00396         }
00397     }
00398     else
00399     {
00400         ROS_ERROR_STREAM("Frame ID not found: " << this->constraint_params_.id_);
00401     }
00402 }
00403 /* END CollisionAvoidance ***************************************************************************************/
00404 
00405 #endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_CA_IMPL_H


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Jun 6 2019 21:19:26