NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tsTestRig.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 // This module
26 #include "tsTestRig.h"
27 // This library
30 
31 #include "core/tgBasicActuator.h"
32 #include "core/tgRod.h"
33 #include "tgcreator/tgBuildSpec.h"
35 #include "tgcreator/tgRodInfo.h"
36 #include "tgcreator/tgStructure.h"
38 // The Bullet Physics library
39 #include "LinearMath/btVector3.h"
40 // The C++ Standard Library
41 #include <stdexcept>
42 
47 namespace
48 {
53  const struct Config
54  {
55  double density;
56  double radius;
57  double stiffness;
58  double damping;
59  double triangle_length;
60  double triangle_height;
61  double prism_height;
62  double pretension;
63  } c =
64  {
65  2, // density (mass / length^3)
66  0.31, // radius (length)
67  1000.0, // stiffness (mass / sec^2)
68  10.0, // damping (mass / sec)
69  10.0, // triangle_length (length)
70  10.0, // triangle_height (length)
71  20.0, // prism_height (length)
72  0.05 // Pretension (percentage)
73  };
74 } // namespace
75 
76 tsTestRig::tsTestRig(bool kinematic) :
77 useKinematic(kinematic),
78 tgModel()
79 {
80 }
81 
83 {
84 
85 }
86 
87 void tsTestRig::addNodes(tgStructure& s)
88 {
89  s.addNode(0.0, -1.0, 0.0); // 0
90  s.addNode(0.0, 0.0, 0.0); // 1
91  s.addNode( 0.0, 10.0, 0.0); // 2
92  s.addNode(0.0, 11.0, 0.0); // 3
93 
94 }
95 
96 void tsTestRig::addRods(tgStructure& s)
97 {
98  s.addPair( 0, 1, "rod");
99  s.addPair( 2, 3, "rod2");
100 }
101 
102 void tsTestRig::addMuscles(tgStructure& s)
103 {
104 
105  s.addPair(1, 2, "muscle");
106 
107 }
108 
110 {
111  // Define the configurations of the rods and strings
112  const tgRod::Config rodConfig(c.radius, c.density);
113  const tgRod::Config rodConfig2(c.radius, 0.0);
114  // String config needs to be inside boolean switch
115 
116  // Create a structure that will hold the details of this model
117  tgStructure s;
118 
119  // Add nodes to the structure
120  addNodes(s);
121 
122  // Add rods to the structure
123  addRods(s);
124 
125  // Add muscles to the structure
126  addMuscles(s);
127 
128  // Move the structure so it doesn't start in the ground
129  s.move(btVector3(0, 5, 0));
130 
131  // Create the build spec that uses tags to turn the structure into a real model
132  // The top rod will be massless - fixed in space
133  tgBuildSpec spec;
134  spec.addBuilder("rod", new tgRodInfo(rodConfig));
135  spec.addBuilder("rod2", new tgRodInfo(rodConfig2));
136 
137  if (useKinematic)
138  {
139  const tgKinematicActuator::Config muscleConfig(c.stiffness, c.damping);
140  spec.addBuilder("muscle", new tgKinematicActuatorInfo(muscleConfig));
141  }
142  else
143  {
144  const tgBasicActuator::Config muscleConfig(c.stiffness, c.damping);
145  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
146  }
147 
148  // Create your structureInfo
149  tgStructureInfo structureInfo(s, spec);
150 
151  // Use the structureInfo to build ourselves
152  structureInfo.buildInto(*this, world);
153 
154  // We could now use tgCast::filter or similar to pull out the
155  // models (e.g. muscles) that we want to control.
156  allMuscles = tgCast::filter<tgModel, tgSpringCableActuator> (getDescendants());
157 
158  // Notify controllers that setup has finished.
159  notifySetup();
160 
161  // Actually setup the children
162  tgModel::setup(world);
163 
164  totalTime = 0.0;
165  reached = false;
166 }
167 
168 void tsTestRig::step(double dt)
169 {
170  // Precondition
171  if (dt <= 0.0)
172  {
173  throw std::invalid_argument("dt is not positive");
174  }
175  else
176  {
177  totalTime += dt;
178  // Notify observers (controllers) of the step so that they can take action
179  notifyStep(dt);
180 
181  if (useKinematic)
182  {
183  allMuscles[0]->setControlInput(-580.0);
184  }
185  else
186  {
187  allMuscles[0]->setControlInput(5.0, dt);
188  }
189  if (allMuscles[0]->getRestLength() <= 5.0 && !reached)
190  {
191  std::cout << "Rest length below 5.0 at: " << totalTime << std::endl;
192  reached = true;
193  }
194 
195  tgModel::step(dt); // Step any children
196  //std::cout << allMuscles[0]->getRestLength() << std::endl;
197  }
198 
199 }
200 
202 {
203  // Example: m_rod->getRigidBody()->dosomething()...
204  tgModel::onVisit(r);
205 }
206 
207 const std::vector<tgSpringCableActuator*>& tsTestRig::getAllMuscles() const
208 {
209  return allMuscles;
210 }
211 
213 {
214  notifyTeardown();
216 }
virtual void teardown()
Definition: tsTestRig.cpp:212
virtual void step(double dt)
Definition: tsTestRig.cpp:168
virtual void teardown()
Definition: tgModel.cpp:68
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
Definition of class tgRodInfo.
const std::vector< tgSpringCableActuator * > & getAllMuscles() const
Definition: tsTestRig.cpp:207
virtual ~tsTestRig()
Definition: tsTestRig.cpp:82
virtual void step(double dt)
Definition: tgModel.cpp:84
virtual void setup(tgWorld &world)
Definition: tsTestRig.cpp:109
virtual void onVisit(tgModelVisitor &r)
Definition: tsTestRig.cpp:201
tsTestRig(bool kinematic=true)
Definition: tsTestRig.cpp:76
Definition of class tgBasicActuatorInfo.
virtual void onVisit(const tgModelVisitor &r) const
Definition: tgModel.cpp:107
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:66
Contains the definition of class tgBasicActuator.
Definition of class tgStructure.
Definition of class tgStructureInfo.
Contains the definition of class tgKinematicActuator.
Definition of class tgKinematicActuatorInfo.
Contains the definition of class tgRod.
Definition of class tgBuildSpec.
Defines a 3 strut 9 string tensegrity model.
void notifyStep(double dt)
std::vector< tgModel * > getDescendants() const
Definition: tgModel.cpp:172
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:56