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
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;
59  double stiffness_in;
60  double damping;
61  double damping_in;
62  double rod_length;
63  double rod_space;
64  double payload_h;
65  double density_pay;
66  double radius_pay;
67  double friction;
68  double rollFriction;
69  double restitution;
70  double pretension;
71  bool history;
72  double maxTens;
73  double targetVelocity;
74  } c =
75  {
76  0.5366, // density (kg / length^3)
77  0.3175/2.0, // radius (length)
78  250000.0, // stiffness of outer muscles (kg / sec^2)
79  500.0, // stiffness of inner muscles (kg/sec^2)
80  25000.0, // damping of outer muscles (kg / sec)
81  50.0, //damping of inner muscles (kg/sec)
82  15.0, // rod_length (length)
83  3.75, // rod_space (length)
84  0.5, // half payload height (length)
85  20/M_PI, //payload density (kg/lenght^3)
86  0.5, //payload radius (length)
87  1.0, // friction (unitless)
88  0.1, // rollFriction (unitless)
89  0.0, // restitution (?)
90  10.0, // pretension (force)
91  false, // history
92  1000000, // maxTens
93  10000 // targetVelocity
94 #if (0)
95  20000 // maxAcc
96 #endif // removed 12/10/14
97  };
98 } // namespace
99 
101 {
102  //data observer
103  // m_dataObserver("Data_test");
104 }
105 
107 {
108 }
109 
110 void T6Model::addNodes(tgStructure& s)
111 {
112  const double half_length = c.rod_length / 2;
113 
114  // Nodes for struts
115  s.addNode(-c.rod_space, -half_length, 0); // 0
116  s.addNode(-c.rod_space, half_length, 0); // 1
117  s.addNode( c.rod_space, -half_length, 0); // 2
118  s.addNode( c.rod_space, half_length, 0); // 3
119  s.addNode(0, -c.rod_space, -half_length); // 4
120  s.addNode(0, -c.rod_space, half_length); // 5
121  s.addNode(0, c.rod_space, -half_length); // 6
122  s.addNode(0, c.rod_space, half_length); // 7
123  s.addNode(-half_length, 0, c.rod_space); // 8
124  s.addNode( half_length, 0, c.rod_space); // 9
125  s.addNode(-half_length, 0, -c.rod_space); // 10
126  s.addNode( half_length, 0, -c.rod_space); // 11
127 
128  //Nodes for payload
129  s.addNode(0,c.payload_h,0);
130  s.addNode(0,-c.payload_h,0);
131 }
132 
133 void T6Model::addRods(tgStructure& s)
134 {
135  // Struts
136  s.addPair( 0, 1, "rod");
137  s.addPair( 2, 3, "rod");
138  s.addPair( 4, 5, "rod");
139  s.addPair( 6, 7, "rod");
140  s.addPair( 8, 9, "rod");
141  s.addPair(10, 11, "rod");
142 
143  // Payload
144  s.addPair(12, 13, "payload_rod");
145 
146 }
147 
148 void T6Model::addMuscles(tgStructure& s)
149 {
150  // Outer Cables
151  s.addPair(0, 4, "muscle");
152  s.addPair(0, 5, "muscle");
153  s.addPair(0, 8, "muscle");
154  s.addPair(0, 10, "muscle");
155 
156  s.addPair(1, 6, "muscle");
157  s.addPair(1, 7, "muscle");
158  s.addPair(1, 8, "muscle");
159  s.addPair(1, 10, "muscle");
160 
161  s.addPair(2, 4, "muscle");
162  s.addPair(2, 5, "muscle");
163  s.addPair(2, 9, "muscle");
164  s.addPair(2, 11, "muscle");
165 
166  s.addPair(3, 7, "muscle");
167  s.addPair(3, 6, "muscle");
168  s.addPair(3, 9, "muscle");
169  s.addPair(3, 11, "muscle");
170 
171  s.addPair(4, 2, "muscle");
172  s.addPair(4, 10, "muscle");
173  s.addPair(4, 11, "muscle");
174 
175  s.addPair(5, 8, "muscle");
176  s.addPair(5, 9, "muscle");
177 
178  s.addPair(6, 10, "muscle");
179  s.addPair(6, 11, "muscle");
180 
181  s.addPair(7, 8, "muscle");
182  s.addPair(7, 9, "muscle");
183 
184  // Payload Muscles
185  s.addPair(0, 13, "muscle_in");
186  s.addPair(1, 12, "muscle_in");
187  s.addPair(2, 13, "muscle_in");
188  s.addPair(3, 12, "muscle_in");
189  s.addPair(4, 13, "muscle_in");
190  s.addPair(5, 13, "muscle_in");
191  s.addPair(6, 12, "muscle_in");
192  s.addPair(7, 12, "muscle_in");
193  s.addPair(8, 13, "muscle_in");
194  s.addPair(9, 12, "muscle_in");
195  s.addPair(10, 13, "muscle_in");
196  s.addPair(11, 12, "muscle_in");
197 
198 
199 }
200 
201 void T6Model::setup(tgWorld& world)
202 {
203 
204  const tgRod::Config rodConfig(c.radius, c.density, c.friction,
205  c.rollFriction, c.restitution);
206 
207  const tgRod::Config payConfig(c.radius_pay, c.density_pay, c.friction,
208  c.rollFriction, c.restitution);
209 
211  tgSpringCableActuator::Config muscleConfig(c.stiffness, c.damping, c.pretension * c.stiffness / c.stiffness_in, c.history,
212  c.maxTens, c.targetVelocity);
213 
214  tgSpringCableActuator::Config muscleInConfig(c.stiffness_in, c.damping_in, c.pretension, c.history,
215  c.maxTens, c.targetVelocity);
216 
217  // Start creating the structure
218  tgStructure s;
219  addNodes(s);
220  addRods(s);
221  addMuscles(s);
222  s.move(btVector3(0, 30, 0));
223 
224  // Add a rotation to land the struture on a V.
225  btVector3 rotationPoint1 = btVector3(0, 0, 0); // origin
226  btVector3 rotationAxis1 = btVector3(1, 0, 0); // x-axis
227  double rotationAngle1 = 0.4636; //M_PI/2;
228  s.addRotation(rotationPoint1, rotationAxis1, rotationAngle1);
229  // Add a rotation to move structure towards triangle.
230  btVector3 rotationPoint2 = btVector3(0, 0, 0); // origin
231  btVector3 rotationAxis2 = btVector3(0, 1, 0); // z-axis
232  double rotationAngle2 = 1.991;
233  s.addRotation(rotationPoint2, rotationAxis2, rotationAngle2);
234  // Add a rotation to land the struture on a triangle.
235  btVector3 rotationPoint3 = btVector3(0, 0, 0); // origin
236  btVector3 rotationAxis3 = btVector3(-1, 0, 0); // x-axis
237  double rotationAngle3 = 0.58895;
238  s.addRotation(rotationPoint3, rotationAxis3, rotationAngle3);
239 
240  // Create the build spec that uses tags to turn the structure into a real model
241  tgBuildSpec spec;
242  spec.addBuilder("rod", new tgRodInfo(rodConfig));
243  spec.addBuilder("payload_rod", new tgRodInfo(payConfig));
244  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
245  spec.addBuilder("muscle_in", new tgBasicActuatorInfo(muscleInConfig));
246 
247  // Create your structureInfo
248  tgStructureInfo structureInfo(s, spec);
249 
250  // Use the structureInfo to build ourselves
251  structureInfo.buildInto(*this, world);
252 
253  // We could now use tgCast::filter or similar to pull out the
254  // models (e.g. muscles) that we want to control.
255  allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
256 
257  // call the onSetup methods of all observed things e.g. controllers
258  notifySetup();
259 
260  // Actually setup the children
261  tgModel::setup(world);
262 }
263 
264 void T6Model::step(double dt)
265 {
266  // Precondition
267  if (dt <= 0.0)
268  {
269  throw std::invalid_argument("dt is not positive");
270  }
271  else
272  {
273  // Notify observers (controllers) of the step so that they can take action
274  notifyStep(dt);
275  tgModel::step(dt); // Step any children
276  }
277 }
278 
280 {
281  tgModel::onVisit(r);
282 }
283 
284 const std::vector<tgSpringCableActuator*>& T6Model::getAllMuscles() const
285 {
286  return allMuscles;
287 }
288 
289 void T6Model::teardown()
290 {
292 }
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
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
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
void teardown()
Definition: T6Model.cpp:268
virtual void step(double dt)
Definition: T6Model.cpp:227
virtual ~T6Model()
Definition: T6Model.cpp:104
Definition of class tgStructure.
Definition of class tgStructureInfo.
T6Model()
Definition: T6Model.cpp:100
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