NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
BigPuppySymmetricArching.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 
29 //This application
31 
32 // This library
33 #include "core/tgModel.h"
34 #include "core/tgSimView.h"
35 #include "core/tgSimViewGraphics.h"
36 #include "core/tgSimulation.h"
37 #include "core/tgWorld.h"
38 #include "core/tgBasicActuator.h"
41 #include "core/tgRod.h"
43 #include "core/tgString.h"
44 #include "tgcreator/tgBuildSpec.h"
46 #include "tgcreator/tgRodInfo.h"
47 #include "tgcreator/tgStructure.h"
49 // The Bullet Physics library
50 #include "LinearMath/btVector3.h"
51 // The C++ Standard Library
52 #include <iostream>
53 #include <stdexcept>
54 
55 //#define USE_KINEMATIC
56 //#define PASSIVE_STRUCTURE
57 
58 BigPuppySymmetricArching::BigPuppySymmetricArching(int segments, int hips, int legs, int feet) :
59 BaseQuadModelLearning(segments, hips),
60 m_legs(legs),
61 m_feet(feet)
62 {
63  m_subStructures = segments + hips + legs + feet;
64 }
65 
66 BigPuppySymmetricArching::~BigPuppySymmetricArching()
67 {
68 }
69 
70 void BigPuppySymmetricArching::addNodesFoot(tgStructure& s, double r1, double r2){
71  s.addNode(r2,0,r2);//0
72  s.addNode(r2,0,-r2);//1
73  s.addNode(-r2,0,-r2);//2
74  s.addNode(-r2,0,r2);//3
75  s.addNode(r2/2,r1/2,0);//4
76  s.addNode(0,r1/2,-r2/2);//5
77  s.addNode(-r2/2,r1/2,0);//6
78  s.addNode(0,r1/2,r2/2);//7
79 }
80 
81 void BigPuppySymmetricArching::addRodsFoot(tgStructure& s){
82  s.addPair(0,6,"rod");
83  s.addPair(1,7,"rod");
84  s.addPair(2,4,"rod");
85  s.addPair(3,5,"rod");
86  //s.addPair(0,1,"rod");
87  //s.addPair(0,3,"rod");
88  //s.addPair(1,2,"rod");
89  //s.addPair(2,3,"rod");
90 }
91 
92 void BigPuppySymmetricArching::addNodesLeg(tgStructure& s, double r){
93  s.addNode(0,0,0); //0: Bottom Center of lower leg segment
94  s.addNode(0,r,0); //1: Center of lower leg segment
95  s.addNode(r,r,0); //2: Right of lower leg segment
96  s.addNode(-r,r,0); //3: Left of lower leg segment
97  s.addNode(0,2*r,0); //4: Top of lower leg segment
98  s.addNode(0,-r/2,0); //5: Leg segment extension for connections to foot.
99 }
100 
101 void BigPuppySymmetricArching::addRodsLeg(tgStructure& s){
102  s.addPair(0,1,"rod");
103  s.addPair(1,2,"rod");
104  s.addPair(1,3,"rod");
105  s.addPair(1,4,"rod");
106  s.addPair(0,5,"rod");
107 }
108 
109 void BigPuppySymmetricArching::addNodesHip(tgStructure& s, double r){
110  s.addNode(0,0,0); //Node 0
111  s.addNode(0,r,r); //Node 1
112  s.addNode(0,-r,-r); //Node 2
113  s.addNode(0,-r,r); //Node 3
114 }
115 
116 void BigPuppySymmetricArching::addRodsHip(tgStructure& s){
117  s.addPair(0,1,"rod");
118  s.addPair(0,2,"rod");
119  s.addPair(0,3,"rod");
120 }
121 
122 void BigPuppySymmetricArching::addNodesVertebra(tgStructure& s, double r){
123  s.addNode(0,0,0); //Node 0
124  s.addNode(r,0,r); //Node 1
125  s.addNode(r,0,-r); //Node 2
126  s.addNode(-r,0,-r); //Node 3
127  s.addNode(-r,0,r); //Node 4
128 }
129 
130 void BigPuppySymmetricArching::addRodsVertebra(tgStructure& s){
131  s.addPair(0,1,"rod");
132  s.addPair(0,2,"rod");
133  s.addPair(0,3,"rod");
134  s.addPair(0,4,"rod");
135 }
136 
137 void BigPuppySymmetricArching::addSegments(tgStructure& puppy, tgStructure& vertebra, tgStructure& hip, tgStructure& leg, tgStructure& foot,
138  double r){
139  const double offsetDist = r+1;
140  const double offsetDist2 = offsetDist*6;
141  const double offsetDist3 = offsetDist2+2;
142  const double yOffset_leg = -(2*r+1);
143  const double yOffset_foot = -(2*r+6);
144 
145  //Vertebrae
146  btVector3 offset(offsetDist,0.0,0);
147  //Hips
148  btVector3 offset1(offsetDist*2,0.0,offsetDist);
149  btVector3 offset2(offsetDist2,0.0,offsetDist);
150  btVector3 offset3(offsetDist*2,0.0,-offsetDist);
151  btVector3 offset4(offsetDist2,0.0,-offsetDist);
152  //Lower legs
153  btVector3 offset5(offsetDist3,yOffset_leg,offsetDist);
154  btVector3 offset6(offsetDist3,yOffset_leg,-offsetDist);
155  btVector3 offset7(r*2,yOffset_leg,offsetDist);
156  btVector3 offset8(r*2,yOffset_leg,-offsetDist);
157  //Feet
158  btVector3 offset9(offsetDist3+1,yOffset_foot,offsetDist);
159  btVector3 offset10(offsetDist3+1,yOffset_foot,-offsetDist);
160  btVector3 offset11(r*2+1,yOffset_foot,offsetDist);
161  btVector3 offset12(r*2+1,yOffset_foot,-offsetDist);
162 
163  for(std::size_t i = 0; i < m_segments; i++) { //Connect segments for spine of puppy
164  tgStructure* t = new tgStructure (vertebra);
165  t->addTags(tgString("spine segment num", i + 1));
166  t->move((i + 1)*offset);
167 
168  if (i % 2 == 1){
169 
170  t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), 0.0);
171 
172  }
173  else{
174 
175  t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), M_PI/2.0);
176 
177  }
178 
179  puppy.addChild(t); //Add a segment to the puppy
180  }
181 
182  for(std::size_t i = m_segments; i < (m_segments + 2); i++) {//deal with right hip and shoulder first
183  tgStructure* t = new tgStructure (hip);
184  t->addTags(tgString("segment num", i + 1));
185 
186  if(i % 2 == 0){
187  t->move(offset2);
188  t->addRotation(btVector3(offsetDist2, 0.0, offsetDist), btVector3(0, 1, 0), M_PI);
189 
190  }
191  else{
192  t->move(offset1);
193  t->addRotation(btVector3(offsetDist*2, 0.0, offsetDist), btVector3(0, 1, 0), M_PI);
194  }
195 
196  puppy.addChild(t); //Add a segment to the puppy
197  }
198 
199  for(std::size_t i = (m_segments + 2); i < (m_segments + m_hips); i++) {//deal with left hip and shoulder now
200  tgStructure* t = new tgStructure (hip);
201  t->addTags(tgString("segment num", i + 1));
202 
203  if(i % 2 == 0){
204  t->move(offset4);
205  }
206  else{
207  t->move(offset3);
208  }
209 
210  puppy.addChild(t); //Add a segment to the puppy
211 
212  }
213 
214  for(std::size_t i = (m_segments + m_hips); i < (m_segments + m_hips + 2); i++) {//right front and back legs
215  tgStructure* t = new tgStructure (leg);
216  t->addTags(tgString("segment num", i + 1));
217 
218  if(i % 2 == 0){
219  t->move(offset5);
220  t->addRotation(btVector3(offsetDist3, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI);
221 
222  }
223  else{
224  t->move(offset7);
225  t->addRotation(btVector3(r*2, yOffset_leg, offsetDist), btVector3(0, 1, 0), M_PI);
226  //the rotations for the legs are a remnant of the earlier design. Removing them now
227  //would mean changing all my muscle attachments. I will do this someday.
228 
229  }
230 
231  puppy.addChild(t); //Add a segment to the puppy
232  }
233 
234  for(std::size_t i = (m_segments + m_hips + 2); i < (m_segments + m_hips + m_legs); i++) {//left front and back legs
235  tgStructure* t = new tgStructure (leg);
236  t->addTags(tgString("segment num", i + 1));
237 
238  if(i % 2 == 0){
239  t->move(offset6);
240  t->addRotation(btVector3(offsetDist3, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI);
241 
242  }
243  else{
244  t->move(offset8);
245  t->addRotation(btVector3(r*2, yOffset_leg, -offsetDist), btVector3(0, 1, 0), M_PI);
246  }
247 
248  puppy.addChild(t); //Add a segment to the puppy
249  }
250 
251  for(std::size_t i = (m_segments + m_hips + m_legs); i < (m_segments + m_hips + m_legs + 2); i++) {//right front and back feet
252  tgStructure* t = new tgStructure (foot);
253  t->addTags(tgString("segment num", i + 1));
254 
255  if(i % 2 == 0){
256  t->move(offset9);
257  t->addRotation(btVector3(offsetDist3+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0);
258 
259  }
260  else{
261  t->move(offset11);
262  t->addRotation(btVector3(r*2+1, yOffset_foot, offsetDist), btVector3(0, 1, 0), 0.0);
263  }
264 
265  puppy.addChild(t); //Add a segment to the puppy
266  }
267 
268  for(std::size_t i = (m_segments + m_hips + m_legs + 2); i < (m_segments + m_hips + m_legs + m_feet); i++) {//left front and back feet
269  tgStructure* t = new tgStructure (foot);
270  t->addTags(tgString("segment num", i + 1));
271 
272  if(i % 2 == 0){
273  t->move(offset10);
274  t->addRotation(btVector3(offsetDist3+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0);
275 
276  }
277  else{
278  t->move(offset12);
279  t->addRotation(btVector3(r*2+1, yOffset_foot, -offsetDist), btVector3(0, 1, 0), 0.0);
280  }
281 
282  puppy.addChild(t); //Add a segment to the puppy
283  }
284 }
285 
286 void BigPuppySymmetricArching::addMuscles(tgStructure& puppy){
287  //Time to add the muscles to the structure. Todo: try to clean this up some more.
288  std::vector<tgStructure*> children = puppy.getChildren();
289  for(std::size_t i = 2; i < (children.size() - (m_hips + m_legs + m_feet)); i++) {
290 
291  tgNodes n0 = children[i-2]->getNodes();
292  tgNodes n1 = children[i-1]->getNodes();
293  tgNodes n2 = children[i]->getNodes();
294 
295 
296  if(i==2){
297  //Extra muscles, to keep front vertebra from swinging.
298  puppy.addPair(n0[3], n1[3], tgString("spine main front upper right muscleAct seg", i-2) + tgString(" seg", i-1));
299  puppy.addPair(n0[3], n1[4], tgString("spine main front upper left muscleAct seg", i-2) + tgString(" seg", i-1));
300 
301  puppy.addPair(n0[4], n1[3], tgString("spine main front lower right muscleAct seg", i-2) + tgString(" seg", i-1));
302  puppy.addPair(n0[4], n1[4], tgString("spine main front lower left muscleAct seg", i-2) + tgString(" seg", i-1));
303 
304 
305  }
306 
307  //Add muscles to the puppy
308  if(i < 3){
309  if(i % 2 == 0){ //front
310  puppy.addPair(n0[1], n1[3], tgString("spine main front lower right muscleAct seg", i-2) + tgString(" seg", i-1));
311  puppy.addPair(n0[1], n1[4], tgString("spine main front lower left muscleAct seg", i-2) + tgString(" seg", i-1));
312  puppy.addPair(n0[2], n1[3], tgString("spine main front upper right muscleAct seg", i-2) + tgString(" seg", i-1));
313  puppy.addPair(n0[2], n1[4], tgString("spine main front upper left muscleAct seg", i-2) + tgString(" seg", i-1));
314  }
315  else{ //rear
316  puppy.addPair(n0[1], n1[3], tgString("spine main rear upper left muscleAct seg", i-2) + tgString(" seg", i-1));
317  puppy.addPair(n0[1], n1[4], tgString("spine main rear lower left muscleAct seg", i-2) + tgString(" seg", i-1));
318  puppy.addPair(n0[2], n1[3], tgString("spine main rear upper right muscleAct seg", i-2) + tgString(" seg", i-1));
319  puppy.addPair(n0[2], n1[4], tgString("spine main rear lower right muscleAct seg", i-2) + tgString(" seg", i-1));
320  }
321  }
322  if(i < 7){//Was 6
323  if(i % 2 == 0){
324  puppy.addPair(n0[1], n2[4], tgString("spine2 bottom muscleAct seg", i-2) + tgString(" seg", i-1));
325  puppy.addPair(n0[2], n2[3], tgString("spine2 top muscleAct seg", i-2) + tgString(" seg", i-1));
326  }
327  else{
328  puppy.addPair(n0[1], n2[4], tgString("spine2 lateral left muscleAct seg", i-2) + tgString(" seg", i-1));
329  puppy.addPair(n0[2], n2[3], tgString("spine2 lateral right muscleAct seg", i-2) + tgString(" seg", i-1));
330 
331  }
332  }
333  if(i > 0 && i < 7){
334  if(i % 2 == 0){//rear
335  puppy.addPair(n1[1], n2[3], tgString("spine main rear upper left muscleAct seg", i-1) + tgString(" seg", i));
336  puppy.addPair(n1[1], n2[4], tgString("spine main rear lower left muscleAct seg", i-1) + tgString(" seg", i));
337  puppy.addPair(n1[2], n2[3], tgString("spine main rear upper right muscleAct seg", i-1) + tgString(" seg", i));
338  puppy.addPair(n1[2], n2[4], tgString("spine main rear lower right muscleAct seg", i-1) + tgString(" seg", i));
339  }
340  else{//front
341 
342  puppy.addPair(n1[1], n2[3], tgString("spine main front lower right muscleAct seg", i-1) + tgString(" seg", i));
343  puppy.addPair(n1[1], n2[4], tgString("spine main front lower left muscleAct seg", i-1) + tgString(" seg", i));
344  puppy.addPair(n1[2], n2[3], tgString("spine main front upper right muscleAct seg", i-1) + tgString(" seg", i));
345  puppy.addPair(n1[2], n2[4], tgString("spine main front upper left muscleAct seg", i-1) + tgString(" seg", i));
346  }
347  }
348  if(i == 6){
349  //rear
350  puppy.addPair(n1[1], n2[2], tgString("spine rear lower left muscleAct seg", i-1) + tgString(" seg", i));
351  puppy.addPair(n1[2], n2[2], tgString("spine rear lower right muscleAct seg", i-1) + tgString(" seg", i));
352  puppy.addPair(n1[1], n2[1], tgString("spine rear upper left muscleAct seg", i-1) + tgString(" seg", i));
353  puppy.addPair(n1[2], n2[1], tgString("spine rear upper right muscleAct seg", i-1) + tgString(" seg", i));
354  }
355 
356  }
357 
358 
359  //Now add muscles to hips....
360  tgNodes n0 = children[0]->getNodes();
361  tgNodes n1 = children[1]->getNodes();
362  tgNodes n2 = children[2]->getNodes();
363  tgNodes n3 = children[3]->getNodes();
364  tgNodes n4 = children[4]->getNodes();
365  tgNodes n5 = children[5]->getNodes();
366  tgNodes n6 = children[6]->getNodes();
367  tgNodes n7 = children[7]->getNodes();
368  tgNodes n8 = children[8]->getNodes();
369  tgNodes n9 = children[9]->getNodes();
370  tgNodes n10 = children[10]->getNodes();
371  tgNodes n11 = children[11]->getNodes();
372  tgNodes n12 = children[12]->getNodes();
373  tgNodes n13 = children[13]->getNodes();
374  tgNodes n14 = children[14]->getNodes();
375 
376  //Adding long muscles to spine, for bending/arching:
377  puppy.addPair(n0[2], n6[3], tgString("spine secondary top arching muscleAct seg", 0) + tgString(" seg", 6)); //Change these to something other than "spine " or "spine2" when it's time to implement new code for them!
378  //puppy.addPair(n0[1], n6[4], tgString("spine bottom arching muscleAct seg", 0) + tgString(" seg", 6));
379  //puppy.addPair(n1[4], n5[1], tgString("spine right lateral arching muscleAct seg", 1) + tgString(" seg", 5));
380  //puppy.addPair(n1[3], n5[2], tgString("spine left lateral arching muscleAct seg", 1) + tgString(" seg", 5));
381 
382  //Left shoulder muscles
383  puppy.addPair(n7[1], n1[1], tgString("left shoulder rear upper muscleAct seg", 6) + tgString(" seg", 1));
384  puppy.addPair(n7[1], n1[4], tgString("left shoulder front upper muscleAct seg", 6) + tgString(" seg", 1));
385  puppy.addPair(n7[1], n0[2], tgString("left shoulder front top muscleAct seg", 6) + tgString(" seg", 0));
386  puppy.addPair(n7[1], n2[3], tgString("left shoulder rear top muscleAct seg", 6) + tgString(" seg", 2));
387 
388  puppy.addPair(n7[3], n1[1], tgString("left shoulder rear lower muscleAct seg", 6) + tgString(" seg", 1));
389  puppy.addPair(n7[3], n1[4], tgString("left shoulder front lower muscleAct seg", 6) + tgString(" seg", 1));
390  puppy.addPair(n7[3], n0[1], tgString("left shoulder front bottom muscleAct seg", 6) + tgString(" seg", 0));
391  puppy.addPair(n7[3], n2[4], tgString("left shoulder rear bottom muscleAct seg", 6) + tgString(" seg", 2));
392 
393  //Extra muscles, to move left shoulder forward and back:
394  puppy.addPair(n7[0], n1[1], tgString("left shoulder rear mid muscleAct seg", 6) + tgString(" seg", 1));
395  puppy.addPair(n7[0], n1[4], tgString("left shoulder front mid muscleAct seg", 6) + tgString(" seg", 1));
396 
397  //Left hip muscles
398  puppy.addPair(n8[1], n5[1], tgString("left hip rear upper muscleAct seg", 7) + tgString(" seg", 5));
399  puppy.addPair(n8[1], n5[4], tgString("left hip front upper muscleAct seg", 7) + tgString(" seg", 5));
400  puppy.addPair(n8[1], n4[2], tgString("left hip front top muscleAct seg", 7) + tgString(" seg", 4));
401  puppy.addPair(n8[1], n6[3], tgString("left hip rear top muscleAct seg", 7) + tgString(" seg", 4));
402 
403  puppy.addPair(n8[3], n5[1], tgString("left hip rear lower muscleAct seg", 7) + tgString(" seg", 5));
404  puppy.addPair(n8[3], n5[4], tgString("left hip front lower muscleAct seg", 7) + tgString(" seg", 5));
405  puppy.addPair(n8[3], n4[1], tgString("left hip front bottom muscleAct seg", 7) + tgString(" seg", 4));
406  puppy.addPair(n8[3], n6[4], tgString("left hip front bottom muscleAct seg", 7) + tgString(" seg", 6));
407 
408  //Extra muscles, to move left hip forward and back:
409  puppy.addPair(n8[0], n5[1], tgString("left hip rear mid muscleAct seg", 7) + tgString(" seg", 3)); //could also be n3[3]
410  puppy.addPair(n8[0], n5[4], tgString("left hip front mid muscleAct seg", 7) + tgString(" seg", 5));
411 
412  //Right shoulder muscles
413  puppy.addPair(n9[1], n1[2], tgString("right shoulder rear upper muscleAct seg", 8) + tgString(" seg", 1));
414  puppy.addPair(n9[1], n1[3], tgString("right shoulder front upper muscleAct seg", 8) + tgString(" seg", 1));
415  puppy.addPair(n9[1], n0[2], tgString("right shoulder front top muscleAct seg", 8) + tgString(" seg", 0));
416  puppy.addPair(n9[1], n2[3], tgString("right shoulder rear top muscleAct seg", 8) + tgString(" seg", 2));
417 
418  puppy.addPair(n9[3], n1[2], tgString("right shoulder rear lower muscleAct seg", 8) + tgString(" seg", 1));
419  puppy.addPair(n9[3], n1[3], tgString("right shoulder front lower muscleAct seg", 8) + tgString(" seg", 1));
420  puppy.addPair(n9[3], n0[1], tgString("right shoulder front bottom muscleAct seg", 8) + tgString(" seg", 0));
421  puppy.addPair(n9[3], n2[4], tgString("right shoulder rear bottom muscleAct seg", 8) + tgString(" seg", 2));
422 
423  //Extra muscles, to move right shoulder forward and back:
424  puppy.addPair(n9[0], n1[2], tgString("right shoulder rear mid muscleAct seg", 8) + tgString(" seg", 1));
425  puppy.addPair(n9[0], n1[3], tgString("right shoulder front mid muscleAct seg", 8) + tgString(" seg", 1));
426 
427  //Right hip muscles
428  puppy.addPair(n10[1], n5[2], tgString("right hip rear upper muscleAct seg", 9) + tgString(" seg", 5));
429  puppy.addPair(n10[1], n5[3], tgString("right hip front upper muscleAct seg", 9) + tgString(" seg", 5));
430  puppy.addPair(n10[1], n4[2], tgString("right hip front top muscleAct seg", 9) + tgString(" seg", 4));
431  puppy.addPair(n10[1], n6[3], tgString("right hip rear top muscleAct seg", 9) + tgString(" seg", 4));
432 
433  puppy.addPair(n10[3], n5[2], tgString("right hip rear lower muscleAct seg", 9) + tgString(" seg", 5));
434  puppy.addPair(n10[3], n5[3], tgString("right hip front lower muscleAct seg", 9) + tgString(" seg", 5));
435  puppy.addPair(n10[3], n4[1], tgString("right hip bottom muscleAct seg", 9) + tgString(" seg", 4));
436  puppy.addPair(n10[3], n6[4], tgString("right hip bottom muscleAct seg", 9) + tgString(" seg", 6));
437 
438  //Extra muscles, to move right hip forward and back:
439  puppy.addPair(n10[0], n5[2], tgString("right hip rear mid muscleAct seg", 9) + tgString(" seg", 3)); //could also be n3[3]
440  puppy.addPair(n10[0], n5[3], tgString("right hip front mid muscleAct seg", 9) + tgString(" seg", 5));
441 
442  //Leg/hip connections:
443 
444  //Left front leg/shoulder
445  puppy.addPair(n11[4], n7[3], tgString("right outer bicep muscle seg", 10) + tgString(" seg", 6));
446  puppy.addPair(n11[4], n7[2], tgString("right inner bicep muscle seg", 10) + tgString(" seg", 6));
447  puppy.addPair(n11[4], n1[4], tgString("right front abdomen connection muscle seg", 10) + tgString(" seg", 1));
448 
449  puppy.addPair(n11[3], n7[3], tgString("right outer tricep muscle seg", 10) + tgString(" seg", 6));
450  puppy.addPair(n11[3], n7[2], tgString("right inner tricep muscle seg", 10) + tgString(" seg", 6));
451 
452  puppy.addPair(n11[2], n7[3], tgString("right outer front tricep muscle seg", 10) + tgString(" seg", 6));
453  puppy.addPair(n11[2], n7[2], tgString("right inner front tricep muscle seg", 10) + tgString(" seg", 6));
454 
455  //Adding muscle to pull up on right front leg:
456  puppy.addPair(n11[4], n7[1], tgString("right mid bicep muscle3 seg", 10) + tgString(" seg", 6));
457 
458  //Right front leg/shoulder
459  puppy.addPair(n13[4], n9[2], tgString("left inner bicep muscle seg", 12) + tgString(" seg", 8));
460  puppy.addPair(n13[4], n9[3], tgString("left outer bicep muscle seg", 12) + tgString(" seg", 8));
461  puppy.addPair(n13[4], n1[3], tgString("left front abdomen connection muscle seg", 12) + tgString(" seg", 1)); //Was n1[2]
462 
463  puppy.addPair(n13[3], n9[2], tgString("left inner tricep muscle seg", 12) + tgString(" seg", 8));
464  puppy.addPair(n13[3], n9[3], tgString("left outer tricep muscle seg", 12) + tgString(" seg", 8));
465 
466  puppy.addPair(n13[2], n9[2], tgString("left inner front tricep muscle seg", 12) + tgString(" seg", 8));
467  puppy.addPair(n13[2], n9[3], tgString("left outer front tricep muscle seg", 12) + tgString(" seg", 8));
468 
469  //Adding muscle to pull up on left front leg:
470  puppy.addPair(n13[4], n9[1], tgString("left mid bicep muscle3 seg", 12) + tgString(" seg", 8));
471 
472  //Left rear leg/hip
473  puppy.addPair(n12[4], n8[3], tgString("right outer thigh muscle seg", 11) + tgString(" seg", 7));
474  puppy.addPair(n12[4], n8[2], tgString("right inner thigh muscle seg", 11) + tgString(" seg", 7));
475 
476  puppy.addPair(n12[4], n3[1],tgString("right rear abdomen connection muscle seg", 11) + tgString(" seg", 3));
477  puppy.addPair(n12[3], n5[1],tgString("right rear abdomen connection muscle seg", 11) + tgString(" seg", 5));
478 
479  puppy.addPair(n12[3], n8[3], tgString("right outer calf muscle seg", 11) + tgString(" seg", 7));
480  puppy.addPair(n12[3], n8[2], tgString("right inner calf muscle seg", 11) + tgString(" seg", 7));
481 
482  puppy.addPair(n12[2], n8[3], tgString("right outer front calf muscle seg", 11) + tgString(" seg", 7));
483  puppy.addPair(n12[2], n8[2], tgString("right inner front calf muscle seg", 11) + tgString(" seg", 7));
484 
485  //Adding muscle to pull rear right leg up:
486  puppy.addPair(n12[4], n8[1], tgString("right central thigh muscle3 seg", 11) + tgString(" seg", 7));
487 
488  //Right rear leg/hip
489  puppy.addPair(n14[4], n10[2], tgString("left inner thigh muscle seg", 13) + tgString(" seg", 9));
490  puppy.addPair(n14[4], n10[3], tgString("left outer thigh muscle seg", 13) + tgString(" seg", 9));
491 
492  puppy.addPair(n14[4], n3[2], tgString("left rear abdomen connection muscle seg", 13) + tgString(" seg", 3));
493  puppy.addPair(n14[3], n5[2], tgString("left rear abdomen connection muscle seg", 13) + tgString(" seg", 5));
494 
495  puppy.addPair(n14[3], n10[2], tgString("left inner calf muscle seg", 13) + tgString(" seg", 9));
496  puppy.addPair(n14[3], n10[3], tgString("left outer calf muscle seg", 13) + tgString(" seg", 9));
497 
498  puppy.addPair(n14[2], n10[2], tgString("left inner front calf muscle seg", 13) + tgString(" seg", 9));
499  puppy.addPair(n14[2], n10[3], tgString("left outer front calf muscle seg", 13) + tgString(" seg", 9));
500 
501  //Adding muscle to pull rear left leg up:
502  puppy.addPair(n14[4], n10[1], tgString("left central thigh muscle3 seg", 13) + tgString(" seg", 9));
503 
504  //Populate feet with muscles. Todo: think up names to differentiate each!
505  for(std::size_t i = (m_segments + m_hips + m_legs); i < children.size(); i++) {
506  tgNodes ni = children[i]->getNodes();
507  tgNodes ni4 = children[i-4]->getNodes();
508 
509  puppy.addPair(ni[0],ni[1],tgString("foot muscle seg", i));
510  puppy.addPair(ni[0],ni[3],tgString("foot muscle seg", i));
511  puppy.addPair(ni[1],ni[2],tgString("foot muscle seg", i));
512  puppy.addPair(ni[2],ni[3],tgString("foot muscle seg", i));
513  puppy.addPair(ni[0],ni[7],tgString("foot muscle2 seg", i));
514  puppy.addPair(ni[1],ni[4],tgString("foot muscle2 seg", i));
515  puppy.addPair(ni[2],ni[5],tgString("foot muscle2 seg", i));
516  puppy.addPair(ni[3],ni[6],tgString("foot muscle2 seg", i));
517  puppy.addPair(ni[4],ni[5],tgString("foot muscle2 seg", i));
518  puppy.addPair(ni[4],ni[7],tgString("foot muscle2 seg", i));
519  puppy.addPair(ni[5],ni[6],tgString("foot muscle2 seg", i));
520  puppy.addPair(ni[6],ni[7],tgString("foot muscle2 seg", i));
521 
522  //Connect feet to legs:
523  puppy.addPair(ni4[5],ni[0],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
524  puppy.addPair(ni4[5],ni[1],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
525  puppy.addPair(ni4[5],ni[2],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
526  puppy.addPair(ni4[5],ni[3],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
527 
528  puppy.addPair(ni4[0],ni[4],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
529  puppy.addPair(ni4[0],ni[5],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
530  puppy.addPair(ni4[0],ni[6],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
531  puppy.addPair(ni4[0],ni[7],tgString("foot muscle2 seg", i) + tgString(" seg", i-4));
532 
533  }
534 
535 }
536 
538 {
539  //Rod and Muscle configuration.
540  const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart...
541  const double radius = 0.5;
542  const double rod_space = 10.0;
543  const double rod_space2 = 8.0;
544  const double friction = 0.5;
545  const double rollFriction = 0.0;
546  const double restitution = 0.0;
547 
548  const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
549 
550  const double stiffness = 1000.0;
551  const double stiffnessPassive = 4000.0;
552  const double damping = .01*stiffness;
553  const double pretension = 0.0;
554  const bool history = true;
555  const double maxTens = 7000.0;
556  const double maxSpeed = 12.0;
557 
558  const double passivePretension = 1000;
559  const double passivePretension2 = 3500;
560  const double passivePretension3 = 3500;
561 
562 #ifdef USE_KINEMATIC
563 
564  const double mRad = 1.0;
565  const double motorFriction = 10.0;
566  const double motorInertia = 1.0;
567  const bool backDrivable = false;
568  #ifdef PASSIVE_STRUCTURE
569  tgKinematicActuator::Config motorConfig(2000, 20, passivePretension,
570  mRad, motorFriction, motorInertia, backDrivable,
571  history, maxTens, maxSpeed);
572  tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
573  mRad, motorFriction, motorInertia, backDrivable,
574  history, maxTens, maxSpeed);
575 
576  tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension,
577  mRad, motorFriction, motorInertia, backDrivable,
578  history, maxTens, maxSpeed);
579  tgKinematicActuator::Config motorConfigLegs(stiffnessPassive, damping, passivePretension3,
580  mRad, motorFriction, motorInertia, backDrivable,
581  history, maxTens, maxSpeed);
582  #else
583  tgKinematicActuator::Config motorConfigSpine(stiffness, damping, pretension,
584  mRad, motorFriction, motorInertia, backDrivable,
585  history, maxTens, maxSpeed);
586 
587  tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
588  mRad, motorFriction, motorInertia, backDrivable,
589  history, maxTens, maxSpeed);
590 
591  tgKinematicActuator::Config motorConfigFeet(stiffnessPassive, damping, passivePretension,
592  mRad, motorFriction, motorInertia, backDrivable,
593  history, maxTens, maxSpeed);
594  tgKinematicActuator::Config motorConfigLegs(stiffnessPassive, damping, passivePretension3,
595  mRad, motorFriction, motorInertia, backDrivable,
596  history, maxTens, maxSpeed);
597  #endif
598 
599 #else
600 
601  #ifdef PASSIVE_STRUCTURE
602  tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension);
603  tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2);
604  tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension);
605  tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3);
606 
607  #else
608  tgSpringCableActuator::Config muscleConfigSpine(stiffness, damping, pretension, history, maxTens, 2*maxSpeed);
609  tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2);
610  tgSpringCableActuator::Config muscleConfigFeet(stiffnessPassive, damping, passivePretension);
611  tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3);
612  #endif
613 
614 #endif
615 
616  //Foot:
617  tgStructure foot;
618  addNodesFoot(foot,rod_space,rod_space2);
619  addRodsFoot(foot);
620 
621  //Leg:
622  tgStructure leg;
623  addNodesLeg(leg,rod_space);
624  addRodsLeg(leg);
625 
626  //Create the basic unit of the puppy
627  tgStructure vertebra;
628  addNodesVertebra(vertebra,rod_space);
629  addRodsVertebra(vertebra);
630 
631  //Create the basic unit for the hips/shoulders.
632  tgStructure hip;
633  addNodesHip(hip,rod_space);
634  addRodsHip(hip);
635 
636  //Build the puppy
637  tgStructure puppy;
638 
639  const double yOffset_foot = -(2*rod_space+6);
640 
641  addSegments(puppy,vertebra,hip,leg,foot,rod_space); //,m_segments,m_hips,m_legs,m_feet
642 
643  puppy.move(btVector3(0.0,-yOffset_foot,0.0));
644 
645  addMuscles(puppy); //,m_segments,m_hips,m_legs,m_feet
646 
647  //Time to add the muscles to the structure. Todo: make this a function; also try to clean this up.
648  std::vector<tgStructure*> children = puppy.getChildren();
649 
650  // Create the build spec that uses tags to turn the structure into a real model
651  tgBuildSpec spec;
652  spec.addBuilder("rod", new tgRodInfo(rodConfig));
653 
654 #ifdef USE_KINEMATIC
655 
656  #ifdef PASSIVE_STRUCTURE
657  spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(motorConfig));
658  spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
659  spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet));
660  spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
661  #else
662  spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(motorConfigSpine));
663  spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
664  spec.addBuilder("muscle2 ", new tgKinematicContactCableInfo(motorConfigFeet));
665  spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
666 
667  #endif
668 
669 #else
670  #ifdef PASSIVE_STRUCTURE
671  spec.addBuilder("muscleAct", new tgBasicActuatorInfo(muscleConfig));
672  spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
673  spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet));
674  spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
675  #else
676  spec.addBuilder("muscleAct" , new tgBasicActuatorInfo(muscleConfigSpine));
677  spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
678  spec.addBuilder("muscle2 " , new tgBasicActuatorInfo(muscleConfigFeet));
679  spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
680  #endif
681 
682 #endif
683 
684 
685 
686  // Create your structureInfo
687  tgStructureInfo structureInfo(puppy, spec);
688 
689  // Use the structureInfo to build ourselves
690  structureInfo.buildInto(*this, world);
691 
692  // We could now use tgCast::filter or similar to pull out the
693  // models (e.g. muscles) that we want to control.
694  m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
695 
696  m_allSegments = this->find<tgModel> ("segment");
697 
698  // Actually setup the children, notify controller that the setup has finished
700 
701  children.clear();
702 }
703 
705 {
706  // Precondition
707  if (dt <= 0.0)
708  {
709  throw std::invalid_argument("dt is not positive");
710  }
711  else
712  {
713  // Notify observers (controllers) of the step so that they can take action
715  }
716 }
717 
719 {
721 }
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Implementing the Flemons quadruped model (roughly), but as a subclass of Brian's BaseSpineModelLearni...
virtual void setup(tgWorld &world)
Convenience function for combining strings with ints, mostly for naming structures.
virtual void step(double dt)
Definition of class tgBasicActuatorInfo.
Contains the definition of class tgSimulation.
Contains the definition of class tgModel.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
Contains the definition of class tgSimViewGraphics.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
virtual void setup(tgWorld &world)
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Contains the definition of class tgBasicActuator.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Contains the definition of class tgWorld $Id$.
Definition of class tgStructure.
Definition of class tgStructureInfo.
Contains the definition of class tgSimView.
Definition of class tgKinematicActuatorInfo.
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
Definition of class tgKinematicContactCableInfo.
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:170
void buildInto(tgModel &model, tgWorld &world)
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70