NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
SuperBallModel.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 
25 // This module
26 #include "SuperBallModel.h"
27 // This library
28 #include "core/tgBasicActuator.h"
29 #include "core/tgRod.h"
30 #include "core/abstractMarker.h"
31 #include "tgcreator/tgBuildSpec.h"
33 #include "tgcreator/tgRodInfo.h"
34 #include "tgcreator/tgStructure.h"
36 // The Bullet Physics library
37 #include "LinearMath/btVector3.h"
38 // The C++ Standard Library
39 #include <stdexcept>
40 #include "BulletDynamics/Dynamics/btRigidBody.h"
41 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
42 
43 namespace
44 {
45  // see tgBaseString.h for a descripton of some of these rod parameters
46  // (specifically, those related to the motor moving the strings.)
47  // NOTE that any parameter that depends on units of length will scale
48  // with the current gravity scaling. E.g., with gravity as 98.1,
49  // the length units below are in decimeters.
50 
51  // Note: This current model of the SUPERball rod is 1.5m long by 3 cm radius,
52  // which is 0.00424 m^3.
53  // For SUPERball v1.5, mass = 3.5kg per strut, which comes out to
54  // 0.825 kg / (decimeter^3).
55 
56  // similarly, frictional parameters are for the tgRod objects.
57  const struct Config
58  {
59  double density;
60  double radius;
61  double stiffness;
62  double damping;
63  double rod_length;
64  double rod_space;
65  double friction;
66  double rollFriction;
67  double restitution;
68  double pretension;
69  bool history;
70  double maxTens;
71  double targetVelocity;
72  } c =
73  {
74  0.825, // density (kg / length^3)
75  0.31, // radius (length)
76  3000.0, // stiffness (kg / sec^2)
77  200.0, // damping (kg / sec)
78  15.0, // rod_length (length)
79  7.5, // rod_space (length)
80  1.0, // friction (unitless)
81  0.01, // rollFriction (unitless)
82  0.2, // restitution (?)
83  0, // pretension
84  false, // history
85  100000, // maxTens
86  10000, // targetVelocity
87 #if (0) // acceleration constrant removed 12/10/14
88  20000 // maxAcc
89 #endif
90  // Use the below values for earlier versions of simulation.
91  // 1.006,
92  // 0.31,
93  // 300000.0,
94  // 3000.0,
95  // 15.0,
96  // 7.5,
97  };
98 } // namespace
99 
100 /*
101  * helper arrays for node and rod numbering schema
102  */
103 /*returns the number of the rod for a given node */
104 const int rodNumbersPerNode[13]={0,1,2,3,3,4,0,1,2,5,5,4,6};
105 
106 /*returns the node that is at the other end of the given node */
107 const int otherEndOfTheRod[13]={6,7,8,4,3,11,0,1,2,10,9,5,12};
108 
109 /*returns the node that is at the parallel rod
110  * and at the same end of the given node
111  */
112 const int parallelNode[13]={1,0,5,9,10,2,7,6,11,3,4,8,12};
113 
115 {
116  this->m_world=world;
117 }
118 
120 {
121 }
122 
123 //Node numbers seen from Front
124 // -----0-------1------
125 // ---------2----------
126 // 3------------------4
127 // ---------5----------
128 // -----6-------7------
129 //
130 //Node numbers seen from Back
131 // -----0-------1------
132 // ---------8----------
133 // 9-----------------10
134 // ---------11---------
135 // -----6-------7------
136 //
137 
138 
139 void SuperBallModel::addNodes(tgStructure& s)
140 {
141  const double half_length = c.rod_length / 2;
142 
143  nodePositions.push_back(btVector3(-half_length, c.rod_space, 0)); // 0
144  nodePositions.push_back(btVector3( half_length, c.rod_space, 0)); // 1
145  nodePositions.push_back(btVector3(0, half_length, -c.rod_space)); // 2
146  nodePositions.push_back(btVector3(-c.rod_space, 0, -half_length)); // 3
147  nodePositions.push_back(btVector3( c.rod_space, 0, -half_length)); // 4
148  nodePositions.push_back(btVector3(0, -half_length, -c.rod_space)); // 5
149  nodePositions.push_back(btVector3(-half_length, -c.rod_space, 0)); // 6
150  nodePositions.push_back(btVector3( half_length, -c.rod_space, 0)); // 7
151  nodePositions.push_back(btVector3(0, half_length, c.rod_space)); // 8
152  nodePositions.push_back(btVector3(-c.rod_space, 0, half_length)); // 9
153  nodePositions.push_back(btVector3( c.rod_space, 0, half_length)); // 10
154  nodePositions.push_back(btVector3(0, -half_length, c.rod_space)); // 11
155 
156  for(int i=0;i<12;i++)
157  {
158  s.addNode(nodePositions[i][0],nodePositions[i][1],nodePositions[i][2]);
159  }
160 }
161 
162 void SuperBallModel::addRods(tgStructure& s)
163 {
164  s.addPair( 0, 6, "r1 rod");
165  s.addPair( 1, 7, "r2 rod");
166  s.addPair( 2, 8, "r3 rod");
167  s.addPair( 3, 4, "r4 rod");
168  s.addPair( 5, 11, "r5 rod");
169  s.addPair( 9, 10, "r6 rod");
170 
171 }
172 
173 
174 void SuperBallModel::addMarkers()
175 {
176  std::vector<tgRod *> rods=find<tgRod>("rod");
177 
178  for(int i=0;i<12;i++)
179  {
180  const btRigidBody* bt = rods[rodNumbersPerNode[i]]->getPRigidBody();
181  btTransform inverseTransform = bt->getWorldTransform().inverse();
182  btVector3 pos = inverseTransform * (nodePositions[i]);
183  abstractMarker tmp=abstractMarker(bt,pos,btVector3(0.08*i,1.0 - 0.08*i,.0),i);
184  this->addMarker(tmp);
185  }
186 }
187 
188 void SuperBallModel::addSensors()
189 {
190  std::cout<<"Adding sensors"<<std::endl;
191  std::vector<tgRod *> rods=find<tgRod>("rod");
192 
193  for(int i=0;i<12;i++)
194  {
195  const btRigidBody* bt = rods[rodNumbersPerNode[i]]->getPRigidBody();
196  btTransform inverseTransform = bt->getWorldTransform().inverse();
197  btVector3 pos = inverseTransform * (nodePositions[i]);
198  heightSensor tmp=heightSensor(bt,pos,i,m_world);
199  this->heightSensors.push_back(tmp);
200  }
201 }
202 
203 std::vector<double> SuperBallModel::getSensorInfo()
204 {
205  std::vector<double> sensorInfo;
206  for(int i=0;i<heightSensors.size();i++)
207  {
208  sensorInfo.push_back(heightSensors.at(i).getHeight());
209  }
210 
211  return sensorInfo;
212 }
213 
214 void SuperBallModel::addMuscles(tgStructure& s)
215 {
216  musclesPerNodes.resize(13);
217  for(int i=0;i<13;i++)
218  {
219  musclesPerNodes[i].resize(13);
220  for(int j=0;j<13;j++)
221  musclesPerNodes[i][j]=NULL;
222  }
223  for(int i=0;i<13;i++)
224  for(int j=0;j<13;j++)
225  muscleConnections[i][j]=-1;
226 
227  muscleConnections[0][3]=0;
228  muscleConnections[3][2]=0;
229  muscleConnections[2][0]=0;
230  muscleConnections[4][5]=0;
231  muscleConnections[5][7]=0;
232  muscleConnections[7][4]=0;
233  muscleConnections[1][8]=0;
234  muscleConnections[8][10]=0;
235  muscleConnections[10][1]=0;
236  muscleConnections[9][11]=0;
237  muscleConnections[11][6]=0;
238  muscleConnections[6][9]=0;
239  muscleConnections[1][2]=1;
240  muscleConnections[2][4]=1;
241  muscleConnections[4][1]=1;
242  muscleConnections[3][5]=1;
243  muscleConnections[5][6]=1;
244  muscleConnections[6][3]=1;
245  muscleConnections[0][8]=1;
246  muscleConnections[8][9]=1;
247  muscleConnections[9][0]=1;
248  muscleConnections[11][7]=1;
249  muscleConnections[7][10]=1;
250  muscleConnections[10][11]=1;
251 
252  for(int i=0;i<13;i++)
253  {
254  for(int j=0;j<13;j++)
255  {
256  if(muscleConnections[i][j]>=0)
257  {
258  std::stringstream tag;
259  tag<<"muscle "<<i<<"-"<<j;
260  s.addPair(i, j, tag.str());
261  }
262  }
263  }
264 }
265 
266 
268 {
269 
270  const tgRod::Config rodConfig(c.radius, c.density, c.friction,
271  c.rollFriction, c.restitution);
273  tgBasicActuator::Config muscleConfig(c.stiffness, c.damping, c.pretension,
274  c.history, c.maxTens, c.targetVelocity);
275 
276  // Start creating the structure
277  tgStructure s;
278  addNodes(s);
279  addRods(s);
280  addMuscles(s);
281 
282 // // Add a rotation. This is needed if the ground slopes too much,
283 // // otherwise glitches put a rod below the ground.
284 // btVector3 rotationPoint = btVector3(0, 0, 0); // origin
285 // btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis
286 // double rotationAngle = M_PI/2;
287 // s.addRotation(rotationPoint, rotationAxis, rotationAngle);
288 
289  //s.move(btVector3(0,30,0));
290 
291  // Create the build spec that uses tags to turn the structure into a real model
292  tgBuildSpec spec;
293  spec.addBuilder("rod", new tgRodInfo(rodConfig));
294  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
295 
296  // Create your structureInfo
297  tgStructureInfo structureInfo(s, spec);
298 
299  // Use the structureInfo to build ourselves
300  structureInfo.buildInto(*this, world);
301 
302  // We could now use tgCast::filter or similar to pull out the
303  // models (e.g. muscles) that we want to control.
304  allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants());
305 
306  // call the onSetup methods of all observed things e.g. controllers
307  notifySetup();
308 
309  // Actually setup the children
310  tgModel::setup(world);
311 
312  this->m_world = world;
313 
314  //map the rods and add the markers to them
315  addMarkers();
316  addSensors();
317 
318  fillNodeNumberingSchema(6,11,9);
319  fillMusclesPerNode();
320 
321  //move rotate and give initial speed to the structure
322  btVector3 location(0,13.0,0);
323  btVector3 rotation(0.0,0.6,0.8);
324  btVector3 speed(0,10,0);
325  this->moveModel(location,rotation,speed);
326 
327 }
328 
329 void SuperBallModel::step(double dt)
330 {
331  // Precondition
332  if (dt <= 0.0)
333  {
334  throw std::invalid_argument("dt is not positive");
335  }
336  else
337  {
338  // Notify observers (controllers) of the step so that they can take action
339  notifyStep(dt);
340  tgModel::step(dt); // Step any children
341  }
342 }
343 
345 {
346  tgModel::onVisit(r);
347 }
348 
349 const std::vector<tgBasicActuator*>& SuperBallModel::getAllMuscles() const
350 {
351  return allMuscles;
352 }
353 
355 {
356  notifyTeardown();
357 
358  std::cout<<"Tearing down model"<<std::endl;
359  this->nodeNumberingSchema.clear();
360  this->heightSensors.clear();
361  this->nodePositions.clear();
362 
364 }
365 
366 void SuperBallModel::moveModel(btVector3 positionVector,btVector3 rotationVector,btVector3 speedVector)
367 {
368  std::vector<tgRod *> rods=find<tgRod>("rod");
369 
370  btQuaternion initialRotationQuat;
371  initialRotationQuat.setEuler(rotationVector[0],rotationVector[1],rotationVector[2]);
372  btTransform initialTransform;
373  initialTransform.setIdentity();
374  initialTransform.setRotation(initialRotationQuat);
375  initialTransform.setOrigin(positionVector);
376  for(int i=0;i<rods.size();i++)
377  {
378  rods[i]->getPRigidBody()->setLinearVelocity(speedVector);
379  rods[i]->getPRigidBody()->setWorldTransform(initialTransform * rods[i]->getPRigidBody()->getWorldTransform());
380  }
381 }
382 
383 std::vector< btVector3 > SuperBallModel::getSensorPositions()
384 {
385  std::vector<btVector3 > positions;
386  for(int i=0;i<heightSensors.size();i++)
387  {
388  positions.push_back(heightSensors[i].getWorldPosition());
389  }
390  return positions;
391 }
392 
393 std::vector< btVector3 > SuperBallModel::getSensorOrientations()
394 {
395  btVector3 temp;
396  std::vector<btVector3 > directions;
397  for(int i=0;i<12;i++)
398  {
399  temp = heightSensors[i].getWorldPosition() - heightSensors[otherEndOfTheRod[i]].getWorldPosition();
400  temp /= temp.length();
401  directions.push_back(temp);
402  }
403  return directions;
404 }
405 
406 
407 btVector3 SuperBallModel::getCenter()
408 {
409  btVector3 pos(0.0,0.0,0.0);
410  for(int i=0;i< heightSensors.size();i++)
411  {
412  pos += heightSensors[i].getWorldPosition();
413  }
414  pos /= heightSensors.size();
415  return pos;
416 }
417 
418 
419 btDynamicsWorld *SuperBallModel::getWorld()
420 {
421  return &(tgBulletUtil::worldToDynamicsWorld(this->m_world));
422 }
423 
424 void SuperBallModel::fillNodeNumberingSchema(int base1,int base2,int base3)
425 {
426  nodeNumberingSchema.resize(3);
427  for (int i = 0; i < 3; ++i)
428  {
429  nodeNumberingSchema[i].resize(2);
430  for (int j = 0; j < 2; ++j)
431  {
432  nodeNumberingSchema[i][j].resize(2);
433  for (int k=0;k<2;k++)
434  {
435  nodeNumberingSchema[i][j][k]=-1;
436  }
437  }
438  }
439 
440  nodeNumberingSchema[0][0][0]=base1;
441  nodeNumberingSchema[1][0][0]=base2;
442  nodeNumberingSchema[2][0][0]=base3;
443  //For each base node
444  for(int i=0;i<3;i++)
445  {
446  //Other end of the rod that this node is in
447  nodeNumberingSchema[i][0][1]=otherEndOfTheRod[nodeNumberingSchema[i][0][0]];
448  //Same end of the parallel rod
449  nodeNumberingSchema[i][1][0]=parallelNode[nodeNumberingSchema[i][0][0]];
450  //Other end of the parallel rod
451  nodeNumberingSchema[i][1][1]=otherEndOfTheRod[nodeNumberingSchema[i][1][0]];
452  }
453 }
454 
455 void SuperBallModel::fillNodeMappingFromBasePoints(int a,int b,int c)
456 {
457  for(int i=0;i<13;i++)
458  {
459  nodeMapping[i]=-1;
460  nodeMappingReverse[i]=-1;
461  }
462  nodeMapping[12]=12;
463 
464  nodeMapping[a]=nodeNumberingSchema[0][0][0];
465  nodeMapping[b]=nodeNumberingSchema[1][0][0];
466  nodeMapping[c]=nodeNumberingSchema[2][0][0];
467 
468  nodeMapping[otherEndOfTheRod[a]]=nodeNumberingSchema[0][0][1];
469  nodeMapping[otherEndOfTheRod[b]]=nodeNumberingSchema[1][0][1];
470  nodeMapping[otherEndOfTheRod[c]]=nodeNumberingSchema[2][0][1];
471 
472  nodeMapping[parallelNode[a]]=nodeNumberingSchema[0][1][0];
473  nodeMapping[parallelNode[b]]=nodeNumberingSchema[1][1][0];
474  nodeMapping[parallelNode[c]]=nodeNumberingSchema[2][1][0];
475 
476  nodeMapping[otherEndOfTheRod[parallelNode[a]]]=nodeNumberingSchema[0][1][1];
477  nodeMapping[otherEndOfTheRod[parallelNode[b]]]=nodeNumberingSchema[1][1][1];
478  nodeMapping[otherEndOfTheRod[parallelNode[c]]]=nodeNumberingSchema[2][1][1];
479 
480  for(int i=0;i<13;i++)
481  {
482 // cout<<"mapping old: "<<i<<" new: "<<nodeMapping[i]<<endl;
483  nodeMappingReverse[nodeMapping[i]]=i;
484  }
485 }
486 
487 
488 int SuperBallModel::getOtherEndOfTheRod(int i)
489 {
490  if(i>=0 && i<13)
491  return otherEndOfTheRod[i];
492  else
493  return -1;
494 }
495 
496 void SuperBallModel::fillMusclesPerNode()
497 {
498  for(int i=0;i<13;i++)
499  {
500  for(int j=0;j<13;j++)
501  {
502  if(muscleConnections[i][j]>=0)
503  {
504  std::stringstream tag;
505  tag<<"muscle "<<i<<"-"<<j;
506  std::vector<tgBasicActuator*> foundStr = this->find<tgBasicActuator>(tag.str());
507  if(!foundStr.empty())
508  {
509 // std::cout<<"Found muscle "<<tag.str()<<std::endl;
510  musclesPerNodes[i][j]=foundStr[0];
511  musclesPerNodes[j][i]=musclesPerNodes[i][j];
512  }
513  }
514  }
515  }
516 }
std::vector< double > getSensorInfo()
virtual void teardown()
Definition: tgModel.cpp:68
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
Definition of class tgRodInfo.
std::vector< btVector3 > getSensorOrientations()
static btDynamicsWorld & worldToDynamicsWorld(const tgWorld &world)
virtual void step(double dt)
Definition: tgModel.cpp:84
Contains the definition of class SuperBallModel. $Id$.
const std::vector< tgBasicActuator * > & getAllMuscles() const
Markers for specific places on a tensegrity.
Definition of class tgBasicActuatorInfo.
virtual void onVisit(const tgModelVisitor &r) const
Definition: tgModel.cpp:107
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:66
virtual void step(double dt)
Contains the definition of class tgBasicActuator.
Definition of class tgStructure.
virtual ~SuperBallModel()
Definition of class tgStructureInfo.
virtual void setup(tgWorld &world)
std::vector< btVector3 > getSensorPositions()
virtual void onVisit(tgModelVisitor &r)
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
SuperBallModel(tgWorld &world)
void notifyStep(double dt)
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:172
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:56