NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
htSpineSine.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 
28 #include "htSpineSine.h"
29 
30 #include <string>
31 
32 
33 // Should include tgString, but compiler complains since its been
34 // included from TetraSpineLearningModel. Perhaps we should move things
35 // to a cpp over there
38 
41 
42 #include "dev/btietz/hardwareSineWaves/tgSineStringControl.h"
43 
44 // JSON Serialization
45 #include "helpers/FileHelpers.h"
46 #include <json/json.h>
47 
48 // The C++ Standard Library
49 #include <stdexcept>
50 
51 //#define LOGGING
52 
59 m_dataObserver("logs/TCData")
60 {
61 }
62 
64 {
65 
66  setupWaves(subject);
67 
68 #ifdef LOGGING // Conditional compile for data logging
69  m_dataObserver.onSetup(subject);
70 #endif
71 
72 
73  m_updateTime = 0.0;
74 }
75 
77 {
79  m_updateTime += dt;
80  if (m_updateTime >= m_controlTime)
81  {
82 
83 #ifdef LOGGING // Conditional compile for data logging
84  m_dataObserver.onStep(subject, m_updateTime);
85 #endif
86  m_updateTime = 0;
87  }
88 }
89 
91 {
92 
93  for(size_t i = 0; i < m_sineControllers.size(); i++)
94  {
95  delete m_sineControllers[i];
96  }
97  m_sineControllers.clear();
98 }
99 
101 {
102  std::vector <tgSpringCableActuator*> allMuscles = subject.getAllMuscles();
103 
104  double tension;
105  double kPosition;
106  double kVelocity;
107  double controlLength;
108 
109  double amplitude;
110  double phase;
111  double offset;
112 
113  Json::Value root; // will contains the root value after parsing.
114  Json::Reader reader;
115 
116  bool parsingSuccessful = reader.parse( FileHelpers::getFileString("controlVars.json"), root );
117  if ( !parsingSuccessful )
118  {
119  // report to the user the failure and their locations in the document.
120  std::cout << "Failed to parse configuration\n"
121  << reader.getFormattedErrorMessages();
123  throw std::invalid_argument("Bad JSON filename");
124  }
125 
126  m_controlTime = root.get("updateFrequency", "UTF-8").asDouble();
127  double frequency = root.get("cpg_frequency", "UTF-8").asDouble();
128 
129  for (std::size_t i = 0; i < allMuscles.size(); i++)
130  {
131  if (allMuscles[i]->hasTag("inner top"))
132  {
133  tension = 2000.0;
134  kPosition = 500.0;
135 
136  controlLength = allMuscles[i]->getStartLength();
137  //controlLength = 19.0;
138  if (allMuscles[i]->hasTag("seg1"))
139  {
140  amplitude = root.get("in_top_amp_a", "UTF-8").asDouble();
141  phase = root.get("front_offset", "UTF-8").asDouble();
142  kVelocity = 50;
143  }
144  else if(allMuscles[i]->hasTag("seg2"))
145  {
146  amplitude = root.get("in_top_amp_b", "UTF-8").asDouble();
147  phase = root.get("back_offset", "UTF-8").asDouble();
148  kVelocity = 100.0;
149  }
150  else
151  {
152  throw std::runtime_error("Missing tags!");
153  }
154  }
155  else if (allMuscles[i]->hasTag("outer top"))
156  {
157  tension = 1000.0;
158  kPosition = 500.0;
159  kVelocity = 100.0;
160 
161  if (allMuscles[i]->hasTag("seg1"))
162  {
163  amplitude = root.get("out_top_amp_a", "UTF-8").asDouble();
164  phase = root.get("front_offset", "UTF-8").asDouble();
165 
166  controlLength = 18.5;
167  }
168  else if(allMuscles[i]->hasTag("seg2"))
169  {
170  amplitude = root.get("out_top_amp_b", "UTF-8").asDouble();
171  phase = root.get("back_offset", "UTF-8").asDouble();
172  controlLength = 19.8;
173  }
174  else
175  {
176  throw std::runtime_error("Missing tags!");
177  }
178  }
179  else if (allMuscles[i]->hasTag("inner"))
180  {
181  tension = 1000.0;
182  kPosition = 300.0;
183  kVelocity = 100.0;
184  controlLength = allMuscles[i]->getStartLength();
185  //controlLength = 21.5;
186  if (allMuscles[i]->hasTag("seg1"))
187  {
188  amplitude = root.get("in_bottom_amp_a", "UTF-8").asDouble();
189  phase = root.get("front_offset", "UTF-8").asDouble();
190  }
191  else if(allMuscles[i]->hasTag("seg2"))
192  {
193  amplitude = root.get("in_bottom_amp_b", "UTF-8").asDouble();
194  phase = root.get("back_offset", "UTF-8").asDouble();
195  }
196  else
197  {
198  throw std::runtime_error("Missing tags!");
199  }
200  }
201  else if (allMuscles[i]->hasTag("outer"))
202  {
203  tension = 500.0;
204  kPosition = 300.0;
205  kVelocity = 100.0;
206  controlLength = 19.0 ;
207  if (allMuscles[i]->hasTag("seg1"))
208  {
209  amplitude = root.get("out_bottom_amp_a", "UTF-8").asDouble();
210  phase = root.get("front_offset", "UTF-8").asDouble();
211  }
212  else if(allMuscles[i]->hasTag("seg2"))
213  {
214  amplitude = root.get("out_bottom_amp_b", "UTF-8").asDouble();
215  phase = root.get("back_offset", "UTF-8").asDouble();
216  }
217  else
218  {
219  throw std::runtime_error("Missing tags!");
220  }
221  }
222  else
223  {
224  throw std::runtime_error("Missing tags!");
225  }
226 
227  tgImpedanceController* p_ipc = new tgImpedanceController( tension,
228  kPosition,
229  kVelocity);
230 
231  // In Won's convention
232  offset = amplitude;
233 
234  tgSineStringControl* pStringControl = new tgSineStringControl(m_controlTime,
235  p_ipc,
236  amplitude,
237  frequency,
238  phase,
239  offset,
240  controlLength);
241 
242 
243  allMuscles[i]->attach(pStringControl);
244  m_sineControllers.push_back(pStringControl);
245  }
246 
247  assert(m_sineControllers.size() == allMuscles.size());
248 
249 }
250 
Contains the definition of class ImpedanceControl. $Id$.
virtual void setupWaves(BaseSpineModelLearning &subject)
virtual void onTeardown(BaseSpineModelLearning &subject)
Definition: htSpineSine.cpp:90
Controller for TetraSpineLearningModel.
A class to read a learning configuration from a .ini file.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
A series of functions to assist with file input/output.
Contains the definition of class AnnealEvolution. Adapting NeuroEvolution to do Simulated Annealing...
virtual void onStep(tgModel &model, double dt)
virtual void onSetup(BaseSpineModelLearning &subject)
Definition: htSpineSine.cpp:63
virtual void onSetup(tgModel &model)
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Definition: htSpineSine.cpp:76