NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
BigPuppySpineOnly.cpp
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 "BigPuppySpineOnly.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"
41 #include "core/tgRod.h"
43 #include "core/tgString.h"
44 #include "tgcreator/tgBuildSpec.h"
46 #include "tgcreator/tgRodInfo.h"
47 #include "tgcreator/tgStructure.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 BigPuppySpineOnly::BigPuppySpineOnly(int segments, int hips) :
59 BaseSpineModelLearning(segments),
60 m_hips(hips)
61 {
62 }
63 
64 BigPuppySpineOnly::~BigPuppySpineOnly()
65 {
66 }
67 
68 void BigPuppySpineOnly::addNodesHip(tgStructure& s, double r){
69  s.addNode(0,0,0); //Node 0
70  s.addNode(0,r,r); //Node 1
71  s.addNode(0,-r,-r); //Node 2
72  s.addNode(0,-r,r); //Node 3
73  s.addNode(0,-r-2,-r-2); //node 4
74  //s.addNode(0,-r-2,r+2); //node 5
75 }
76 
77 void BigPuppySpineOnly::addRodsHip(tgStructure& s){
78  s.addPair(0,1,"rod");
79  s.addPair(0,2,"rod");
80  s.addPair(0,3,"rod");
81  s.addPair(2,4,"rod");
82  //s.addPair(3,5,"rod");
83 }
84 
85 void BigPuppySpineOnly::addNodesVertebra(tgStructure& s, double r){
86  s.addNode(0,0,0); //Node 0
87  s.addNode(r,0,r); //Node 1
88  s.addNode(r,0,-r); //Node 2
89  s.addNode(-r,0,-r); //Node 3
90  s.addNode(-r,0,r); //Node 4
91 }
92 
93 void BigPuppySpineOnly::addRodsVertebra(tgStructure& s){
94  s.addPair(0,1,"rod");
95  s.addPair(0,2,"rod");
96  s.addPair(0,3,"rod");
97  s.addPair(0,4,"rod");
98 }
99 
100 void BigPuppySpineOnly::addSegments(tgStructure& puppy, tgStructure& vertebra, tgStructure& hip, double r){
101  const double offsetDist = r+1;
102  const double offsetDist2 = offsetDist*6;
103  const double offsetDist3 = offsetDist2+2;
104 
105  //Vertebrae
106  btVector3 offset(offsetDist,0.0,0);
107  //Hips
108  btVector3 offset1(offsetDist*2,0.0,offsetDist);
109  btVector3 offset2(offsetDist2,0.0,offsetDist);
110  btVector3 offset3(offsetDist*2,0.0,-offsetDist);
111  btVector3 offset4(offsetDist2,0.0,-offsetDist);
112 
113 
114  for(std::size_t i = 0; i < m_segments; i++) { //Connect segments for spine of puppy
115  tgStructure* t = new tgStructure (vertebra);
116  t->addTags(tgString("spine segment num", i + 1));
117  t->move((i + 1)*offset);
118 
119  if (i % 2 == 1){
120 
121  t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), 0.0);
122 
123  }
124  else{
125 
126  t->addRotation(btVector3((i + 1) * offsetDist, 0.0, 0.0), btVector3(1, 0, 0), M_PI/2.0);
127 
128  }
129 
130  puppy.addChild(t); //Add a segment to the puppy
131  }
132 
133  for(std::size_t i = m_segments; i < (m_segments + 2); i++) {//deal with right hip and shoulder first
134  tgStructure* t = new tgStructure (hip);
135  t->addTags(tgString("segment num", i + 1));
136 
137  if(i % 2 == 0){
138  t->move(offset2);
139  t->addRotation(btVector3(offsetDist2, 0.0, offsetDist), btVector3(0, 1, 0), M_PI);
140 
141  }
142  else{
143  t->move(offset1);
144  t->addRotation(btVector3(offsetDist*2, 0.0, offsetDist), btVector3(0, 1, 0), M_PI);
145  }
146 
147  puppy.addChild(t); //Add a segment to the puppy
148  }
149 
150  for(std::size_t i = (m_segments + 2); i < (m_segments + m_hips); i++) {//deal with left hip and shoulder now
151  tgStructure* t = new tgStructure (hip);
152  t->addTags(tgString("segment num", i + 1));
153 
154  if(i % 2 == 0){
155  t->move(offset4);
156  }
157  else{
158  t->move(offset3);
159  }
160 
161  puppy.addChild(t); //Add a segment to the puppy
162 
163  }
164 }
165 
166 void BigPuppySpineOnly::addMuscles(tgStructure& puppy){
167  //Time to add the muscles to the structure. Todo: try to clean this up some more.
168  std::vector<tgStructure*> children = puppy.getChildren();
169  for(std::size_t i = 2; i < (children.size() - (m_hips)); i++) {
170 
171  tgNodes n0 = children[i-2]->getNodes();
172  tgNodes n1 = children[i-1]->getNodes();
173  tgNodes n2 = children[i]->getNodes();
174 
175 
176  if(i==2){
177  //Extra muscles, to keep front vertebra from swinging.
178  puppy.addPair(n0[3], n1[3], tgString("spine front upper right muscleAct seg", i-2) + tgString(" seg", i-1));
179  puppy.addPair(n0[3], n1[4], tgString("spine front upper left muscleAct seg", i-2) + tgString(" seg", i-1));
180 
181  puppy.addPair(n0[4], n1[3], tgString("spine front lower right muscleAct seg", i-2) + tgString(" seg", i-1));
182  puppy.addPair(n0[4], n1[4], tgString("spine front lower left muscleAct seg", i-2) + tgString(" seg", i-1));
183 
184 
185  }
186 
187  //Add muscles to the puppy
188  if(i < 3){
189  if(i % 2 == 0){ //front
190  puppy.addPair(n0[1], n1[3], tgString("spine front lower right muscleAct seg", i-2) + tgString(" seg", i-1));
191  puppy.addPair(n0[1], n1[4], tgString("spine front lower left muscleAct seg", i-2) + tgString(" seg", i-1));
192  puppy.addPair(n0[2], n1[3], tgString("spine front upper right muscleAct seg", i-2) + tgString(" seg", i-1));
193  puppy.addPair(n0[2], n1[4], tgString("spine front upper left muscleAct seg", i-2) + tgString(" seg", i-1));
194  }
195  else{ //rear
196  puppy.addPair(n0[1], n1[3], tgString("spine rear upper left muscleAct seg", i-2) + tgString(" seg", i-1));
197  puppy.addPair(n0[1], n1[4], tgString("spine rear lower left muscleAct seg", i-2) + tgString(" seg", i-1));
198  puppy.addPair(n0[2], n1[3], tgString("spine rear upper right muscleAct seg", i-2) + tgString(" seg", i-1));
199  puppy.addPair(n0[2], n1[4], tgString("spine rear lower right muscleAct seg", i-2) + tgString(" seg", i-1));
200  }
201  }
202  if(i < 7){//Was 6
203  if(i % 2 == 0){
204  puppy.addPair(n0[1], n2[4], tgString("spine2 bottom muscleAct seg", i-2) + tgString(" seg", i-1));
205  puppy.addPair(n0[2], n2[3], tgString("spine2 top muscleAct seg", i-2) + tgString(" seg", i-1));
206  }
207  else{
208  puppy.addPair(n0[1], n2[4], tgString("spine2 lateral left muscleAct seg", i-2) + tgString(" seg", i-1));
209  puppy.addPair(n0[2], n2[3], tgString("spine2 lateral right muscleAct seg", i-2) + tgString(" seg", i-1));
210 
211  }
212  }
213  if(i > 0 && i < 7){
214  if(i % 2 == 0){//rear
215  puppy.addPair(n1[1], n2[3], tgString("spine rear upper left muscleAct seg", i-1) + tgString(" seg", i));
216  puppy.addPair(n1[1], n2[4], tgString("spine rear lower left muscleAct seg", i-1) + tgString(" seg", i));
217  puppy.addPair(n1[2], n2[3], tgString("spine rear upper right muscleAct seg", i-1) + tgString(" seg", i));
218  puppy.addPair(n1[2], n2[4], tgString("spine rear lower right muscleAct seg", i-1) + tgString(" seg", i));
219  }
220  else{//front
221 
222  puppy.addPair(n1[1], n2[3], tgString("spine front lower right muscleAct seg", i-1) + tgString(" seg", i));
223  puppy.addPair(n1[1], n2[4], tgString("spine front lower left muscleAct seg", i-1) + tgString(" seg", i));
224  puppy.addPair(n1[2], n2[3], tgString("spine front upper right muscleAct seg", i-1) + tgString(" seg", i));
225  puppy.addPair(n1[2], n2[4], tgString("spine front upper left muscleAct seg", i-1) + tgString(" seg", i));
226  }
227  }
228  if(i == 6){
229  //rear
230  puppy.addPair(n1[1], n2[2], tgString("spine rear lower left muscleAct seg", i-1) + tgString(" seg", i));
231  puppy.addPair(n1[2], n2[2], tgString("spine rear lower right muscleAct seg", i-1) + tgString(" seg", i));
232  puppy.addPair(n1[1], n2[1], tgString("spine rear upper left muscleAct seg", i-1) + tgString(" seg", i));
233  puppy.addPair(n1[2], n2[1], tgString("spine rear upper right muscleAct seg", i-1) + tgString(" seg", i));
234  }
235 
236  }
237 
238 
239  //Now add muscles to hips....
240  tgNodes n0 = children[0]->getNodes();
241  tgNodes n1 = children[1]->getNodes();
242  tgNodes n2 = children[2]->getNodes();
243  tgNodes n3 = children[3]->getNodes();
244  tgNodes n4 = children[4]->getNodes();
245  tgNodes n5 = children[5]->getNodes();
246  tgNodes n6 = children[6]->getNodes();
247  tgNodes n7 = children[7]->getNodes();
248  tgNodes n8 = children[8]->getNodes();
249  tgNodes n9 = children[9]->getNodes();
250  tgNodes n10 = children[10]->getNodes();
251 
252 
253  //Left shoulder muscles
254  puppy.addPair(n7[1], n1[1], tgString("left shoulder rear upper muscleAct seg", 6) + tgString(" seg", 1));
255  puppy.addPair(n7[1], n1[4], tgString("left shoulder front upper muscleAct seg", 6) + tgString(" seg", 1));
256  puppy.addPair(n7[1], n0[2], tgString("left shoulder front top muscleAct seg", 6) + tgString(" seg", 0));
257  puppy.addPair(n7[1], n2[3], tgString("left shoulder rear top muscleAct seg", 6) + tgString(" seg", 2));
258 
259  puppy.addPair(n7[3], n1[1], tgString("left shoulder rear lower muscleAct seg", 6) + tgString(" seg", 1));
260  puppy.addPair(n7[3], n1[4], tgString("left shoulder front lower muscleAct seg", 6) + tgString(" seg", 1));
261  puppy.addPair(n7[3], n0[1], tgString("left shoulder front bottom muscleAct seg", 6) + tgString(" seg", 0));
262  puppy.addPair(n7[3], n2[4], tgString("left shoulder rear bottom muscleAct seg", 6) + tgString(" seg", 2));
263 
264  //Extra muscles, to move left shoulder forward and back:
265  puppy.addPair(n7[0], n1[1], tgString("left shoulder rear mid muscleAct seg", 6) + tgString(" seg", 1));
266  puppy.addPair(n7[0], n1[4], tgString("left shoulder front mid muscleAct seg", 6) + tgString(" seg", 1));
267 
268  //Left hip muscles
269  puppy.addPair(n8[1], n5[1], tgString("left hip rear upper muscleAct seg", 7) + tgString(" seg", 5));
270  puppy.addPair(n8[1], n5[4], tgString("left hip front upper muscleAct seg", 7) + tgString(" seg", 5));
271  puppy.addPair(n8[1], n4[2], tgString("left hip front top muscleAct seg", 7) + tgString(" seg", 4));
272  puppy.addPair(n8[1], n6[3], tgString("left hip rear top muscleAct seg", 7) + tgString(" seg", 4));
273 
274  puppy.addPair(n8[3], n5[1], tgString("left hip rear lower muscleAct seg", 7) + tgString(" seg", 5));
275  puppy.addPair(n8[3], n5[4], tgString("left hip front lower muscleAct seg", 7) + tgString(" seg", 5));
276  puppy.addPair(n8[3], n4[1], tgString("left hip front bottom muscleAct seg", 7) + tgString(" seg", 4));
277  puppy.addPair(n8[3], n6[4], tgString("left hip front bottom muscleAct seg", 7) + tgString(" seg", 6));
278 
279  //Extra muscles, to move left hip forward and back:
280  puppy.addPair(n8[0], n5[1], tgString("left hip rear mid muscleAct seg", 7) + tgString(" seg", 3)); //could also be n3[3]
281  puppy.addPair(n8[0], n5[4], tgString("left hip front mid muscleAct seg", 7) + tgString(" seg", 5));
282 
283  //Right shoulder muscles
284  puppy.addPair(n9[1], n1[2], tgString("right shoulder rear upper muscleAct seg", 8) + tgString(" seg", 1));
285  puppy.addPair(n9[1], n1[3], tgString("right shoulder front upper muscleAct seg", 8) + tgString(" seg", 1));
286  puppy.addPair(n9[1], n0[2], tgString("right shoulder front top muscleAct seg", 8) + tgString(" seg", 0));
287  puppy.addPair(n9[1], n2[3], tgString("right shoulder rear top muscleAct seg", 8) + tgString(" seg", 2));
288 
289  puppy.addPair(n9[3], n1[2], tgString("right shoulder rear lower muscleAct seg", 8) + tgString(" seg", 1));
290  puppy.addPair(n9[3], n1[3], tgString("right shoulder front lower muscleAct seg", 8) + tgString(" seg", 1));
291  puppy.addPair(n9[3], n0[1], tgString("right shoulder front bottom muscleAct seg", 8) + tgString(" seg", 0));
292  puppy.addPair(n9[3], n2[4], tgString("right shoulder rear bottom muscleAct seg", 8) + tgString(" seg", 2));
293 
294  //Extra muscles, to move right shoulder forward and back:
295  puppy.addPair(n9[0], n1[2], tgString("right shoulder rear mid muscleAct seg", 8) + tgString(" seg", 1));
296  puppy.addPair(n9[0], n1[3], tgString("right shoulder front mid muscleAct seg", 8) + tgString(" seg", 1));
297 
298  //Right hip muscles
299  puppy.addPair(n10[1], n5[2], tgString("right hip rear upper muscleAct seg", 9) + tgString(" seg", 5));
300  puppy.addPair(n10[1], n5[3], tgString("right hip front upper muscleAct seg", 9) + tgString(" seg", 5));
301  puppy.addPair(n10[1], n4[2], tgString("right hip front top muscleAct seg", 9) + tgString(" seg", 4));
302  puppy.addPair(n10[1], n6[3], tgString("right hip rear top muscleAct seg", 9) + tgString(" seg", 4));
303 
304  puppy.addPair(n10[3], n5[2], tgString("right hip rear lower muscleAct seg", 9) + tgString(" seg", 5));
305  puppy.addPair(n10[3], n5[3], tgString("right hip front lower muscleAct seg", 9) + tgString(" seg", 5));
306  puppy.addPair(n10[3], n4[1], tgString("right hip bottom muscleAct seg", 9) + tgString(" seg", 4));
307  puppy.addPair(n10[3], n6[4], tgString("right hip bottom muscleAct seg", 9) + tgString(" seg", 6));
308 
309  //Extra muscles, to move right hip forward and back:
310  puppy.addPair(n10[0], n5[2], tgString("right hip rear mid muscleAct seg", 9) + tgString(" seg", 3)); //could also be n3[3]
311  puppy.addPair(n10[0], n5[3], tgString("right hip front mid muscleAct seg", 9) + tgString(" seg", 5));
312 
313 }
314 
316 {
317  //Rod and Muscle configuration.
318  const double density = 4.2/300.0; //Note: this needs to be high enough or things fly apart...
319  const double radius = 0.5;
320  const double rod_space = 10.0;
321  const double rod_space2 = 8.0;
322  const double friction = 0.5;
323  const double rollFriction = 0.0;
324  const double restitution = 0.0;
325 
326  const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
327 
328  const double stiffness = 1000.0;
329  const double damping = .01*stiffness;
330  const double pretension = 0.0;
331  const bool history = true;
332  const double maxTens = 7000.0;
333  const double maxSpeed = 12.0;
334 
335  const double passivePretension = 1000; // 5 N. Todo: make this a #ifdef
336 
337 #ifdef USE_KINEMATIC
338 
339  const double mRad = 1.0;
340  const double motorFriction = 10.0;
341  const double motorInertia = 1.0;
342  const bool backDrivable = false;
343  #ifdef PASSIVE_STRUCTURE
344  tgKinematicActuator::Config motorConfig(3000, 30, passivePretension,
345  mRad, motorFriction, motorInertia, backDrivable,
346  history, maxTens, maxSpeed);
347  #else
348  tgKinematicActuator::Config motorConfigSpine(stiffness, damping, pretension,
349  mRad, motorFriction, motorInertia, backDrivable,
350  history, maxTens, maxSpeed);
351 
352  tgKinematicActuator::Config motorConfigOther(3000, damping*3, passivePretension,
353  mRad, motorFriction, motorInertia, backDrivable,
354  history, maxTens, maxSpeed);
355  #endif
356 
357 #else
358 
359  #ifdef PASSIVE_STRUCTURE
360  tgSpringCableActuator::Config muscleConfig(2000, 20, passivePretension);
361  #else
362  tgSpringCableActuator::Config muscleConfigSpine(stiffness, damping, pretension, history, maxTens, 2*maxSpeed);
363  tgSpringCableActuator::Config muscleConfigOther(3000, damping*3, passivePretension); //May need different configs for different muscles in legs/feet... but using one to start with.
364  #endif
365 
366 #endif
367 
368  //Create the basic unit of the puppy
369  tgStructure vertebra;
370  addNodesVertebra(vertebra,rod_space);
371  addRodsVertebra(vertebra);
372 
373  //Create the basic unit for the hips/shoulders.
374  tgStructure hip;
375  addNodesHip(hip,rod_space);
376  addRodsHip(hip);
377 
378  //Build the puppy
379  tgStructure puppy;
380 
381  //const double yOffset_foot = -(2*rod_space+6);
382 
383  addSegments(puppy,vertebra,hip,rod_space); //,m_segments,m_hips,m_legs,m_feet
384 
385  puppy.move(btVector3(0.0,12.0,0.0));
386 
387  addMuscles(puppy); //,m_segments,m_hips,m_legs,m_feet
388 
389  //Time to add the muscles to the structure. Todo: make this a function; also try to clean this up.
390  std::vector<tgStructure*> children = puppy.getChildren();
391 
392  // Create the build spec that uses tags to turn the structure into a real model
393  tgBuildSpec spec;
394  spec.addBuilder("rod", new tgRodInfo(rodConfig));
395 
396 #ifdef USE_KINEMATIC
397 
398  #ifdef PASSIVE_STRUCTURE
399  spec.addBuilder("muscle", new tgKinematicContactCableInfo(motorConfig));
400  #else
401  spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(motorConfigSpine));
402  spec.addBuilder("muscle ", new tgKinematicContactCableInfo(motorConfigOther));
403  #endif
404 
405 #else
406  #ifdef PASSIVE_STRUCTURE
407  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
408  #else
409  spec.addBuilder("muscleAct" , new tgBasicActuatorInfo(muscleConfigSpine));
410  spec.addBuilder("muscle " , new tgBasicActuatorInfo(muscleConfigOther));
411  #endif
412 
413 #endif
414 
415 
416 
417  // Create your structureInfo
418  tgStructureInfo structureInfo(puppy, spec);
419 
420  // Use the structureInfo to build ourselves
421  structureInfo.buildInto(*this, world);
422 
423  // We could now use tgCast::filter or similar to pull out the
424  // models (e.g. muscles) that we want to control.
425  m_allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
426 
427  m_allSegments = this->find<tgModel> ("spine segment");
428 
429  // Actually setup the children, notify controller that the setup has finished
431 
432  children.clear();
433 }
434 
435 void BigPuppySpineOnly::step(double dt)
436 {
437  // Precondition
438  if (dt <= 0.0)
439  {
440  throw std::invalid_argument("dt is not positive");
441  }
442  else
443  {
444  // Notify observers (controllers) of the step so that they can take action
446  }
447 }
448 
450 {
452 }
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Convenience function for combining strings with ints, mostly for naming structures.
virtual void teardown()
virtual void setup(tgWorld &world)
virtual void setup(tgWorld &world)
Definition of class tgBasicActuatorInfo.
Contains the definition of class tgSimulation.
Contains the definition of class tgModel.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
Contains the definition of class tgSimViewGraphics.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
virtual void step(double dt)
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Contains the definition of class tgBasicActuator.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Contains the definition of class tgWorld $Id$.
Definition of class tgStructure.
Definition of class tgStructureInfo.
Contains the definition of class tgSimView.
Definition of class tgKinematicActuatorInfo.
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
virtual void step(double dt)
Definition of class tgKinematicContactCableInfo.
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:170
void buildInto(tgModel &model, tgWorld &world)
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70