NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
ConnectorTestModel.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/tgBasicActuator.h"
24 #include "tgcreator/tgNodes.h"
25 #include "core/tgSubject.h"
26 #include "core/tgModelVisitor.h"
27 
28 #include "btBulletDynamicsCommon.h"
29 #include <iostream>
30 #include <vector>
31 #include <set>
32 
33 #include "tgcreator/tgRodInfo.h"
36 
37 
38 class ConnectorTestModel: public tgSubject<ConnectorTestModel>, public tgModel
39 {
40 public:
41 
42  ConnectorTestModel(std::string name) : tgModel(name)
43  {
44 
45  }
46 
48  {
49  }
50 
51  virtual void setup(tgWorld& world)
52  {
53  // 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)
54 
55  const double density = 0.9;
56  const double radius = 0.3;
57  const tgRod::Config rodConfig(radius, density);
58 
59  tgBasicActuator::Config muscleConfig(1000, 10);
60 
61  tgNodes nodes;
62  nodes.addNode(0,0,0);
63  nodes.addNode(0,3,0);
64  nodes.addNode(2,5,0);
65  nodes.addNode(-2,5,0);
66 
67  nodes.addNode(0, 10, 0);
68  nodes.addNode(0, 7, 0);
69  nodes.addNode(0, 5, 2);
70  nodes.addNode(0, 5,-2);
71 
72  tgPairs pairs;
73  pairs.addPair(nodes.pair(0, 1), "rod");
74  pairs.addPair(nodes.pair(1, 2), "rod");
75  pairs.addPair(nodes.pair(1, 3), "rod");
76 
77  pairs.addPair(nodes.pair(4, 5), "rod");
78  pairs.addPair(nodes.pair(5, 6), "rod");
79  pairs.addPair(nodes.pair(5, 7), "rod");
80 
81 
82  tgPairs musclePairs;
83  musclePairs.addPair(nodes.pair(2, 6), "muscle");
84  musclePairs.addPair(nodes.pair(3, 7), "muscle");
85  musclePairs.addPair(nodes.pair(2, 7), "muscle");
86  musclePairs.addPair(nodes.pair(3, 6), "muscle");
87 
88 
89 
90 
91  // Start with a set of tgRigidInfo*. A tgStructureInfo will also make this available.
92  std::vector<tgRigidInfo*> rigids;
93  for(int i = 0; i < pairs.size(); i++) {
94  rigids.push_back(new tgRodInfo(rodConfig, pairs[i]));
95  }
96 
97  // Create compound rigids out of rigids that share nodes
98  tgRigidAutoCompound autoCompound(rigids);
99  autoCompound.execute(); // After this we have all rigids grouped (even if they're in their own group)
100 
101  // need to init the models in the world...
102  std::vector<tgRigidInfo*>::iterator it;
103  for(it = rigids.begin(); it != rigids.end(); it++) {
104  (*it)->initRigidBody(world);
105  }
106 
107  // not using this right now...
108  //tgBuildSpec spec;
109  //spec.addBuilder("rod", new tgRodInfo(rodConfig)); // @todo: should probably have tgRigidBuilder be abstract and use something like tgRodBuilder
110 
111 
112 
113 
114  std::set<tgConnectorInfo*> muscles;
115  for(int i = 0; i < musclePairs.size(); i++) {
116  tgConnectorInfo* s = new tgBasicActuatorInfo(muscleConfig, musclePairs[i]);
117 
118  std::cout << "tgConnectorInfo: " << std::endl;
119  std::cout << *s << std::endl;
120 
121  try {
122  s->chooseRigids(rigids);
123  } catch(std::exception e) {
124  std::cout << "caught exception " << e.what() << std::endl;
125  throw;
126  }
127  s->initConnector(world);
128  addChild(s->createModel(world));
129  //m_testString = s->createModel(world);
130  //muscles.insert(s);
131  //addChild(m_testString);
132  }
133 
134  for(it = rigids.begin(); it != rigids.end(); it++) {
135  (*it)->createModel(world);
136  }
137 
138  }
139 
140  virtual void step(double dt)
141  {
142  //btTransform trans;
143  //m_rod->getRigidBody()->getMotionState()->getWorldTransform(trans);
144  //std::cout << "rod height: " << trans.getOrigin().getY() << std::endl;
145 
146  // Notify observers (controllers) of the step so that they can take action
147  notifyStep(dt);
148 
149  tgModel::step(dt); // Step any children
150  }
151 
152  virtual void onVisit(const tgModelVisitor& r) const
153  {
154  // Example: m_rod->getRigidBody()->dosomething()...
155 
156  //m_testString->onRender(r);
157 
158  tgModel::onVisit(r);
159  //std::cout << " ConnectorTestModel has special rendering requirements!" << std::endl;
160  }
161 
162  //Does this make myModel an observer as well??
163  void changeMuscle (double length)
164  {
165  //Do we need error checking here too?
166  //newString->setRestLength(length); // commented out for testing
167  }
168 
169 private:
170  tgRodInfo* m_rod;
171  tgRodInfo* m_rod2;
172 
173  //tgBasicActuator* newString;
174  tgModel* m_testString;
175 };
176 
177 #endif
Definition of class tgRodInfo.
void addChild(tgModel *pChild)
Definition: tgModel.cpp:124
virtual void step(double dt)
Definition of tgSubject class.
tgPair pair(int from, int to, std::string tags="")
Definition: tgNodes.cpp:30
virtual void step(double dt)
Definition: tgModel.cpp:84
Definition of class tgBasicActuatorInfo.
virtual void onVisit(const tgModelVisitor &r) const
Definition: tgModel.cpp:107
Contains the definition of class tgModel.
Definition of class tgNodes.
Contains the definition of interface class tgModelVisitor.
tgModel()
Definition: tgModel.cpp:33
Contains the definition of class tgBasicActuator.
virtual void setup(tgWorld &world)
virtual void onVisit(const tgModelVisitor &r) const
int addNode(const btVector3 &node)
Definition: tgNodes.h:129
Definition of class tgRigidAutoCompound.