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