NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
NestedStructureTestModel_rb.h
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 #ifndef CONNECTOR_TEST_MODEL_H
20 #define CONNECTOR_TEST_MODEL_H
21 
22 #include "core/tgModel.h"
23 #include "core/tgLinearString.h"
24 #include "dev/btietz/tgRBString.h"
25 
26 #include "core/tgSubject.h"
27 
28 #include "btBulletDynamicsCommon.h"
29 #include <iostream>
30 #include <set>
31 #include <map>
32 
33 #include "tgcreator/tgRodInfo.h"
34 #include "tgcreator/tgLinearStringInfo.h"
35 #include "tgcreator/tgRBStringInfo.h"
37 #include "tgcreator/tgBuildSpec.h"
38 
39 #include "tgcreator/tgStructure.h"
41 #include "core/tgUtil.h"
42 #include "core/tgString.h"
43 
44 #include "core/tgCast.h"
45 
46 class NestedStructureTestModel: public tgSubject<NestedStructureTestModel>, public tgModel
47 {
48 public:
49 
50  NestedStructureTestModel(int segments) :
51  m_segments(segments),
52  tgModel()
53  {
54  }
55 
56  virtual ~NestedStructureTestModel()
57  {}
58 
59  virtual void setup(tgWorld* world)
60  {
61  // This is basically a manual setup of a model. There are things that do this for us (@todo: reference the things that do this for us)
62 
63  // Rod and Muscle configuration
64 
65  const double density = 4.2/300.0; // Note: This needs to be high enough or things fly apart...
66  const double radius = 0.5;
67  const tgRod::Config rodConfig(radius, density);
68 
69  tgLinearString::Config muscleConfig(1000, 10);
70 
71  // Config for rods that are a part of RB Strings
72  const double strRadius = 0.2;
73  const double strDensity = 2; // Note: This needs to be high enough or things fly apart...
74  const tgRod::Config strRodConfig(strRadius, strDensity);
75 
76  const double stiffness = 1000.0;
77  const double damping = .01*stiffness;
78  const int segments = 10;
79  const tgLinearString::Config stringConfig(stiffness, damping);
80  const tgRBStringInfo::Config rbConfig(segments, strRodConfig, stringConfig, 3.0);
81 
82  // Calculations for the tetrahedra model
83  double edge = 30.0;
84  double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
85 
86  std::cout << "edge: " << edge << "; height: " << height << std::endl;
87 
88  // Create the tetrahedra
89  tgStructure tetra;
90 
91  tetra.addNode(-edge/2,0,0); // right
92  tetra.addNode(edge/2,0,0); // left
93  tetra.addNode(0,height,0); // top
94  tetra.addNode(0,height/2, tgUtil::round(std::sqrt(3.0)/2 * height)); // front
95 
96  tetra.addPair(0,1, "back bottom rod");
97  tetra.addPair(0,2, "back right rod");
98  tetra.addPair(0,3, "front right rod");
99  tetra.addPair(1,2, "back left rod");
100  tetra.addPair(1,3, "front left rod");
101  tetra.addPair(2,3, "front top rod");
102 
103  // Move the first one so we can create a longer snake. Or you could move the snake at the end, up to you.
104  tetra.move(btVector3(0.0,2.0,100.0));
105 
106  // Create our snake segments
107  tgStructure snake;
108  btVector3 offset(0,0,-edge * 1.15); // @todo: there seems to be an issue with Muscle2P connections if the front of a tetra is inside the next one.
109  for(int i = 0; i < m_segments; i++) {
110  // @todo: the snake is a temporary variable -- will its destructor be called? If not, where do we delete its children?
111  tgStructure* t = new tgStructure(tetra);
112  t->addTags(tgString("segment", i + 1));
113  t->move((i + 1)*offset);
114  snake.addChild(t); // Add a child to the snake
115  }
116  //conditionally compile for debugging
117  #if (1)
118  // Add muscles that connect the segments
119  std::vector<tgStructure*> children = snake.getChildren();
120  for(int i = 1; i < children.size(); i++) {
121  tgNodes n0 = children[i-1]->getNodes();
122  tgNodes n1 = children[i]->getNodes();
123 
124  snake.addPair(n0[0], n1[0], "outer right muscle");
125  snake.addPair(n0[1], n1[1], "outer left muscle");
126  snake.addPair(n0[2], n1[2], "outer top muscle");
127 
128  snake.addPair(n0[0], n1[3], "inner right muscle");
129  snake.addPair(n0[1], n1[3], "inner left muscle");
130  snake.addPair(n0[2], n1[3], "inner top muscle");
131 
132  }
133  #endif
134  // Create the build spec that uses tags to turn the structure into a real model
135  tgBuildSpec spec;
136  spec.addBuilder("rod", new tgRodInfo(rodConfig));
137 
138  #if (1)
139  spec.addBuilder("muscle", new tgRBStringInfo(rbConfig));
140  #endif
141 
142  // Create your structureInfo
143  tgStructureInfo structureInfo(&snake, &spec);
144 
145  // Use the structureInfo to build ourselves
146  structureInfo.buildInto(this, world);
147 
148  // We could now use tgCast::filter or similar to pull out the models (e.g. muscles) that we want to control.
149 
150  allMuscles = tgCast::filter<tgModel, tgLinearString> (getDescendants());
151 
152  // Note that tags don't need to match exactly, we could create
153  // supersets if we wanted to
154  muscleMap["inner left"] = this->find<tgLinearString>("inner left muscle");
155  muscleMap["inner right"] = this->find<tgLinearString>("inner right muscle");
156  muscleMap["inner top"] = this->find<tgLinearString>("inner top muscle");
157  muscleMap["outer left"] = this->find<tgLinearString>("outer left muscle");
158  muscleMap["outer right"] = this->find<tgLinearString>("outer right muscle");
159  muscleMap["outer top"] = this->find<tgLinearString>("outer top muscle");
160 
161  // Debug printing
162  std::cout << "StructureInfo:" << std::endl;
163  std::cout << structureInfo << std::endl;
164 
165  std::cout << "Model: " << std::endl;
166  std::cout << *this << std::endl;
167 
168 
169  // Showing the find function
170  std::vector<tgLinearString*> outerMuscles = find<tgLinearString>("outer");
171  for(int i = 0; i < outerMuscles.size(); i++) {
172  std::cout << "Outer muscle: " << *(outerMuscles[i]) << std::endl;
173  }
174 
175 
176  }
177 
178  virtual void step(double dt)
179  {
180  // Notify observers (controllers) of the step so that they can take action
181  notifyStep(dt);
182 
183  tgModel::step(dt); // Step any children
184 
185  }
186 
187  //Does this make myModel an observer as well??
188  void changeMuscle (double length, double dt)
189  {
190  for( int i = 0; i < allMuscles.size(); i++){
191  allMuscles[i]->setRestLength(length, dt);
192  }
193  }
194  const std::vector<tgLinearString*> getMuscles (std::string key)
195  {
196  return muscleMap[key];
197  }
198 
199  const int getSegments()
200  {
201  return m_segments;
202  }
203 
204 private:
205  std::vector<tgLinearString*> allMuscles;
206  std::map<std::string, std::vector<tgLinearString*> > muscleMap;
207 
208  const int m_segments;
209 };
210 
211 #endif
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:131
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Definition of tgSubject class.
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 tgModel.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:66
tgModel()
Definition: tgModel.cpp:33
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Definition of class tgStructure.
Definition of class tgStructureInfo.
Contains the definition of class tgRBString. A string with small rigid bodies to create contact dynam...
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