NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
CPGEquationsFB.cpp
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 "CPGEquationsFB.h"
28 
29 #include "core/tgCast.h"
30 
31 #include "boost/array.hpp"
32 #include "boost/numeric/odeint.hpp"
33 
34 // The Bullet Physics Library
35 #include "LinearMath/btQuickprof.h"
36 
37 // The C++ Standard Library
38 #include <assert.h>
39 #include <stdexcept>
40 #include <iterator>
41 
42 using namespace boost::numeric::odeint;
43 
44 typedef std::vector<double > cpgVars_type;
45 
46 CPGEquationsFB::CPGEquationsFB(int maxSteps) :
47 CPGEquations(maxSteps)
48  {}
49 CPGEquationsFB::CPGEquationsFB(std::vector<CPGNode*>& newNodeList, int maxSteps) :
50 CPGEquations(newNodeList, maxSteps)
51 {
52 }
53 
54 CPGEquationsFB::~CPGEquationsFB()
55 {
56  //CPGEquations
57 }
58 
59 int CPGEquationsFB::addNode(std::vector<double>& newParams)
60 {
61  int index = nodeList.size();
62  CPGNodeFB* newNode = new CPGNodeFB(index, newParams);
63  nodeList.push_back(newNode);
64 
65  return index;
66 }
67 
68 std::vector<double>& CPGEquationsFB::getXVars() {
69 #ifndef BT_NO_PROFILE
70  BT_PROFILE("CPGEquationsFB:getXVars");
71 #endif //BT_NO_PROFILE
72  XVars.clear();
73 
74  for (int i = 0; i != nodeList.size(); i++){
75  CPGNodeFB* currentNode = tgCast::cast<CPGNode, CPGNodeFB>(nodeList[i]);
76  assert(currentNode);
77  XVars.push_back(currentNode->phiValue);
78  XVars.push_back(currentNode->rValue);
79  XVars.push_back(currentNode->omega);
80  }
81 
82  return XVars;
83 }
84 
85 std::vector<double>& CPGEquationsFB::getDXVars() {
86 #ifndef BT_NO_PROFILE
87  BT_PROFILE("CPGEquationsFB:getDXVars");
88 #endif //BT_NO_PROFILE
89  DXVars.clear();
90 
91  for (int i = 0; i != nodeList.size(); i++){
92  CPGNodeFB* currentNode = tgCast::cast<CPGNode, CPGNodeFB>(nodeList[i]);
93  DXVars.push_back(currentNode->phiDotValue);
94  DXVars.push_back(currentNode->rDotValue);
95  DXVars.push_back(currentNode->omegaDot);
96  }
97 
98  return DXVars;
99 }
100 
101 void CPGEquationsFB::updateNodes(std::vector<double>& descCom)
102 {
103 #ifndef BT_NO_PROFILE
104  BT_PROFILE("CPGEquationsFB:updateNodes");
105 #endif //BT_NO_PROFILE
106  std::vector<double>::iterator comIt = descCom.begin();
107 
108  assert(descCom.size() == nodeList.size() * 3);
109 
110  for(int i = 0; i != nodeList.size(); i++){
111  CPGNodeFB* currentNode = tgCast::cast<CPGNode, CPGNodeFB>(nodeList[i]);
112  std::vector<double> comGroup(comIt, comIt + 3);
113  currentNode->updateDTs(comGroup);
114 
115  comIt += 3;
116  }
117 }
118 
119 void CPGEquationsFB::updateNodeData(std::vector<double> newXVals)
120 {
121 #ifndef BT_NO_PROFILE
122  BT_PROFILE("CPGEquationsFB::updateNodeData");
123 #endif //BT_NO_PROFILE
124  assert(newXVals.size()==3*nodeList.size());
125 
126  for(int i = 0; i!=nodeList.size(); i++){
127  CPGNodeFB* currentNode = tgCast::cast<CPGNode, CPGNodeFB>(nodeList[i]);
128  currentNode->updateNodeValues(newXVals[3*i], newXVals[3*i+1], newXVals[3*i+2]);
129  }
130 }
double omega
Definition: CPGNodeFB.h:68
Utility class for class casting and filtering collections by type.
virtual void updateDTs(const std::vector< double > &feedback)
Definition: CPGNodeFB.cpp:55
Definition of class CPGEquationsFB.