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