chomp_cost.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan */
36 
39 #include <eigen3/Eigen/LU>
40 
41 using namespace Eigen;
42 using namespace std;
43 
44 namespace chomp
45 {
46 ChompCost::ChompCost(const ChompTrajectory& trajectory, int joint_number, const std::vector<double>& derivative_costs,
47  double ridge_factor)
48 {
49  int num_vars_all = trajectory.getNumPoints();
50  int num_vars_free = num_vars_all - 2 * (DIFF_RULE_LENGTH - 1);
51  MatrixXd diff_matrix = MatrixXd::Zero(num_vars_all, num_vars_all);
52  quad_cost_full_ = MatrixXd::Zero(num_vars_all, num_vars_all);
53 
54  // construct the quad cost for all variables, as a sum of squared differentiation matrices
55  double multiplier = 1.0;
56  for (unsigned int i = 0; i < derivative_costs.size(); i++)
57  {
58  multiplier *= trajectory.getDiscretization();
59  diff_matrix = getDiffMatrix(num_vars_all, &DIFF_RULES[i][0]);
60  quad_cost_full_ += (derivative_costs[i] * multiplier) * (diff_matrix.transpose() * diff_matrix);
61  }
62  quad_cost_full_ += MatrixXd::Identity(num_vars_all, num_vars_all) * ridge_factor;
63 
64  // extract the quad cost just for the free variables:
65  quad_cost_ = quad_cost_full_.block(DIFF_RULE_LENGTH - 1, DIFF_RULE_LENGTH - 1, num_vars_free, num_vars_free);
66 
67  // invert the matrix:
68  quad_cost_inv_ = quad_cost_.inverse();
69 
70  // cout << quad_cost_inv_ << endl;
71 }
72 
73 Eigen::MatrixXd ChompCost::getDiffMatrix(int size, const double* diff_rule) const
74 {
75  MatrixXd matrix = MatrixXd::Zero(size, size);
76  for (int i = 0; i < size; i++)
77  {
78  for (int j = -DIFF_RULE_LENGTH / 2; j <= DIFF_RULE_LENGTH / 2; j++)
79  {
80  int index = i + j;
81  if (index < 0)
82  continue;
83  if (index >= size)
84  continue;
85  matrix(i, index) = diff_rule[j + DIFF_RULE_LENGTH / 2];
86  }
87  }
88  return matrix;
89 }
90 
91 double ChompCost::getMaxQuadCostInvValue() const
92 {
93  return quad_cost_inv_.maxCoeff();
94 }
95 
96 void ChompCost::scale(double scale)
97 {
98  double inv_scale = 1.0 / scale;
99  quad_cost_inv_ *= inv_scale;
100  quad_cost_ *= scale;
101  quad_cost_full_ *= scale;
102 }
103 
104 ChompCost::~ChompCost()
105 {
106 }
107 }
static const int DIFF_RULE_LENGTH
Definition: chomp_utils.h:46
unsigned int index
int getNumPoints() const
Gets the number of points in the trajectory.
static const double DIFF_RULES[3][DIFF_RULE_LENGTH]
Definition: chomp_utils.h:49
Represents a discretized joint-space trajectory for CHOMP.
double getDiscretization() const
Gets the discretization time interval of the trajectory.


chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Sun Oct 18 2020 13:17:08