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