NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
RBStringTest.cpp
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 #include "RBStringTest.h"
20 
21 #include "btBulletDynamicsCommon.h"
22 #include <iostream>
23 
24 #include "tgcreator/tgRodInfo.h"
28 #include "tgcreator/tgBuildSpec.h"
29 
30 #include "tgcreator/tgStructure.h"
32 #include "tgcreator/tgUtil.h"
33 #include "core/tgString.h"
34 #include "tgcreator/tgNode.h"
35 
36 #include "core/tgCast.h"
37 
38 RBStringTest::Config::Config( int segments,
39  const tgRod::Config& rodConf,
40  const tgBasicActuator::Config& stringConf,
41  double minTotalLength) :
42 m_segments(segments),
43 m_rodConfig(rodConf),
44 m_stringConfig(stringConf),
45 m_minTotalLength(minTotalLength)
46 {
47 }
48 
49 RBStringTest::RBStringTest(tgNode* start, tgNode* end, const RBStringTest::Config& config) :
50  m_pStartNode(start),
51  m_pEndNode(end),
52  m_config(config),
53  tgModel()
54 {
55 }
56 
58 {
59 
60  // todo - figure out a way to change only the one parameter with less code
61  const double stiffness = m_config.m_stringConfig.stiffness;
62  const double damping = m_config.m_stringConfig.damping;
63  const double pretension = 0.0;
64  tgBasicActuator::Config muscleConfig1(stiffness, damping, pretension, false, 1000.0, 100.0, 0.1, 0.1, -M_PI / 2.0);
65  tgBasicActuator::Config muscleConfig2(stiffness, damping, pretension, false, 1000.0, 100.0, 0.1, 0.1, M_PI / 2.0);
66  tgBasicActuator::Config muscleConfig3(stiffness, damping, pretension, false, 1000.0, 100.0, 0.1, 0.1, M_PI);
67  tgBasicActuator::Config muscleConfig4(stiffness, damping, pretension, false, 1000.0, 100.0, 0.1, 0.1, 0);
68 
69  // Calculations for the flemons spine model
70  double v_size = m_config.m_minTotalLength / (double) m_config.m_segments;
71 
72  // tgNode is a subclass, so this should be fine
73  btVector3 buildVec = (*m_pEndNode - *m_pStartNode);
74 
75  double spacing = buildVec.length() / (double) m_config.m_segments;
76  // After this buildVec stays normalized.
77  btVector3 offset = buildVec.normalize() * spacing;
78 
79  // Create the tetrahedra
80  tgStructure tetra;
81 
82  tetra.addNode(0,0,0); // Bottom
83  tgNode endNode(v_size * buildVec); //Fancy upcast
84  tetra.addNode(endNode); // Top
85 
86  tetra.addPair(0,1, "a rod");
87 
88  // Move the first one into position
89  tetra.move(*m_pStartNode);
90 
91 
92  // Create our snake segments - these will be tgModels with
93  // tgRods as children
94  tgStructure snake;
95 
96  for(int i = 0; i < m_config.m_segments; i++) {
97  // @todo: the snake is a temporary variable -- will its destructor be called? If not, where do we delete its children?
98  tgStructure* t = new tgStructure(tetra);
99  t->addTags(tgString("segment num", i + 1));
100  t->move((i + 1)*offset);
101  snake.addChild(t); // Add a child to the snake
102  }
103 
104  // Add muscles that connect the segments
105  std::vector<tgStructure*> children = snake.getChildren();
106  for(int i = 1; i < children.size(); i++) {
107  tgNodes n0 = children[i-1]->getNodes();
108  tgNodes n1 = children[i]->getNodes();
109 
110  snake.addPair(n0[1], n1[0], tgString("muscle seg", i-1) + tgString(" seg", i));
111  snake.addPair(n0[1], n1[0], tgString("muscle2 seg", i-1) + tgString(" seg", i));
112  snake.addPair(n0[1], n1[0], tgString("muscle3 seg", i-1) + tgString(" seg", i));
113  snake.addPair(n0[1], n1[0], tgString("muscle4 seg", i-1) + tgString(" seg", i));
114  }
115 
116  // Create the build spec that uses tags to turn the structure into a real model
117  tgBuildSpec spec;
118  spec.addBuilder("rod", new tgRodInfo(m_config.m_rodConfig));
119 
120  spec.addBuilder("muscle", new tgBasicActuatorInfo(muscleConfig1));
121  spec.addBuilder("muscle2", new tgBasicActuatorInfo(muscleConfig2));
122  spec.addBuilder("muscle3", new tgBasicActuatorInfo(muscleConfig3));
123  spec.addBuilder("muscle4", new tgBasicActuatorInfo(muscleConfig4));
124 
125  // Create your structureInfo
126  tgStructureInfo structureInfo(snake, spec);
127 
128  // Use the structureInfo to build ourselves
129  structureInfo.buildInto(*this, world);
130 
131  // We could now use tgCast::filter or similar to pull out the models (e.g. muscles) that we want to control.
132  allMuscles = tgCast::filter<tgModel, tgBasicActuator> (getDescendants());
133 
134  // Debug printing
135  std::cout << "StructureInfo:" << std::endl;
136  std::cout << structureInfo << std::endl;
137 
138  std::cout << "Model: " << std::endl;
139  std::cout << *this << std::endl;
140 
141 
142 }
143 
144 void RBStringTest::step(double dt)
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 }
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:131
void addChild(tgStructure *child)
Definition of class tgRodInfo.
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.
Definition of class tgBasicActuatorInfo.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:66
virtual void step(double dt)
Definition of class tgConnectorInfo.
Definition of class tgNode.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Definition: tgNode.h:45
Definition of class tgStructure.
Definition of class tgStructureInfo.
virtual void setup(tgWorld &world)
Contains the definition of class tgUtil and overloaded operator<<() free functions.
Definition of class tgBuildSpec.
Definition of class tgRigidAutoCompound.
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