NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
CPGNode.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 "CPGNode.h"
28 //#include "CPGEdge.h"
29 
30 // The C++ Standard Library
31 #include <algorithm> //for_each
32 #include <math.h>
33 #include <assert.h>
34 
35 CPGNode::CPGNode(int nodeNum, const std::vector<double> & params):
36 nodeValue(0),
37 phiValue(0),
38 phiDotValue(0),
39 rValue(0),
40 rDotValue(0),
41 m_nodeNumber(nodeNum),
42 rDoubleDotValue(0),
43 rConst(params[4]),
44 frequencyOffset(params[0]),
45 frequencyScale(params[1]),
46 radiusOffset(params[2]),
47 radiusScale(params[3]),
48 dMin(params[5]),
49 dMax(params[6])
50 {
51  //Precondition
52  assert(params.size() >= 7);
53 }
54 
55 CPGNode::~CPGNode()
56 {
57  couplingList.clear();
58 }
59 
60 void CPGNode::addCoupling( CPGNode* cNode,
61  const double cWeight,
62  const double cPhase)
63 {
64  couplingList.push_back(cNode);
65  weightList.push_back(cWeight);
66  phaseList.push_back(cPhase);
67 
68  assert(couplingList.size() == weightList.size() && couplingList.size() == phaseList.size());
69 }
70 
71 void CPGNode::updateDTs(double descCom)
72 {
73  phiDotValue = 2 * M_PI * nodeEquation(descCom, frequencyOffset, frequencyScale);
74 
80  const std::size_t n = couplingList.size();
81  for (std::size_t i = 0; i != n; i++){
82  const CPGNode& targetNode = *(couplingList[i]);
83  phiDotValue += weightList[i] * targetNode.rValue * sin (targetNode.phiValue - phiValue - phaseList[i]);
84  }
85 
86  rDoubleDotValue = rConst * (rConst / 4 * (nodeEquation(descCom, radiusOffset, radiusScale)
87  - rValue) - rDotValue);
88 }
89 
90 double CPGNode::nodeEquation( double d,
91  double c0,
92  double c1)
93 {
94  //@todo: Make parameters dMax, dMin
95  if(d >= dMin && d <= dMax){
96  return c1 * d + c0;
97  }
98  else{
99  return 0;
100  }
101 }
102 
103 void CPGNode::updateNodeValues (double newPhi,
104  double newR,
105  double newRD)
106 
107 {
108  rValue = newR;
109  rDotValue = newRD;
110  phiValue = newPhi;
111  nodeValue = rValue*cos(phiValue);
112 }
113 
114 std::string CPGNode::toString(const std::string& prefix) const
115 {
116  std::string p = " ";
117  std::ostringstream os;
118  os << prefix << "CPGNode(" << p << m_nodeNumber << std::endl;
119 
120  // TODO: add something about parameters of this node?
121 
122  os << prefix << p << "Connectivity:" << std::endl;
123  for(int i = 0; i < couplingList.size(); i++) {
124  os << prefix << p << p << *(couplingList[i]);
125  }
126 
127  os << prefix << ")";
128  return os.str();
129 }
double nodeEquation(double d, double c0, double c1)
Definition: CPGNode.cpp:90
const int m_nodeNumber
Definition: CPGNode.h:106
const double rConst
Definition: CPGNode.h:111
Definition of class CPGNode.
CPGNode(int nodeNum, const std::vector< double > &params)
Definition: CPGNode.cpp:35
double nodeValue
Definition: CPGNode.h:92
virtual void updateDTs(double descCom)
Definition: CPGNode.cpp:71