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