NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
NestedBoxTestModel.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 
26 // This module
27 #include "NestedBoxTestModel.h"
28 // This library
29 #include "core/tgCast.h"
31 #include "core/tgBox.h"
32 #include "core/tgRod.h"
33 #include "core/tgSphere.h"
34 #include "core/tgString.h"
35 #include "tgcreator/tgBuildSpec.h"
38 #include "tgcreator/tgRodInfo.h"
39 #include "tgcreator/tgBoxInfo.h"
40 #include "tgcreator/tgSphereInfo.h"
41 #include "tgcreator/tgStructure.h"
43 #include "tgcreator/tgUtil.h"
44 // The Bullet Physics library
45 #include "btBulletDynamicsCommon.h"
46 // The C++ Standard Library
47 #include <iostream>
48 #include <stdexcept>
49 
51  m_segments(segments),
52  tgModel()
53 {
54 }
55 
59 namespace
60 {
61  void addNodes(tgStructure& tetra, double edge, double height)
62  {
63  // right
64  tetra.addNode(-edge / 2.0, 0, 0, "heavy");
65  // left
66  tetra.addNode( edge / 2.0, 0, 0, "heavy");
67  // top
68  tetra.addNode(0, height, -1.0 * tgUtil::round(std::sqrt(3.0) / 2.0 * height), "light");
69  // front
70  tetra.addNode(0, height, tgUtil::round(std::sqrt(3.0) / 2.0 * height), "heavy");
71  }
72 
73  void addPairs(tgStructure& tetra)
74  {
75  tetra.addPair(0, 1, "back bottom rod");
76  tetra.addPair(0, 2, "back right rod");
77  tetra.addPair(0, 3, "front right rod");
78  tetra.addPair(1, 2, "back left rod");
79  tetra.addPair(1, 3, "front left rod");
80  tetra.addPair(2, 3, "front top rod");
81  }
82 
83  void addSegments(tgStructure& snake, const tgStructure& tetra, double edge,
84  size_t segmentCount)
85  {
86 
87  const btVector3 offset(0, 0, -edge * 2);
88  for (size_t i = 0; i < segmentCount; ++i)
89  {
90  tgStructure* const t = new tgStructure(tetra);
91  t->addTags(tgString("segment", i + 1));
92  t->move((i + 1) * offset);
93  // Add a child to the snake
94  snake.addChild(t);
95  }
96  }
97 
98  // Add muscles that connect the segments
99  void addMuscles(tgStructure& snake)
100  {
101  const std::vector<tgStructure*> children = snake.getChildren();
102  for (size_t i = 1; i < children.size(); ++i)
103  {
104  tgNodes n0 = children[i-1]->getNodes();
105  tgNodes n1 = children[i ]->getNodes();
106 
107  snake.addPair(n0[0], n1[0], "outer right muscle");
108  snake.addPair(n0[1], n1[1], "outer left muscle");
109  snake.addPair(n0[2], n1[2], "outer top muscle");
110 
111  snake.addPair(n0[0], n1[3], "inner right muscle");
112  snake.addPair(n0[1], n1[3], "inner left muscle");
113  snake.addPair(n0[2], n1[3], "inner top muscle");
114  }
115  }
116 
117  void mapMuscles(NestedBoxTestModel::MuscleMap& muscleMap,
118  tgModel& model)
119  {
120  // Note that tags don't need to match exactly, we could create
121  // supersets if we wanted to
122  muscleMap["inner left"] = model.find<tgSpringCableActuator>("inner left muscle");
123  muscleMap["inner right"] = model.find<tgSpringCableActuator>("inner right muscle");
124  muscleMap["inner top"] = model.find<tgSpringCableActuator>("inner top muscle");
125  muscleMap["outer left"] = model.find<tgSpringCableActuator>("outer left muscle");
126  muscleMap["outer right"] = model.find<tgSpringCableActuator>("outer right muscle");
127  muscleMap["outer top"] = model.find<tgSpringCableActuator>("outer top muscle");
128  }
129 
130  void trace(const tgStructureInfo& structureInfo, tgModel& model)
131  {
132  std::cout << "StructureInfo:" << std::endl
133  << structureInfo << std::endl
134  << "Model: " << std::endl
135  << model << std::endl;
136  }
137 
138 } // namespace
139 
141 {
142  const double edge = 30.0;
143  const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
144  std::cout << "edge: " << edge << "; height: " << height << std::endl;
145 
146  // Create the tetrahedra
147  tgStructure tetra;
148  addNodes(tetra, edge, height);
149  addPairs(tetra);
150 
151  // Move the first one so we can create a longer snake.
152  // Or you could move the snake at the end, up to you.
153  tetra.move(btVector3(0.0, 2.0, 100.0));
154 
155  // Create our snake segments
156  tgStructure snake;
157  addSegments(snake, tetra, edge, m_segments);
158  addMuscles(snake);
159 
160  // Create the build spec that uses tags to turn the structure into a real model
161  // Note: This needs to be high enough or things fly apart...
162  const double density = 4.2 / 3000.0; // kg / length^3 - see app for length
163  const double radius = 0.5;
164  const double h = 0.5;
165  const tgRod::Config rodConfig(radius, density);
166  tgBuildSpec spec;
167  spec.addBuilder("rod", new tgRodInfo(rodConfig));
168 
169  tgSpringCableActuator::Config muscleConfig(1000, 10);
170  //spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
171 
172  const tgSphere::Config sphereConfig(0.5, 0.5);
173  spec.addBuilder("light", new tgSphereInfo(sphereConfig));
174 
175  const tgSphere::Config sphereConfig2(0.5, 2.5);
176  spec.addBuilder("light", new tgSphereInfo(sphereConfig2));
177 
178  // Create your structureInfo
179  tgStructureInfo structureInfo(snake, spec);
180  // Use the structureInfo to build ourselves
181  structureInfo.buildInto(*this, world);
182 
183  // We could now use tgCast::filter or similar to pull out the models (e.g. muscles)
184  // that we want to control.
185  allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
186  mapMuscles(muscleMap, *this);
187 
188  trace(structureInfo, *this);
189 
190  // Actually setup the children
191  tgModel::setup(world);
192 }
193 
195 {
196  if (dt < 0.0)
197  {
198  throw std::invalid_argument("dt is not positive");
199  }
200  else
201  {
202  // Notify observers (controllers) of the step so that they can take action
203  notifyStep(dt);
204  // Step any children
205  tgModel::step(dt);
206  }
207 }
208 
209 const std::vector<tgSpringCableActuator*>&
210 NestedBoxTestModel::getMuscles (const std::string& key) const
211 {
212  const MuscleMap::const_iterator it = muscleMap.find(key);
213  if (it == muscleMap.end())
214  {
215  throw std::invalid_argument("Key '" + key + "' not found in muscle map");
216  }
217  else
218  {
219  return it->second;
220  }
221 }
222 
Create a box shape as an obstacle or add it to your tensegrity.
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:131
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
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(double dt)
Definition: tgModel.cpp:84
Utility class for class casting and filtering collections by type.
virtual void step(const double dt)
Definition of class tgBasicActuatorInfo.
Class that interfaces with Bullet to build the boxes.
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...
virtual void setup(tgWorld &world)
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Definition of class tgStructure.
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:125
Definition of class tgStructureInfo.
Contains the definition of class NestedBoxTestModel.
const std::vector< tgSpringCableActuator * > & getMuscles(const std::string &key) const
Contains the definition of class tgUtil and overloaded operator<<() free functions.
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
NestedBoxTestModel(size_t segments)
Definition of class tgRigidAutoCompound.
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:172
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:56
std::map< std::string, std::vector< tgSpringCableActuator * > > MuscleMap