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