constraint_ca_impl.h
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #ifndef COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_CA_IMPL_H
19 #define COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_CA_IMPL_H
20 
21 #include <vector>
22 #include <string>
23 #include <limits>
24 #include <sstream>
25 #include <ros/ros.h>
26 
27 #include <boost/shared_ptr.hpp>
28 #include <boost/pointer_cast.hpp>
29 
30 #include <kdl/chainjnttojacsolver.hpp>
31 #include <kdl/jntarray.hpp>
32 
34 
37 
40 
41 /* BEGIN CollisionAvoidance *************************************************************************************/
42 template <typename T_PARAMS, typename PRIO>
44 {
45  const std::string frame_id = this->constraint_params_.id_;
46  std::ostringstream oss;
47  oss << this->member_inst_cnt_;
48  oss << "_";
49  oss << frame_id;
50  oss << "_";
51  oss << this->priority_;
52  std::string taskid = "CollisionAvoidance_" + oss.str();
53  return taskid;
54 }
55 
61 template <typename T_PARAMS, typename PRIO>
63 {
64  return this->task_jacobian_;
65 }
66 
72 template <typename T_PARAMS, typename PRIO>
74 {
75  return this->derivative_values_;
76 }
77 
78 template <typename T_PARAMS, typename PRIO>
80 {
81  const ConstraintParams& params = this->constraint_params_.params_;
82 
83  this->calcValue();
84  this->calcDerivativeValue();
85  this->calcPartialValues();
86  this->calcPredictionValue();
87 
88  const double pred_min_dist = this->getPredictionValue();
89  const double activation = params.thresholds.activation;
90  const double critical = params.thresholds.critical;
91  const double activation_buffer = params.thresholds.activation_with_buffer;
92  const double crit_min_distance = this->getCriticalValue();
93 
94  if (this->state_.getCurrent() == CRITICAL && pred_min_dist < crit_min_distance)
95  {
96  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.");
97  }
98  else if (crit_min_distance < critical || pred_min_dist < critical)
99  {
100  this->state_.setState(CRITICAL);
101  }
102  else if (crit_min_distance < activation_buffer)
103  {
104  this->state_.setState(DANGER);
105  }
106  else
107  {
108  this->state_.setState(NORMAL);
109  }
110 }
111 
112 template <typename T_PARAMS, typename PRIO>
113 double CollisionAvoidance<T_PARAMS, PRIO>::getActivationGain(double current_cost_func_value) const
114 {
115  const ConstraintParams& params = this->constraint_params_.params_;
116  double activation_gain;
117  const double activation = params.thresholds.activation;
118  const double activation_buffer_region = params.thresholds.activation_with_buffer;
119 
120  if (current_cost_func_value < activation) // activation == d_m
121  {
122  activation_gain = 1.0;
123  }
124  else if (current_cost_func_value < activation_buffer_region) // activation_buffer_region == d_i
125  {
126  activation_gain = 0.5 * (1.0 + cos(M_PI * (current_cost_func_value - activation) / (activation_buffer_region - activation)));
127  }
128  else
129  {
130  activation_gain = 0.0;
131  }
132 
133  return activation_gain;
134 }
135 
136 template <typename T_PARAMS, typename PRIO>
138 {
139  return 1.0;
140 }
141 
143 template <typename T_PARAMS, typename PRIO>
144 double CollisionAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(double current_distance_value) const
145 {
146  const ConstraintParams& params = this->constraint_params_.params_;
147  const double activation_with_buffer = params.thresholds.activation_with_buffer;
148  double magnitude = 0.0;
149 
150  if (current_distance_value < activation_with_buffer)
151  {
152  if (current_distance_value > 0.0)
153  {
154  magnitude = pow(activation_with_buffer / current_distance_value, 2.0) - 1.0;
155  }
156  else
157  {
158  magnitude = activation_with_buffer / 0.000001; // strong magnitude
159  }
160  }
161 
162  double k_H = params.k_H;
163  return k_H * magnitude;
164 }
165 
167 template <typename T_PARAMS, typename PRIO>
168 double CollisionAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const
169 {
170  return 1.0;
171 }
172 
173 template <typename T_PARAMS, typename PRIO>
175 {
176  double min_distance = std::numeric_limits<double>::max();
177  for (std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
178  it != this->constraint_params_.current_distances_.end();
179  ++it)
180  {
181  if (it->min_distance < min_distance)
182  {
183  min_distance = it->min_distance;
184  }
185  }
186 
187  return min_distance;
188 }
189 
190 template <typename T_PARAMS, typename PRIO>
192 {
193  const ConstraintParams& params = this->constraint_params_.params_;
194  std::vector<double> relevant_values;
195  for (std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
196  it != this->constraint_params_.current_distances_.end();
197  ++it)
198  {
199  if (params.thresholds.activation_with_buffer > it->min_distance)
200  {
201  const double activation_gain = this->getActivationGain(it->min_distance);
202  const double magnitude = std::abs(this->getSelfMotionMagnitude(it->min_distance)); // important only for task!
203  double value = activation_gain * magnitude * pow(it->min_distance - params.thresholds.activation_with_buffer, 2.0);
204  relevant_values.push_back(value);
205  }
206  }
207 
208  if (relevant_values.size() > 0)
209  {
210  this->values_ = Eigen::VectorXd::Zero(relevant_values.size());
211  }
212 
213  for (uint32_t idx = 0; idx < relevant_values.size(); ++idx)
214  {
215  this->values_(idx) = relevant_values.at(idx);
216  }
217 }
218 
219 template <typename T_PARAMS, typename PRIO>
221 {
222  this->derivative_value_ = -0.1 * this->value_; // exponential decay experimentally chosen -0.1
223  this->derivative_values_ = -0.1 * this->values_;
224 }
225 
232 template <typename T_PARAMS, typename PRIO>
234 {
235  // ROS_INFO_STREAM("CollisionAvoidance::calcPartialValues:");
236  Eigen::VectorXd partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
237  Eigen::VectorXd sum_partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
238  this->partial_values_ = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
239 
240  const ConstraintParams& params = this->constraint_params_.params_;
241  std::vector<Eigen::VectorXd> vec_partial_values;
242 
243  // ROS_INFO_STREAM("this->jacobian_data_.cols: " << this->jacobian_data_.cols());
244  // ROS_INFO_STREAM("this->joint_states_.current_q_.rows: " << this->joint_states_.current_q_.rows());
245 
246  std::vector<std::string>::const_iterator str_it = std::find(this->constraint_params_.frame_names_.begin(),
247  this->constraint_params_.frame_names_.end(),
248  this->constraint_params_.id_);
249 
250  for (std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
251  it != this->constraint_params_.current_distances_.end();
252  ++it)
253  {
254  if (params.thresholds.activation_with_buffer > it->min_distance)
255  {
256  if (this->constraint_params_.frame_names_.end() != str_it)
257  {
258  Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector;
259  Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector;
260 
261  // Create a skew-symm matrix as transformation between the segment root and the critical point.
262  Eigen::Matrix3d skew_symm;
263  skew_symm << 0.0, collision_pnt_vector.z(), -collision_pnt_vector.y(),
264  -collision_pnt_vector.z(), 0.0, collision_pnt_vector.x(),
265  collision_pnt_vector.y(), -collision_pnt_vector.x(), 0.0;
266 
267  Eigen::Matrix3d ident = Eigen::Matrix3d::Identity();
268  Eigen::Matrix<double, 6, 6> T;
269  T.block(0, 0, 3, 3) << ident;
270  T.block(0, 3, 3, 3) << skew_symm;
271  T.block(3, 0, 3, 3) << Eigen::Matrix3d::Zero();
272  T.block(3, 3, 3, 3) << ident;
273  // ****************************************************************************************************
274 
275  uint32_t idx = str_it - this->constraint_params_.frame_names_.begin();
276  uint32_t frame_number = idx + 1; // segment nr not index represents frame number
277 
278  KDL::JntArray ja = this->joint_states_.current_q_;
279  KDL::Jacobian new_jac_chain(this->joint_states_.current_q_.rows());
280 
281  // ROS_INFO_STREAM("frame_number: " << frame_number);
282  // ROS_INFO_STREAM("ja.rows: " << ja.rows());
283  // ROS_INFO_STREAM("new_jac_chain: " << new_jac_chain.rows() << " x " << new_jac_chain.columns());
284 
285  if (0 != this->jnt_to_jac_.JntToJac(ja, new_jac_chain, frame_number))
286  {
287  ROS_ERROR_STREAM("Failed to calculate JntToJac. Error Code: " << this->jnt_to_jac_.getError() << " (" << this->jnt_to_jac_.strError(this->jnt_to_jac_.getError()) << ")");
288  ROS_ERROR_STREAM("This is likely due to using a KinematicExtension! The ChainJntToJac-Solver is configured for the main chain only!");
289  return;
290  }
291  // ROS_INFO_STREAM("new_jac_chain.columns: " << new_jac_chain.columns());
292 
293  Matrix6Xd_t jac_extension = this->jacobian_data_;
294  jac_extension.block(0, 0, new_jac_chain.data.rows(), new_jac_chain.data.cols()) = new_jac_chain.data;
295  Matrix6Xd_t crit_pnt_jac = T * jac_extension;
296  Eigen::Matrix3Xd m_transl = Eigen::Matrix3Xd::Zero(3, crit_pnt_jac.cols());
297  m_transl << crit_pnt_jac.row(0),
298  crit_pnt_jac.row(1),
299  crit_pnt_jac.row(2);
300 
301  double vec_norm = distance_vec.norm();
302  vec_norm = (vec_norm > 0.0) ? vec_norm : DIV0_SAFE;
303  Eigen::VectorXd term_2nd = (m_transl.transpose()) * (distance_vec / vec_norm); // use the unit vector only for direction!
304 
305  // Gradient of the cost function from: Strasse O., Escande A., Mansard N. et al.
306  // "Real-Time (Self)-Collision Avoidance Task on a HRP-2 Humanoid Robot", 2008 IEEE International Conference
307  const double denom = it->min_distance > 0.0 ? it->min_distance : DIV0_SAFE;
308  const double activation_gain = this->getActivationGain(it->min_distance);
309  const double magnitude = this->getSelfMotionMagnitude(it->min_distance);
310  partial_values = (2.0 * ((it->min_distance - params.thresholds.activation_with_buffer) / denom) * term_2nd);
311  // only consider the gain for the partial values, because of GPM, not for the task jacobian!
312  sum_partial_values += (activation_gain * magnitude * partial_values);
313  vec_partial_values.push_back(partial_values);
314  }
315  else
316  {
317  ROS_ERROR_STREAM("Frame id not found: " << this->constraint_params_.id_);
318  }
319  }
320  else
321  {
322  // ROS_INFO_STREAM("min_dist not within activation_buffer: " << params.thresholds.activation_with_buffer << " <= " << it->min_distance);
323  }
324  }
325  // ROS_INFO_STREAM("Done all distances");
326  // ROS_INFO_STREAM("vec_partial_values.size:" << vec_partial_values.size());
327 
328  if (vec_partial_values.size() > 0)
329  {
330  this->task_jacobian_.resize(vec_partial_values.size(), this->jacobian_data_.cols());
331  }
332 
333  for (uint32_t idx = 0; idx < vec_partial_values.size(); ++idx)
334  {
335  this->task_jacobian_.block(idx, 0, 1, this->jacobian_data_.cols()) = vec_partial_values.at(idx).transpose();
336  }
337  // ROS_INFO_STREAM("this->task_jacobian_.rows:" << this->task_jacobian_.rows());
338  // ROS_INFO_STREAM("this->task_jacobian_.cols:" << this->task_jacobian_.cols());
339 
340  this->partial_values_ = sum_partial_values;
341 }
342 
343 template <typename T_PARAMS, typename PRIO>
345 {
346  const ConstraintParams& params = this->constraint_params_.params_;
347  this->prediction_value_ = std::numeric_limits<double>::max();
348 
350  double cycle = (now - this->last_pred_time_).toSec();
351  this->last_pred_time_ = now;
352 
353  std::vector<std::string>::const_iterator str_it = std::find(this->constraint_params_.frame_names_.begin(),
354  this->constraint_params_.frame_names_.end(),
355  this->constraint_params_.id_);
356  // ROS_INFO_STREAM("constraint_params_.id_: " << this->constraint_params_.id_);
357 
358  if (this->constraint_params_.frame_names_.end() != str_it)
359  {
360  if (this->constraint_params_.current_distances_.size() > 0)
361  {
362  uint32_t frame_number = (str_it - this->constraint_params_.frame_names_.begin()) + 1; // segment nr not index represents frame number
363  KDL::FrameVel frame_vel;
364 
365  // Calculate prediction for pos and vel
366  int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number);
367  if (error != 0)
368  {
369  ROS_ERROR_STREAM("Could not calculate twist for frame: " << frame_number << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")");
370  ROS_ERROR_STREAM("This is likely due to using a KinematicExtension! The ChainFkSolverVel is configured for the main chain only!");
371  return;
372  }
373  // ROS_INFO_STREAM("Calculated twist for frame: " << frame_number);
374 
375  KDL::Twist twist = frame_vel.GetTwist(); // predicted frame twist
376 
377  Eigen::Vector3d pred_twist_vel;
378  tf::vectorKDLToEigen(twist.vel, pred_twist_vel);
379 
380  Eigen::Vector3d pred_twist_rot;
381  tf::vectorKDLToEigen(twist.rot, pred_twist_rot);
382 
383  std::vector<ObstacleDistanceData>::const_iterator it = this->constraint_params_.current_distances_.begin();
384  ObstacleDistanceData critical_data = *it;
385  for ( ; it != this->constraint_params_.current_distances_.end(); ++it)
386  {
387  if (it->min_distance < critical_data.min_distance)
388  {
389  critical_data = *it;
390  }
391  }
392 
393  Eigen::Vector3d delta_pred_vel = pred_twist_vel + pred_twist_rot.cross(critical_data.nearest_point_frame_vector);
394  Eigen::Vector3d pred_pos = critical_data.nearest_point_frame_vector + delta_pred_vel * cycle;
395  this->prediction_value_ = (critical_data.nearest_point_obstacle_vector - pred_pos).norm();
396  }
397  }
398  else
399  {
400  ROS_ERROR_STREAM("Frame ID not found: " << this->constraint_params_.id_);
401  }
402 }
403 /* END CollisionAvoidance ***************************************************************************************/
404 
405 #endif // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_CA_IMPL_H
double now()
Vector vel
virtual Eigen::VectorXd getTaskDerivatives() const
IMETHOD Twist GetTwist() const
virtual double getCriticalValue() const
virtual double getSelfMotionMagnitude(const Eigen::MatrixXd &particular_solution, const Eigen::MatrixXd &homogeneous_solution) const
Returns a value for k_H to weight the partial values for e.g. GPM.
virtual void calculate()
Vector rot
def error(args, kwargs)
virtual Eigen::MatrixXd getTaskJacobian() const
#define ROS_WARN_STREAM(args)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix< double, 3, 1 > &e)
virtual double getActivationGain() const
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd_t
static Time now()
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
#define ROS_ERROR_STREAM(args)
virtual std::string getTaskId() const


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Apr 8 2021 02:40:00