NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
MountainGoatFM0.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 "MountainGoatFM0.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 MountainGoatFM0::MountainGoatFM0(int segments, int hips, int legs) :
60 BaseQuadModelLearning(segments, hips),
61 m_legs(legs)
62 {
63  m_subStructures = segments + hips + legs;
64 }
65 
66 MountainGoatFM0::~MountainGoatFM0()
67 {
68 }
69 
70 void MountainGoatFM0::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 MountainGoatFM0::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 MountainGoatFM0::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 MountainGoatFM0::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 MountainGoatFM0::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 MountainGoatFM0::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 MountainGoatFM0::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 MountainGoatFM0::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 < m_segments){//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 < m_segments){
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 muscleAct1 seg", i-1) + tgString(" seg", i));
301  goat.addPair(n1[1], n2[4], tgString("spine all main front lower left muscleAct1 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 < m_segments){
307  goat.addPair(n0[4], n2[3], tgString("spine all vertical muscleAct1 seg", i-1) + tgString(" seg", i));
308  goat.addPair(n0[2], n2[1], tgString("spine all vertical muscleAct1 seg", i-1) + tgString(" seg", i));
309 
310  goat.addPair(n0[3], n2[4], tgString("spine all vertical muscleAct1 seg", i-1) + tgString(" seg", i));
311  goat.addPair(n0[1], n2[2], tgString("spine all vertical muscleAct1 seg", i-1) + tgString(" seg", i));
312  }
313  if(i == m_segments - 1){
314  //rear
315  goat.addPair(n1[1], n2[2], tgString("spine all rear lower left muscleAct2 seg", i-1) + tgString(" seg", i));
316  goat.addPair(n1[2], n2[2], tgString("spine all rear lower right muscleAct2 seg", i-1) + tgString(" seg", i));
317  goat.addPair(n1[1], n2[1], tgString("spine all rear upper left muscleAct1 seg", i-1) + tgString(" seg", i));
318  goat.addPair(n1[2], n2[1], tgString("spine all rear upper right muscleAct1 seg", i-1) + tgString(" seg", i));
319  }
320 
321  }
322 
323 
324  //Now add muscles to hips....
325  tgNodes n0 = children[0]->getNodes();
326  tgNodes n1 = children[1]->getNodes();
327  tgNodes n2 = children[2]->getNodes();
328  tgNodes n3 = children[3]->getNodes();
329  tgNodes n4 = children[4]->getNodes();
330  tgNodes n5 = children[5]->getNodes();
331  tgNodes n6 = children[6]->getNodes();
332  tgNodes n7 = children[7]->getNodes();
333  tgNodes n8 = children[8]->getNodes();
334  tgNodes n9 = children[9]->getNodes();
335  tgNodes n10 = children[10]->getNodes();
336  tgNodes n11 = children[11]->getNodes();
337  tgNodes n12 = children[12]->getNodes();
338  tgNodes n13 = children[13]->getNodes();
339  tgNodes n14 = children[14]->getNodes();
340 
341  //Adding long muscles to spine, for bending/arching:
342  //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!
343  //goat.addPair(n0[1], n6[4], tgString("spine bottom arching muscleAct seg", 0) + tgString(" seg", 6));
344  //goat.addPair(n1[4], n5[1], tgString("spine right lateral arching muscleAct seg", 1) + tgString(" seg", 5));
345  //goat.addPair(n1[3], n5[2], tgString("spine left lateral arching muscleAct seg", 1) + tgString(" seg", 5));
346 
347  //Left shoulder muscles
348  goat.addPair(n7[1], n1[1], tgString("all left_shoulder hip rear upper muscleAct1 seg", 7) + tgString(" seg", 1));
349  goat.addPair(n7[1], n1[4], tgString("all left_shoulder hip front upper muscleAct1 seg", 7) + tgString(" seg", 1));
350  goat.addPair(n7[1], n0[2], tgString("all left_shoulder hip front top muscleAct1 seg", 7) + tgString(" seg", 0));
351  goat.addPair(n7[1], n2[3], tgString("all left_shoulder hip rear top muscleAct1 seg", 7) + tgString(" seg", 2));
352 
353  goat.addPair(n7[3], n1[1], tgString("all left_shoulder hip rear lower muscleAct1 seg", 7) + tgString(" seg", 1));
354  goat.addPair(n7[3], n1[4], tgString("all left_shoulder hip front lower muscleAct1 seg", 7) + tgString(" seg", 1));
355  goat.addPair(n7[3], n0[1], tgString("all left_shoulder front bottom muscleAct1 seg", 7) + tgString(" seg", 0));
356  goat.addPair(n7[3], n2[4], tgString("all left_shoulder hip rear bottom muscleAct1 seg", 7) + tgString(" seg", 2));
357 
358  //Extra muscles, to move left shoulder forward and back:
359  goat.addPair(n7[0], n1[1], tgString("all left_shoulder hip rear mid muscleAct1 seg", 7) + tgString(" seg", 1));
360  goat.addPair(n7[0], n1[4], tgString("all left_shoulder hip front mid muscleAct1 seg", 7) + tgString(" seg", 1));
361 
362  //Left hip muscles
363  goat.addPair(n8[1], n5[1], tgString("all left_hip rear upper muscleAct1 seg", 8) + tgString(" seg", 5));
364  goat.addPair(n8[1], n5[4], tgString("all left_hip front upper muscleAct1 seg", 8) + tgString(" seg", 5));
365  goat.addPair(n8[1], n4[2], tgString("all left_hip front top muscleAct1 seg", 8) + tgString(" seg", 4));
366  goat.addPair(n8[1], n6[3], tgString("all left_hip rear top muscleAct1 seg", 8) + tgString(" seg", 6));
367 
368  goat.addPair(n8[3], n5[1], tgString("all left_hip rear lower muscleAct1 seg", 8) + tgString(" seg", 5));
369  goat.addPair(n8[3], n5[4], tgString("all left_hip front lower muscleAct1 seg", 8) + tgString(" seg", 5));
370  goat.addPair(n8[3], n4[1], tgString("all left_hip front bottom muscleAct1 seg", 8) + tgString(" seg", 4));
371  goat.addPair(n8[3], n6[4], tgString("all left_hip front bottom muscleAct1 seg", 8) + tgString(" seg", 6)); //all left_hip
372 
373  //Extra muscles, to move left hip forward and back:
374  goat.addPair(n8[0], n5[1], tgString("all left_hip rear mid muscleAct1 seg", 8) + tgString(" seg", 5));
375  goat.addPair(n8[0], n5[4], tgString("all left_hip front mid muscleAct1 seg", 8) + tgString(" seg", 5));
376 
377  //Right shoulder muscles
378  goat.addPair(n9[1], n1[2], tgString("all right_shoulder hip rear upper muscleAct1 seg", 9) + tgString(" seg", 1));
379  goat.addPair(n9[1], n1[3], tgString("all right_shoulder hip front upper muscleAct1 seg", 9) + tgString(" seg", 1));
380  goat.addPair(n9[1], n0[2], tgString("all right_shoulder hip front top muscleAct1 seg", 9) + tgString(" seg", 0));
381  goat.addPair(n9[1], n2[3], tgString("all right_shoulder hip rear top muscleAct1 seg", 9) + tgString(" seg", 2));
382 
383  goat.addPair(n9[3], n1[2], tgString("all right_shoulder hip rear lower muscleAct1 seg", 9) + tgString(" seg", 1));
384  goat.addPair(n9[3], n1[3], tgString("all right_shoulder hip front lower muscleAct1 seg", 9) + tgString(" seg", 1));
385  goat.addPair(n9[3], n0[1], tgString("all right_shoulder hip front bottom muscleAct1 seg", 9) + tgString(" seg", 0));
386  goat.addPair(n9[3], n2[4], tgString("all right_shoulder hip rear bottom muscleAct1 seg", 9) + tgString(" seg", 2));
387 
388  //Extra muscles, to move right shoulder forward and back:
389  goat.addPair(n9[0], n1[2], tgString("all right_shoulder hip rear mid muscleAct1 seg", 9) + tgString(" seg", 1));
390  goat.addPair(n9[0], n1[3], tgString("all right_shoulder hip front mid muscleAct1 seg", 9) + tgString(" seg", 1));
391 
392  //Right hip muscles
393  goat.addPair(n10[1], n5[2], tgString("all right_hip rear upper muscleAct1 seg", 10) + tgString(" seg", 5));
394  goat.addPair(n10[1], n5[3], tgString("all right_hip front upper muscleAct1 seg", 10) + tgString(" seg", 5));
395  goat.addPair(n10[1], n4[2], tgString("all right_hip front top muscleAct1 seg", 10) + tgString(" seg", 4)); //all right_hip
396  goat.addPair(n10[1], n6[3], tgString("all right_hip rear top muscleAct1 seg", 10) + tgString(" seg", 6));
397 
398  goat.addPair(n10[3], n5[2], tgString("all right_hip rear lower muscleAct1 seg", 10) + tgString(" seg", 5));
399  goat.addPair(n10[3], n5[3], tgString("all right_hip front lower muscleAct1 seg", 10) + tgString(" seg", 5));
400  goat.addPair(n10[3], n4[1], tgString("all right_hip bottom muscleAct1 seg", 10) + tgString(" seg", 4));
401  goat.addPair(n10[3], n6[4], tgString("all right_hip bottom muscleAct1 seg", 10) + tgString(" seg", 6));
402 
403  //Extra muscles, to move right hip forward and back:
404  goat.addPair(n10[0], n5[2], tgString("all right_hip rear mid muscleAct1 seg", 10) + tgString(" seg", 5));
405  goat.addPair(n10[0], n5[3], tgString("all right_hip front mid muscleAct1 seg", 10) + tgString(" seg", 5));
406 
407  //Leg/hip connections:
408 
409  //Left front leg/shoulder
410  goat.addPair(n11[4], n7[3], tgString("all left_foreleg outer bicep muscle seg", 11) + tgString(" seg", 7));
411  goat.addPair(n11[4], n7[2], tgString("all left_foreleg inner bicep muscle seg", 11) + tgString(" seg", 7));
412  goat.addPair(n11[4], n1[4], tgString("all left_foreleg front abdomen connection muscle seg", 11) + tgString(" seg", 1));
413  goat.addPair(n11[3], n1[1],tgString("all left_foreleg front abdomen connection muscle3 seg", 11) + tgString(" seg", 1)); //Active
414  goat.addPair(n11[2], n1[4],tgString("all left_foreleg rear abdomen connection muscle3 seg", 11) + tgString(" seg", 1)); //Active
415 
416  goat.addPair(n11[3], n7[3], tgString("all left_foreleg outer tricep muscle seg", 11) + tgString(" seg", 7));
417  goat.addPair(n11[3], n7[2], tgString("all left_foreleg inner tricep muscle seg", 11) + tgString(" seg", 7));
418 
419  goat.addPair(n11[2], n7[3], tgString("all left_foreleg outer front tricep muscle seg", 11) + tgString(" seg", 7));
420  goat.addPair(n11[2], n7[2], tgString("all left_foreleg inner front tricep muscle seg", 11) + tgString(" seg", 7));
421 
422  //Adding muscle to pull up on right front leg:
423  goat.addPair(n11[4], n7[1], tgString("all left_foreleg mid bicep muscle3 seg", 11) + tgString(" seg", 7)); //Active
424 
425  //Right front leg/shoulder
426  goat.addPair(n13[4], n9[2], tgString("all right_foreleg inner bicep muscle seg", 13) + tgString(" seg", 9));
427  goat.addPair(n13[4], n9[3], tgString("all right_foreleg outer bicep muscle seg", 13) + tgString(" seg", 9));
428  goat.addPair(n13[4], n1[3], tgString("all right_foreleg front abdomen connection muscle seg", 13) + tgString(" seg", 1));
429  goat.addPair(n13[3], n1[2], tgString("all right_foreleg front abdomen connection muscle3 seg", 13) + tgString(" seg", 1)); //Active
430  goat.addPair(n13[2], n1[3], tgString("all right_foreleg rear abdomen connection muscle3 seg", 13) + tgString(" seg", 1)); //Active
431 
432 
433  goat.addPair(n13[3], n9[2], tgString("all right_foreleg inner tricep muscle seg", 13) + tgString(" seg", 9));
434  goat.addPair(n13[3], n9[3], tgString("all right_foreleg outer tricep muscle seg", 13) + tgString(" seg", 9));
435 
436  goat.addPair(n13[2], n9[2], tgString("all right_foreleg inner front tricep muscle seg", 13) + tgString(" seg", 9));
437  goat.addPair(n13[2], n9[3], tgString("all right_foreleg outer front tricep muscle seg", 13) + tgString(" seg", 9));
438 
439  //Adding muscle to pull up on left front leg:
440  goat.addPair(n13[4], n9[1], tgString("all right_foreleg mid bicep muscle3 seg", 13) + tgString(" seg", 9)); //Active
441 
442  //Left rear leg/hip
443  goat.addPair(n12[4], n8[3], tgString("all left_hindleg outer thigh muscle seg", 12) + tgString(" seg", 8));
444  goat.addPair(n12[4], n8[2], tgString("inner thigh muscle seg", 12) + tgString(" seg", 8));
445 
446  goat.addPair(n12[4], n3[1],tgString("all left_hindleg rear abdomen connection muscle seg", 12) + tgString(" seg", 3));
447  goat.addPair(n12[3], n5[1],tgString("all left_hindleg front abdomen connection muscle3 seg", 12) + tgString(" seg", 5)); //Active
448  goat.addPair(n12[2], n5[4],tgString("all left_hindleg rear abdomen connection muscle3 seg", 12) + tgString(" seg", 5)); //Active
449 
450  goat.addPair(n12[3], n8[3], tgString("all left_hindleg outer calf muscle seg", 12) + tgString(" seg", 8));
451  goat.addPair(n12[3], n8[2], tgString("all left_hindleg inner calf muscle seg", 12) + tgString(" seg", 8));
452 
453  goat.addPair(n12[2], n8[3], tgString("all left_hindleg outer front calf muscle seg", 12) + tgString(" seg", 8));
454  goat.addPair(n12[2], n8[2], tgString("all left_hindleg inner front calf muscle seg", 12) + tgString(" seg", 8));
455 
456  //Adding muscle to pull rear right leg up:
457  goat.addPair(n12[4], n8[1], tgString("all left_hindleg central thigh muscle3 seg", 12) + tgString(" seg", 8)); //Active
458 
459  //Right rear leg/hip
460  goat.addPair(n14[4], n10[2], tgString("all right_hindleg inner thigh muscle seg", 14) + tgString(" seg", 10));
461  goat.addPair(n14[4], n10[3], tgString("all right_hindleg outer thigh muscle seg", 14) + tgString(" seg", 10));
462 
463  goat.addPair(n14[4], n3[2], tgString("all right_hindleg rear abdomen connection muscle seg", 14) + tgString(" seg", 3));
464  goat.addPair(n14[3], n5[2], tgString("all right_hindleg front abdomen connection muscle3 seg", 14) + tgString(" seg", 5));
465  goat.addPair(n14[2], n5[3], tgString("all right_hindleg rear abdomen connection muscle3 seg", 14) + tgString(" seg", 5));
466 
467 
468  goat.addPair(n14[3], n10[2], tgString("all right_hindleg inner calf muscle seg", 14) + tgString(" seg", 10));
469  goat.addPair(n14[3], n10[3], tgString("all right_hindleg outer calf muscle seg", 14) + tgString(" seg", 10));
470 
471  goat.addPair(n14[2], n10[2], tgString("all right_hindleg inner front calf muscle seg", 14) + tgString(" seg", 10));
472  goat.addPair(n14[2], n10[3], tgString("all right_hindleg outer front calf muscle seg", 14) + tgString(" seg", 10));
473 
474  //Adding muscle to pull rear left leg up:
475  goat.addPair(n14[4], n10[1], tgString("all right_hindleg central thigh muscle3 seg", 14) + tgString(" seg", 10));
476 
477 }
478 
479 void MountainGoatFM0::setup(tgWorld& world)
480 {
481  //Rod and Muscle configuration.
482  const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart...
483  const double radius = 0.5;
484  const double rod_space = 10.0;
485  const double rod_space2 = 8.0;
486  const double friction = 0.5;
487  const double rollFriction = 0.0;
488  const double restitution = 0.0;
489 
490  const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
491 
492  const double stiffness = 1000.0;
493  const double stiffnessPassive = 5000.0; //4000
494  const double stiffnessPassive2 = 4000.0;
495  const double stiffnessPassive3 = 10000.0;
496  const double damping = .01*stiffness;
497  const double pretension = 0.0;
498  const bool history = true;
499  const double maxTens = 7000.0;
500  const double maxSpeed = 12.0;
501 
502  const double passivePretension = 1000;
503  const double passivePretension2 = 3500;
504  const double passivePretension3 = 3500;
505  const double passivePretension4 = 5000.0;
506 
507 #ifdef USE_KINEMATIC
508 
509  const double mRad = 1.0;
510  const double motorFriction = 10.0;
511  const double motorInertia = 1.0;
512  const bool backDrivable = false;
513  #ifdef PASSIVE_STRUCTURE
514  tgKinematicActuator::Config motorConfig(stiffness, 20, passivePretension,
515  mRad, motorFriction, motorInertia, backDrivable,
516  history, maxTens, maxSpeed);
517  tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
518  mRad, motorFriction, motorInertia, backDrivable,
519  history, maxTens, maxSpeed);
520 
521  tgKinematicActuator::Config motorConfigStomach(stiffnessPassive2, damping, passivePretension4,
522  mRad, motorFriction, motorInertia, backDrivable,
523  history, maxTens, maxSpeed);
524  tgKinematicActuator::Config motorConfigLegs(stiffnessPassive3, damping, passivePretension3,
525  mRad, motorFriction, motorInertia, backDrivable,
526  history, maxTens, maxSpeed);
527  #else
528  tgKinematicActuator::Config motorConfigSpine(stiffness, damping, pretension,
529  mRad, motorFriction, motorInertia, backDrivable,
530  history, maxTens, maxSpeed);
531 
532  tgKinematicActuator::Config motorConfigOther(stiffnessPassive, damping, passivePretension2,
533  mRad, motorFriction, motorInertia, backDrivable,
534  history, maxTens, maxSpeed);
535 
536  tgKinematicActuator::Config motorConfigStomach(stiffnessPassive2, damping, passivePretension4,
537  mRad, motorFriction, motorInertia, backDrivable,
538  history, maxTens, maxSpeed);
539  tgKinematicActuator::Config motorConfigLegs(stiffnessPassive3, damping, passivePretension3,
540  mRad, motorFriction, motorInertia, backDrivable,
541  history, maxTens, maxSpeed);
542  #endif
543 
544 #else
545 
546  #ifdef PASSIVE_STRUCTURE
547  tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension);
548  tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2);
549  tgSpringCableActuator::Config muscleConfigStomach(stiffnessPassive2, damping, passivePretension4);
550  tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3);
551 
552  #else
553  tgSpringCableActuator::Config muscleConfigSpine(stiffness, damping, pretension, history, maxTens, 2*maxSpeed);
554  tgSpringCableActuator::Config muscleConfigOther(stiffnessPassive, damping, passivePretension2, history);
555  tgSpringCableActuator::Config muscleConfigStomach(stiffnessPassive2, damping, passivePretension4, history);
556  tgSpringCableActuator::Config muscleConfigLegs(stiffnessPassive, damping, passivePretension3, history);
557  #endif
558 
559 #endif
560 
561  //Leg:
562  tgStructure leg;
563  addNodesLeg(leg,rod_space);
564  addRodsLeg(leg);
565 
566  //Create the basic unit of the goat
567  tgStructure vertebra;
568  addNodesVertebra(vertebra,rod_space);
569  addRodsVertebra(vertebra);
570 
571  //Create the basic unit for the hips/shoulders.
572  tgStructure hip;
573  addNodesHip(hip,rod_space);
574  addRodsHip(hip);
575 
576  //Build the goat
577  tgStructure goat;
578 
579  const double yOffset_foot = -(2*rod_space+6);
580 
581  addSegments(goat,vertebra,hip,leg,rod_space); //,m_segments,m_hips,m_legs,m_feet
582 
583  goat.move(btVector3(0.0,-yOffset_foot,0.0));
584 
585  addMuscles(goat); //,m_segments,m_hips,m_legs,m_feet
586 
587  std::vector<tgStructure*> children = goat.getChildren();
588 
589  // Create the build spec that uses tags to turn the structure into a real model
590  tgBuildSpec spec;
591  spec.addBuilder("rod", new tgRodInfo(rodConfig));
592 
593 #ifdef USE_KINEMATIC
594 
595  #ifdef PASSIVE_STRUCTURE
596  spec.addBuilder("muscleAct1", new tgKinematicContactCableInfo(motorConfig));
597  spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
598  spec.addBuilder("muscleAct2 ", new tgKinematicContactCableInfo(motorConfigStomach));
599  spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
600  #else
601  spec.addBuilder("muscleAct1", new tgKinematicContactCableInfo(motorConfigSpine));
602  spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
603  spec.addBuilder("muscleAct2 ", new tgKinematicContactCableInfo(motorConfigStomach));
604  spec.addBuilder("muscle3 ", new tgKinematicContactCableInfo(motorConfigLegs));
605 
606  #endif
607 
608 #else
609  #ifdef PASSIVE_STRUCTURE
610  spec.addBuilder("muscleAct1", new tgBasicActuatorInfo(muscleConfig));
611  spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
612  spec.addBuilder("muscleAct2 " , new tgBasicActuatorInfo(muscleConfigStomach));
613  spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
614  #else
615  spec.addBuilder("muscleAct1" , new tgBasicActuatorInfo(muscleConfigSpine));
616  spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
617  spec.addBuilder("muscleAct2 " , new tgBasicActuatorInfo(muscleConfigStomach));
618  spec.addBuilder("muscle3 " , new tgBasicActuatorInfo(muscleConfigLegs));
619  #endif
620 
621 #endif
622 
623 
624 
625  // Create your structureInfo
626  tgStructureInfo structureInfo(goat, spec);
627 
628  // Use the structureInfo to build ourselves
629  structureInfo.buildInto(*this, world);
630 
631  // We could now use tgCast::filter or similar to pull out the
632  // models (e.g. muscles) that we want to control.
633  m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
634 
635  m_allSegments = this->find<tgModel> ("segment");
636 
637  // Actually setup the children, notify controller that the setup has finished
638  BaseQuadModelLearning::setup(world);
639 
640  children.clear();
641 }
642 
643 void MountainGoatFM0::step(double dt)
644 {
645  // Precondition
646  if (dt <= 0.0)
647  {
648  throw std::invalid_argument("dt is not positive");
649  }
650  else
651  {
652  // Notify observers (controllers) of the step so that they can take action
653  BaseQuadModelLearning::step(dt);
654  }
655 }
656 
657 void MountainGoatFM0::teardown()
658 {
659  BaseQuadModelLearning::teardown();
660 }
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
MountainGoatFM0::setup
virtual void setup(tgWorld &world)
Definition: MountainGoatFM0.cpp:479
tgBasicActuator.h
Contains the definition of class tgBasicActuator.
MountainGoatFM0::teardown
virtual void teardown()
Definition: MountainGoatFM0.cpp:657
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.
MountainGoatFM0::step
virtual void step(double dt)
Definition: MountainGoatFM0.cpp:643
tgSimView.h
Contains the definition of class tgSimView.
tgKinematicActuatorInfo.h
Definition of class tgKinematicActuatorInfo.
tgRod.h
Contains the definition of class tgRod.
tgBuildSpec.h
Definition of class tgBuildSpec.
MountainGoatFM0.h
Implementing MountainGoat, but as a subclass of Brian's BaseSpineModelLearning module. Seeing what will happen when the number of actuators is reduced, and their placement has changed.
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