NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
T6SphereModel.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 
25 // This module
26 #include "T6SphereModel.h"
27 // This library
29 #include "core/tgRod.h"
30 #include "core/tgSphere.h"
31 #include "tgcreator/tgBuildSpec.h"
32 #include "tgcreator/tgRodInfo.h"
33 #include "tgcreator/tgSphereInfo.h"
34 #include "tgcreator/tgStructure.h"
37 // The Bullet Physics library
38 #include "LinearMath/btVector3.h"
39 // The C++ Standard Library
40 #include <stdexcept>
41 
42 namespace
43 {
44  // see tgBaseString.h for a descripton of some of these rod parameters
45  // (specifically, those related to the motor moving the strings.)
46  // NOTE that any parameter that depends on units of length will scale
47  // with the current gravity scaling. E.g., with gravity as 98.1,
48  // the length units below are in decimeters.
49 
50  // Note: This current model of the SUPERball rod is 1.5m long by 3 cm radius,
51  // which is 0.00424 m^3.
52  // For SUPERball v1.5, mass = 3.5kg per strut, which comes out to
53  // 0.825 kg / (decimeter^3).
54 
55  // similarly, frictional parameters are for the tgRod objects.
56  const struct Config
57  {
58  double density;
59  double radius;
60  double stiffness;
61  double damping;
62  double rod_length;
63  double rod_space;
64  double friction;
65  double rollFriction;
66  double restitution;
67  double pretension;
68  bool history;
69  double maxTens;
70  double targetVelocity;
71  double maxAcc;
72  } c =
73  {
74  0.688, // density (kg / length^3)
75  0.31, // radius (length)
76  613.0, // stiffness (kg / sec^2) was 1500
77  200.0, // damping (kg / sec)
78  16.84, // rod_length (length)
79  7.5, // rod_space (length)
80  0.99, // friction (unitless)
81  0.01, // rollFriction (unitless)
82  0.0, // restitution (?)
83  0.0, // pretension (force)
84  false, // history (boolean)
85  100000, // maxTens
86  10000, // targetVelocity
87  20000 // maxAcc
88 
89  // Use the below values for earlier versions of simulation.
90  // 1.006,
91  // 0.31,
92  // 300000.0,
93  // 3000.0,
94  // 15.0,
95  // 7.5,
96  };
97 } // namespace
98 
100 {
101 }
102 
104 {
105 }
106 
107 void T6SphereModel::addNodes(tgStructure& s)
108 {
109  const double half_length = c.rod_length / 2;
110 
111  s.addNode(-c.rod_space, -half_length, 0, "sphere1"); // 0
112  s.addNode(-c.rod_space, half_length, 0, "sphere2"); // 1
113  s.addNode( c.rod_space, -half_length, 0, "sphere1"); // 2
114  s.addNode( c.rod_space, half_length, 0, "sphere2"); // 3
115  s.addNode(0, -c.rod_space, -half_length, "sphere1"); // 4
116  s.addNode(0, -c.rod_space, half_length, "sphere1"); // 5
117  s.addNode(0, c.rod_space, -half_length, "sphere2"); // 6
118  s.addNode(0, c.rod_space, half_length, "sphere1"); // 7
119  s.addNode(-half_length, 0, c.rod_space, "sphere2"); // 8
120  s.addNode( half_length, 0, c.rod_space, "sphere1"); // 9
121  s.addNode(-half_length, 0, -c.rod_space, "sphere2"); // 10
122  s.addNode( half_length, 0, -c.rod_space, "sphere2"); // 11
123 }
124 
125 void T6SphereModel::addRods(tgStructure& s)
126 {
127  s.addPair( 0, 1, "rod");
128  s.addPair( 2, 3, "rod");
129  s.addPair( 4, 5, "rod");
130  s.addPair( 6, 7, "rod");
131  s.addPair( 8, 9, "rod");
132  s.addPair(10, 11, "rod");
133 }
134 
135 void T6SphereModel::addMuscles(tgStructure& s)
136 {
137  s.addPair(0, 4, "muscle");
138  s.addPair(0, 5, "muscle");
139  s.addPair(0, 8, "muscle");
140  s.addPair(0, 10, "muscle");
141 
142  s.addPair(1, 6, "muscle");
143  s.addPair(1, 7, "muscle");
144  s.addPair(1, 8, "muscle");
145  s.addPair(1, 10, "muscle");
146 
147  s.addPair(2, 4, "muscle");
148  s.addPair(2, 5, "muscle");
149  s.addPair(2, 9, "muscle");
150  s.addPair(2, 11, "muscle");
151 
152  s.addPair(3, 7, "muscle");
153  s.addPair(3, 6, "muscle");
154  s.addPair(3, 9, "muscle");
155  s.addPair(3, 11, "muscle");
156 
157  s.addPair(4, 2, "muscle");
158  s.addPair(4, 10, "muscle");
159  s.addPair(4, 11, "muscle");
160 
161  s.addPair(5, 8, "muscle");
162  s.addPair(5, 9, "muscle");
163 
164  s.addPair(6, 10, "muscle");
165  s.addPair(6, 11, "muscle");
166 
167  s.addPair(7, 8, "muscle");
168  s.addPair(7, 9, "muscleN");
169 
170 }
171 
173 {
174 
175  const tgRod::Config rodConfig(c.radius, c.density, c.friction,
176  c.rollFriction, c.restitution);
177 
178  tgSpringCableActuator::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.history,
179  c.maxTens, c.targetVelocity,
180  c.maxAcc);
181 
182  const tgSphere::Config sphereConfig(0.5, 0.25);
183 
184  const tgSphere::Config sphereConfig2(0.5, 2.5);
185 
186  // Start creating the structure
187  tgStructure s;
188  addNodes(s);
189  addRods(s);
190  addMuscles(s);
191  s.move(btVector3(0, 10, 0));
192 
193  // Add a rotation. This is needed if the ground slopes too much,
194  // otherwise glitches put a rod below the ground.
195  btVector3 rotationPoint = btVector3(0, 0, 0); // origin
196  btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis
197  double rotationAngle = M_PI/2;
198  s.addRotation(rotationPoint, rotationAxis, rotationAngle);
199 
200  // Create the build spec that uses tags to turn the structure into a real model
201  tgBuildSpec spec;
202  spec.addBuilder("rod", new tgRodInfo(rodConfig));
203  spec.addBuilder("muscle", new tgBasicContactCableInfo(muscleConfig));
204  //spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
205  //spec.addBuilder("sphere1", new tgSphereInfo(sphereConfig));
206  //spec.addBuilder("sphere2", new tgSphereInfo(sphereConfig));
207 
208  // Create your structureInfo
209  tgStructureInfo structureInfo(s, spec);
210 
211  // Use the structureInfo to build ourselves
212  structureInfo.buildInto(*this, world);
213 
214  // We could now use tgCast::filter or similar to pull out the
215  // models (e.g. muscles) that we want to control.
216  allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
217 
218  // call the onSetup methods of all observed things e.g. controllers
219  notifySetup();
220 
221  // Actually setup the children
222  tgModel::setup(world);
223 }
224 
225 void T6SphereModel::step(double dt)
226 {
227  // Precondition
228  if (dt <= 0.0)
229  {
230  throw std::invalid_argument("dt is not positive");
231  }
232  else
233  {
234  // Notify observers (controllers) of the step so that they can take action
235  notifyStep(dt);
236  tgModel::step(dt); // Step any children
237  }
238 }
239 
241 {
242  tgModel::onVisit(r);
243 }
244 
245 const std::vector<tgSpringCableActuator*>& T6SphereModel::getAllMuscles() const
246 {
247  return allMuscles;
248 }
249 
251 {
253 }
virtual void teardown()
Definition: tgModel.cpp:68
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
Definition of class tgRodInfo.
Definition of class tgSphereInfo.
Contains the definition of class T6SphereModel. $Id$.
virtual void onVisit(tgModelVisitor &r)
Contains the definition of class tgSphere.
virtual void step(double dt)
Definition: tgModel.cpp:84
const std::vector< tgSpringCableActuator * > & getAllMuscles() const
virtual void onVisit(const tgModelVisitor &r) const
Definition: tgModel.cpp:107
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
virtual ~T6SphereModel()
Definition of class tgBasicContactCableInfo.
Definition of class tgStructure.
virtual void setup(tgWorld &world)
virtual void step(double dt)
Definition of class tgStructureInfo.
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
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