20 #include "TetraSpineStaticModel_hf.h"
36 #include "LinearMath/btVector3.h"
52 TetraSpineStaticModel_hf::TetraSpineStaticModel_hf(
size_t segments) :
57 TetraSpineStaticModel_hf::~TetraSpineStaticModel_hf()
62 void addNodes(
tgStructure& tetra,
double edge,
double height)
66 tetra.
addNode(-edge / 2.0, 0, 0,
"base");
68 tetra.
addNode( edge / 2.0, 0, 0,
"base");
70 tetra.
addNode(0, height, 0,
"base");
72 tetra.
addNode(0, edge / 2.0 * tan(M_PI / 6.0), 25.0,
"tip");
76 tgNode nn1 = (oldNodes[0] + oldNodes[2])/2.0;
79 tgNode nn2 = (oldNodes[1] + oldNodes[2])/2.0;
83 std::cout << (oldNodes[3] - oldNodes[2]).length() << std::endl;
84 std::cout << (oldNodes[3] - oldNodes[1]).length() << std::endl;
85 std::cout << (oldNodes[3] - oldNodes[0]).length() << std::endl;
91 tetra.
addPair(0, 1,
"back bottom rod");
92 tetra.
addPair(0, 4,
"back rightBottom rod");
93 tetra.
addPair(4, 2,
"back rightTop rod");
94 tetra.
addPair(0, 3,
"front right rod");
95 tetra.
addPair(1, 5,
"back leftBottom rod");
96 tetra.
addPair(5, 2,
"back leftTop rod");
97 tetra.
addPair(1, 3,
"front left rod");
98 tetra.
addPair(2, 3,
"front top rod");
105 const btVector3 offset(0, 0, -23.0);
106 for (
size_t i = 0; i < segmentCount; ++i)
111 t->addTags(
tgString(
"segment num", i ));
113 t->move((i + 1) * offset);
124 const std::vector<tgStructure*> children = snake.
getChildren();
125 for (
size_t i = 1; i < children.size(); ++i)
127 tgNodes n0 = children[i-1]->getNodes();
128 tgNodes n1 = children[i ]->getNodes();
140 void mapMuscles(TetraSpineStaticModel_hf::MuscleMap& muscleMap,
156 const std::vector<tgStructure*> children = structure.
getChildren();
157 tgNodes n0 = children[0]->getNodes();
160 btRigidBody* firstBody = model.getAllRigids()[0]->getPRigidBody();
162 std::vector<tgBaseRigid*> myRigids = model.getAllRigids();
164 for (
int i =0; i < myRigids.size(); i++)
166 std::cout << myRigids[i]->mass() <<
" " <<myRigids[i]->getPRigidBody() << std::endl;
170 abstractMarker marker1(firstBody, n0[3] - firstBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
172 model.addMarker(marker1);
174 tgNodes n1 = children[1]->getNodes();
176 btRigidBody* secondBody = model.getAllRigids()[15]->getPRigidBody();
178 abstractMarker marker2(secondBody, n1[3] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
180 model.addMarker(marker2);
182 abstractMarker marker3(secondBody, n1[1] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
184 model.addMarker(marker3);
186 abstractMarker marker4(secondBody, n1[0] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
188 model.addMarker(marker4);
190 tgNodes n2 = children[2]->getNodes();
192 btRigidBody* thirdBody = model.getAllRigids()[29]->getPRigidBody();
194 abstractMarker marker5(thirdBody, n2[3] - thirdBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
196 model.addMarker(marker5);
201 std::cout <<
"StructureInfo:" << std::endl
202 << structureInfo << std::endl
203 <<
"Model: " << std::endl
204 << model << std::endl;
206 const std::vector<tgSpringCableActuator*> outerMuscles =
208 for (
size_t i = 0; i < outerMuscles.size(); ++i)
211 assert(pMuscle != NULL);
212 std::cout <<
"Outer muscle: " << *pMuscle << std::endl;
222 const double edge = 38;
223 const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
224 std::cout <<
"edge: " << edge <<
"; height: " << height << std::endl;
228 addNodes(tetra, edge, height);
233 tetra.move(btVector3(0.0, 2.0, 100.0));
237 addSegments(snake, tetra, edge, m_segments);
245 const double oldDensity = .00311;
246 const double radius = 0.635 / 2.0;
247 const double density = 0.0201 / (pow(radius, 2) * M_PI * edge);
248 const double friction = 0.15;
251 spec.addBuilder(
"rod",
new tgRodInfo(rodConfig));
254 const double sphereVolume = 1000.0 * 4.0 / 3.0 * M_PI * pow(radius, 3);
256 const double baseCornerFrontD = 140.0 / sphereVolume;
257 const tgSphere::Config baseCornerFrontConfig(radius, baseCornerFrontD, friction);
258 spec.addBuilder(
"num0 base",
new tgSphereInfo(baseCornerFrontConfig));
260 const double baseCornerMidD = 180.0 / sphereVolume;
261 const tgSphere::Config baseCornerMidConfig(radius, baseCornerMidD, friction);
262 spec.addBuilder(
"num1 base",
new tgSphereInfo(baseCornerMidConfig));
264 const double baseCornerRearD = 100.0 / sphereVolume;
265 const tgSphere::Config baseCornerRearConfig(radius, baseCornerRearD, friction);
266 spec.addBuilder(
"num2 base",
new tgSphereInfo(baseCornerRearConfig));
268 const double tipCornerFrontD = 40.0 / sphereVolume;
269 const tgSphere::Config tipCornerFrontConfig(radius, tipCornerFrontD, friction);
270 spec.addBuilder(
"num0 tip",
new tgSphereInfo(tipCornerFrontConfig));
272 const double tipCornerMidD = 120.0 / sphereVolume;
274 spec.addBuilder(
"num1 tip",
new tgSphereInfo(tipCornerMidConfig));
275 spec.addBuilder(
"num2 tip",
new tgSphereInfo(tipCornerMidConfig));
277 const double PCBD = 70.0 / sphereVolume;
279 spec.addBuilder(
"PCB num0",
new tgSphereInfo(PCB_1_Config));
280 spec.addBuilder(
"PCB num1",
new tgSphereInfo(PCB_1_Config));
282 const double PCB2D = 95.0 / sphereVolume;
284 spec.addBuilder(
"PCB num2",
new tgSphereInfo(PCB_2_Config));
298 structureInfo.buildInto(*
this, world);
302 m_allMuscles = this->find<tgSpringCableActuator> (
"muscle");
303 m_allSegments = this->find<tgModel> (
"segment");
304 mapMuscles(m_muscleMap, *
this);
306 addMarkers(snake, *
this);
309 trace(structureInfo, *
this);
327 throw std::invalid_argument(
"dt is not positive");
338 std::vector<double> maxTens;
344 for(
int i=0; i<m_allMuscles.size(); i++)
346 tgBasicActuator& m_sca = *(tgCast::cast<tgSpringCableActuator, tgBasicActuator>(m_allMuscles[i]));
350 maxTens.push_back( *(std::max_element(tensionHist.begin(), tensionHist.end())) );
const std::vector< tgStructure * > & getChildren() const
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Definition of class tgSphereInfo.
std::deque< double > tensionHistory
Convenience function for combining strings with ints, mostly for naming structures.
Contains the definition of class tgSphere.
virtual void step(const double dt)
Utility class for class casting and filtering collections by type.
virtual void setup(tgWorld &world)
Markers for specific places on a tensegrity.
virtual void setup(tgWorld &world)
const tgNodes & getNodes() const
Definition of class tgBasicActuatorInfo.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
virtual const tgSpringCableActuator::SpringCableActuatorHistory & getHistory() const
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
Contains the definition of class tgBasicActuator.
std::string tgString(std::string s, int i)
Definition of class tgStructure.
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition of class tgStructureInfo.
std::vector< double > getStringMaxTensions() const
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="")