NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
EscapeModel.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 "EscapeModel.h"
27 // This library
28 #include "core/tgBasicActuator.h"
29 #include "core/tgRod.h"
30 #include "tgcreator/tgBuildSpec.h"
32 #include "tgcreator/tgRodInfo.h"
33 #include "tgcreator/tgStructure.h"
35 // The Bullet Physics library
36 #include "LinearMath/btVector3.h"
37 // The C++ Standard Library
38 #include <stdexcept>
39 
40 namespace
41 {
42  // see tgBaseString.h for a descripton of some of these rod parameters
43  // (specifically, those related to the motor moving the strings.)
44  // NOTE that any parameter that depends on units of length will scale
45  // with the current gravity scaling. E.g., with gravity as 98.1,
46  // the length units below are in decimeters.
47 
48  // Note: This current model of the SUPERball rod is 1.5m long by 3 cm radius,
49  // which is 0.00424 m^3.
50  // For SUPERball v1.5, mass = 3.5kg per strut, which comes out to
51  // 0.825 kg / (decimeter^3).
52 
53  // similarly, frictional parameters are for the tgRod objects.
54  const struct Config
55  {
56  double density;
57  double radius;
58  //double stiffness_passive;
59  double stiffness;
60  double damping;
61  double rod_length;
62  double rod_space;
63  double friction;
64  double rollFriction;
65  double restitution;
66  bool hist;
67  double rotation;
68  double maxTens;
69  double targetVelocity;
70  } c =
71  {
72  0.40374, // density (kg / length^3)
73  0.40, // radius (length)
74  //998.25, // stiffness_passive (kg / sec^2)
75  3152.36, // stiffness_active (kg / sec^2)
76  50.0, // damping (kg / sec)
77  17.00, // rod_length (length)
78  17.00/4, // rod_space (length)
79  0.99, // friction (unitless)
80  0.01, // rollFriction (unitless)
81  0.0, // restitution (?)
82  0, // History logging (boolean)
83  0, // rotation
84  100000, // maxTens
85  10000, // targetVelocity
86 #if (0) // Acceleration limit removed 12/10/14
87  5000 // maxAcc
88 #endif
89  //100000, // maxTens
90  //10000, // targetVelocity
91  //20000 // maxAcc
92 
93  // Use the below values for earlier versions of simulation.
94  // 1.006,
95  // 0.31,
96  // 300000.0,
97  // 3000.0,
98  // 15.0,
99  // 7.5,
100  };
101 
102 } // namespace
103 
105 {
106 }
107 
109 {
110 }
111 
112 void EscapeModel::addNodes(tgStructure& s)
113 {
114  const double half_length = c.rod_length / 2;
115 
116  s.addNode(-c.rod_space, -half_length, 0); // 0
117  s.addNode(-c.rod_space, half_length, 0); // 1
118  s.addNode( c.rod_space, -half_length, 0); // 2
119  s.addNode( c.rod_space, half_length, 0); // 3
120  s.addNode(0, -c.rod_space, -half_length); // 4
121  s.addNode(0, -c.rod_space, half_length); // 5
122  s.addNode(0, c.rod_space, -half_length); // 6
123  s.addNode(0, c.rod_space, half_length); // 7
124  s.addNode(-half_length, 0, c.rod_space); // 8
125  s.addNode( half_length, 0, c.rod_space); // 9
126  s.addNode(-half_length, 0, -c.rod_space); // 10
127  s.addNode( half_length, 0, -c.rod_space); // 11
128 }
129 
130 void EscapeModel::addRods(tgStructure& s)
131 {
132  s.addPair( 0, 1, "rod");
133  s.addPair( 2, 3, "rod");
134  s.addPair( 4, 5, "rod");
135  s.addPair( 6, 7, "rod");
136  s.addPair( 8, 9, "rod");
137  s.addPair(10, 11, "rod");
138 }
139 
140 // 24 muscles in total
141 void EscapeModel::addMuscles(tgStructure& s)
142 {
143  s.addPair(0, 4, "muscle");
144  s.addPair(0, 5, "muscle");
145  s.addPair(0, 8, "muscle");
146  s.addPair(0, 10, "muscle");
147 
148  s.addPair(1, 6, "muscle");
149  s.addPair(1, 7, "muscle");
150  s.addPair(1, 8, "muscle");
151  s.addPair(1, 10, "muscle");
152 
153  s.addPair(2, 4, "muscle");
154  s.addPair(2, 5, "muscle");
155  s.addPair(2, 9, "muscle");
156  s.addPair(2, 11, "muscle");
157 
158  s.addPair(3, 7, "muscle");
159  s.addPair(3, 6, "muscle");
160  s.addPair(3, 9, "muscle");
161  s.addPair(3, 11, "muscle");
162 
163  s.addPair(4, 10, "muscle");
164  s.addPair(4, 11, "muscle");
165 
166  s.addPair(5, 8, "muscle");
167  s.addPair(5, 9, "muscle");
168 
169  s.addPair(6, 10, "muscle");
170  s.addPair(6, 11, "muscle");
171 
172  s.addPair(7, 8, "muscle");
173  s.addPair(7, 9, "muscle");
174 
175 }
176 
178 {
179 
180  const tgRod::Config rodConfig(c.radius, c.density, c.friction,
181  c.rollFriction, c.restitution);
182 
184  tgBasicActuator::Config activeMuscleConfig(c.stiffness, c.damping, c.hist, c.rotation,
185  c.maxTens, c.targetVelocity);
186 /*
187  tgBasicActuator::Config passiveMuscleConfig(c.stiffness_passive, c.damping, c.hist, c.rotation,
188  c.maxTens, c.targetVelocity,
189  c.maxAcc);*/
190 
191  // Start creating the structure
192  tgStructure s;
193  addNodes(s);
194  addRods(s);
195  addMuscles(s);
196  s.move(btVector3(0, 10, 0));
197 
198  // Add a rotation. This is needed if the ground slopes too much,
199  // otherwise glitches put a rod below the ground.
200  btVector3 rotationPoint = btVector3(0, 0, 0); // origin
201  btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis
202  double rotationAngle = M_PI/2;
203  s.addRotation(rotationPoint, rotationAxis, rotationAngle);
204 
205  // Create the build spec that uses tags to turn the structure into a real model
206  tgBuildSpec spec;
207  spec.addBuilder("rod", new tgRodInfo(rodConfig));
208  spec.addBuilder("muscle", new tgBasicActuatorInfo(activeMuscleConfig));
209  //spec.addBuilder("active muscle", new tgBasicActuatorInfo(activeMuscleConfig));
210  //spec.addBuilder("passive muscle", new tgBasicActuatorInfo(passiveMuscleConfig));
211 
212  // Create your structureInfo
213  tgStructureInfo structureInfo(s, spec);
214 
215  // Use the structureInfo to build ourselves
216  structureInfo.buildInto(*this, world);
217 
218  // We could now use tgCast::filter or similar to pull out the
219  // models (e.g. muscles) that we want to control.
220  allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants());
221 
222  // call the onSetup methods of all observed things e.g. controllers
223  notifySetup();
224 
225  // Actually setup the children
226  tgModel::setup(world);
227 }
228 
229 void EscapeModel::step(double dt)
230 {
231  // Precondition
232  if (dt <= 0.0)
233  {
234  throw std::invalid_argument("dt is not positive");
235  }
236  else
237  {
238  // Notify observers (controllers) of the step so that they can take action
239  notifyStep(dt);
240  tgModel::step(dt); // Step any children
241  }
242 }
243 
245 {
246  tgModel::onVisit(r);
247 }
248 
249 const std::vector<tgBasicActuator*>& EscapeModel::getAllMuscles() const
250 {
251  return allMuscles;
252 }
253 
255 {
256  notifyTeardown();
258 }
259 
260 // Return the center of mass of this model
261 // Pre-condition: This model has 6 rods
262 std::vector<double> EscapeModel::getBallCOM() {
263  std::vector <tgRod*> rods = find<tgRod>("rod");
264  assert(!rods.empty());
265 
266  btVector3 ballCenterOfMass(0, 0, 0);
267  double ballMass = 0.0;
268  for (std::size_t i = 0; i < rods.size(); i++) {
269  const tgRod* const rod = rods[i];
270  assert(rod != NULL);
271  const double rodMass = rod->mass();
272  const btVector3 rodCenterOfMass = rod->centerOfMass();
273  ballCenterOfMass += rodCenterOfMass * rodMass;
274  ballMass += rodMass;
275  }
276 
277  assert(ballMass > 0.0);
278  ballCenterOfMass /= ballMass;
279 
280  // Copy to the result std::vector
281  std::vector<double> result(3);
282  for (size_t i = 0; i < 3; ++i) { result[i] = ballCenterOfMass[i]; }
283  //std::cout<<"COM: (" << result[0] << ", " << result[1] << ", " << result[2] << ")\n";
284 
285  return result;
286 }
287 
288 
Contains the definition of class EscapeModel. $Id$.
virtual void teardown()
Definition: tgModel.cpp:68
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
Definition of class tgRodInfo.
virtual void step(double dt)
virtual void step(double dt)
Definition: tgModel.cpp:84
virtual btVector3 centerOfMass() const
Definition: tgBaseRigid.cpp:74
Definition of class tgBasicActuatorInfo.
virtual void onVisit(const tgModelVisitor &r) const
Definition: tgModel.cpp:107
void teardown()
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:66
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Definition: tgStructure.cpp:99
Contains the definition of class tgBasicActuator.
virtual void onVisit(tgModelVisitor &r)
Definition of class tgStructure.
const std::vector< tgBasicActuator * > & getAllMuscles() const
Definition of class tgStructureInfo.
virtual double mass() const
Definition: tgBaseRigid.h:65
virtual void setup(tgWorld &world)
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
virtual ~EscapeModel()
Definition: tgRod.h:43
std::vector< double > getBallCOM()
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