NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
TetraSpineStaticModel_hf.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 
19 // This module
20 #include "TetraSpineStaticModel_hf.h"
21 // This library
22 #include "core/tgCast.h"
24 #include "core/tgBasicActuator.h"
25 #include "core/tgString.h"
26 #include "core/tgSphere.h"
27 #include "core/abstractMarker.h"
28 #include "tgcreator/tgBuildSpec.h"
30 #include "tgcreator/tgRodInfo.h"
31 #include "tgcreator/tgSphereInfo.h"
32 #include "tgcreator/tgStructure.h"
34 #include "tgcreator/tgUtil.h"
35 // The Bullet Physics library
36 #include "LinearMath/btVector3.h"
37 // The C++ Standard Library
38 #include <iostream>
39 #include <stdexcept>
40 #include <algorithm>
41 #include <deque>
42 
52 TetraSpineStaticModel_hf::TetraSpineStaticModel_hf(size_t segments) :
53  BaseSpineModelLearning(segments)
54 {
55 }
56 
57 TetraSpineStaticModel_hf::~TetraSpineStaticModel_hf()
58 {
59 }
60 namespace
61 {
62  void addNodes(tgStructure& tetra, double edge, double height)
63  {
64  // Front segment
65  // right
66  tetra.addNode(-edge / 2.0, 0, 0, "base");
67  // left
68  tetra.addNode( edge / 2.0, 0, 0, "base");
69  // top
70  tetra.addNode(0, height, 0, "base");
71  // front
72  tetra.addNode(0, edge / 2.0 * tan(M_PI / 6.0), 25.0, "tip");
73 
74  // Get the next two nodes from existing nodes:
75  tgNodes oldNodes = tetra.getNodes();
76  tgNode nn1 = (oldNodes[0] + oldNodes[2])/2.0; // 4 mid
77  nn1.addTags("PCB");
78  tetra.addNode(nn1);
79  tgNode nn2 = (oldNodes[1] + oldNodes[2])/2.0; // 5 mid
80  nn2.addTags("PCB");
81  tetra.addNode(nn2);
82 
83  std::cout << (oldNodes[3] - oldNodes[2]).length() << std::endl;
84  std::cout << (oldNodes[3] - oldNodes[1]).length() << std::endl;
85  std::cout << (oldNodes[3] - oldNodes[0]).length() << std::endl;
86 
87  }
88 
89  void addPairs(tgStructure& tetra)
90  {
91  tetra.addPair(0, 1, "back bottom rod");
92  tetra.addPair(0, 4, "back rightBottom rod");
93  tetra.addPair(4, 2, "back rightTop rod");
94  tetra.addPair(0, 3, "front right rod");
95  tetra.addPair(1, 5, "back leftBottom rod");
96  tetra.addPair(5, 2, "back leftTop rod");
97  tetra.addPair(1, 3, "front left rod");
98  tetra.addPair(2, 3, "front top rod");
99  }
100 
101 
102  void addSegments(tgStructure& snake, const tgStructure& tetra, double edge,
103  size_t segmentCount)
104  {
105  const btVector3 offset(0, 0, -23.0);
106  for (size_t i = 0; i < segmentCount; ++i)
107  {
110  tgStructure* const t = new tgStructure(tetra);
111  t->addTags(tgString("segment num", i )); // Use num0, num1, num2 as a tag!!
112 
113  t->move((i + 1) * offset);
114 
115  // Add a child to the snake
116  snake.addChild(t);
117 
118  }
119  }
120 
121  // Add muscles that connect the segments
122  void addMuscles(tgStructure& snake)
123  {
124  const std::vector<tgStructure*> children = snake.getChildren();
125  for (size_t i = 1; i < children.size(); ++i)
126  {
127  tgNodes n0 = children[i-1]->getNodes();
128  tgNodes n1 = children[i ]->getNodes();
129 
130  snake.addPair(n0[0], n1[0], tgString("outer right muscle seg", i));
131  snake.addPair(n0[1], n1[1], tgString("outer left muscle seg", i));
132  snake.addPair(n0[2], n1[2], tgString("outer top muscle seg", i));
133 
134  snake.addPair(n0[0], n1[3], tgString("inner right muscle seg", i));
135  snake.addPair(n0[1], n1[3], tgString("inner left muscle seg", i));
136  snake.addPair(n0[2], n1[3], tgString("inner top muscle seg", i));
137  }
138  }
139 
140  void mapMuscles(TetraSpineStaticModel_hf::MuscleMap& muscleMap,
141  tgModel& model)
142  {
143  // Note that tags don't need to match exactly, we could create
144  // supersets if we wanted to
145  muscleMap["inner left"] = model.find<tgSpringCableActuator>("inner left muscle");
146  muscleMap["inner right"] = model.find<tgSpringCableActuator>("inner right muscle");
147  muscleMap["inner top"] = model.find<tgSpringCableActuator>("inner top muscle");
148  muscleMap["outer left"] = model.find<tgSpringCableActuator>("outer left muscle");
149  muscleMap["outer right"] = model.find<tgSpringCableActuator>("outer right muscle");
150  muscleMap["outer top"] = model.find<tgSpringCableActuator>("outer top muscle");
151  }
152 
153  void addMarkers(tgStructure& structure, TetraSpineStaticModel_hf& model)
154  {
155 
156  const std::vector<tgStructure*> children = structure.getChildren();
157  tgNodes n0 = children[0]->getNodes();
158 
159  // TODO: consider using the segments vector here
160  btRigidBody* firstBody = model.getAllRigids()[0]->getPRigidBody();
161 
162  std::vector<tgBaseRigid*> myRigids = model.getAllRigids();
163 #if (0)
164  for (int i =0; i < myRigids.size(); i++)
165  {
166  std::cout << myRigids[i]->mass() << " " <<myRigids[i]->getPRigidBody() << std::endl;
167  }
168 #endif
169 
170  abstractMarker marker1(firstBody, n0[3] - firstBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
171 
172  model.addMarker(marker1);
173 
174  tgNodes n1 = children[1]->getNodes();
175 
176  btRigidBody* secondBody = model.getAllRigids()[15]->getPRigidBody();
177 
178  abstractMarker marker2(secondBody, n1[3] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
179 
180  model.addMarker(marker2);
181 
182  abstractMarker marker3(secondBody, n1[1] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
183 
184  model.addMarker(marker3);
185 
186  abstractMarker marker4(secondBody, n1[0] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
187 
188  model.addMarker(marker4);
189 
190  tgNodes n2 = children[2]->getNodes();
191 
192  btRigidBody* thirdBody = model.getAllRigids()[29]->getPRigidBody();
193 
194  abstractMarker marker5(thirdBody, n2[3] - thirdBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
195 
196  model.addMarker(marker5);
197  }
198 
199  void trace(const tgStructureInfo& structureInfo, tgModel& model)
200  {
201  std::cout << "StructureInfo:" << std::endl
202  << structureInfo << std::endl
203  << "Model: " << std::endl
204  << model << std::endl;
205  // Showing the find function
206  const std::vector<tgSpringCableActuator*> outerMuscles =
207  model.find<tgSpringCableActuator>("outer");
208  for (size_t i = 0; i < outerMuscles.size(); ++i)
209  {
210  const tgSpringCableActuator* const pMuscle = outerMuscles[i];
211  assert(pMuscle != NULL);
212  std::cout << "Outer muscle: " << *pMuscle << std::endl;
213  }
214  }
215 
216 } // namespace
217 
218 // This is basically a manual setup of a model.
219 // There are things that do this for us (@todo: reference the things that do this for us)
221 {
222  const double edge = 38;
223  const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
224  std::cout << "edge: " << edge << "; height: " << height << std::endl;
225 
226  // Create the tetrahedra
227  tgStructure tetra;
228  addNodes(tetra, edge, height);
229  addPairs(tetra);
230 
231  // Move the first one so we can create a longer snake.
232  // Or you could move the snake at the end, up to you.
233  tetra.move(btVector3(0.0, 2.0, 100.0));
234 
235  // Create our snake segments
236  tgStructure snake;
237  addSegments(snake, tetra, edge, m_segments);
238  addMuscles(snake);
239 
240  // Create the build spec that uses tags to turn the structure into a real model
241  // Note: This needs to be high enough or things fly apart...
242 
243 
244  // Params for In Won
245  const double oldDensity = .00311;
246  const double radius = 0.635 / 2.0;
247  const double density = 0.0201 / (pow(radius, 2) * M_PI * edge); // Mass divided by volume... should there be a way to set this automatically??
248  const double friction = 0.15;
249  const tgRod::Config rodConfig(radius, density, friction);
250  tgBuildSpec spec;
251  spec.addBuilder("rod", new tgRodInfo(rodConfig));
252 
253  // 1000 is so the units below can be in grams
254  const double sphereVolume = 1000.0 * 4.0 / 3.0 * M_PI * pow(radius, 3);
255 
256  const double baseCornerFrontD = 140.0 / sphereVolume;
257  const tgSphere::Config baseCornerFrontConfig(radius, baseCornerFrontD, friction);
258  spec.addBuilder("num0 base", new tgSphereInfo(baseCornerFrontConfig));
259 
260  const double baseCornerMidD = 180.0 / sphereVolume;
261  const tgSphere::Config baseCornerMidConfig(radius, baseCornerMidD, friction);
262  spec.addBuilder("num1 base", new tgSphereInfo(baseCornerMidConfig));
263 
264  const double baseCornerRearD = 100.0 / sphereVolume;
265  const tgSphere::Config baseCornerRearConfig(radius, baseCornerRearD, friction);
266  spec.addBuilder("num2 base", new tgSphereInfo(baseCornerRearConfig));
267 
268  const double tipCornerFrontD = 40.0 / sphereVolume;
269  const tgSphere::Config tipCornerFrontConfig(radius, tipCornerFrontD, friction);
270  spec.addBuilder("num0 tip", new tgSphereInfo(tipCornerFrontConfig));
271 
272  const double tipCornerMidD = 120.0 / sphereVolume;
273  const tgSphere::Config tipCornerMidConfig(radius, tipCornerMidD, friction);
274  spec.addBuilder("num1 tip", new tgSphereInfo(tipCornerMidConfig));
275  spec.addBuilder("num2 tip", new tgSphereInfo(tipCornerMidConfig));
276 
277  const double PCBD = 70.0 / sphereVolume;
278  const tgSphere::Config PCB_1_Config(radius, PCBD, friction);
279  spec.addBuilder("PCB num0", new tgSphereInfo(PCB_1_Config));
280  spec.addBuilder("PCB num1", new tgSphereInfo(PCB_1_Config));
281 
282  const double PCB2D = 95.0 / sphereVolume;
283  const tgSphere::Config PCB_2_Config(radius, PCB2D, friction);
284  spec.addBuilder("PCB num2", new tgSphereInfo(PCB_2_Config));
285 
286  // Two different string configs
288  tgSpringCableActuator::Config muscleConfig(229.16 * 2.0, 20, 0.0, true, 5000, 7.0, 10.0, 10.0);
289  tgSpringCableActuator::Config muscleConfig2(229.16, 20, 0.0, true, 5000, 7.0, 10.0, 10.0);
290  spec.addBuilder("top muscle", new tgBasicActuatorInfo(muscleConfig));
291  spec.addBuilder("left muscle", new tgBasicActuatorInfo(muscleConfig2));
292  spec.addBuilder("right muscle", new tgBasicActuatorInfo(muscleConfig2));
293 
294  // Create your structureInfo
295  tgStructureInfo structureInfo(snake, spec);
296 
297  // Use the structureInfo to build ourselves
298  structureInfo.buildInto(*this, world);
299 
300  // We could now use tgCast::filter or similar to pull out the models (e.g. muscles)
301  // that we want to control.
302  m_allMuscles = this->find<tgSpringCableActuator> ("muscle");
303  m_allSegments = this->find<tgModel> ("segment");
304  mapMuscles(m_muscleMap, *this);
305 
306  addMarkers(snake, *this);
307 
308  #if (0)
309  trace(structureInfo, *this);
310  #endif
311 
312  // Actually setup the children
314 }
315 
317 {
318 
320 
321 }
322 
324 {
325  if (dt < 0.0)
326  {
327  throw std::invalid_argument("dt is not positive");
328  }
329  else
330  {
331  // Step any children, notify observers
333  }
334 }
335 
337 {
338  std::vector<double> maxTens;
339 
344  for(int i=0; i<m_allMuscles.size(); i++)
345  {
346  tgBasicActuator& m_sca = *(tgCast::cast<tgSpringCableActuator, tgBasicActuator>(m_allMuscles[i]));
347 
349  std::deque<double>& tensionHist = stringHist.tensionHistory;
350  maxTens.push_back( *(std::max_element(tensionHist.begin(), tensionHist.end())) );
351  }
352 
353  return maxTens;
354 }
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:131
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Definition of class tgSphereInfo.
Convenience function for combining strings with ints, mostly for naming structures.
Contains the definition of class tgSphere.
virtual void step(const double dt)
Utility class for class casting and filtering collections by type.
virtual void setup(tgWorld &world)
Markers for specific places on a tensegrity.
virtual void setup(tgWorld &world)
const tgNodes & getNodes() const
Definition: tgStructure.h:113
Definition of class tgBasicActuatorInfo.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:66
virtual const tgSpringCableActuator::SpringCableActuatorHistory & getHistory() const
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
Contains the definition of class tgBasicActuator.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Definition: tgNode.h:45
Definition of class tgStructure.
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:125
Definition of class tgStructureInfo.
std::vector< double > getStringMaxTensions() const
Contains the definition of class tgUtil and overloaded operator<<() free functions.
Definition of class tgBuildSpec.
virtual void step(double dt)
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:56