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