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