ocra-recipes
Doxygen documentation for the ocra-recipes repository
SubtractionFunction.cpp
Go to the documentation of this file.
1 #if 0
2 
4 #include "ocra/optim/FunctionProperties.h"
5 
6 #include <stdexcept>
7 
8 
9 namespace ocra
10 {
11  // TODO [todo] :
12 /* SubtractionFunction::SubtractionFunction(Function* f1, Function* f2)
13  : Function(*FunctionProperties::getConcatenatedVariable(*f1, *f2), f1->getDimension(),
14  FunctionProperties::getAddType(*f1,*f2), C_UNDEFINED,
15  FunctionProperties::getAddContinuityProperty(*f1,*f2),
16  FunctionProperties::computeHessian(*f1,*f2), FunctionProperties::computeGradient(*f1,*f2))
17  , _f1(f1), _f2(f2), _case(0)
18  {
19  //TODO [todo] : size checks
20  _f1->attach(*this);
21  _f2->attach(*this);
22  }*/
23 
24  SubtractionFunction::SubtractionFunction(Function* f, const Vector& v)
25  : Function(f->getVariable(), f->getDimension(), f->getType(), f->getConvexityProperty(),
26  f->getContinuityProperty(), f->canComputeHessian(), f->canComputeGradient())
27  , _f1(f), _v2(v), _f2(NULL), _case(1)
28  {
29  CML_scal(-1., _v2);
30  _f1->attach(*this);
31  }
32 
33  SubtractionFunction::SubtractionFunction(const Vector& v, Function* f)
34  : Function(f->getVariable(), f->getDimension(), f->getType(), FunctionProperties::getOppositeConvexityProperty(*f),
35  f->getContinuityProperty(), f->canComputeHessian(), f->canComputeGradient())
36  , _f2(f), _v1(v), _f1(NULL), _case(2)
37  {
38  _f2->attach(*this);
39  }
40 
42  {
43  //TODO [todo] : delete if no other function use _x
44 /* if (_f1 == NULL || _f2 == NULL)
45  delete _x;
46 */
47  switch (_case)
48  {
49  case 0:
50  _f1->detach(*this);
51  _f2->detach(*this);
52  break;
53  case 1:
54  _f1->detach(*this);
55  break;
56  case 2:
57  _f2->detach(*this);
58  break;
59  }
60  }
61 
62 
63  void SubtractionFunction::changeV(const Vector& v)
64  {
65  switch (_case)
66  {
67  case 0:
68  throw std::runtime_error("[ocra::SubtractionFunction::changeV]: no vector involved in this SubtractionFunction");
69  break;
70  case 1:
71  _v2.copyValuesFrom(v);
72  CML_scal(-1., _v2);
73  break;
74  case 2:
75  _v1.copyValuesFrom(v);
76  break;
77  }
78  invalidate();
79  }
80 
81 
82 
83  void SubtractionFunction::computeValue(void) const
84  {
85  switch (_case)
86  {
87  case 0:
88  _value.copyValuesFrom(_f1->getValues());
89  CML_axpy(-1., _f2->getValues(), _value);
90  break;
91  case 1:
92  _value.copyValuesFrom(_v2); //assume _v2 contains -v2
93  CML_axpy(1., _f1->getValues(), _value);
94  break;
95  case 2:
96  _value.copyValuesFrom(_v1);
97  CML_axpy(-1., _f2->getValues(), _value);
98  break;
99  }
100  }
101 
102 
103  void SubtractionFunction::computeGradient(void) const
104  {
105  switch (_case)
106  {
107  case 0:
108  _gradient.copyValuesFrom(_f1->getGradients());
109  for (cfl_size_t i=0; i<_gradient.get_ncols(); ++i)
110  {
111  VectorWrap gradientColumn = _gradient.getColumnAsVector(i);
112  CML_axpy(-1., _f2->getGradients().getColumnAsVector(i), gradientColumn);
113  }
114  break;
115  case 1:
116  _gradient.copyValuesFrom(_f1->getGradients());
117  break;
118  case 2:
119  _gradient.copyValuesFrom(_f2->getGradients());
120  _gradient.scalarMultInPlace(-1.);
121  break;
122  }
123  }
124 
125 
126  void SubtractionFunction::computeHessian(void) const
127  {
128  switch (_case)
129  {
130  case 0:
131  for (cfl_size_t i=0; i<_dimension; ++i)
132  {
133  (const_cast<Matrix*>(_hessians[i]))->copyValuesFrom(*_f1->getHessian(i));
134  for (cfl_size_t j=0; j<_gradient.get_ncols(); ++j)
135  {
136  VectorWrap f2HessianColumn = (const_cast<Matrix*>(_f2->getHessian(i)))->getColumnAsVector(j);
137  VectorWrap hessianIColumn = (const_cast<Matrix*>(_hessians[i]))->getColumnAsVector(j);
138  CML_axpy(-1., f2HessianColumn, hessianIColumn);
139  }
140  }
141  break;
142  case 1:
143  for (cfl_size_t i=0; i<_dimension; ++i)
144  (const_cast<Matrix*>(_hessians[i]))->copyValuesFrom(*_f1->getHessian(i));
145  break;
146  case 2:
147  for (cfl_size_t i=0; i<_dimension; ++i)
148  {
149  (const_cast<Matrix*>(_hessians[i]))->copyValuesFrom(*_f2->getHessian(i));
150  (const_cast<Matrix*>(_hessians[i]))->scalarMultInPlace(-1.);
151  }
152  break;
153  }
154  }
155 
156 
157  void SubtractionFunction::computeJdot(void) const
158  {
159  switch (_case)
160  {
161  case 0:
162  _Jdot.copyValuesFrom(_f1->getJdot());
163  for (cfl_size_t i=0; i<_Jdot.get_ncols(); ++i)
164  {
165  VectorWrap gradientColumn = _Jdot.getColumnAsVector(i);
166  CML_axpy(-1., _f2->getJdot().getColumnAsVector(i), gradientColumn);
167  }
168  break;
169  case 1:
170  _Jdot.copyValuesFrom(_f1->getJdot());
171  break;
172  case 2:
173  _Jdot.copyValuesFrom(_f2->getJdot());
174  _Jdot.scalarMultInPlace(-1.);
175  break;
176  }
177  }
178 
179 
180  void SubtractionFunction::computeJdotXdot(void) const
181  {
182  switch (_case)
183  {
184  case 0:
185  _JdotXdot.copyValuesFrom(_f1->getJdotXdot());
186  CML_axpy(-1., _f2->getJdotXdot(), _JdotXdot);
187  break;
188  case 1:
189  _JdotXdot.copyValuesFrom(_v2); //assume _v2 contains -v2
190  CML_axpy(1., _f1->getJdotXdot(), _JdotXdot);
191  break;
192  case 2:
193  _JdotXdot.copyValuesFrom(_v1);
194  CML_axpy(-1., _f2->getJdotXdot(), _JdotXdot);
195  break;
196  }
197  }
198 
199 
201  {
202  }
203 
204 
205  void SubtractionFunction::initHessian(void)
206  {
207  for (cfl_size_t i=0; i<_dimension; ++i)
208  _hessians[i] = new Matrix(_x->getSize(), _x->getSize());
209  }
210 }
211 
212 #endif
213 
214 // cmake:sourcegroup=toBeUpdated
215 
virtual void doUpdateSize(void)
virtual void computeJdot(void) const
int getOppositeConvexityProperty(const Function &f)
virtual void computeJdotXdot(void) const
void changeV(const Vector &v)
virtual void computeGradient(void) const
virtual void computeHessian(void) const
virtual void computeValue(void) const
Optimization-based Robot Controller namespace. a library of classes to write and solve optimization p...
Declaration file of the SubtractionFunction class.