NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
VerticalSpineRestLengthController.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 // This application
30 #include "VerticalSpineModel.h"
31 // This library
32 #include "core/tgBasicActuator.h"
33 #include "core/tgString.h"
34 // The C++ Standard Library
35 #include <cassert>
36 #include <stdexcept>
37 #include <vector>
38 
40 {
41  verticalRL = 7.38 ; // cm
42  saddleRL1 = 13.037 ; // cm
43  saddleRL2 = 13.613 ; // cm
44  saddleRL3 = 14.189 ; // cm
45  saddleRL4 = 14.766 ; // cm
46 }
47 
49 {
50  // Do a one-time update of all cable rest lengths
51  // First, get all muscles (cables)
52  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
53 
54  // get all vertical muscles
55  const std::vector<tgBasicActuator*> v_musclesA = subject.getMuscles("vertical a");
56  const std::vector<tgBasicActuator*> v_musclesB = subject.getMuscles("vertical b");
57  const std::vector<tgBasicActuator*> v_musclesC = subject.getMuscles("vertical c");
58  const std::vector<tgBasicActuator*> v_musclesD = subject.getMuscles("vertical d");
59 
60  // get saddle muscles and put them in individual vectors
61  const std::vector<tgBasicActuator*> s_muscles0 = subject.getMuscles(tgString("saddle", 0));
62  const std::vector<tgBasicActuator*> s_muscles1 = subject.getMuscles(tgString("saddle", 1));
63  const std::vector<tgBasicActuator*> s_muscles2 = subject.getMuscles(tgString("saddle", 2));
64  const std::vector<tgBasicActuator*> s_muscles3 = subject.getMuscles(tgString("saddle", 3));
65 
66  // set string length for vertical muscles
67  for (size_t i = 0; i < v_musclesA.size(); ++ i)
68  {
69  // A
70  tgBasicActuator * const pMuscleA = v_musclesA[i];
71  assert(pMuscleA != NULL);
72  pMuscleA->setRestLengthSingleStep(verticalRL);
73 
74  //B
75  tgBasicActuator * const pMuscleB = v_musclesB[i];
76  assert(pMuscleB != NULL);
77  pMuscleB->setRestLengthSingleStep(verticalRL);
78 
79  //C
80  tgBasicActuator * const pMuscleC = v_musclesC[i];
81  assert(pMuscleC != NULL);
82  pMuscleC->setRestLengthSingleStep(verticalRL);
83 
84  //D
85  tgBasicActuator * const pMuscleD = v_musclesD[i];
86  assert(pMuscleD != NULL);
87  pMuscleD->setRestLengthSingleStep(verticalRL);
88  }
89 
90  // set string lengths for saddle muscles
91 
92  for (size_t i = 0; i < s_muscles0.size(); ++i)
93  {
94 
95  // 1
96  tgBasicActuator * const pMuscle1 = s_muscles0[i];
97  assert(pMuscle1 != NULL);
98  pMuscle1->setRestLengthSingleStep(saddleRL1);
99 
100  // 2
101  tgBasicActuator * const pMuscle2 = s_muscles1[i];
102  assert(pMuscle2 != NULL);
103  pMuscle2->setRestLengthSingleStep(saddleRL2);
104 
105  // 3
106  tgBasicActuator * const pMuscle3 = s_muscles2[i];
107  assert(pMuscle3 != NULL);
108  pMuscle3->setRestLengthSingleStep(saddleRL3);
109 
110  // 4
111  tgBasicActuator * const pMuscle4 = s_muscles3[i];
112  assert(pMuscle4 != NULL);
113  pMuscle4->setRestLengthSingleStep(saddleRL4);
114  }
115 
116 
117  // // first vertical
118 // for (size_t i = 0; i < 4; ++i)
119 // {
120 // tgBasicActuator * const pMuscle = muscles[i];
121 // assert(pMuscle != NULL);
122 //
123 // double desiredRestLength = verticalRL;
124 // // Note that the single step version of setRestLength is used here,
125 // // since we only want to call it once (not iteratively like the original.)
126 // pMuscle->setRestLengthSingleStep(desiredRestLength);
127 // }
128 //
129 // // 1st saddle
130 // for (size_t i = 4; i < 8; ++i)
131 // {
132 // tgBasicActuator * const pMuscle = muscles[i];
133 // assert(pMuscle != NULL);
134 //
135 // double desiredRestLength = saddleRL1;
136 // // Note that the single step version of setRestLength is used here,
137 // // since we only want to call it once (not iteratively like the original.)
138 // pMuscle->setRestLengthSingleStep(desiredRestLength);
139 // }
140 //
141 // // 2nd vertical
142 // for (size_t i = 8; i < 12; ++i)
143 // {
144 // tgBasicActuator * const pMuscle = muscles[i];
145 // assert(pMuscle != NULL);
146 //
147 // double desiredRestLength = verticalRL;
148 // // Note that the single step version of setRestLength is used here,
149 // // since we only want to call it once (not iteratively like the original.)
150 // pMuscle->setRestLengthSingleStep(desiredRestLength);
151 // }
152 //
153 // // 2nd saddle
154 // for (size_t i = 12; i < 16; ++i)
155 // {
156 // tgBasicActuator * const pMuscle = muscles[i];
157 // assert(pMuscle != NULL);
158 //
159 // double desiredRestLength = saddleRL2;
160 // // Note that the single step version of setRestLength is used here,
161 // // since we only want to call it once (not iteratively like the original.)
162 // pMuscle->setRestLengthSingleStep(desiredRestLength);
163 // }
164 //
165 // // 3rd vertical
166 // for (size_t i = 16; i < 20; ++i)
167 // {
168 // tgBasicActuator * const pMuscle = muscles[i];
169 // assert(pMuscle != NULL);
170 //
171 // double desiredRestLength = verticalRL;
172 // // Note that the single step version of setRestLength is used here,
173 // // since we only want to call it once (not iteratively like the original.)
174 // pMuscle->setRestLengthSingleStep(desiredRestLength);
175 // }
176 //
177  // 3rd saddle
178 // for (size_t i = 20; i < 24; ++i)
179 // {
180 // tgBasicActuator * const pMuscle = muscles[i];
181 // assert(pMuscle != NULL);
182 //
183 // double desiredRestLength = saddleRL3;
184 // // Note that the single step version of setRestLength is used here,
185 // // since we only want to call it once (not iteratively like the original.)
186 // pMuscle->setRestLengthSingleStep(desiredRestLength);
187 // }
188 //
189 // // 4th vertical
190 // for (size_t i = 24; i < 28; ++i)
191 // {
192 // tgBasicActuator * const pMuscle = muscles[i];
193 // assert(pMuscle != NULL);
194 //
195 // double desiredRestLength = verticalRL;
196 // // Note that the single step version of setRestLength is used here,
197 // // since we only want to call it once (not iteratively like the original.)
198 // pMuscle->setRestLengthSingleStep(desiredRestLength);
199 // }
200 //
201  // 4th saddle
202 // for (size_t i = 28; i < 32; ++i)
203 // {
204 // tgBasicActuator * const pMuscle = muscles[i];
205 // assert(pMuscle != NULL);
206 //
207 // double desiredRestLength = saddleRL4;
208 // // Note that the single step version of setRestLength is used here,
209 // // since we only want to call it once (not iteratively like the original.)
210 // pMuscle->setRestLengthSingleStep(desiredRestLength);
211 // }
212 //
213 //
214 }
215 
217 {
218  if (dt <= 0.0)
219  {
220  throw std::invalid_argument("dt is not positive");
221  }
222  else
223  {
224  // Do a one-time update of all cable rest lengths
225  // First, get all muscles (cables)
226  const std::vector<tgBasicActuator*> muscles = subject.getAllMuscles();
227 
228  // get all vertical muscles
229  const std::vector<tgBasicActuator*> v_musclesA = subject.getMuscles("vertical a");
230  const std::vector<tgBasicActuator*> v_musclesB = subject.getMuscles("vertical b");
231  const std::vector<tgBasicActuator*> v_musclesC = subject.getMuscles("vertical c");
232  const std::vector<tgBasicActuator*> v_musclesD = subject.getMuscles("vertical d");
233 
234  // get saddle muscles and put them in individual vectors
235  const std::vector<tgBasicActuator*> s_muscles0 = subject.getMuscles(tgString("saddle", 0));
236  const std::vector<tgBasicActuator*> s_muscles1 = subject.getMuscles(tgString("saddle", 1));
237  const std::vector<tgBasicActuator*> s_muscles2 = subject.getMuscles(tgString("saddle", 2));
238  const std::vector<tgBasicActuator*> s_muscles3 = subject.getMuscles(tgString("saddle", 3));
239 
240  // set string length for vertical muscles
241  for (size_t i = 0; i < v_musclesA.size(); ++ i)
242  {
243  // A
244  tgBasicActuator * const pMuscleA = v_musclesA[i];
245  assert(pMuscleA != NULL);
246  pMuscleA->setRestLengthSingleStep(verticalRL);
247 
248  //B
249  tgBasicActuator * const pMuscleB = v_musclesB[i];
250  assert(pMuscleB != NULL);
251  pMuscleB->setRestLengthSingleStep(verticalRL);
252 
253  //C
254  tgBasicActuator * const pMuscleC = v_musclesC[i];
255  assert(pMuscleC != NULL);
256  pMuscleC->setRestLengthSingleStep(verticalRL);
257 
258  //D
259  tgBasicActuator * const pMuscleD = v_musclesD[i];
260  assert(pMuscleD != NULL);
261  pMuscleD->setRestLengthSingleStep(verticalRL);
262  }
263 
264  // set string lengths for saddle muscles
265 
266  for (size_t i = 0; i < s_muscles0.size(); ++i)
267  {
268 
269  // 1
270  tgBasicActuator * const pMuscle1 = s_muscles0[i];
271  assert(pMuscle1 != NULL);
272  pMuscle1->setRestLengthSingleStep(saddleRL1);
273 
274  // 2
275  tgBasicActuator * const pMuscle2 = s_muscles1[i];
276  assert(pMuscle2 != NULL);
277  pMuscle2->setRestLengthSingleStep(saddleRL2);
278 
279  // 3
280  tgBasicActuator * const pMuscle3 = s_muscles2[i];
281  assert(pMuscle3 != NULL);
282  pMuscle3->setRestLengthSingleStep(saddleRL3);
283 
284  // 4
285  tgBasicActuator * const pMuscle4 = s_muscles3[i];
286  assert(pMuscle4 != NULL);
287  pMuscle4->setRestLengthSingleStep(saddleRL4);
288  }
289 
290 //
291 // // first vertical
292 // for (size_t i = 0; i < 4; ++i)
293 // {
294 // tgBasicActuator * const pMuscle = muscles[i];
295 // assert(pMuscle != NULL);
296 //
297 // double desiredRestLength = verticalRL;
298 // // Note that the single step version of setRestLength is used here,
299 // // since we only want to call it once (not iteratively like the original.)
300 // pMuscle->setRestLengthSingleStep(desiredRestLength);
301 // }
302 //
303 // // 1st saddle
304 // for (size_t i = 4; i < 8; ++i)
305 // {
306 // tgBasicActuator * const pMuscle = muscles[i];
307 // assert(pMuscle != NULL);
308 //
309 // double desiredRestLength = saddleRL1;
310 // // Note that the single step version of setRestLength is used here,
311 // // since we only want to call it once (not iteratively like the original.)
312 // pMuscle->setRestLengthSingleStep(desiredRestLength);
313 // }
314 //
315 // // 2nd vertical
316 // for (size_t i = 8; i < 12; ++i)
317 // {
318 // tgBasicActuator * const pMuscle = muscles[i];
319 // assert(pMuscle != NULL);
320 //
321 // double desiredRestLength = verticalRL;
322 // // Note that the single step version of setRestLength is used here,
323 // // since we only want to call it once (not iteratively like the original.)
324 // pMuscle->setRestLengthSingleStep(desiredRestLength);
325 // }
326 //
327 // // 2nd saddle
328 // for (size_t i = 12; i < 16; ++i)
329 // {
330 // tgBasicActuator * const pMuscle = muscles[i];
331 // assert(pMuscle != NULL);
332 //
333 // double desiredRestLength = saddleRL2;
334 // // Note that the single step version of setRestLength is used here,
335 // // since we only want to call it once (not iteratively like the original.)
336 // pMuscle->setRestLengthSingleStep(desiredRestLength);
337 // }
338 //
339 // // 3rd vertical
340 // for (size_t i = 16; i < 20; ++i)
341 // {
342 // tgBasicActuator * const pMuscle = muscles[i];
343 // assert(pMuscle != NULL);
344 //
345 // double desiredRestLength = verticalRL;
346 // // Note that the single step version of setRestLength is used here,
347 // // since we only want to call it once (not iteratively like the original.)
348 // pMuscle->setRestLengthSingleStep(desiredRestLength);
349 // }
350 //
351 // // 3rd saddle
352 // for (size_t i = 20; i < 24; ++i)
353 // {
354 // tgBasicActuator * const pMuscle = muscles[i];
355 // assert(pMuscle != NULL);
356 //
357 // double desiredRestLength = saddleRL3;
358 // // Note that the single step version of setRestLength is used here,
359 // // since we only want to call it once (not iteratively like the original.)
360 // pMuscle->setRestLengthSingleStep(desiredRestLength);
361 // }
362 //
363 // // 4th vertical
364 // for (size_t i = 24; i < 28; ++i)
365 // {
366 // tgBasicActuator * const pMuscle = muscles[i];
367 // assert(pMuscle != NULL);
368 //
369 // double desiredRestLength = verticalRL;
370 // // Note that the single step version of setRestLength is used here,
371 // // since we only want to call it once (not iteratively like the original.)
372 // pMuscle->setRestLengthSingleStep(desiredRestLength);
373 // }
374 //
375 // // 4th saddle
376 // for (size_t i = 28; i < 32; ++i)
377 // {
378 // tgBasicActuator * const pMuscle = muscles[i];
379 // assert(pMuscle != NULL);
380 //
381 // double desiredRestLength = saddleRL4;
382 // // Note that the single step version of setRestLength is used here,
383 // // since we only want to call it once (not iteratively like the original.)
384 // pMuscle->setRestLengthSingleStep(desiredRestLength);
385 // }
386 //
387 
388 
389  }
390 }
virtual void onSetup(VerticalSpineModel &subject)
Convenience function for combining strings with ints, mostly for naming structures.
Contains the definition of class VerticalSpineModel.
const std::vector< tgBasicActuator * > & getMuscles(const std::string &key) const
Contains the definition of class tgBasicActuator.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
virtual void onStep(VerticalSpineModel &subject, double dt)
Contains the definition of class VerticalSpineRestLengthController.