NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
CPGEquations.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17 */
18 
27 #include "CPGEquations.h"
28 
29 #include "boost/array.hpp"
30 #include "boost/numeric/odeint.hpp"
31 
32 // The Bullet Physics Library
33 #include "LinearMath/btQuickprof.h"
34 
35 
36 // The C++ Standard Library
37 #include <assert.h>
38 #include <stdexcept>
39 
40 using namespace boost::numeric::odeint;
41 
42 typedef std::vector<double > cpgVars_type;
43 
44 CPGEquations::CPGEquations(int maxSteps) :
45 stepSize(0.1),
46 numSteps(0),
47 m_maxSteps(maxSteps)
48  {}
49 CPGEquations::CPGEquations(std::vector<CPGNode*>& newNodeList, int maxSteps) :
50 nodeList(newNodeList),
51 stepSize(0.1), //TODO: specify as a parameter somewhere
52 numSteps(0),
53 m_maxSteps(maxSteps)
54 {
55 }
56 
57 CPGEquations::~CPGEquations()
58 {
59  for (std::size_t i = 0; i < nodeList.size(); i++)
60  {
61  delete nodeList[i];
62  }
63  nodeList.clear();
64 }
65 
66 // Params needs size 7 to fill all of the params.
67 // TODO: consider changing to a config struct
68 int CPGEquations::addNode(std::vector<double>& newParams)
69 {
70  int index = nodeList.size();
71  CPGNode* newNode = new CPGNode(index, newParams);
72  nodeList.push_back(newNode);
73 
74  return index;
75 }
76 
77 void CPGEquations::defineConnections ( int nodeIndex,
78  std::vector<int> connections,
79  std::vector<double> newWeights,
80  std::vector<double> newPhaseOffsets)
81 {
82  assert(connections.size() == newWeights.size());
83  assert(connections.size() == newPhaseOffsets.size());
84  assert(nodeList[nodeIndex] != NULL);
85 
86  for(int i = 0; i != connections.size(); i++){
87  nodeList[nodeIndex]->addCoupling(nodeList[connections[i]], newWeights[i], newPhaseOffsets[i]);
88  }
89 }
90 
91 const double CPGEquations::operator[](const std::size_t i) const
92 {
93 #ifndef BT_NO_PROFILE
94  BT_PROFILE("CPGEquations::[]");
95 #endif //BT_NO_PROFILE
96  double nodeValue;
97  if (i >= nodeList.size())
98  {
99  nodeValue = NAN;
100  throw std::invalid_argument("Node index out of bounds");
101  }
102  else
103  {
104  nodeValue = (*nodeList[i]).nodeValue;
105  }
106 
107  return nodeValue;
108 }
109 
110 std::vector<double>& CPGEquations::getXVars() {
111 #ifndef BT_NO_PROFILE
112  BT_PROFILE("CPGEquations::getXVars");
113 #endif //BT_NO_PROFILE
114  XVars.clear();
115 
116  for (int i = 0; i != nodeList.size(); i++){
117  XVars.push_back(nodeList[i]->phiValue);
118  XVars.push_back(nodeList[i]->rValue);
119  XVars.push_back(nodeList[i]->rDotValue);
120  }
121 
122  return XVars;
123 }
124 
125 std::vector<double>& CPGEquations::getDXVars() {
126  DXVars.clear();
127 
128  for (int i = 0; i != nodeList.size(); i++){
129  DXVars.push_back(nodeList[i]->phiDotValue);
130  DXVars.push_back(nodeList[i]->rDotValue);
131  DXVars.push_back(nodeList[i]->rDoubleDotValue);
132  }
133 
134  return DXVars;
135 }
136 
137 void CPGEquations::updateNodes(std::vector<double>& descCom)
138 {
139 #ifndef BT_NO_PROFILE
140  BT_PROFILE("CPGEquations::updateNodes");
141 #endif //BT_NO_PROFILE
142  for(int i = 0; i != nodeList.size(); i++){
143  nodeList[i]->updateDTs(descCom[i]);
144  }
145 }
146 
147 void CPGEquations::updateNodeData(std::vector<double> newXVals)
148 {
149 #ifndef BT_NO_PROFILE
150  BT_PROFILE("CPGEquations::updateNodeData");
151 #endif //BT_NO_PROFILE
152  assert(newXVals.size()==3*nodeList.size());
153 
154  for(int i = 0; i!=nodeList.size(); i++){
155  nodeList[i]->updateNodeValues(newXVals[3*i], newXVals[3*i+1], newXVals[3*i+2]);
156  }
157 }
158 
163  public:
164 
165  integrate_function(CPGEquations* pCPGs, std::vector<double> newComs) :
166  theseCPGs(pCPGs),
167  descCom(newComs)
168  {
169 
170  }
171 
172  void operator() (const cpgVars_type &x ,
173  cpgVars_type &dxdt ,
174  double t )
175  {
176 #ifndef BT_NO_PROFILE
177  BT_PROFILE("CPGEquations::integrate_function");
178 #endif //BT_NO_PROFILE
179  theseCPGs->updateNodeData(x);
180  theseCPGs->updateNodes(descCom);
181 
185  std::vector<double> dXVars = theseCPGs->getDXVars();
190  for(std::size_t i = 0; i != x.size(); i++){
191  dxdt[i] = dXVars[i];
192  }
193  //std::cout<<"operator call"<<std::endl;
194 
195  theseCPGs->countStep();
196 
197  }
198 
199  private:
200  CPGEquations* theseCPGs;
201  std::vector<double> descCom;
202 };
203 
208  public:
209 
210  output_function(CPGEquations* pCPGs) :
211  theseCPGs(pCPGs)
212  {
213  }
214 
215  void operator() ( const cpgVars_type &x ,
216  const double t )
217  {
221  theseCPGs->updateNodeData(x);
222  #if (0) // Suppress output
223  std::cout << t << '\t' << (*theseCPGs)[0] << '\t' << (*theseCPGs)[1] << '\t' << (*theseCPGs)[2] << std::endl;
224  #endif
225  }
226 
227  private:
228  CPGEquations* theseCPGs;
229 };
230 
231 void CPGEquations::update(std::vector<double>& descCom, double dt)
232 {
233 #ifndef BT_NO_PROFILE
234  BT_PROFILE("CPGEquations::update");
235 #endif //BT_NO_PROFILE
236  if (dt <= 0.1){ //TODO: specify default step size as a parameter during construction
237  stepSize = dt;
238  }
239  else{
240  stepSize = 0.1;
241  }
242 
243  numSteps = 0;
244 
248  std::vector<double>& xVars = getXVars();
249 
253  integrate(integrate_function(this, descCom), xVars, 0.0, dt, stepSize, output_function(this));
254 
255  if (numSteps > m_maxSteps)
256  {
257  std::cout << "Ending trial due to inefficient equations " << numSteps << std::endl;
258  throw std::runtime_error("Inefficient CPG Parameters");
259  }
260 
261  #if (0)
262  std::cout << dt << '\t' << nodeList[0]->nodeValue <<
263  '\t' << nodeList[1]->nodeValue <<
264  '\t' << nodeList[2]->nodeValue << std::endl;
265  #endif
266 
267 }
268 
269 std::string CPGEquations::toString(const std::string& prefix) const
270 {
271  std::string p = " ";
272  std::ostringstream os;
273  os << prefix << "CPGEquations(" << std::endl;
274 
275  os << prefix << p << "Nodes:" << std::endl;
276  for(int i = 0; i < nodeList.size(); i++) {
277  os << prefix << p << p << *(nodeList[i]) << std::endl;
278  }
279 
280  os << prefix << ")" << std::endl;
281  return os.str();
282 }
void update(std::vector< double > &descCom, double dt)
void operator()(const cpgVars_type &x, cpgVars_type &dxdt, double t)
void operator()(const cpgVars_type &x, const double t)
Definition of class CPGEquations.