NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
T6Model.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 "T6Model.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  // Note: This current model of the SUPERball rod is 1.5m long by 4.5cm (0.045m) radius,
43  // which is 1.684m*pi*(0.040m)^2 = 0.008545132m^3.
44  // For SUPERball v1.5, mass = 3.5kg per strut, which density [kg/m^3] comes out to:
45  // density = 3.45kg/0.008545132m^3 = 403.738642402kg/m^3 = 0.403738642402 kg/dm^3
46 
47  // see tgBaseString.h for a descripton of some of these rod parameters
48  // (specifically, those related to the motor moving the strings.)
49  // NOTE that any parameter that depends on units of length will scale
50  // with the current gravity scaling. E.g., with gravity as 98.1
51  // Thus,
52  // Rod Length = 17.00
53  // Rod Radius = 0.40
54  // density = 0.40374 (length is cubed, so decimal moves 3 places)
55 
56  // similarly, frictional parameters are for the tgRod objects.
57  const struct Config
58  {
59  double density;
60  double radius;
61  double stiffnessA;
62  double stiffnessB;
63  double damping;
64  double rod_length;
65  double rod_space;
66  double friction;
67  double rollFriction;
68  double restitution;
69  double pretensionA;
70  double pretensionB;
71  bool hist;
72  double maxTens;
73  double targetVelocity;
74  } c =
75  {
76  0.40374, // density (kg / length^3)
77  0.40, // radius (length)
78  998.25, // stiffnessA (kg / sec^2), generally less stiff than B.
79  3152.36, // stiffnessB (kg / sec^2)
80  50.0, // damping (kg / sec)
81  17.00, // rod_length (length)
82  17.00/4, // rod_space (length)
83  0.99, // friction (unitless)
84  0.01, // rollFriction (unitless)
85  0.0, // restitution (?)
86  399.0, // pretensionA (force), from (0.4)*(998)
87  1261.0, // pretensionB (force), from (0.4)*(3152)
88  0, // History logging (boolean)
89  100000, // maxTens
90  10000 // targetVelocity
91 
92  // Use the below values for earlier versions of simulation.
93  // 1.152, // density (kg / length^3)
94  // 0.45, // radius (length)
95  // 613.0, // stiffness_passive (kg / sec^2)
96  // 2854.5, // stiffness_active (kg / sec^2)
97  };
98 } // namespace
99 
101 {
102 }
103 
105 {
106 }
107 
108 void T6Model::addNodes(tgStructure& s)
109 {
110  const double half_length = c.rod_length / 2;
111 
112  s.addNode(-c.rod_space, -half_length, 0); // 0
113  s.addNode(-c.rod_space, half_length, 0); // 1
114  s.addNode( c.rod_space, -half_length, 0); // 2
115  s.addNode( c.rod_space, half_length, 0); // 3
116  s.addNode(0, -c.rod_space, -half_length); // 4
117  s.addNode(0, -c.rod_space, half_length); // 5
118  s.addNode(0, c.rod_space, -half_length); // 6
119  s.addNode(0, c.rod_space, half_length); // 7
120  s.addNode(-half_length, 0, c.rod_space); // 8
121  s.addNode( half_length, 0, c.rod_space); // 9
122  s.addNode(-half_length, 0, -c.rod_space); // 10
123  s.addNode( half_length, 0, -c.rod_space); // 11
124 }
125 
126 void T6Model::addRods(tgStructure& s)
127 {
128  s.addPair( 0, 1, "rod");
129  s.addPair( 2, 3, "rod");
130  s.addPair( 4, 5, "rod");
131  s.addPair( 6, 7, "rod");
132  s.addPair( 8, 9, "rod");
133  s.addPair(10, 11, "rod");
134 }
135 
136 void T6Model::addMuscles(tgStructure& s)
137 {
138  // There are now two types of cables.
139  // cableA corresponds to the unactuated cables on SUPERball, "passive."
140  // cableB are the ones with motors attached, "active." (stiffer cables.)
141  s.addPair(0, 4, "cableB");
142  s.addPair(0, 5, "cableA");
143  s.addPair(0, 8, "cableB"); //muscle_active
144  s.addPair(0, 10, "cableA");
145 
146  s.addPair(1, 6, "cableB");
147  s.addPair(1, 7, "cableA");
148  s.addPair(1, 8, "cableA");
149  s.addPair(1, 10, "cableB");
150 
151  s.addPair(2, 4, "cableA");
152  s.addPair(2, 5, "cableB");
153  s.addPair(2, 9, "cableB");
154  s.addPair(2, 11, "cableA");
155 
156  s.addPair(3, 7, "cableB");
157  s.addPair(3, 6, "cableA");
158  s.addPair(3, 9, "cableA");
159  s.addPair(3, 11, "cableB");
160 
161  s.addPair(4, 10, "cableB");
162  s.addPair(4, 11, "cableA");
163 
164  s.addPair(5, 8, "cableB");
165  s.addPair(5, 9, "cableA");
166 
167  s.addPair(6, 10, "cableA");
168  s.addPair(6, 11, "cableB");
169 
170  s.addPair(7, 8, "cableA");
171  s.addPair(7, 9, "cableB");
172 
173 }
174 
175 void T6Model::setup(tgWorld& world)
176 {
177  // rod config: same for all rods on this model
178  const tgRod::Config rodConfig(c.radius, c.density, c.friction,
179  c.rollFriction, c.restitution);
180 
181  // cable configs for the two cables
182  // muscleConfigA is for cableA, the "passive" one on SUPERball
183  // @todo acceleration constraint deprecated, replace with tgKinematicActuator
184  tgBasicActuator::Config muscleConfigA(c.stiffnessA, c.damping, c.pretensionA,
185  c.hist, c.maxTens, c.targetVelocity);
186 
187  tgBasicActuator::Config muscleConfigB(c.stiffnessB, c.damping, c.pretensionB,
188  c.hist, c.maxTens, c.targetVelocity);
189 
190  // Start creating the structure
191  tgStructure s;
192  addNodes(s);
193  addRods(s);
194  addMuscles(s);
195  s.move(btVector3(0, 10, 0));
196 
197  // Add a rotation. This is needed if the ground slopes too much,
198  // otherwise glitches put a rod below the ground.
199  btVector3 rotationPoint = btVector3(0, 0, 0); // origin
200  btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis
201  double rotationAngle = M_PI/2;
202  s.addRotation(rotationPoint, rotationAxis, rotationAngle);
203 
204  // Create the build spec that uses tags to turn the structure into a real model
205  tgBuildSpec spec;
206  spec.addBuilder("rod", new tgRodInfo(rodConfig));
207  spec.addBuilder("cableA", new tgBasicActuatorInfo(muscleConfigA));
208  spec.addBuilder("cableB", new tgBasicActuatorInfo(muscleConfigB));
209 
210  // Create your structureInfo
211  tgStructureInfo structureInfo(s, spec);
212 
213  // Use the structureInfo to build ourselves
214  structureInfo.buildInto(*this, world);
215 
216  // We could now use tgCast::filter or similar to pull out the
217  // models (e.g. muscles) that we want to control.
218  // @todo check and confirm these calls actually work correctly.
219  allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants());
220 
221  passiveMuscles = tgCast::find<tgModel, tgBasicActuator>(tgTagSearch("cableA"),
222  getDescendants());
223  activeMuscles = tgCast::find<tgModel, tgBasicActuator>(tgTagSearch("cableB"),
224  getDescendants());
225 
226  // call the onSetup methods of all observed things e.g. controllers
227  notifySetup();
228 
229  // Actually setup the children
230  tgModel::setup(world);
231 }
232 
233 void T6Model::step(double dt)
234 {
235  // Precondition
236  if (dt <= 0.0)
237  {
238  throw std::invalid_argument("dt is not positive");
239  }
240  else
241  {
242  // Notify observers (controllers) of the step so that they can take action
243  notifyStep(dt);
244  tgModel::step(dt); // Step any children
245  }
246 }
247 
249 {
250  tgModel::onVisit(r);
251 }
252 
253 const std::vector<tgBasicActuator*>& T6Model::getAllMuscles() const
254 {
255  return allMuscles;
256 }
257 
258 const std::vector<tgBasicActuator*>& T6Model::getPassiveMuscles() const
259 {
260  return passiveMuscles;
261 }
262 
263 const std::vector<tgBasicActuator*>& T6Model::getActiveMuscles() const
264 {
265  return activeMuscles;
266 }
267 
268 const double T6Model::muscleRatio()
269 {
270  return (c.stiffnessA/c.stiffnessB);
271  //return 2.5;
272 }
273 
274 void T6Model::teardown()
275 {
277 }
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)
Definition: tgModel.cpp:84
virtual void onVisit(tgModelVisitor &r)
Definition: T6Model.cpp:242
const std::vector< tgBasicActuator * > & getAllMuscles() const
Definition: T6Model.cpp:247
const std::vector< tgBasicActuator * > & getPassiveMuscles() const
Definition: T6Model.cpp:252
Definition of class tgBasicActuatorInfo.
virtual void onVisit(const tgModelVisitor &r) const
Definition: tgModel.cpp:107
virtual void setup(tgWorld &world)
Definition: T6Model.cpp:174
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
void teardown()
Definition: T6Model.cpp:268
Contains the definition of class tgBasicActuator.
virtual void step(double dt)
Definition: T6Model.cpp:227
virtual ~T6Model()
Definition: T6Model.cpp:104
Definition of class tgStructure.
Definition of class tgStructureInfo.
const std::vector< tgBasicActuator * > & getActiveMuscles() const
Definition: T6Model.cpp:257
T6Model()
Definition: T6Model.cpp:100
virtual const double muscleRatio()
Definition: T6Model.cpp:262
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
void notifyStep(double dt)
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:170
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70