NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
DuCTTTestModel.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 
25 #include "DuCTTTestModel.h"
26 
28  m_segments(segments),
29  tgModel()
30 {
31 }
32 
33 void DuCTTTestModel::addNodes(tgStructure& tetra, double edge, double height)
34 {
35  // right
36  tetra.addNode(-edge / 2.0, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
37  // left
38  tetra.addNode( edge / 2.0, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
39 
40  // front
41  tetra.addNode(0, edge/2.0, 0);
42  // back
43  tetra.addNode(0, -edge/2.0, 0);
44 
45  // top middle 1
46  tetra.addNode(-0.01, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
47  // top middle 2
48  tetra.addNode(0.01, 0, tgUtil::round(std::sqrt(3.0) / 2.0 * height));
49  // bottom middle 1
50  tetra.addNode(0, -0.01, 0);
51  // bottom middle 2
52  tetra.addNode(0, 0.01, 0);
53 }
54 
55 //add param for top or bottom prismatic actuator
56 void DuCTTTestModel::addPairs(tgStructure& tetra)
57 {
58  tetra.addPair(3, 0, "back right rod");
59  tetra.addPair(3, 1, "back left rod");
60  tetra.addPair(2, 0, "front right rod");
61  tetra.addPair(2, 1, "front left rod");
62 
63 // tetra.addPair(0, 1, "top rod");
64 // tetra.addPair(2, 3, "bottom rod");
65 // tetra.addPair(0, 1, "top prismatic");
66 // tetra.addPair(2, 3, "bottom prismatic");
67  addBottomPairs(tetra);
68 // addTopPairs(tetra);
69 }
70 
71 void DuCTTTestModel::addTopPairs(tgStructure& tetra)
72 {
73  tetra.addPair(0, 4, "top1 rod");
74  tetra.addPair(5, 1, "top2 rod");
75 // tetra.addPair(4, 5, "top muscle");
76  tetra.addPair(4, 5, "top prismatic");
77 }
78 
79 void DuCTTTestModel::addBottomPairs(tgStructure& tetra)
80 {
81  tetra.addPair(2, 6, "bottom1 rod");
82  tetra.addPair(7, 3, "bottom2 rod");
83 // tetra.addPair(6, 7, "bottom muscle");
84  tetra.addPair(6, 7, "bottom prismatic");
85 }
86 
87 void DuCTTTestModel::addSegments(tgStructure& snake, const tgStructure& tetra, double edge,
88  size_t segmentCount)
89 {
90 
91  const btVector3 offset(0, 0, -edge * 1.15);
92  for (size_t i = 0; i < segmentCount; ++i)
93  {
94  tgStructure* const t = new tgStructure(tetra);
95  t->addTags(tgString("segment", i + 1));
96  t->move((i + 1) * offset);
97  // Add a child to the snake
98  snake.addChild(t);
99  }
100 }
101 
102 // Add muscles that connect the segments
103 void DuCTTTestModel::addMuscles(tgStructure& snake)
104 {
105  std::vector<tgStructure*> children = snake.getChildren();
106  for (size_t i = 1; i < children.size(); ++i)
107  {
108  addBottomPairs(*children[i]);
109  addTopPairs(*children[i-1]);
110 
111 // addBottomPairs(*children[i-1]);
112 // addTopPairs(*children[i]);
113 
114  tgNodes n0 = children[i-1]->getNodes();
115  tgNodes n1 = children[i ]->getNodes();
116 
117  snake.addPair(n0[0], n1[0], "top right muscle");
118  snake.addPair(n0[1], n1[1], "top left muscle");
119 
120  snake.addPair(n0[2], n1[0], "front right muscle");
121  snake.addPair(n0[2], n1[1], "front left muscle");
122 
123  snake.addPair(n0[3], n1[0], "back right muscle");
124  snake.addPair(n0[3], n1[1], "back left muscle");
125 
126  snake.addPair(n0[2], n1[2], "bottom front muscle");
127  snake.addPair(n0[3], n1[3], "bottom back muscle");
128  }
129 }
130 
131 void DuCTTTestModel::mapMuscles(DuCTTTestModel::MuscleMap& muscleMap,
132  tgModel& model)
133 {
134  // Note that tags don't need to match exactly, we could create
135  // supersets if we wanted to
136  muscleMap["top right"] = model.find<tgSpringCableActuator>("top right muscle");
137  muscleMap["top left"] = model.find<tgSpringCableActuator>("top left muscle");
138 
139  muscleMap["front right"] = model.find<tgSpringCableActuator>("front right muscle");
140  muscleMap["front left"] = model.find<tgSpringCableActuator>("front left muscle");
141 
142  muscleMap["back right"] = model.find<tgSpringCableActuator>("back right muscle");
143  muscleMap["back left"] = model.find<tgSpringCableActuator>("back left muscle");
144 
145  muscleMap["bottom front"] = model.find<tgSpringCableActuator>("bottom front muscle");
146  muscleMap["bottom back"] = model.find<tgSpringCableActuator>("bottom back muscle");
147 }
148 
149 void DuCTTTestModel::trace(const tgStructureInfo& structureInfo, tgModel& model)
150 {
151  std::cout << "StructureInfo:" << std::endl
152  << structureInfo << std::endl
153  << "Model: " << std::endl
154  << model << std::endl;
155 }
156 
158 {
159  const double edge = 30.0;
160  const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
161  std::cout << "edge: " << edge << "; height: " << height << std::endl;
162 
163  // Create the first tetrahedra
164  tgStructure tetra;
165  addNodes(tetra, edge, height);
166  addPairs(tetra);
167 
168  // Move the first one so we can create the second
169  tetra.move(btVector3(0.0, edge, 10.0));
170 
171  // Create our snake segments
172  tgStructure snake;
173  addSegments(snake, tetra, edge, 1);
174 // addSegments(snake, tetra, edge, m_segments);
175 // addMuscles(snake);
176 
177  // Create the build spec that uses tags to turn the structure into a real model
178  // Note: This needs to be high enough or things fly apart...
179  const double density = 4.2 / 300.0; // kg / length^3 - see app for length
180  const double radius = 0.5;
181  const tgRod::Config rodConfig(radius, density, 0.5, 0, 0.2);
182  const tgPrismatic::Config prismConfig(3);
183 
184  tgBuildSpec spec;
185  spec.addBuilder("rod", new tgRodInfo(rodConfig));
186  spec.addBuilder("prismatic", new tgPrismaticInfo(prismConfig));
187 
188  tgSpringCableActuator::Config muscleConfig(1000, 10);
189  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
190 
191  // Create your structureInfo
192  tgStructureInfo structureInfo(snake, spec);
193  // Use the structureInfo to build ourselves
194  structureInfo.buildInto(*this, world);
195 
196  // We could now use tgCast::filter or similar to pull out the models (e.g. muscles)
197  // that we want to control.
198  allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
199  mapMuscles(muscleMap, *this);
200 
201  trace(structureInfo, *this);
202 
203  // Debug printing
204  std::cout << "StructureInfo:" << std::endl;
205  std::cout << structureInfo << std::endl;
206 
207  std::cout << "Model: " << std::endl;
208  std::cout << *this << std::endl;
209 
210  // Actually setup the children
211  tgModel::setup(world);
212 
213  std::cout << "Finished Setup!" << std::endl;
214 }
215 
216 void DuCTTTestModel::step(double dt)
217 {
218  if (dt < 0.0)
219  {
220  throw std::invalid_argument("dt is not positive");
221  }
222  else
223  {
224  // Notify observers (controllers) of the step so that they can take action
225  notifyStep(dt);
226  // Step any children
227  tgModel::step(dt);
228  }
229 }
230 
231 const std::vector<tgSpringCableActuator*>&
232 DuCTTTestModel::getMuscles (const std::string& key) const
233 {
234  const MuscleMap::const_iterator it = muscleMap.find(key);
235  if (it == muscleMap.end())
236  {
237  throw std::invalid_argument("Key '" + key + "' not found in muscle map");
238  }
239  else
240  {
241  return it->second;
242  }
243 }
244 
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
void addChild(tgStructure *child)
DuCTTTestModel(size_t segments)
virtual void step(const double dt)
virtual void step(double dt)
Definition: tgModel.cpp:84
const std::vector< tgSpringCableActuator * > & getMuscles(const std::string &key) const
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
Contains the definition of class DuCTTTestModel.
std::map< std::string, std::vector< tgSpringCableActuator * > > MuscleMap
std::string tgString(std::string s, int i)
Definition: tgString.h:33
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:128
virtual void setup(tgWorld &world)
void notifyStep(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