NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
OctahedralComplex.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 
28 #include "OctahedralComplex.h"
29 
30 // This library
31 #include "core/tgCast.h"
33 #include "core/tgString.h"
34 #include "core/tgBox.h"
35 #include "tgcreator/tgBoxInfo.h"
36 #include "tgcreator/tgBuildSpec.h"
40 #include "tgcreator/tgRodInfo.h"
41 #include "tgcreator/tgStructure.h"
43 #include "tgcreator/tgUtil.h"
44 
45 #include "LinearMath/btVector3.h"
46 #include <iostream>
47 #include <algorithm> // std::fill
48 #include <map>
49 #include <set>
50 
51 OctahedralComplex::OctahedralComplex(int segments, double goalAngle, double startAngle) :
52  BaseSpineModelGoal(segments, goalAngle),
53  m_startAngle(startAngle)
54 {
55 }
56 
57 OctahedralComplex::~OctahedralComplex()
58 {
59 }
60 
62 {
63  // 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)
64 
65  // Rod and Muscle configuration
66 
67  const double density = 4.2/300.0; // Note: This needs to be high enough or things fly apart...
68  const double radius = 0.5;
69  const double friction = 0.5;
70  const double rollFriction = 0.0;
71  const double restitution = 0.0;
72  const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
73 
74  const double elasticity = 1000.0;
75  const double damping = 10.0;
76  const double pretension = 0.0;
77  const bool history = false;
78  const double maxTens = 7000.0;
79  const double maxSpeed = 24.0;
80 
81  const double mRad = 1.0;
82  const double motorFriction = 10.0;
83  const double motorInertia = 1.0;
84  const bool backDrivable = false;
85  tgKinematicActuator::Config motorConfig(elasticity, damping, pretension,
86  mRad, motorFriction, motorInertia, backDrivable,
87  history, maxTens, maxSpeed);
88 
90 #if (0)
91  const tgSpringCableActuator::Config stringConfig(stiffness, damping, pretension, false, 7000, 24);
92 #endif
93 
94  const double passivePretension = 700; // 5 N
95  tgKinematicActuator::Config muscleConfig(2000, 20, passivePretension,
96  mRad, motorFriction, motorInertia, backDrivable,
97  history, maxTens, maxSpeed);
98 
99  // Calculations for the flemons spine model
100  double v_size = 10.0;
101 
102  // Create the tetrahedra
103  tgStructure tetra;
104 
105  tetra.addNode(0,0,0); // center
106  tetra.addNode(0.0, v_size, 0.0); // Top
107  tetra.addNode(0.0, -v_size, 0.0); // Bottom
108  tetra.addNode(0.0, 0.0, v_size); // front
109  tetra.addNode(0.0, 0.0, -v_size); // back
110  tetra.addNode(v_size, 0.0, 0.0); // right
111  tetra.addNode(-v_size, 0.0, 0.0); // left
112 
113  tetra.addPair(0,1, "top rod");
114  tetra.addPair(0,2, "bottom rod");
115  tetra.addPair(0,3, "front rod");
116  tetra.addPair(0,4, "back rod");
117  tetra.addPair(0,5, "right rod");
118  tetra.addPair(0,6, "left rod");
119 
120 
121  // Create our snake segments
122  tgStructure snake;
123  const double offsetDist = -v_size *1.25;
124  btVector3 offset(0,0,offsetDist); // @todo: there seems to be an issue with Muscle2P connections if the front of a tetra is inside the next one.
125  for(std::size_t i = 0; i < m_segments; i++) {
126  // @todo: the snake is a temporary variable -- will its destructor be called? If not, where do we delete its children?
127  tgStructure* t = new tgStructure(tetra);
128  t->addTags(tgString("segment num", i + 1));
129  t->move((i + 1)*offset);
130 
131  if (i % 2 == 1)
132  {
133  t->addRotation(btVector3(0.0, 0.0, (i + 1) * offsetDist), btVector3(1, 0, 0), M_PI/4.0);
134  }
135  else
136  {
137  t->addRotation(btVector3(0.0, 0.0, (i + 1) * offsetDist), btVector3(0, 1, 0), -M_PI/4.0);
138  }
139 
140  //t->addRotation(btVector3(0.0, 0.0, (i + 1) * offsetDist), btVector3(0, 0, 1), M_PI/4.0);
141 
142 
143  snake.addChild(t); // Add a child to the snake
144  }
145 
146  // Move the snake at the end, up to you.
147  snake.addRotation(btVector3(0.0, 0.0, 0.0), btVector3(0, 0, 1), M_PI/4.0);
148  snake.move(btVector3(0.0,15.0,100.0));
149  //conditionally compile for debugging
150  #if (1)
151  // Add muscles that connect the segments
152  // Tag the muscles with their segment numbers so CPGs can find
153  // them.
154  std::vector<tgStructure*> children = snake.getChildren();
155  for(std::size_t i = 1; i < children.size(); i++) {
156  tgNodes n0 = children[i-1]->getNodes();
157  tgNodes n1 = children[i]->getNodes();
158 
159  if (i % 2 == 0 )
160  {
161 
162  snake.addPair(n0[2], n1[3], tgString("inner front muscle seg", i-1) + tgString(" seg", i));
163  snake.addPair(n0[4], n1[3], tgString("inner right muscle seg", i-1) + tgString(" seg", i));
164  snake.addPair(n0[2], n1[5], tgString("inner left muscle seg", i-1) + tgString(" seg", i));
165  snake.addPair(n0[4], n1[5], tgString("inner back muscle seg", i-1) + tgString(" seg", i));
166 
167  #if (1) // Traditional interior crosslink
168  snake.addPair(n0[5], n1[3], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
169  snake.addPair(n0[6], n1[5], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
170  snake.addPair(n0[2], n1[1], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
171  snake.addPair(n0[4], n1[2], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
172 
173  #else
174  snake.addPair(n0[5], n1[5], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
175  snake.addPair(n0[6], n1[3], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
176  snake.addPair(n0[4], n1[1], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
177  snake.addPair(n0[2], n1[2], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
178  #endif
179 
180  }
181  else
182  {
183 
184  snake.addPair(n0[6], n1[1], tgString("inner front muscle seg", i-1) + tgString(" seg", i));
185  snake.addPair(n0[4], n1[1], tgString("inner right muscle seg", i-1) + tgString(" seg", i));
186  snake.addPair(n0[6], n1[3], tgString("inner left muscle seg", i-1) + tgString(" seg", i));
187  snake.addPair(n0[4], n1[3], tgString("inner back muscle seg", i-1) + tgString(" seg", i));
188 
189  #if (1)
190  snake.addPair(n0[1], n1[3], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
191  snake.addPair(n0[2], n1[1], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
192  snake.addPair(n0[6], n1[5], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
193  snake.addPair(n0[4], n1[6], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
194 
195  #else
196  snake.addPair(n0[4], n1[5], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
197  snake.addPair(n0[2], n1[3], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
198  snake.addPair(n0[1], n1[1], tgString("inner left muscle2 seg", i-1) + tgString(" seg", i));
199  snake.addPair(n0[6], n1[6], tgString("inner back muscle2 seg", i-1) + tgString(" seg", i));
200  #endif
201 
202  }
203  }
204  #endif
205 
206  btVector3 fixedPoint(0.0, 0.0, 0.0);
207  btVector3 axis(0.0, 1.0, 0.0);
208 
209  snake.addRotation(fixedPoint, axis, m_startAngle);
210 
211 
212  // Create the build spec that uses tags to turn the structure into a real model
213  tgBuildSpec spec;
214  spec.addBuilder("rod", new tgRodInfo(rodConfig));
215 
216  #if (1)
217  spec.addBuilder("muscle", new tgKinematicContactCableInfo(muscleConfig));
218  spec.addBuilder("muscle2", new tgKinematicContactCableInfo(motorConfig));
219  #else
220  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig));
221  spec.addBuilder("muscle2", new tgBasicActuatorInfo(stringConfig));
222  #endif
223 
224  // Create your structureInfo
225  tgStructureInfo structureInfo(snake, spec);
226 
227  // Use the structureInfo to build ourselves
228  structureInfo.buildInto(*this, world);
229 
230  // Setup vectors for control
231  m_allMuscles = find<tgSpringCableActuator> ("muscle2");
232  m_saddleMuscles = find<tgSpringCableActuator> ("muscle");
233  m_allSegments = this->find<tgModel> ("segment");
234 
235  #if (0)
236  // Debug printing
237  std::cout << "StructureInfo:" << std::endl;
238  std::cout << structureInfo << std::endl;
239 
240  std::cout << "Model: " << std::endl;
241  std::cout << *this << std::endl;
242 
243  #endif
244 
245  children.clear();
246 
247  // Actually setup the children
249 }
250 
252 {
253 
255 
256 }
257 
258 void OctahedralComplex::step(double dt)
259 {
260  /* CPG update occurs in the controller so that we can decouple it
261  * from the physics update
262  */
263 
264  BaseSpineModelGoal::step(dt); // Step any children
265 }
266 
Create a box shape as an obstacle or add it to your tensegrity.
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Implementing the cross-linked octahedral complex spine inspired by Tom Flemons.
virtual void setup(tgWorld &world)
virtual void setup(tgWorld &world)
virtual void teardown()
Convenience function for combining strings with ints, mostly for naming structures.
Utility class for class casting and filtering collections by type.
virtual void step(double dt)
Definition of class tgBasicActuatorInfo.
Class that interfaces with Bullet to build the boxes.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
virtual void step(double dt)
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Definition of class tgBasicContactCableInfo.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Definition of class tgStructure.
Definition of class tgStructureInfo.
Rand seeding simular to the evolution and terrain classes.
Definition of class tgBuildSpec.
Definition of class tgKinematicContactCableInfo.
void buildInto(tgModel &model, tgWorld &world)
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70