polynomial.cpp
Go to the documentation of this file.
1
28 #include <ros/console.h>
29
33 namespace stomp_moveit
34 {
35
39 namespace utils
40 {
41
45 namespace polynomial
46 {
47
48
50 {
51
52 // Unconstrained Least Squares
53 // |p|=|A*A'|^-1 * |A*y|
54 //
55 // Constrained Least Squares
56 // |p| - | 2*A*A', C |^-1 * | 2*A*b |
57 // |z| - | C', 0 | | d |
58 //
59 // Variable Description:
60 // x_t (A) - Is the Vandermonde matrix of all (constrained and unconstrained) domain values
61 // a_t (C) - Is the Vandermonde matrix of the constrained domain values
62 // y (b) - An array of the values to perform the fit on
63 // yf (d) - An array of the values corresponding to the constrained domain values
64 // poly_params (p) - An array of the polynomial coefficients solved for.
65
66  PolyFitResults result;
67
68  int num_r = request.d + 1;
69  Eigen::MatrixXd x_t(num_r, request.xy.cols());
70  if (!request.hasConstraints())
71  {
72  for (auto r = 0; r < num_r; r++)
73  x_t.row(r) = request.xy.row(0).array().pow(r);
74
75  Eigen::MatrixXd mat = x_t * x_t.transpose();
76  Eigen::VectorXd vec = x_t * request.xy.row(1);
77
79  }
80  else
81  {
82  int num_constraints = request.xyfp.cols() + request.xyfs.cols();
83
84  Eigen::MatrixXd a_t(num_r, num_constraints);
85  Eigen::MatrixXd a_tp(num_r, request.xyfp.cols());
86  Eigen::MatrixXd a_ts(num_r, request.xyfs.cols());
87
88  for (auto r = 0; r < num_r; r++)
89  {
90  x_t.row(r) = request.xy.row(0).array().pow(r);
91
92  if (request.hasPositionConstraints())
93  a_tp.row(r) = request.xyfp.row(0).array().pow(r);
94
95  if (request.hasSlopeConstraints())
96  if (r == 0)
97  a_ts.row(r) = request.xyfs.row(0).array() * 0.0;
98  else
99  a_ts.row(r) = request.xyfs.row(0).array().pow(r - 1) / r;
100
101  }
102
103  Eigen::MatrixXd top(num_r, num_r + num_constraints);
104  Eigen::MatrixXd bot(num_constraints, num_r + num_constraints);
105  Eigen::MatrixXd mat(num_r + num_constraints, num_r + num_constraints);
106  Eigen::VectorXd vec(num_r + num_constraints);
107
108  if (request.hasPositionConstraints() && request.hasSlopeConstraints())
109  {
110  a_t << a_tp, a_ts;
111  vec << 2*x_t*request.xy.row(1).transpose(), request.xyfp.row(1).transpose(), request.xyfs.row(1).transpose();
112  }
113  else if (request.hasPositionConstraints())
114  {
115  a_t << a_tp;
116  vec << 2*x_t*request.xy.row(1).transpose(), request.xyfp.row(1).transpose();
117  }
118  else if (request.hasSlopeConstraints())
119  {
120  a_t << a_ts;
121  vec << 2*x_t*request.xy.row(1).transpose(), request.xyfs.row(1).transpose();
122  }
123
124  top << 2*x_t*x_t.transpose(), a_t;
125  bot << a_t.transpose(), Eigen::MatrixXd::Zero(num_constraints, num_constraints);
126  mat << top,
127  bot;
128
130  }
131
132  if (request.output_domain.size() == 0)
133  {
134  result.x = request.xy.row(0);
135  result.y = x_t.transpose() * result.p;
136  result.successful = true;
137  return result;
138  }
139  else
140  {
141  result.x = request.output_domain;
142  fillVandermondeMatrix(result.x, request.d, x_t);
143  result.y = x_t.transpose() * result.p;
144  result.successful = true;
145  return result;
146  }
147 }
148
149 void fillVandermondeMatrix(const Eigen::ArrayXd &domain_vals, const int &order, Eigen::MatrixXd &v)
150 {
151  v = Eigen::MatrixXd::Ones(order+1, domain_vals.size());
152  for(auto p = 1u; p <= order; p++)
153  v.row(p) = domain_vals.pow(p);
154 }
155
156 void generateMinimumDomainValues(const std::vector<const moveit::core::JointModel *> joint_models,
157  const Eigen::MatrixXd &parameters,Eigen::VectorXd &domain_values)
158 {
159  Eigen::VectorXd distance(parameters.rows());
160  Eigen::VectorXd velocity(parameters.rows());
161  Eigen::VectorXd t(parameters.rows());
162  Eigen::VectorXd domain_dist(parameters.cols());
163  double max_t = 0;
164
165  domain_values.resize(parameters.cols());
166  for(auto r = 0; r < parameters.rows(); r++)
167  {
168  moveit::core::VariableBounds bound = joint_models[r]->getVariableBounds()[0];
169  velocity(r) = bound.max_velocity_;
170  distance(r) = 0.0;
171  domain_dist(0) = 0.0;
172  for(auto c = 1; c < parameters.cols(); c++)
173  {
174  distance(r) += std::abs((parameters(r, c)) - (parameters(r, c - 1)));
175  domain_dist(c) = distance(r);
176  }
177
178  t(r) = distance(r)/velocity(r);
179  if (t(r) > max_t)
180  {
181  max_t = t(r);
182  domain_values = domain_dist/velocity(r);
183  }
184  }
185 }
186
187 bool applyPolynomialSmoothing(moveit::core::RobotModelConstPtr robot_model, const std::string& group_name, Eigen::MatrixXd& parameters,
188  int poly_order, double joint_limit_margin)
189 {
190  using namespace Eigen;
191  using namespace moveit::core;
192
193  const std::vector<const JointModel*> &joint_models = robot_model->getJointModelGroup(group_name)->getActiveJointModels();
194  PolyFitRequest request;
195  int num_timesteps = parameters.cols();
196
197  VectorXd domain_vals;
198  domain_vals.setLinSpaced(num_timesteps,0,1);
199
200  request.d = poly_order;
201  request.xy.resize(2, num_timesteps);
202  request.xy.row(0) = domain_vals;
203  for(auto r = 0; r < parameters.rows(); r++)
204  {
205  request.xy.row(1) = parameters.row(r);
206  request.xyfp.resize(2, 2);
207  request.xyfp(0, 0) = request.xy(0, 0);
208  request.xyfp(1, 0) = request.xy(1, 0);
209  request.xyfp(0, 1) = request.xy(0, num_timesteps - 1);
210  request.xyfp(1, 1) = request.xy(1, num_timesteps - 1);
211
212  PolyFitResults results;
213  results = polyFit(request);
214  for(auto i = 0; i < results.y.size(); ++i)
215  joint_models[r]->enforcePositionBounds(&results.y[i]);
216
217  // Now check if joint trajectory is within joint limits
218  bool finished;
219  double min = results.y.minCoeff();
220  double max = results.y.maxCoeff();
221  finished = joint_models[r]->satisfiesPositionBounds(&min, joint_limit_margin) &&
222  joint_models[r]->satisfiesPositionBounds(&max, joint_limit_margin);
223
224  if ((results.p.array() != results.p.array()).any())
225  {
226  ROS_ERROR("Smoother, joint %s polynomial fit failed!", joint_models[r]->getName().c_str());
227  return false;
228  }
229
230  if(!finished)
231  {
232  ROS_ERROR("Smoother, joint %s not within limits, Min: %f, Max: %f", joint_models[r]->getName().c_str(), min, max);
233  return false;
234  }
235
236  parameters.row(r) = results.y;
237  }
238
239  return true;
240 }
241
242 } // end of namespace smoothing
243 } // end of namespace utils
244 } // end of namespace stomp_moveit
245
246
247
248
Eigen::VectorXd y
The fit values.
Definition: polynomial.h:94
bool hasSlopeConstraints() const
Check if request has slope constraints.
Definition: polynomial.h:82
The Polynomial Fit request data.
Definition: polynomial.h:52
Eigen::VectorXd p
The polynomial parameter found during fit.
Definition: polynomial.h:92
bool hasConstraints() const
Check if request has constraints.
Definition: polynomial.h:64
Eigen::Matrix< double, 2, Eigen::Dynamic > xyfp
The position constraint data.
Definition: polynomial.h:56
std::string getName(void *handle)
Eigen::VectorXd x
The fit domain values.
Definition: polynomial.h:93
The Polynomial Fit results data.
Definition: polynomial.h:90
double min(double a, double b)
Eigen::Matrix< double, 2, Eigen::Dynamic > xy
The fit data.
Definition: polynomial.h:55
bool successful
True if polynomial fit was found, otherwise false.
Definition: polynomial.h:95
void fillVandermondeMatrix(const Eigen::ArrayXd &domain_vals, const int &order, Eigen::MatrixXd &v)
Generates a Vandermonde matrix from the domain values as described here https://en.wikipedia.org/wiki/Vandermonde_matrix. It uses the poly_order parameter in order to set the size of the matrix.
Definition: polynomial.cpp:149
Eigen::VectorXd output_domain
The fit output domain. If not provided it will use the input domain.
Definition: polynomial.h:58
void generateMinimumDomainValues(const std::vector< const moveit::core::JointModel * > joint_models, const Eigen::MatrixXd &parameters, Eigen::VectorXd &domain_values)
Get the polynomial smoother domain values.
Definition: polynomial.cpp:156
bool hasPositionConstraints() const
Check if request has position constraints.
Definition: polynomial.h:73
Eigen::Matrix< double, 2, Eigen::Dynamic > xyfs
The slope constraint data.
Definition: polynomial.h:57
bool applyPolynomialSmoothing(moveit::core::RobotModelConstPtr robot_model, const std::string &group_name, Eigen::MatrixXd &parameters, int poly_order=5, double joint_limit_margin=1e-5)
Applies a polynomial smoothing method to a trajectory. It checks for joint limits and makes correctio...
Definition: polynomial.cpp:187
r
double max(double a, double b)
#define ROS_ERROR(...)
double distance(const urdf::Pose &transform)
PolyFitResults polyFit(const PolyFitRequest &request)
Fit a polynomial with fixed indices.
Definition: polynomial.cpp:49

stomp_moveit
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:47