NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
FlemonsArm.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 "FlemonsArm.h"
27 // This library
28 #include "core/tgBasicActuator.h"
29 #include "core/tgRod.h"
30 #include "core/abstractMarker.h"
31 #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 
45 namespace
46 {
51  const struct Config
52  {
53  double density;
54  double radius;
55  double stiffness;
56  double damping;
57  double pretension_olecranon;
58  double pretension_anconeus;
59  double pretension_brachioradialis;
60  double humerus;
61  double ulna;
62  double mass_fixed;
63 
64  //additional parameters set by steve, ask him to explain why
65  double rollFriction;
66  double friction;
67  double restitution;
68  double pretension_support;
69  double maxTens;
70  double targetVelocity;
71  bool history;
72  } c =
73  {
74  0.05, // density (mass / length^3)
75  0.8, // radius (length)
76  3000.0, // stiffness (mass / sec^2)
77  200.0, // damping (mass / sec)
78  3000.0, // pretension of olecranon (mass * length / sec^2)
79  3000.0/15.55, // pretension of anconeus (mass * length / sec^2)
80  3000.0/262, // pretension of brachioradialis (mass * length / sec^2)
81  15.0, // humerus (length)
82  7.5, //ulna (length)
83  0, // mass is fixed
84 
85  0.01, // rollFriction (unitless)
86  1.0, //friction
87  0.2, // restitution
88  30000.0/1, //pretension_support (force), stiffness/initial length
89  100000, //maxTens (force)
90  10000, //targetVelocity
91  false //history (boolean)
92  };
93 } // namespace
94 
96 tgModel()
97 {
98 }
99 
101 {
102 }
103 
104 void FlemonsArmModel::addNodes(tgStructure& s)
105 {
106  //bottom right (index: 0)
107  //s.addNode(-edge, 0, 0);
108 
109  const double scale = 0.5;
110  const double bone_scale = 0.3;
111  const size_t nNodes = 31; //number of nodes
112 
113  //all in mm for measurements
114  //const double humerusNode = 36.58; // scale * bone_scale * 243.86
115  //const double ulnaNode = 28.2; // scale * bone_scale * 188
116 
117  //Node Positions: Bones
118 
119  //nodePositions.push_back(btVector3(0, humerusNode, 0)); // index: 0
120  //nodePositions.push_back(btVector3(0, 0, 0)); // index: 1
121 
122  //nodePositions.push_back(btVector3(-ulnaNode, 0.0001, 0)); // index: 2
123 
124  //nodePositions.push_back(btVector3(0, 0.0001, 0)); //index: 3
125 
126  //Node Positions: Muscles
127 
128  //nodePositions.push_back(btVector3(-ulnaNode/2, 0.0001, 0)); // index: 4 halfway in the ulna
129 
130  //nodePositions.push_back(btVector3(0, humerusNode/2, 0)); // index: 5
131 
132 
133 // Average Adult Male Measurements with scale
134 // Lengths are in mm
135 const double ulna_w = 22 * scale; //ulnta distal width
136 const double ulna_l = 334 * scale * bone_scale; //ulna length
137 const double humerus_l = 265 * scale * bone_scale; //humerus length //NB: in model, c==b
138 //const double d = 66 * scale; // humerus epicondylar width
139 //const double e = 246 * scale * bone_scale; //radius length
140 const double humerus_head_r = 25 * scale; // humerus head radius
141 const double g = 17 * scale; //ulna proximal width
142 const double x = ulna_w/2;
143 const double z = humerus_l/2;
144 //Format: (x, z, y)
145 nodePositions.push_back(btVector3(g, 0, 0)); // 0
146 nodePositions.push_back(btVector3(0, -g, 0)); //1
147 nodePositions.push_back(btVector3(-ulna_w/2, 0, 0)); // 2
148 nodePositions.push_back(btVector3(0, 0, g)); // 3
149 nodePositions.push_back(btVector3(0, 0, -g)); // 4
150 nodePositions.push_back(btVector3(0, g, 0)); //5
151 nodePositions.push_back(btVector3(0, humerus_l, 0)); // 6
152 nodePositions.push_back(btVector3(x, z, 0)); // 7
153 nodePositions.push_back(btVector3(ulna_l+ulna_w/2, -g, 0)); // 8
154 nodePositions.push_back(btVector3(0, humerus_l+2, humerus_head_r)); // 9
155 nodePositions.push_back(btVector3(0, humerus_l+2, -humerus_head_r)); //10
156 //Added 6/17/15
157 nodePositions.push_back(btVector3(ulna_w/2, -2*g, 0)); //11
158 //ulna and radius
159 nodePositions.push_back(btVector3(3*ulna_w/2, -g, 0)); //12
160 nodePositions.push_back(btVector3(3*ulna_w/4, -g, g)); //13
161 nodePositions.push_back(btVector3(3*ulna_w/4, -g, -g)); //14
162 nodePositions.push_back(btVector3(humerus_head_r, humerus_l+2, 0)); // 15
163 nodePositions.push_back(btVector3(-humerus_head_r, humerus_l+2, 0)); // 16
164 
165 //Added 7/13/15
166 
167 //upper humerus nodes
168 nodePositions.push_back(btVector3(g/2,humerus_l-2,x/2)); //17
169 nodePositions.push_back(btVector3(-g/2,humerus_l-2,x/2)); //18
170 nodePositions.push_back(btVector3(-g/2,humerus_l-2,-x/2)); //19
171 nodePositions.push_back(btVector3(g/2,humerus_l-2,-x/2)); //20
172 
173 //lower humerus nodes
174 nodePositions.push_back(btVector3(x/3,g+7.5,g/2)); //21
175 nodePositions.push_back(btVector3(x/3,g+7.5,-g/2)); //22
176 nodePositions.push_back(btVector3(x/3,g+5,g/2)); //23
177 nodePositions.push_back(btVector3(x/3,g+5,-g/2)); //24
178 
179 //ulna/radius nodes
180 nodePositions.push_back(btVector3(ulna_l/2 -1,g+6,-x/4)); //25
181 nodePositions.push_back(btVector3(-ulna_l/12,g+6,-x/4)); //26
182 nodePositions.push_back(btVector3(ulna_l/2 -1,g+6,x/4)); //27
183 nodePositions.push_back(btVector3(-ulna_l/12,g+6,x/4)); //28
184 
185 //elbow nodes
186 nodePositions.push_back(btVector3(ulna_l/9,g+10,0)); //29
187 nodePositions.push_back(btVector3(ulna_l/9,g+2,0)); //30
188 
189 //nodePositions.push_back(btVector3(0,humerus_l+0.001,0)); //31
190 //nodePositions.push_back(btVector3(0,humerus_l+0.002, 0)); //32
191 
192  for(size_t i = 0; i < nNodes; i++){
193  s.addNode(nodePositions[i][0], nodePositions[i][1], nodePositions[i][2]);
194  }
195 }
196 
197 void FlemonsArmModel::addRods(tgStructure& s)
198 {
199  //vertical sides
200  //s.addPair( 0, 4, "rod");
201 
202  //s.addPair(0, 5, "humerus massless");
203  //s.addPair(1, 5, "humerus massless");
204 
205  //s.addPair(3, 4, "rod");
206  //s.addPair(4, 2, "rod");
207 /*
208 // ulna and radius
209 s.addPair(8, 12, "rod");
210 s.addPair(12, 14, "rod");
211 s.addPair(12, 13, "rod");
212 //olecranon
213 s.addPair(2, 11, "rod");
214 s.addPair(0, 11, "rod");
215 s.addPair(2, 0, "rod");
216 s.addPair(1, 2, "rod");
217 // humerus
218 s.addPair(3, 5, "humerus massless");
219 s.addPair(4, 5, "humerus massless");
220 s.addPair(5, 6, "humerus massless");
221 */
222 
223  //humerus
224  s.addPair(5, 6, "humerus massless"); //humerus bone
225  s.addPair(17, 18, "rod"); //upper humerus parallel 1
226  s.addPair(19, 20, "rod"); //upper humerus parallel 2
227  s.addPair(21, 22, "rod"); //lower humerus parallel 1
228  s.addPair(23, 24, "rod"); //lower humerus parallel 2
229 
230  s.addPair(25, 26, "rod"); //ulna 1
231  s.addPair(27, 28, "rod"); //ulna 2
232 
233  s.addPair(29, 30, "rod"); //elbow
234 }
235 
236 void FlemonsArmModel::addMuscles(tgStructure& s)
237 {
238 
239  s.addPair(6, 17, "anconeus muscle");
240  s.addPair(6, 18, "anconeus muscle");
241  s.addPair(6, 19, "anconeus muscle");
242  s.addPair(6, 20, "anconeus muscle");
243  s.addPair(19, 18, "anconeus muscle");
244  s.addPair(20, 17, "anconeus muscle");
245 
246  s.addPair(17, 21, "anconeus muscle");
247  s.addPair(20, 22, "anconeus muscle");
248  s.addPair(21, 23, "anconeus muscle");
249  s.addPair(22, 24, "anconeus muscle");
250 
251  s.addPair(22, 26, "olecranon muscle");
252  s.addPair(24, 26, "olecranon muscle");
253  s.addPair(19, 26, "olecranon muscle");
254 
255  s.addPair(21, 28, "olecranon muscle");
256  s.addPair(23, 28, "olecranon muscle");
257  s.addPair(18, 28, "olecranon muscle");
258 
259  s.addPair(25, 27, "anconeus muscle");
260  s.addPair(26, 28, "olecranon muscle");
261  s.addPair(5, 23, "olecranon muscle");
262  s.addPair(5, 24, "olecranon muscle");
263  s.addPair(5, 26, "olecranon muscle");
264  s.addPair(5, 28, "olecranon muscle");
265 
266  //elbow
267  s.addPair(29, 21, "olecranon muscle");
268  s.addPair(29, 22, "olecranon muscle");
269  s.addPair(30, 23, "olecranon muscle");
270  s.addPair(30, 24, "olecranon muscle");
271 
272 
273 
274 /*
275 s.addPair(0, 3, "olecranon muscle"); //NB actually fascial tissue
276 s.addPair(0, 4, "olecranon muscle"); //NB actually fascial tissue
277 s.addPair(1, 3, "olecranon muscle"); //NB actually fascial tissue
278 s.addPair(1, 4, "olecranon muscle"); //NB actually fascial tissue
279 s.addPair(2, 3, "olecranon muscle"); //NB actually fascial tissue
280 s.addPair(2, 4, "olecranon muscle"); //NB actually fascial tissue
281 s.addPair(0, 13, "olecranon muscle"); //NB actually fascial tissue
282 s.addPair(1, 13, "olecranon muscle"); //NB actually fascial tissue
283 s.addPair(11, 13, "olecranon muscle"); //NB actually fascial tissue
284 s.addPair(0, 14, "olecranon muscle"); //NB actually fascial tissue
285 s.addPair(1, 14, "olecranon muscle"); //NB actually fascial tissue
286 s.addPair(11, 14, "olecranon muscle"); //NB actually fascial tissue
287 s.addPair(0, 12, "olecranon muscle"); //NB actually fascial tissue
288 s.addPair(11, 12, "olecranon muscle"); //NB actually fascial tissue
289 s.addPair(0, 5, "brachioradialis muscle");
290 s.addPair(2, 5, "olecranon muscle"); //NB actually fascial tissue
291 s.addPair(3, 13, "right anconeus muscle");
292 s.addPair(4, 14, "left anconeus muscle");
293 */
294 }
295 
297 {
298  // Define the configurations of the rods and strings
299  // Note that pretension is defined for this string
300  const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution);
301  const tgRod::Config rodConfigMassless(c.radius, 0.0/*c.density*/, c.friction, c.rollFriction, c.restitution);
302 
303  //have Steve explain
304  //Olecranon Muscle Configuration
305  tgBasicActuator::Config olecranonMuscleConfig(c.stiffness, c.damping, c.pretension_olecranon, c.history, c.maxTens, c.targetVelocity);
306  //Anconeus Muscle Configuration
307  tgBasicActuator::Config anconeusMuscleConfig(c.stiffness, c.damping, c.pretension_anconeus, c.history, c.maxTens, c.targetVelocity);
308  //Brachioradialis Muscle Configuration
309  tgBasicActuator::Config brachioradialisMuscleConfig(c.stiffness, c.damping, c.pretension_brachioradialis, c.history, c.maxTens, c.targetVelocity);
310  //Support String Muscle Configuration
311  tgBasicActuator::Config supportstringMuscleConfig(c.stiffness, c.damping, c.pretension_support, c.history, c.maxTens, c.targetVelocity);
312 
313  // Create a structure that will hold the details of this model
314  tgStructure s;
315 
316  // Add nodes to the structure
317  addNodes(s);
318 
319  // Add rods to the structure
320  addRods(s);
321 
322  // Add muscles to the structure
323  addMuscles(s);
324 
325  // Move the structure so it doesn't start in the ground
326  s.move(btVector3(0, 50.0, 0));
327 
328  // Create the build spec that uses tags to turn the structure into a real model
329  tgBuildSpec spec;
330 
331  spec.addBuilder("rod", new tgRodInfo(rodConfig));
332  spec.addBuilder("massless", new tgRodInfo(rodConfigMassless));
333  spec.addBuilder("olecranon muscle", new tgBasicActuatorInfo(olecranonMuscleConfig));
334  spec.addBuilder("anconeus muscle", new tgBasicActuatorInfo(anconeusMuscleConfig));
335  spec.addBuilder("brachiorodialis muscle", new tgBasicActuatorInfo(brachioradialisMuscleConfig));
336  spec.addBuilder("support muscle", new tgBasicActuatorInfo(supportstringMuscleConfig));
337 
338  // Create your structureInfo
339  tgStructureInfo structureInfo(s, spec);
340 
341  // Use the structureInfo to build ourselves
342  structureInfo.buildInto(*this, world);
343 
344  // We could now use tgCast::filter or similar to pull out the
345  // models (e.g. muscles) that we want to control.
346  allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants());
347 
348  // Notify controllers that setup has finished.
349  notifySetup();
350 
351  // Actually setup the children
352  tgModel::setup(world);
353 }
354 
355 void FlemonsArmModel::step(double dt)
356 {
357  // Precondition
358  if (dt <= 0.0)
359  {
360  throw std::invalid_argument("dt is not positive");
361  }
362  else
363  {
364  // Notify observers (controllers) of the step so that they can take action
365  notifyStep(dt);
366  tgModel::step(dt); // Step any children
367  }
368 }
369 
371 {
372  // Example: m_rod->getRigidBody()->dosomething()...
373  tgModel::onVisit(r);
374 }
375 
376 const std::vector<tgBasicActuator*>& FlemonsArmModel::getAllMuscles() const
377 {
378  return allMuscles;
379 }
380 
382 {
383  notifyTeardown();
385 }
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: FlemonsArm.cpp:355
virtual void setup(tgWorld &world)
Definition: FlemonsArm.cpp:296
virtual ~FlemonsArmModel()
Definition: FlemonsArm.cpp:100
virtual void step(double dt)
Definition: tgModel.cpp:84
virtual void onVisit(tgModelVisitor &r)
Definition: FlemonsArm.cpp:370
Markers for specific places on a tensegrity.
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
Contains the definition of class tgBasicActuator.
Definition of class tgStructure.
Definition of class tgStructureInfo.
const std::vector< tgBasicActuator * > & getAllMuscles() const
Definition: FlemonsArm.cpp:376
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
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