NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
CPGNodeFB.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 "CPGNodeFB.h"
28 
29 // The Bullet Physics Library
30 #include "LinearMath/btQuickprof.h"
31 
32 // The C++ Standard Library
33 #include <algorithm> //for_each
34 #include <math.h>
35 #include <assert.h>
36 
37 CPGNodeFB::CPGNodeFB(int nodeNum, const std::vector<double> & params):
38 CPGNode(nodeNum, params),
39 omega(params[7]),
40 omegaDot(0),
41 kFreq(params[8]),
42 kAmp(params[9]),
43 kPhase(params[10])
44 {
45  //Precondition
46  assert(params.size() >= 11);
47  rValue = sqrt(radiusOffset); // Jumpstart integration
48 }
49 
50 CPGNodeFB::~CPGNodeFB()
51 {
52 
53 }
54 
55 void CPGNodeFB::updateDTs(const std::vector<double>& feedback)
56 {
57 #ifndef BT_NO_PROFILE
58  BT_PROFILE("CPGNodeFB::updateDTs");
59 #endif //BT_NO_PROFILE
60  assert(feedback.size() >= 3);
61 
62  phiDotValue = omega + kPhase * feedback [2];
63 
69  const std::size_t n = couplingList.size();
70  for (std::size_t i = 0; i != n; i++){
71  const CPGNode& targetNode = *couplingList[i];
72  phiDotValue += weightList[i] * targetNode.rValue * sin (targetNode.phiValue - phiValue - phaseList[i]);
73  }
74 
75  omegaDot = kFreq * feedback[0] * sin(phiValue);
76 
77  rDotValue = rConst * (radiusOffset + kAmp * feedback[1] - pow(rValue, 2.0)) * rValue;
78 }
79 
80 void CPGNodeFB::updateNodeValues (double newPhi,
81  double newR,
82  double newO)
83 
84 {
85 #ifndef BT_NO_PROFILE
86  BT_PROFILE("CPGNodeFB::updateNodeValues");
87 #endif //BT_NO_PROFILE
88  rValue = newR;
89  phiValue = newPhi;
90  omega = newO;
91  nodeValue = rValue*cos(phiValue);
92 }
double omega
Definition: CPGNodeFB.h:68
CPGNodeFB(int nodeNum, const std::vector< double > &params)
Definition: CPGNodeFB.cpp:37
Definition of class CPGNodeFB.
virtual void updateDTs(const std::vector< double > &feedback)
Definition: CPGNodeFB.cpp:55
const double rConst
Definition: CPGNode.h:111
double nodeValue
Definition: CPGNode.h:92