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"
39 #include "tgcreator/tgKinematicActuatorInfo.h"
40 #include "tgcreator/tgKinematicContactCableInfo.h"
41 #include "core/tgRod.h"
42 #include "core/tgSpringCableActuator.h"
43 #include "core/tgString.h"
44 #include "tgcreator/tgBuildSpec.h"
45 #include "tgcreator/tgBasicActuatorInfo.h"
46 #include "tgcreator/tgRodInfo.h"
47 #include "tgcreator/tgStructure.h"
48 #include "tgcreator/tgStructureInfo.h"
49 // The Bullet Physics library
50 #include "LinearMath/btVector3.h"
51 // The C++ Standard Library
52 #include <iostream>
53 #include <stdexcept>
54 
55 //#define USE_KINEMATIC
56 #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 
542 void BigPuppyRigidFeet::setup(tgWorld& world)
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
706  BaseSpineModelLearning::setup(world);
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
721  BaseSpineModelLearning::step(dt);
722  }
723 }
724 
725 void BigPuppyRigidFeet::teardown()
726 {
727  BaseSpineModelLearning::teardown();
728 }
tgStructure::getChildren
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
BigPuppyRigidFeet.h
Implementing the Flemons quadruped model (roughly), but as a subclass of Brian's BaseSpineModelLearni...
tgStructure::addChild
void addChild(tgStructure *child)
Definition: tgStructure.cpp:167
tgRodInfo.h
Definition of class tgRodInfo.
BigPuppyRigidFeet::setup
virtual void setup(tgWorld &world)
Definition: BigPuppyRigidFeet.cpp:542
tgString.h
Convenience function for combining strings with ints, mostly for naming structures.
BaseSpineModelLearning::setup
virtual void setup(tgWorld &world)
Definition: BaseSpineModelLearning.cpp:50
BigPuppyRigidFeet::step
virtual void step(double dt)
Definition: BigPuppyRigidFeet.cpp:711
tgBasicActuatorInfo.h
Definition of class tgBasicActuatorInfo.
tgSimulation.h
Contains the definition of class tgSimulation.
tgModel.h
Contains the definition of class tgModel.
tgStructure::addPair
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
tgSimViewGraphics.h
Contains the definition of class tgSimViewGraphics.
tgSpringCableActuator.h
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
tgStructure::addRotation
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Definition: tgStructure.cpp:120
tgBasicActuator.h
Contains the definition of class tgBasicActuator.
tgString
std::string tgString(std::string s, int i)
Definition: tgString.h:33
tgWorld.h
Contains the definition of class tgWorld $Id$.
tgStructure.h
Definition of class tgStructure.
tgStructureInfo.h
Definition of class tgStructureInfo.
tgSimView.h
Contains the definition of class tgSimView.
tgKinematicActuatorInfo.h
Definition of class tgKinematicActuatorInfo.
BigPuppyRigidFeet::teardown
virtual void teardown()
Definition: BigPuppyRigidFeet.cpp:725
tgRod.h
Contains the definition of class tgRod.
tgBuildSpec.h
Definition of class tgBuildSpec.
BaseSpineModelLearning::step
virtual void step(double dt)
Definition: BaseSpineModelLearning.cpp:71
tgKinematicContactCableInfo.h
Definition of class tgKinematicContactCableInfo.
tgModel::getDescendants
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:170
tgStructureInfo::buildInto
void buildInto(tgModel &model, tgWorld &world)
Definition: tgStructureInfo.cpp:277
tgStructure::addNode
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70