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