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