NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
SerializedSpineControl.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
28 #include "SerializedSpineControl.h"
29 
30 // Its subject
31 #include "TetraSpineStaticModel.h"
32 
33 // NTRTSim
34 #include "core/tgSpringCableActuator.h"
35 #include "core/tgBasicActuator.h"
36 #include "core/tgBaseRigid.h"
37 #include "controllers/tgImpedanceController.h"
38 #include "core/abstractMarker.h"
39 #include "tgcreator/tgUtil.h"
40 
41 // The C++ Standard Library
42 #include <stdexcept>
43 #include <string>
44 
45 // JSON Serialization
46 #include "helpers/FileHelpers.h"
47 #include <json/json.h>
48 
49 //#define VERBOSE
50 //#define LOGGING
51 
52 SerializedSpineControl::Config::Config(std::string fileName)
53 {
54 
55  //BEGIN DESERIALIZING
56 
57  Json::Value root; // will contains the root value after parsing.
58  Json::Reader reader;
59 
60  std::string filePath = FileHelpers::getResourcePath("ICRA2015/static/controlVars.json");
61 
62  bool parsingSuccessful = reader.parse( FileHelpers::getFileString(filePath), root );
63  if ( !parsingSuccessful )
64  {
65  // report to the user the failure and their locations in the document.
66  std::cout << "Failed to parse configuration\n"
67  << reader.getFormattedErrorMessages();
68  throw std::invalid_argument("Bad config filename");
69  return;
70  }
71  // Get the value of the member of root named 'encoding', return 'UTF-8' if there is no
72  // such member.
73  double kTens = root.get("inside_imp_ten", "UTF-8").asDouble();
74  double kPos = root.get("inside_imp_pos", "UTF-8").asDouble();
75  double kVel = root.get("inside_imp_vel", "UTF-8").asDouble();
76  in_controller = new tgImpedanceController(kTens, kPos, kVel);
77 
78  kTens = root.get("outside_imp_ten", "UTF-8").asDouble();
79  kPos = root.get("outside_imp_pos", "UTF-8").asDouble();
80  kVel = root.get("outside_imp_vel", "UTF-8").asDouble();
81  out_controller = new tgImpedanceController(kTens, kPos, kVel);
82 
83  kTens = root.get("top_imp_ten", "UTF-8").asDouble();
84  kPos = root.get("top_imp_pos", "UTF-8").asDouble();
85  kVel = root.get("top_imp_vel", "UTF-8").asDouble();
86  top_controller = new tgImpedanceController(kTens, kPos, kVel);
87 
88  rod_edge = root.get("rod_edge", "UTF-8").asDouble();
89  rod_front = root.get("rod_front", "UTF-8").asDouble();
90  rod_offset = root.get("rod_offset", "UTF-8").asDouble();
91 
92 #if (0)
93  insideLength = sqrt(pow( (rod_edge * sin(M_PI/6)), 2) + pow( (rod_front - rod_offset), 2));
94  std::cout << "Computed inside length: " << insideLength << std::endl;
95  insideTopLength = insideLength;
96  outsideLength = rod_offset;
97  outsideTopLength = rod_offset;
98 #endif
99 
100  insideTopTens.clear();
101  insideTopTens.push_back(root.get("in_top_tens_a", "UTF-8").asDouble());
102  insideTopTens.push_back(root.get("in_top_tens_b", "UTF-8").asDouble());
103  insideLeftTens.clear();
104  insideLeftTens.push_back(root.get("in_left_tens_a", "UTF-8").asDouble());
105  insideLeftTens.push_back(root.get("in_left_tens_b", "UTF-8").asDouble());
106  insideRightTens.clear();
107  insideRightTens.push_back(root.get("in_right_tens_a", "UTF-8").asDouble());
108  insideRightTens.push_back(root.get("in_right_tens_b", "UTF-8").asDouble());
109 
110  outsideTopTens.clear();
111  outsideTopTens.push_back(root.get("out_top_tens_a", "UTF-8").asDouble());
112  outsideTopTens.push_back(root.get("out_top_tens_b", "UTF-8").asDouble());
113  outsideLeftTens.clear();
114  outsideLeftTens.push_back(root.get("out_left_tens_a", "UTF-8").asDouble());
115  outsideLeftTens.push_back(root.get("out_left_tens_b", "UTF-8").asDouble());
116  outsideRightTens.clear();
117  outsideRightTens.push_back(root.get("out_right_tens_a", "UTF-8").asDouble());
118  outsideRightTens.push_back(root.get("out_right_tens_b", "UTF-8").asDouble());
119 
120  insideTopLength.clear();
121  insideTopLength.push_back(root.get("in_top_length_a", "UTF-8").asDouble());
122  insideTopLength.push_back(root.get("in_top_length_b", "UTF-8").asDouble());
123  insideLeftLength.clear();
124  insideLeftLength.push_back(root.get("in_left_length_a", "UTF-8").asDouble());
125  insideLeftLength.push_back(root.get("in_left_length_b", "UTF-8").asDouble());
126  insideRightLength.clear();
127  insideRightLength.push_back(root.get("in_right_length_a", "UTF-8").asDouble());
128  insideRightLength.push_back(root.get("in_right_length_b", "UTF-8").asDouble());
129 
130  outsideTopLength.clear();
131  outsideTopLength.push_back(root.get("out_top_length_a", "UTF-8").asDouble());
132  outsideTopLength.push_back(root.get("out_top_length_b", "UTF-8").asDouble());
133  outsideLeftLength.clear();
134  outsideLeftLength.push_back(root.get("out_left_length_a", "UTF-8").asDouble());
135  outsideLeftLength.push_back(root.get("out_left_length_b", "UTF-8").asDouble());
136  outsideRightLength.clear();
137  outsideRightLength.push_back(root.get("out_right_length_a", "UTF-8").asDouble());
138  outsideRightLength.push_back(root.get("out_right_length_b", "UTF-8").asDouble());
139 
140 
141 
142  offsetSpeed = root.get("offset_speed", "UTF-8").asDouble();
143  cpgAmplitude = root.get("cpg_amplitude", "UTF-8").asDouble();
144  cpgFrequency = root.get("cpg_frequency", "UTF-8").asDouble();
145  bodyWaves = root.get("bodyWaves", "UTF-8").asDouble();
146  insideMod = root.get("insideMod", "UTF-8").asDouble();
147 
148  updateFrequency = root.get("updateFrequency", "UTF-8").asDouble();
149 
150  phaseOffsets.clear();
151  phaseOffsets.push_back(root.get("top_offset", "UTF-8").asDouble());
152  phaseOffsets.push_back(root.get("left_offset", "UTF-8").asDouble());
153  phaseOffsets.push_back(root.get("right_offset", "UTF-8").asDouble());
154 
155  //END SERIALIZING
156 
158 }
159 
160 SerializedSpineControl::Config::~Config()
161 {
162  delete in_controller;
163  delete out_controller;
164 }
165 
166 
167 SerializedSpineControl::SerializedSpineControl(std::string fileName) :
168 m_config(fileName),
169 segments(1.0),
170 m_dataObserver("logs/TCData"),
171 simTime(0.0),
172 updateTime(0.0),
173 cycle(0.0),
174 target(0.0)
175 {
176 
177 
178 
179 }
180 
181 SerializedSpineControl::~SerializedSpineControl()
182 {
183 }
184 
185 void SerializedSpineControl::applyImpedanceControlGeneric(tgImpedanceController* controller,
186  const std::vector<tgSpringCableActuator*> stringList,
187  const std::vector<double> stringLengths,
188  const std::vector<double> tensions,
189  double dt,
190  std::size_t phase)
191 {
192  assert(stringList.size() == stringLengths.size() && stringList.size() == tensions.size());
193  assert(controller);
194 
195  for(std::size_t i = 0; i < stringList.size(); i++)
196  {
197  tgBasicActuator& m_sca = *(tgCast::cast<tgSpringCableActuator, tgBasicActuator>(stringList[i]));
198 
199  // This will reproduce the same value until simTime is updated. See onStep
200  // TODO : consider making inside mod a parameter as well...
201  cycle = sin(simTime * m_config.cpgFrequency + 2 * m_config.bodyWaves * M_PI * i / (segments) + m_config.phaseOffsets[phase]);
202  target = m_config.offsetSpeed + cycle*m_config.cpgAmplitude;
203 
204  double setTension = controller->controlTension(m_sca,
205  dt,
206  stringLengths[i],
207  tensions[i],
208  target
209  );
210 #ifdef VERBOSE // Conditional compile for verbose control
211  std::cout << "Top Outside String " << i << " com tension " << setTension
212  << " act tension " << stringList[i]->getMuscle()->getTension()
213  << " length " << stringList[i]->getMuscle()->getActualLength() << std::endl;
214 #endif
215  }
216 }
217 
218 void SerializedSpineControl::onSetup(BaseSpineModelLearning& subject)
219 {
220 #ifdef LOGGING // Conditional compile for data logging
221  m_dataObserver.onSetup(subject);
222 #endif
223 
224 
225  // Setup initial lengths
226  std::vector<tgSpringCableActuator*> stringList;
227 
228  stringList = subject.getMuscles("inner top");
229  m_config.insideTopLength.clear();
230  for(std::size_t i = 0; i < stringList.size(); i++)
231  {
232  m_config.insideTopLength.push_back(stringList[i]->getStartLength());
233  }
234 
235  stringList = subject.getMuscles("inner left");
236  m_config.insideLeftLength.clear();
237  for(std::size_t i = 0; i < stringList.size(); i++)
238  {
239  m_config.insideLeftLength.push_back(stringList[i]->getStartLength());
240  }
241 
242  stringList = subject.getMuscles("inner right");
243  m_config.insideRightLength.clear();
244  for(std::size_t i = 0; i < stringList.size(); i++)
245  {
246  m_config.insideRightLength.push_back(stringList[i]->getStartLength());
247  }
248 #if (0)
249  stringList = subject.getMuscles("outer top");
250  m_config.outsideTopLength.clear();
251  for(std::size_t i = 0; i < stringList.size(); i++)
252  {
253  m_config.outsideTopLength.push_back(stringList[i]->getStartLength());
254  }
255 
256  stringList = subject.getMuscles("outer left");
257  m_config.outsideLeftLength.clear();
258  for(std::size_t i = 0; i < stringList.size(); i++)
259  {
260  m_config.outsideLeftLength.push_back(stringList[i]->getStartLength());
261  }
262 
263  stringList = subject.getMuscles("outer right");
264  m_config.outsideRightLength.clear();
265  for(std::size_t i = 0; i < stringList.size(); i++)
266  {
267  m_config.outsideRightLength.push_back(stringList[i]->getStartLength());
268  }
269 #endif
270 }
271 
272 void SerializedSpineControl::onStep(BaseSpineModelLearning& subject, double dt)
273 {
274 
275  updateTime += dt;
276 
277 
278  if (updateTime >= 1.0/m_config.updateFrequency)
279  {
280  simTime += updateTime;
281 
282 
283 #ifdef LOGGING // Conditional compile for data logging
284  m_dataObserver.onStep(subject, updateTime);
285 #endif
286  updateTime = 0.0;
287  }
288 
289  segments = subject.getSegments();
290 #if (1)
291  applyImpedanceControlGeneric(m_config.top_controller,
292  subject.getMuscles("inner top"),
293  m_config.insideTopLength,
294  m_config.insideTopTens,
295  dt,
296  0);
297 
298  applyImpedanceControlGeneric(m_config.in_controller,
299  subject.getMuscles("inner left"),
300  m_config.insideLeftLength,
301  m_config.insideLeftTens,
302  dt,
303  1);
304 
305  applyImpedanceControlGeneric(m_config.in_controller,
306  subject.getMuscles("inner right"),
307  m_config.insideRightLength,
308  m_config.insideRightTens,
309  dt,
310  2);
311 
312  applyImpedanceControlGeneric(m_config.top_controller,
313  subject.getMuscles("outer top"),
314  m_config.outsideTopLength,
315  m_config.outsideTopTens,
316  dt,
317  0);
318 
319  applyImpedanceControlGeneric(m_config.out_controller,
320  subject.getMuscles("outer left"),
321  m_config.outsideLeftLength,
322  m_config.outsideLeftTens,
323  dt,
324  1);
325 
326  applyImpedanceControlGeneric(m_config.out_controller,
327  subject.getMuscles("outer right"),
328  m_config.outsideRightLength,
329  m_config.outsideRightTens,
330  dt,
331  2);
332 
333 #endif
334  std::vector<tgBaseRigid*> rigids = subject.getAllRigids();
335  btRigidBody* seg1Body = rigids[0]->getPRigidBody();
336  btRigidBody* seg2Body = rigids[15]->getPRigidBody();
337  btRigidBody* seg3Body = rigids[29]->getPRigidBody();
338 
339  const abstractMarker marker = subject.getMarkers()[0];
340  const abstractMarker marker2 = subject.getMarkers()[1];
341  const abstractMarker marker3 = subject.getMarkers()[2];
342  const abstractMarker marker4 = subject.getMarkers()[3];
343  const abstractMarker marker5 = subject.getMarkers()[4];
344 
345  btVector3 force(0.0, 0.0, 0.0);
346  // 2 kg times gravity
347  if (simTime > 30.0 && simTime < 35.0)
348  {
349  force = btVector3(0.0, 0.0, 2.3 * 981.0 * (simTime - 30) / 5.0);
350  }
351  else if (simTime >= 35.0 && simTime < 40.0)
352  {
353  force = btVector3(0.0, 0.0, 2.3 * 981.0);
354  }
355  else if (simTime >= 40.0 && simTime < 45.0)
356  {
357  force = btVector3(0.0, 0.0, 2.3 * 981.0 * (45 - simTime)/5.0);
358  }
359  else
360  {
361  force = btVector3(0.0, 0.0, 0.0);
362  }
363  seg1Body->applyForce(force, marker.getRelativePosition());
364  seg2Body->applyForce(-force / 2.0 * 0.9, marker2.getRelativePosition());
365  seg3Body->applyForce(-force / 2.0 * 1.1, marker5.getRelativePosition());
366  //seg2Body->applyForce(-force / 2.0, marker4.getRelativePosition());
367 }
368 
369 
tgImpedanceController.h
Contains the definition of class ImpedanceControl. $Id$.
tgBaseRigid.h
Create a box shape as an obstacle or add it to your tensegrity.
abstractMarker::getRelativePosition
btVector3 getRelativePosition() const
Definition: abstractMarker.cpp:45
SerializedSpineControl.h
A Sine Wave controller for TetraSpine using JSON serialization.
abstractMarker.h
Markers for specific places on a tensegrity.
tgSpringCableActuator.h
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
SerializedSpineControl::Config::in_controller
tgImpedanceController * in_controller
Definition: SerializedSpineControl.h:60
FileHelpers.h
A series of functions to assist with file input/output.
SerializedSpineControl::onStep
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Definition: SerializedSpineControl.cpp:272
tgBasicActuator.h
Contains the definition of class tgBasicActuator.
SerializedSpineControl::SerializedSpineControl
SerializedSpineControl(std::string fileName)
Definition: SerializedSpineControl.cpp:167
SerializedSpineControl::onSetup
virtual void onSetup(BaseSpineModelLearning &subject)
Definition: SerializedSpineControl.cpp:218
tgDataObserver::onStep
virtual void onStep(tgModel &model, double dt)
Definition: tgDataObserver.cpp:142
tgUtil.h
Contains the definition of class tgUtil and overloaded operator<<() free functions.
tgDataObserver::onSetup
virtual void onSetup(tgModel &model)
Definition: tgDataObserver.cpp:59
TetraSpineStaticModel.h
Tetraspine, configured for learning in the NTRT simulator.