NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
ScarrArmModel.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2015, 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 "ScarrArmModel.h"
27 // This library
28 #include "core/tgBasicActuator.h"
29 #include "core/tgRod.h"
30 #include "core/abstractMarker.h"
31 #include "tgcreator/tgBuildSpec.h"
34 #include "tgcreator/tgRodInfo.h"
35 #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  // 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  const struct ConfigRod {
50  double density;
51  double radius;
52  double rod_length;
53  double rod_space;
54  double friction;
55  double rollFriction;
56  double restitution;
57  } cRod = {
58  0.05, // density (kg / length^3)
59  0.8, // radius (length)
60  15.0, // rod_length (length)
61  7.5, // rod_space (length)
62  1.0, // friction (unitless)
63  0.01, // rollFriction (unitless)
64  0.2 // restitution (?)
65  };
66 
67  const struct ConfigCable {
68  double elasticity;
69  double damping;
70  double stiffness;
71  double pretension_olecranon;
72  double pretension_anconeus;
73  double pretension_brachioradialis;
74  double pretension_support;
75  bool history;
76  double maxTens;
77  double targetVelocity;
78  double mRad;
79  double motorFriction;
80  double motorInertia;
81  bool backDrivable;
82  } cCable = {
83  1000.0, // elasticity
84  200.0, // damping (kg/s)
85  3000.0, // stiffness (kg / sec^2)
86  3000.0/1, // pretension_olecranon (force), stiffness/initial length
87  3000.0/15.55, // pretension_anconeus (force), stiffness/initial length
88  3000.0/262, // pretension_brachioradialis (force), stiffness/initial length
89  30000.0/1, // pretension_support (force), stiffness/initial length
90  false, // history (boolean)
91  100000, // maxTens
92  10000, // targetVelocity
93  1.0, // mRad
94  10.0, // motorFriction
95  1.0, // motorInertia
96  false // backDrivable (boolean)
97  };
98 } // namespace
99 
101 
103 
104 void ScarrArmModel::addNodes(tgStructure& s) {
105  const double scale = 0.5;
106  const double bone_scale = 0.3;
107  const size_t nNodes = 15 + 2; //2 for massless rod
108 
109  // Average Adult Male Measurements with scale
110  // Lengths are in mm
111  const double a = 22 * scale; //ulnta distal width
112  const double b = 334 * scale * bone_scale; //ulna length
113  const double c = 265 * scale * bone_scale; //humerus length //NB: in model, c==b
114  //const double d = 66 * scale; // humerus epicondylar width
115  //const double e = 246 * scale * bone_scale; //radius length
116  const double f = 25 * scale; // humerus head radius
117  const double g = 17 * scale; //ulna proximal width
118  const double x = a/2;
119  const double z = c/2;
120 
121  //Format: (x, z, y)
122  nodePositions.push_back(btVector3(g, 0, 0));
123  nodePositions.push_back(btVector3(0, -g, 0));
124  nodePositions.push_back(btVector3(-a/2, 0, 0));
125  nodePositions.push_back(btVector3(0, 0, g));
126  nodePositions.push_back(btVector3(0, 0, -g));
127  nodePositions.push_back(btVector3(0, g, 0));
128  nodePositions.push_back(btVector3(0, c, 0));
129  nodePositions.push_back(btVector3(x, z, 0));
130  nodePositions.push_back(btVector3(b+a/2, -g, 0));
131  nodePositions.push_back(btVector3(0, c+2, f));
132  nodePositions.push_back(btVector3(0, c+2, -f));
133 
134  //Added 6/17/15
135  nodePositions.push_back(btVector3(a/2, -2*g, 0));
136 
137  //ulna and radius
138  nodePositions.push_back(btVector3(3*a/2, -g, 0));
139  nodePositions.push_back(btVector3(3*a/4, -g, g));
140  nodePositions.push_back(btVector3(3*a/4, -g, -g));
141 
142  nodePositions.push_back(btVector3(f, c+2, 0));
143  nodePositions.push_back(btVector3(-f, c+2, 0));
144 
145  for(size_t i=0;i<nNodes;i++) {
146  s.addNode(nodePositions[i][0],nodePositions[i][1],nodePositions[i][2]);
147  }
148 }
149 
150 void ScarrArmModel::addRods(tgStructure& s) {
151  // ulna and radius
152  s.addPair(8, 12, "rod");
153  s.addPair(12, 14, "rod");
154  s.addPair(12, 13, "rod");
155 
156  //olecranon
157  s.addPair(0, 1, "rod");
158  s.addPair(1, 2, "rod");
159  s.addPair(1, 11, "rod");
160 
161  // humerus
162  s.addPair(3, 5, "humerus massless");
163  s.addPair(4, 5, "humerus massless");
164  s.addPair(5, 6, "humerus massless");
165 }
166 
167 void ScarrArmModel::addMuscles(tgStructure& s) {
168  const std::vector<tgStructure*> children = s.getChildren();
169 
170  s.addPair(0, 3, "olecranon muscle"); //NB actually fascial tissue
171  s.addPair(0, 4, "olecranon muscle"); //NB actually fascial tissue
172  s.addPair(1, 3, "olecranon muscle"); //NB actually fascial tissue
173  s.addPair(1, 4, "olecranon muscle"); //NB actually fascial tissue
174  s.addPair(2, 3, "olecranon muscle"); //NB actually fascial tissue
175  s.addPair(2, 4, "olecranon muscle"); //NB actually fascial tissue
176 
177  s.addPair(0, 13, "olecranon muscle"); //NB actually fascial tissue
178  s.addPair(1, 13, "olecranon muscle"); //NB actually fascial tissue
179  s.addPair(11, 13, "olecranon muscle"); //NB actually fascial tissue
180  s.addPair(0, 14, "olecranon muscle"); //NB actually fascial tissue
181  s.addPair(1, 14, "olecranon muscle"); //NB actually fascial tissue
182  s.addPair(11, 14, "olecranon muscle"); //NB actually fascial tissue
183 
184  s.addPair(0, 12, "olecranon muscle"); //NB actually fascial tissue
185  s.addPair(11, 12, "olecranon muscle"); //NB actually fascial tissue
186  s.addPair(0, 5, "brachioradialis muscle");
187  s.addPair(2, 5, "olecranon muscle"); //NB actually fascial tissue
188  s.addPair(3, 13, "right anconeus muscle");
189  s.addPair(4, 14, "left anconeus muscle");
190 }
191 
192 /*
193 void ScarrArmModel::addMarkers(tgStructure &s) {
194  std::vector<tgRod *> rods=find<tgRod>("rod");
195 
196  for(int i=0;i<10;i++)
197  {
198  const btRigidBody* bt = rods[rodNumbersPerNode[i]]->getPRigidBody();
199  btTransform inverseTransform = bt->getWorldTransform().inverse();
200  btVector3 pos = inverseTransform * (nodePositions[i]);
201  abstractMarker tmp=abstractMarker(bt,pos,btVector3(0.08*i,1.0 - 0.08*i,.0),i);
202  this->addMarker(tmp);
203  }
204 }
205 */
206 
208  const tgRod::Config rodConfig(cRod.radius, cRod.density, cRod.friction, cRod.rollFriction, cRod.restitution);
209  const tgRod::Config rodConfigMassless(cRod.radius, 0.00/*c.density*/, cRod.friction, cRod.rollFriction, cRod.restitution);
211 
212  tgBasicActuator::Config olecranonMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_olecranon,
213  cCable.history, cCable.maxTens, cCable.targetVelocity);
214  tgBasicActuator::Config anconeusMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_anconeus,
215  cCable.history, cCable.maxTens, cCable.targetVelocity);
216  tgBasicActuator::Config brachioradialisMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_brachioradialis,
217  cCable.history, cCable.maxTens, cCable.targetVelocity);
218  tgBasicActuator::Config supportstringMuscleConfig(cCable.stiffness, cCable.damping, cCable.pretension_support,
219  cCable.history, cCable.maxTens, cCable.targetVelocity);
220 
221  // Start creating the structure
222  tgStructure s;
223  addNodes(s);
224  addRods(s);
225  addMuscles(s);
226 
227  // Move the arm out of the ground
228  btVector3 offset(0.0, 50.0, 0.0);
229  s.move(offset);
230 
231  // Create the build spec that uses tags to turn the structure into a real model
232  tgBuildSpec spec;
233  spec.addBuilder("massless", new tgRodInfo(rodConfigMassless));
234  spec.addBuilder("rod", new tgRodInfo(rodConfig));
235  spec.addBuilder("olecranon muscle", new tgBasicActuatorInfo(olecranonMuscleConfig));
236  spec.addBuilder("anconeus muscle", new tgBasicActuatorInfo(anconeusMuscleConfig));
237  spec.addBuilder("brachioradialis muscle", new tgBasicActuatorInfo(brachioradialisMuscleConfig));
238  spec.addBuilder("support muscle", new tgBasicActuatorInfo(supportstringMuscleConfig));
239 
240  // Create your structureInfo
241  tgStructureInfo structureInfo(s, spec);
242 
243  // Use the structureInfo to build ourselves
244  structureInfo.buildInto(*this, world);
245 
246  // We could now use tgCast::filter or similar to pull out the
247  // models (e.g. muscles) that we want to control.
248  allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants());
249 
250  // call the onSetup methods of all observed things e.g. controllers
251  notifySetup();
252 
253  // Actually setup the children
254  tgModel::setup(world);
255 
256  //map the rods and add the markers to them
257  //addMarkers(s);
258 
259 }
260 
261 void ScarrArmModel::step(double dt)
262 {
263  // Precondition
264  if (dt <= 0.0) {
265  throw std::invalid_argument("dt is not positive");
266  } else {
267  // Notify observers (controllers) of the step so that they can take action
268  notifyStep(dt);
269  tgModel::step(dt); // Step any children
270  }
271 }
272 
274 {
275  tgModel::onVisit(r);
276 }
277 
278 const std::vector<tgBasicActuator*>& ScarrArmModel::getAllMuscles() const
279 {
280  return allMuscles;
281 }
282 
284 {
285  notifyTeardown();
287 }
288 
virtual void onVisit(tgModelVisitor &r)
virtual void setup(tgWorld &world)
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
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 ~ScarrArmModel()
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.
Contains the definition of class ScarrArmModel. $Id$.
const std::vector< tgBasicActuator * > & getAllMuscles() const
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
Definition of class tgKinematicContactCableInfo.
void notifyStep(double dt)
virtual void step(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