NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
NestedStructureSineWaves.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 // This module
29 
30 // Its subject
32 
33 // NTRTSim
34 #include "core/tgBasicActuator.h"
36 #include "tgcreator/tgUtil.h"
37 
39  in_controller(new tgImpedanceController(100, 500, 50)),
40  out_controller(new tgImpedanceController(100, 500, 100)),
41  segments(1.0),
42  insideLength(16.5),
43  outsideLength(19.5),
44  offsetSpeed(0.0),
45  cpgAmplitude(50.0),
46  cpgFrequency(2.51),
47  bodyWaves(2.0),
48  simTime(0.0),
49  cycle(0.0),
50  target(0.0)
51 {
52  phaseOffsets.clear();
53  phaseOffsets.push_back(M_PI/2);
54  phaseOffsets.push_back(0);
55  phaseOffsets.push_back(0);
56 }
57 
59 {
60  delete in_controller;
61  delete out_controller;
62 }
63 
64 void NestedStructureSineWaves::applyImpedanceControlInside(const std::vector<tgBasicActuator*> stringList, double dt)
65 {
66  for(std::size_t i = 0; i < stringList.size(); i++)
67  {
68  double setTension = in_controller->control(*(stringList[i]),
69  dt,
70  insideLength
71  );
72  #if (0) // Conditional compile for verbose control
73  std::cout << "Inside String " << i << " tension " << setTension
74  << " act tension " << stringList[i]->getMuscle()->getTension()
75  << " length " << stringList[i]->getMuscle()->getActualLength() << std::endl;
76  #endif
77  }
78 }
79 
80 void NestedStructureSineWaves::applyImpedanceControlOutside(const std::vector<tgBasicActuator*> stringList,
81  double dt,
82  std::size_t phase)
83 {
84  for(std::size_t i = 0; i < stringList.size(); i++)
85  {
86  cycle = sin(simTime * cpgFrequency + 2 * bodyWaves * M_PI * i / (segments) + phaseOffsets[phase]);
87  target = offsetSpeed + cycle*cpgAmplitude;
88 
89  double setTension = out_controller->control(*(stringList[i]),
90  dt,
91  outsideLength,
92  target
93  );
94  #if(0) // Conditional compile for verbose control
95  std::cout << "Outside String " << i << " com tension " << setTension
96  << " act tension " << stringList[i]->getMuscle()->getTension()
97  << " length " << stringList[i]->getMuscle()->getActualLength() << std::endl;
98  #endif
99  }
100 }
101 
103 {
104  simTime += dt;
105 
106  segments = subject.getSegments();
107 
108  applyImpedanceControlInside(subject.getActuators("inner top"), dt);
109  applyImpedanceControlInside(subject.getActuators("inner left") , dt);
110  applyImpedanceControlInside(subject.getActuators("inner right"), dt);
111 
112  applyImpedanceControlOutside(subject.getActuators("outer top"), dt, 0);
113  applyImpedanceControlOutside(subject.getActuators("outer left"), dt, 1);
114  applyImpedanceControlOutside(subject.getActuators("outer right"), dt, 2);
115 }
116 
117 
Contains the definition of class ImpedanceControl. $Id$.
Contains the definition of class NestedStructureTestModel.
void applyImpedanceControlOutside(const std::vector< tgBasicActuator * > stringList, double dt, std::size_t phase)
Contains the definition of class tgBasicActuator.
Contains the definition of class NestedStructureSineWaves.
void applyImpedanceControlInside(const std::vector< tgBasicActuator * > stringList, double dt)
virtual void onStep(NestedStructureTestModel &subject, double dt)
const std::vector< tgBasicActuator * > & getActuators(const std::string &key) const
Contains the definition of class tgUtil and overloaded operator<<() free functions.
double control(tgBasicController &mLocalController, double deltaTimeSeconds, double newPosition, double offsetVel=0)
Control Functions.