NTRT Simulator  Version: Master
 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 #if (0)
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 #endif
249 
250 #if (0)
251  stringList = subject.getMuscles("outer top");
252  m_config.outsideTopLength.clear();
253  for(std::size_t i = 0; i < stringList.size(); i++)
254  {
255  m_config.outsideTopLength.push_back(stringList[i]->getStartLength());
256  }
257 
258  stringList = subject.getMuscles("outer left");
259  m_config.outsideLeftLength.clear();
260  for(std::size_t i = 0; i < stringList.size(); i++)
261  {
262  m_config.outsideLeftLength.push_back(stringList[i]->getStartLength());
263  }
264 
265  stringList = subject.getMuscles("outer right");
266  m_config.outsideRightLength.clear();
267  for(std::size_t i = 0; i < stringList.size(); i++)
268  {
269  m_config.outsideRightLength.push_back(stringList[i]->getStartLength());
270  }
271 #endif
272 }
273 
274 void SerializedSpineControl::onStep(BaseSpineModelLearning& subject, double dt)
275 {
276 
277  updateTime += dt;
278 
279 
280  if (updateTime >= 1.0/m_config.updateFrequency)
281  {
282  simTime += updateTime;
283 
284 
285 #ifdef LOGGING // Conditional compile for data logging
286  m_dataObserver.onStep(subject, updateTime);
287 #endif
288  updateTime = 0.0;
289  }
290 
291  segments = subject.getSegments();
292 #if (1)
293  applyImpedanceControlGeneric(m_config.top_controller,
294  subject.getMuscles("inner top"),
295  m_config.insideTopLength,
296  m_config.insideTopTens,
297  dt,
298  0);
299 
300  applyImpedanceControlGeneric(m_config.in_controller,
301  subject.getMuscles("inner left"),
302  m_config.insideLeftLength,
303  m_config.insideLeftTens,
304  dt,
305  1);
306 
307  applyImpedanceControlGeneric(m_config.in_controller,
308  subject.getMuscles("inner right"),
309  m_config.insideRightLength,
310  m_config.insideRightTens,
311  dt,
312  2);
313 
314  applyImpedanceControlGeneric(m_config.top_controller,
315  subject.getMuscles("outer top"),
316  m_config.outsideTopLength,
317  m_config.outsideTopTens,
318  dt,
319  0);
320 
321  applyImpedanceControlGeneric(m_config.out_controller,
322  subject.getMuscles("outer left"),
323  m_config.outsideLeftLength,
324  m_config.outsideLeftTens,
325  dt,
326  1);
327 
328  applyImpedanceControlGeneric(m_config.out_controller,
329  subject.getMuscles("outer right"),
330  m_config.outsideRightLength,
331  m_config.outsideRightTens,
332  dt,
333  2);
334 
335 #endif
336  std::vector<tgBaseRigid*> rigids = subject.getAllRigids();
337  btRigidBody* seg1Body = rigids[0]->getPRigidBody();
338  btRigidBody* seg2Body = rigids[15]->getPRigidBody();
339  btRigidBody* seg3Body = rigids[29]->getPRigidBody();
340 
341  const abstractMarker marker = subject.getMarkers()[0];
342  const abstractMarker marker2 = subject.getMarkers()[1];
343  const abstractMarker marker3 = subject.getMarkers()[2];
344  const abstractMarker marker4 = subject.getMarkers()[3];
345  const abstractMarker marker5 = subject.getMarkers()[4];
346 
347  btVector3 force(0.0, 0.0, 0.0);
348  // 2 kg times gravity
349  if (simTime > 30.0 && simTime < 35.0)
350  {
351  force = btVector3(0.0, 0.0, 2.3 * 981.0 * (simTime - 30) / 5.0);
352  }
353  else if (simTime >= 35.0 && simTime < 40.0)
354  {
355  force = btVector3(0.0, 0.0, 2.3 * 981.0);
356  }
357  else if (simTime >= 40.0 && simTime < 45.0)
358  {
359  force = btVector3(0.0, 0.0, 2.3 * 981.0 * (45 - simTime)/5.0);
360  }
361  else
362  {
363  force = btVector3(0.0, 0.0, 0.0);
364  }
365  seg1Body->applyForce(force, marker.getRelativePosition());
366  seg2Body->applyForce(-force / 2.0 * 0.9, marker2.getRelativePosition());
367  seg3Body->applyForce(-force / 2.0 * 1.1, marker5.getRelativePosition());
368  //seg2Body->applyForce(-force / 2.0, marker4.getRelativePosition());
369 }
370 
371 
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.
FileHelpers::getResourcePath
static std::string getResourcePath(std::string relPath)
Definition: FileHelpers.cpp:40
SerializedSpineControl::onStep
virtual void onStep(BaseSpineModelLearning &subject, double dt)
Definition: SerializedSpineControl.cpp:274
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:159
tgUtil.h
Rand seeding simular to the evolution and terrain classes.
tgDataObserver::onSetup
virtual void onSetup(tgModel &model)
Definition: tgDataObserver.cpp:60
TetraSpineStaticModel.h
Tetraspine, configured for learning in the NTRT simulator.