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