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