diff --git a/bin/setup/patches/OpenGLPatch.diff b/bin/setup/patches/OpenGLPatch.diff index 1c110b9a1..11dc40e64 100644 --- a/bin/setup/patches/OpenGLPatch.diff +++ b/bin/setup/patches/OpenGLPatch.diff @@ -1,6 +1,6 @@ -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/CMakeLists.txt Tensegrity/trunk/NTRT/src/core/bullet/CMakeLists.txt ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/CMakeLists.txt 2014-05-20 16:21:25.833574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/CMakeLists.txt 2014-05-20 16:18:50.277568956 -0400 +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/CMakeLists.txt NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/CMakeLists.txt +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/CMakeLists.txt 2014-11-10 09:15:18.680913618 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/CMakeLists.txt 2014-08-18 20:29:02.998097341 -0700 @@ -1,67 +1,36 @@ -# This is basically the overall name of the project in Visual Studio this is the name of the Solution File - @@ -93,774 +93,11 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/CMake + ENDIF (INSTALL_EXTRA_LIBS) ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/OpenGLPatch.diff.rej Tensegrity/trunk/NTRT/src/core/bullet/OpenGLPatch.diff.rej ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/OpenGLPatch.diff.rej 1969-12-31 19:00:00.000000000 -0500 -+++ Tensegrity/trunk/NTRT/src/core/bullet/OpenGLPatch.diff.rej 2014-05-20 15:58:12.465526976 -0400 -@@ -0,0 +1,760 @@ -+--- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/OpenGLPatch.diff 2014-05-20 15:46:12.269502551 -0400 -++++ Tensegrity/trunk/NTRT/src/core/bullet/OpenGLPatch.diff 1969-12-31 19:00:00.000000000 -0500 -+@@ -1,757 +0,0 @@ -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/CMakeLists.txt Tensegrity/trunk/NTRT/src/core/bullet/CMakeLists.txt -+-1,15d0 -+-< # This is basically the overall name of the project in Visual Studio this is the name of the Solution File -+-< -+-< -+-< # For every executable you have with a main method you should have an add_executable line below. -+-< # For every add executable line you should list every .cpp and .h file you have associated with that executable. -+-< -+-< -+-< -+-< -+-< # You shouldn't have to modify anything below this line -+-< ######################################################## -+-< -+-< -+-< -+-< -+-20,49c5,17 -+-< -+-< ADD_LIBRARY(OpenGLSupport -+-< GLDebugFont.cpp -+-< GLDebugFont.h -+-< GL_DialogDynamicsWorld.cpp -+-< GL_DialogDynamicsWorld.h -+-< GL_DialogWindow.cpp -+-< GL_DialogWindow.h -+-< GL_ShapeDrawer.cpp -+-< GL_ShapeDrawer.h -+-< GL_Simplex1to4.cpp -+-< GL_Simplex1to4.h -+-< GLDebugDrawer.cpp -+-< GLDebugDrawer.h -+-< -+-< RenderTexture.cpp -+-< RenderTexture.h -+-< DemoApplication.cpp -+-< DemoApplication.h -+-< -+-< GlutDemoApplication.cpp -+-< GlutDemoApplication.h -+-< GlutStuff.cpp -+-< GlutStuff.h -+-< -+-< stb_image.cpp -+-< stb_image.h -+-< -+-< Win32DemoApplication.cpp -+-< Win32DemoApplication.h -+---- -+-> -+-> ADD_LIBRARY(tgOpenGLSupport SHARED -+-> tgDemoApplication.cpp -+-> tgDemoApplication.h -+-> -+-> tgGlutDemoApplication.cpp -+-> tgGlutDemoApplication.h -+-> tgGlutStuff.cpp -+-> tgGlutStuff.h -+-> -+-> tgGLDebugDrawer.cpp -+-> #Win32DemoApplication.cpp -+-> #Win32DemoApplication.h -+-52d19 -+-< -+-54c21 -+-< TARGET_LINK_LIBRARIES(OpenGLSupport BulletDynamics BulletCollision ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) -+---- -+-> TARGET_LINK_LIBRARIES(tgOpenGLSupport OpenGLSupport BulletDynamics BulletCollision ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}) -+-59,66c26,33 -+-< IF(INSTALL_EXTRA_LIBS) -+-< IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -+-< INSTALL(TARGETS OpenGLSupport DESTINATION .) -+-< ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -+-< INSTALL(TARGETS OpenGLSupport DESTINATION lib) -+-< INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") -+-< ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -+-< ENDIF (INSTALL_EXTRA_LIBS) -+---- -+-> IF(INSTALL_EXTRA_LIBS) -+-> IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -+-> INSTALL(TARGETS tgOpenGLSupport OpenGLSupport DESTINATION .) -+-> ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -+-> INSTALL(TARGETS tgOpenGLSupport OpenGLSupport DESTINATION lib) -+-> INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") -+-> ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) -+-> ENDIF (INSTALL_EXTRA_LIBS) -+-67a35 -+-> -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.cpp -+-17c17 -+-< #include "DemoApplication.h" -+---- -+-> #include "tgDemoApplication.h" -+-35a36 -+-> #include -+-58c59 -+-< DemoApplication::DemoApplication() -+---- -+-> tgDemoApplication::tgDemoApplication() -+-87a89,91 -+-> exitFlag(false), -+-> m_autocam(false), -+-> -+-104c108 -+-< DemoApplication::~DemoApplication() -+---- -+-> tgDemoApplication::~tgDemoApplication() -+-114a119,120 -+-> -+-> std::cout<< "deleted tgDemoApplication" < void tgDemoApplication::overrideGLShapeDrawer (GL_ShapeDrawer* shapeDrawer) -+-125c131 -+-< void DemoApplication::myinit(void) -+---- -+-> void tgDemoApplication::myinit(void) -+-161c167 -+-< void DemoApplication::setCameraDistance(float dist) -+---- -+-> void tgDemoApplication::setCameraDistance(float dist) -+-166c172 -+-< float DemoApplication::getCameraDistance() -+---- -+-> float tgDemoApplication::getCameraDistance() -+-173c179 -+-< void DemoApplication::toggleIdle() { -+---- -+-> void tgDemoApplication::toggleIdle() { -+-185c191 -+-< void DemoApplication::updateCamera() { -+---- -+-> void tgDemoApplication::updateCamera() { -+-257c263 -+-< void DemoApplication::stepLeft() -+---- -+-> void tgDemoApplication::stepLeft() -+-261c267 -+-< void DemoApplication::stepRight() -+---- -+-> void tgDemoApplication::stepRight() -+-265c271 -+-< void DemoApplication::stepFront() -+---- -+-> void tgDemoApplication::stepFront() -+-269c275 -+-< void DemoApplication::stepBack() -+---- -+-> void tgDemoApplication::stepBack() -+-273c279 -+-< void DemoApplication::zoomIn() -+---- -+-> void tgDemoApplication::zoomIn() -+-280c286 -+-< void DemoApplication::zoomOut() -+---- -+-> void tgDemoApplication::zoomOut() -+-295c301 -+-< void DemoApplication::reshape(int w, int h) -+---- -+-> void tgDemoApplication::reshape(int w, int h) -+-308c314 -+-< void DemoApplication::keyboardCallback(unsigned char key, int x, int y) -+---- -+-> void tgDemoApplication::keyboardCallback(unsigned char key, int x, int y) -+-329,347d334 -+-< case 8: -+-< { -+-< int numObj = getDynamicsWorld()->getNumCollisionObjects(); -+-< if (numObj) -+-< { -+-< btCollisionObject* obj = getDynamicsWorld()->getCollisionObjectArray()[numObj-1]; -+-< -+-< getDynamicsWorld()->removeCollisionObject(obj); -+-< btRigidBody* body = btRigidBody::upcast(obj); -+-< if (body && body->getMotionState()) -+-< { -+-< delete body->getMotionState(); -+-< } -+-< delete obj; -+-< -+-< -+-< } -+-< break; -+-< } -+-351a339 -+-> exitFlag = true; -+-353c341 -+-< exit(0); -+---- -+-> exit(0); -+-509a498,503 -+-> -+-> case ';': -+-> { -+-> m_autocam=!m_autocam; -+-> break; -+-> } -+-523c517 -+-< void DemoApplication::setDebugMode(int mode) -+---- -+-> void tgDemoApplication::setDebugMode(int mode) -+-535c529 -+-< void DemoApplication::moveAndDisplay() -+---- -+-> void tgDemoApplication::moveAndDisplay() -+-546c540 -+-< void DemoApplication::displayCallback() -+---- -+-> void tgDemoApplication::displayCallback() -+-552c546 -+-< void DemoApplication::setShootBoxShape () -+---- -+-> void tgDemoApplication::setShootBoxShape () -+-562c556 -+-< void DemoApplication::shootBox(const btVector3& destination) -+---- -+-> void tgDemoApplication::shootBox(const btVector3& destination) -+-600c594 -+-< btScalar gOldPickingDist = 0.f; -+---- -+-> float gOldPickingDist = 0.f; -+-604c598 -+-< btVector3 DemoApplication::getRayTo(int x,int y) -+---- -+-> btVector3 tgDemoApplication::getRayTo(int x,int y) -+-677c671 -+-< void DemoApplication::mouseFunc(int button, int state, int x, int y) -+---- -+-> void tgDemoApplication::mouseFunc(int button, int state, int x, int y) -+-776,781d769 -+-< btVector3 pickPos = rayCallback.m_hitPointWorld; -+-< -+-< pickObject(pickPos, rayCallback.m_collisionObject); -+-< -+-< gOldPickingPos = rayTo; -+-< gHitPos = pickPos; -+-783c771,845 -+-< gOldPickingDist = (pickPos-rayFrom).length(); -+---- -+-> btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); -+-> if (body) -+-> { -+-> //other exclusions? -+-> if (!(body->isStaticObject() || body->isKinematicObject())) -+-> { -+-> pickedBody = body; -+-> pickedBody->setActivationState(DISABLE_DEACTIVATION); -+-> -+-> -+-> btVector3 pickPos = rayCallback.m_hitPointWorld; -+-> //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); -+-> -+-> -+-> btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; -+-> -+-> -+-> -+-> -+-> -+-> -+-> if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0) -+-> { -+-> btTransform tr; -+-> tr.setIdentity(); -+-> tr.setOrigin(localPivot); -+-> btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false); -+-> dof6->setLinearLowerLimit(btVector3(0,0,0)); -+-> dof6->setLinearUpperLimit(btVector3(0,0,0)); -+-> dof6->setAngularLowerLimit(btVector3(0,0,0)); -+-> dof6->setAngularUpperLimit(btVector3(0,0,0)); -+-> -+-> m_dynamicsWorld->addConstraint(dof6,true); -+-> m_pickConstraint = dof6; -+-> -+-> dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0); -+-> dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1); -+-> dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2); -+-> dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3); -+-> dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4); -+-> dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5); -+-> -+-> dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0); -+-> dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1); -+-> dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2); -+-> dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3); -+-> dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4); -+-> dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5); -+-> } else -+-> { -+-> btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); -+-> m_dynamicsWorld->addConstraint(p2p,true); -+-> m_pickConstraint = p2p; -+-> p2p->m_setting.m_impulseClamp = mousePickClamping; -+-> //very weak constraint for picking -+-> p2p->m_setting.m_tau = 0.001f; -+-> /* -+-> p2p->setParam(BT_CONSTRAINT_CFM,0.8,0); -+-> p2p->setParam(BT_CONSTRAINT_CFM,0.8,1); -+-> p2p->setParam(BT_CONSTRAINT_CFM,0.8,2); -+-> p2p->setParam(BT_CONSTRAINT_ERP,0.1,0); -+-> p2p->setParam(BT_CONSTRAINT_ERP,0.1,1); -+-> p2p->setParam(BT_CONSTRAINT_ERP,0.1,2); -+-> */ -+-> -+-> -+-> } -+-> -+-> //save mouse position for dragging -+-> gOldPickingPos = rayTo; -+-> gHitPos = pickPos; -+-> -+-> gOldPickingDist = (pickPos-rayFrom).length(); -+-> } -+-> } -+-802,874c864 -+-< void DemoApplication::pickObject(const btVector3& pickPos, const btCollisionObject* hitObj) -+-< { -+-< -+-< btRigidBody* body = (btRigidBody*)btRigidBody::upcast(hitObj); -+-< if (body) -+-< { -+-< //other exclusions? -+-< if (!(body->isStaticObject() || body->isKinematicObject())) -+-< { -+-< pickedBody = body; -+-< pickedBody->setActivationState(DISABLE_DEACTIVATION); -+-< -+-< -+-< //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); -+-< -+-< -+-< btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; -+-< -+-< if ((m_modifierKeys& BT_ACTIVE_SHIFT)!=0) -+-< { -+-< btTransform tr; -+-< tr.setIdentity(); -+-< tr.setOrigin(localPivot); -+-< btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false); -+-< dof6->setLinearLowerLimit(btVector3(0,0,0)); -+-< dof6->setLinearUpperLimit(btVector3(0,0,0)); -+-< dof6->setAngularLowerLimit(btVector3(0,0,0)); -+-< dof6->setAngularUpperLimit(btVector3(0,0,0)); -+-< -+-< m_dynamicsWorld->addConstraint(dof6,true); -+-< m_pickConstraint = dof6; -+-< -+-< dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0); -+-< dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1); -+-< dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2); -+-< dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3); -+-< dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4); -+-< dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5); -+-< -+-< dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0); -+-< dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1); -+-< dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2); -+-< dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3); -+-< dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4); -+-< dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5); -+-< } else -+-< { -+-< btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); -+-< m_dynamicsWorld->addConstraint(p2p,true); -+-< m_pickConstraint = p2p; -+-< p2p->m_setting.m_impulseClamp = mousePickClamping; -+-< //very weak constraint for picking -+-< p2p->m_setting.m_tau = 0.001f; -+-< /* -+-< p2p->setParam(BT_CONSTRAINT_CFM,0.8,0); -+-< p2p->setParam(BT_CONSTRAINT_CFM,0.8,1); -+-< p2p->setParam(BT_CONSTRAINT_CFM,0.8,2); -+-< p2p->setParam(BT_CONSTRAINT_ERP,0.1,0); -+-< p2p->setParam(BT_CONSTRAINT_ERP,0.1,1); -+-< p2p->setParam(BT_CONSTRAINT_ERP,0.1,2); -+-< */ -+-< -+-< -+-< } -+-< -+-< //save mouse position for dragging -+-< -+-< } -+-< } -+-< -+-< } -+-< -+-< void DemoApplication::removePickingConstraint() -+---- -+-> void tgDemoApplication::removePickingConstraint() -+-888c878 -+-< void DemoApplication::mouseMotionFunc(int x,int y) -+---- -+-> void tgDemoApplication::mouseMotionFunc(int x,int y) -+-1009c999 -+-< btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape) -+---- -+-> btRigidBody* tgDemoApplication::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape) -+-1042c1032 -+-< void DemoApplication::setOrthographicProjection() -+---- -+-> void tgDemoApplication::setOrthographicProjection() -+-1066c1056 -+-< void DemoApplication::resetPerspectiveProjection() -+---- -+-> void tgDemoApplication::resetPerspectiveProjection() -+-1080c1070 -+-< void DemoApplication::displayProfileString(int xOffset,int yStart,char* message) -+---- -+-> void tgDemoApplication::displayProfileString(int xOffset,int yStart,char* message) -+-1087c1077 -+-< void DemoApplication::showProfileInfo(int& xOffset,int& yStart, int yIncr) -+---- -+-> void tgDemoApplication::showProfileInfo(int& xOffset,int& yStart, int yIncr) -+-1161c1151 -+-< void DemoApplication::renderscene(int pass) -+---- -+-> void tgDemoApplication::renderscene(int pass) -+-1208,1209c1198,1199 -+-< btVector3 aabbMin(0,0,0),aabbMax(0,0,0); -+-< //m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); -+---- -+-> btVector3 aabbMin,aabbMax; -+-> m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); -+-1231c1221 -+-< void DemoApplication::renderme() -+---- -+-> void tgDemoApplication::renderme() -+-1233a1224,1225 -+-> -+-> const btVector3 ps = getPs(); -+-1234a1227,1231 -+-> if (m_autocam) -+-> { -+-> m_cameraTargetPosition += (ps - m_cameraTargetPosition) * 0.05; -+-> } -+-> -+-1336a1334,1358 -+-> btVector3 tgDemoApplication::getPs() -+-> { -+-> btVector3 ps(0.0, 0.0, 0.0); -+-> size_t nps = 0; -+-> -+-> const btCollisionObjectArray btarr = -+-> getDynamicsWorld()->getCollisionObjectArray(); -+-> const size_t count = btarr.size(); -+-> nps += count; -+-> /// @todo Use std::for_each() -+-> for (int ib = 0; ib < count; ++ib) -+-> { -+-> const btRigidBody* const pRigid = -+-> static_cast(btarr[ib]); -+-> ps += pRigid->getCenterOfMassPosition(); -+-> } -+-> -+-> if (nps != 0) -+-> { -+-> ps /= static_cast(nps); -+-> } -+-> -+-> return ps; -+-> } -+-> -+-1340c1362 -+-< void DemoApplication::clientResetScene() -+---- -+-> void tgDemoApplication::clientResetScene() -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.h Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.h -+-16,17c16,17 -+-< #ifndef DEMO_APPLICATION_H -+-< #define DEMO_APPLICATION_H -+---- -+-> #ifndef TG_DEMO_APPLICATION_H -+-> #define TG_DEMO_APPLICATION_H -+-20c20 -+-< #include "GlutStuff.h" -+---- -+-> #include "tgGlutStuff.h" -+-41c41 -+-< class DemoApplication -+---- -+-> class tgDemoApplication -+-60,62d59 -+-< virtual void pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj); -+-< -+-< -+-75a73,74 -+-> -+-> bool m_autocam; -+-108a108,111 -+-> private: -+-> -+-> bool exitFlag; -+-> -+-111c114 -+-< DemoApplication(); -+---- -+-> tgDemoApplication(); -+-113c116 -+-< virtual ~DemoApplication(); -+---- -+-> virtual ~tgDemoApplication(); -+-120a124,125 -+-> -+-> virtual void exitPhysics() = 0; -+-155,159d159 -+-< -+-< void setEle(float ele) -+-< { -+-< m_ele = ele; -+-< } -+-211c211 -+-< ///Demo functions -+---- -+-> ///tgDemo functions -+-260c260,266 -+-< -+---- -+-> -+-> btVector3 getPs(); -+-> -+-> bool getExitFlag() -+-> { -+-> return exitFlag; -+-> } -+-264c270 -+-< #endif //DEMO_APPLICATION_H -+---- -+-> #endif //TG_DEMO_APPLICATION_H -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgGLDebugDrawer.cpp -+-2c2 -+-< #include "GLDebugDrawer.h" -+---- -+-> #include "tgGLDebugDrawer.h" -+-4c4 -+-< #include "GlutStuff.h" -+---- -+-> #include "tgGlutStuff.h" -+-9c9 -+-< GLDebugDrawer::GLDebugDrawer() -+---- -+-> tgGLDebugDrawer::tgGLDebugDrawer() -+-15c15 -+-< GLDebugDrawer::~GLDebugDrawer() -+---- -+-> tgGLDebugDrawer::~tgGLDebugDrawer() -+-19c19 -+-< void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor) -+---- -+-> void tgGLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor) -+-29c29 -+-< void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color) -+---- -+-> void tgGLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color) -+-34c34 -+-< void GLDebugDrawer::drawSphere (const btVector3& p, btScalar radius, const btVector3& color) -+---- -+-> void tgGLDebugDrawer::drawSphere (const btVector3& p, btScalar radius, const btVector3& color) -+-72c72 -+-< void GLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) -+---- -+-> void tgGLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) -+-87c87 -+-< void GLDebugDrawer::setDebugMode(int debugMode) -+---- -+-> void tgGLDebugDrawer::setDebugMode(int debugMode) -+-93c93 -+-< void GLDebugDrawer::draw3dText(const btVector3& location,const char* textString) -+---- -+-> void tgGLDebugDrawer::draw3dText(const btVector3& location,const char* textString) -+-99c99 -+-< void GLDebugDrawer::reportErrorWarning(const char* warningString) -+---- -+-> void tgGLDebugDrawer::reportErrorWarning(const char* warningString) -+-104c104 -+-< void GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) -+---- -+-> void tgGLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h Tensegrity/trunk/NTRT/src/core/bullet/tgGLDebugDrawer.h -+-1,2c1,2 -+-< #ifndef GL_DEBUG_DRAWER_H -+-< #define GL_DEBUG_DRAWER_H -+---- -+-> #ifndef tgGL_DEBUG_DRAWER_H -+-> #define tgGL_DEBUG_DRAWER_H -+-8c8 -+-< class GLDebugDrawer : public btIDebugDraw -+---- -+-> class tgGLDebugDrawer : public btIDebugDraw -+-14,15c14,15 -+-< GLDebugDrawer(); -+-< virtual ~GLDebugDrawer(); -+---- -+-> tgGLDebugDrawer(); -+-> virtual ~tgGLDebugDrawer(); -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgGlutDemoApplication.cpp -+-4c4 -+-< #include "GlutDemoApplication.h" -+---- -+-> #include "tgGlutDemoApplication.h" -+-6c6 -+-< #include "GlutStuff.h" -+---- -+-> #include "tgGlutStuff.h" -+-11c11 -+-< void GlutDemoApplication::updateModifierKeys() -+---- -+-> void tgGlutDemoApplication::updateModifierKeys() -+-24c24 -+-< void GlutDemoApplication::specialKeyboard(int key, int x, int y) -+---- -+-> void tgGlutDemoApplication::specialKeyboard(int key, int x, int y) -+-46,60c46,47 -+-< int numObj = getDynamicsWorld()->getNumCollisionObjects(); -+-< if (numObj) -+-< { -+-< btCollisionObject* obj = getDynamicsWorld()->getCollisionObjectArray()[numObj-1]; -+-< -+-< getDynamicsWorld()->removeCollisionObject(obj); -+-< btRigidBody* body = btRigidBody::upcast(obj); -+-< if (body && body->getMotionState()) -+-< { -+-< delete body->getMotionState(); -+-< } -+-< delete obj; -+-< -+-< -+-< } -+---- -+-> -+-> exitPhysics(); -+-79c66 -+-< void GlutDemoApplication::swapBuffers() -+---- -+-> void tgGlutDemoApplication::swapBuffers() -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h Tensegrity/trunk/NTRT/src/core/bullet/tgGlutDemoApplication.h -+-17,18c17,18 -+-< #ifndef GLUT_DEMO_APPLICATION_H -+-< #define GLUT_DEMO_APPLICATION_H -+---- -+-> #ifndef TG_GLUT_DEMO_APPLICATION_H -+-> #define TG_GLUT_DEMO_APPLICATION_H -+-20c20 -+-< #include "DemoApplication.h" -+---- -+-> #include "tgDemoApplication.h" -+-22c22 -+-< ATTRIBUTE_ALIGNED16(class) GlutDemoApplication : public DemoApplication -+---- -+-> ATTRIBUTE_ALIGNED16(class) tgGlutDemoApplication : public tgDemoApplication -+-35c35 -+-< #endif //GLUT_DEMO_APPLICATION_H -+---- -+-> #endif //TG_GLUT_DEMO_APPLICATION_H -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgGlutStuff.cpp -+-18c18,19 -+-< #include "DemoApplication.h" -+---- -+-> #include "tgDemoApplication.h" -+-> #include // Debugging only -+-20,21c21,22 -+-< //glut is C code, this global gDemoApplication links glut to the C++ demo -+-< static DemoApplication* gDemoApplication = 0; -+---- -+-> //glut is C code, this global gtgDemoApplication links glut to the C++ demo -+-> static tgDemoApplication* gtgDemoApplication = 0; -+-24c25 -+-< #include "GlutStuff.h" -+---- -+-> #include "tgGlutStuff.h" -+-28c29 -+-< gDemoApplication->keyboardCallback(key,x,y); -+---- -+-> gtgDemoApplication->keyboardCallback(key,x,y); -+-33c34 -+-< gDemoApplication->keyboardUpCallback(key,x,y); -+---- -+-> gtgDemoApplication->keyboardUpCallback(key,x,y); -+-38c39 -+-< gDemoApplication->specialKeyboard(key,x,y); -+---- -+-> gtgDemoApplication->specialKeyboard(key,x,y); -+-43c44 -+-< gDemoApplication->specialKeyboardUp(key,x,y); -+---- -+-> gtgDemoApplication->specialKeyboardUp(key,x,y); -+-49c50 -+-< gDemoApplication->reshape(w,h); -+---- -+-> gtgDemoApplication->reshape(w,h); -+-54c55 -+-< gDemoApplication->moveAndDisplay(); -+---- -+-> gtgDemoApplication->moveAndDisplay(); -+-59c60 -+-< gDemoApplication->mouseFunc(button,state,x,y); -+---- -+-> gtgDemoApplication->mouseFunc(button,state,x,y); -+-65c66 -+-< gDemoApplication->mouseMotionFunc(x,y); -+---- -+-> gtgDemoApplication->mouseMotionFunc(x,y); -+-71c72 -+-< gDemoApplication->displayCallback(); -+---- -+-> gtgDemoApplication->displayCallback(); -+-75c76 -+-< int glutmain(int argc, char **argv,int width,int height,const char* title,DemoApplication* demoApp) { -+---- -+-> void tgglutmain(int width,int height,const char* title,tgDemoApplication* demoApp) { -+-77,78c78,83 -+-< gDemoApplication = demoApp; -+-< -+---- -+-> gtgDemoApplication = demoApp; -+-> -+-> //Faking argc and argv since GLUT doesn't need to receive them -+-> static char** argv = NULL; -+-> static int argc = 0; -+-> -+-81c86 -+-< glutInitWindowPosition(width/2, height/2); -+---- -+-> glutInitWindowPosition(0, 0); -+-88c93 -+-< gDemoApplication->myinit(); -+---- -+-> gtgDemoApplication->myinit(); -+-113,116d117 -+-< -+-< -+-< glutMainLoop(); -+-< return 0; -+-118a120,125 -+-> #if (0) -+-> void tgGlutMainEventLoop() -+-> { -+-> glutMainLoopEvent(); -+-> } -+-> #endif -+-diff -r SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutStuff.h Tensegrity/trunk/NTRT/src/core/bullet/tgGlutStuff.h -+-43c43 -+-< #define BT_ACTIVE_SHIFT VK_LSHIFT -+---- -+-> -+-70c70,72 -+-< class DemoApplication; -+---- -+-> class tgDemoApplication; -+-> -+-> void tgglutmain(int width,int height,const char* title,tgDemoApplication* demoApp); -+-72c74 -+-< int glutmain(int argc, char **argv,int width,int height,const char* title,DemoApplication* demoApp); -+---- -+-> void tgGlutMainEventLoop(); -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.cpp ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp 2014-05-20 16:21:25.837574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.cpp 2014-05-12 12:02:18.297093465 -0400 -@@ -14,7 +14,7 @@ +Binary files NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/libtgOpenGLSupport.a and NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/libtgOpenGLSupport.a differ +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp 2014-11-10 09:15:18.692913618 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp 2014-10-21 14:06:06.471590379 -0700 +@@ -14,7 +14,7 @@ subject to the following restrictions: */ @@ -869,7 +106,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem #include "LinearMath/btIDebugDraw.h" #include "BulletDynamics/Dynamics/btDynamicsWorld.h" -@@ -33,6 +33,7 @@ +@@ -33,6 +33,7 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" #include "GLDebugFont.h" @@ -877,7 +114,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem extern bool gDisableDeactivation; int numObjects = 0; -@@ -55,7 +56,7 @@ +@@ -55,7 +56,7 @@ extern int gTotalBytesAlignedAllocs; #endif // @@ -886,7 +123,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem //see btIDebugDraw.h for modes : m_dynamicsWorld(0), -@@ -85,6 +86,9 @@ +@@ -85,6 +86,9 @@ m_ShootBoxInitialSpeed(40.f), m_stepping(true), m_singleStep(false), m_idle(false), @@ -896,7 +133,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem m_enableshadows(false), m_sundirection(btVector3(1,-2,1)*1000), -@@ -101,7 +105,7 @@ +@@ -101,7 +105,7 @@ m_defaultContactProcessingThreshold(BT_L @@ -905,7 +142,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { #ifndef BT_NO_PROFILE CProfileManager::Release_Iterator(m_profileIterator); -@@ -112,17 +116,19 @@ +@@ -112,17 +116,19 @@ DemoApplication::~DemoApplication() if (m_shapeDrawer) delete m_shapeDrawer; @@ -927,7 +164,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) }; -@@ -158,19 +164,19 @@ +@@ -158,19 +164,19 @@ void DemoApplication::myinit(void) } @@ -950,7 +187,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem if (m_idle) { m_idle = false; } -@@ -182,7 +188,7 @@ +@@ -182,7 +188,7 @@ void DemoApplication::toggleIdle() { @@ -959,7 +196,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem glMatrixMode(GL_PROJECTION); -@@ -254,30 +260,30 @@ +@@ -254,30 +260,30 @@ void DemoApplication::updateCamera() { const float STEPSIZE = 5; @@ -996,7 +233,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { m_cameraDistance += btScalar(m_zoomStepSize); updateCamera(); -@@ -292,7 +298,7 @@ +@@ -292,7 +298,7 @@ void DemoApplication::zoomOut() @@ -1005,7 +242,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { GLDebugResetFont(w,h); -@@ -305,7 +311,7 @@ +@@ -305,7 +311,7 @@ void DemoApplication::reshape(int w, int @@ -1014,7 +251,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { (void)x; (void)y; -@@ -326,31 +332,13 @@ +@@ -326,31 +332,13 @@ void DemoApplication::keyboardCallback(u switch (key) { @@ -1048,7 +285,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem #endif break; -@@ -507,6 +495,12 @@ +@@ -507,6 +495,12 @@ void DemoApplication::keyboardCallback(u m_ShootBoxInitialSpeed -= 10.f; break; } @@ -1061,7 +298,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem default: // std::cout << "unused key : " << key << std::endl; -@@ -520,7 +514,7 @@ +@@ -520,7 +514,7 @@ void DemoApplication::keyboardCallback(u } @@ -1070,7 +307,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { m_debugMode = mode; if (getDynamicsWorld() && getDynamicsWorld()->getDebugDrawer()) -@@ -532,7 +526,7 @@ +@@ -532,7 +526,7 @@ void DemoApplication::setDebugMode(int m @@ -1079,7 +316,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { if (!m_idle) clientMoveAndDisplay(); -@@ -543,13 +537,13 @@ +@@ -543,13 +537,13 @@ void DemoApplication::moveAndDisplay() @@ -1095,7 +332,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { if (!m_shootBoxShape) { -@@ -559,7 +553,7 @@ +@@ -559,7 +553,7 @@ void DemoApplication::setShootBoxShape ( } } @@ -1104,7 +341,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { if (m_dynamicsWorld) -@@ -597,11 +591,11 @@ +@@ -597,11 +591,11 @@ void DemoApplication::shootBox(const btV int gPickingConstraintId = 0; btVector3 gOldPickingPos; btVector3 gHitPos(-1,-1,-1); @@ -1118,7 +355,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { -@@ -674,7 +668,7 @@ +@@ -674,7 +668,7 @@ btVector3 DemoApplication::getRayTo(int btScalar mousePickClamping = 30.f; @@ -1127,7 +364,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { if (state == 0) { -@@ -773,14 +767,82 @@ +@@ -773,14 +767,82 @@ void DemoApplication::mouseFunc(int butt if (rayCallback.hasHit()) { @@ -1217,7 +454,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem } } -@@ -799,79 +861,7 @@ +@@ -799,79 +861,7 @@ void DemoApplication::mouseFunc(int butt } @@ -1298,7 +535,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { if (m_pickConstraint && m_dynamicsWorld) { -@@ -885,7 +875,7 @@ +@@ -885,7 +875,7 @@ void DemoApplication::removePickingConst } } @@ -1307,7 +544,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { if (m_pickConstraint) -@@ -1006,7 +996,7 @@ +@@ -1006,7 +996,7 @@ void DemoApplication::mouseMotionFunc(in @@ -1316,7 +553,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE)); -@@ -1039,7 +1029,7 @@ +@@ -1039,7 +1029,7 @@ btRigidBody* DemoApplication::localCreat } //See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho @@ -1325,7 +562,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { // switch to projection mode -@@ -1063,7 +1053,7 @@ +@@ -1063,7 +1053,7 @@ void DemoApplication::setOrthographicPro } @@ -1334,7 +571,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { glMatrixMode(GL_PROJECTION); -@@ -1077,14 +1067,14 @@ +@@ -1077,14 +1067,14 @@ void DemoApplication::resetPerspectivePr extern CProfileIterator * m_profileIterator; @@ -1351,7 +588,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { #ifndef BT_NO_PROFILE -@@ -1158,7 +1148,7 @@ +@@ -1158,7 +1148,7 @@ void DemoApplication::showProfileInfo(in // @@ -1360,18 +597,114 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { btScalar m[16]; btMatrix3x3 rot;rot.setIdentity(); -@@ -1205,8 +1195,8 @@ +@@ -1167,71 +1157,82 @@ void DemoApplication::renderscene(int pa + for(int i=0;igetCollisionObjectArray()[i]; +- btRigidBody* body=btRigidBody::upcast(colObj); +- if(body&&body->getMotionState()) +- { +- btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState(); +- myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); +- rot=myMotionState->m_graphicsWorldTrans.getBasis(); +- } +- else +- { +- colObj->getWorldTransform().getOpenGLMatrix(m); +- rot=colObj->getWorldTransform().getBasis(); +- } +- btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation +- if(i&1) wireColor=btVector3(0.f,0.0f,1.f); +- ///color differently for active, sleeping, wantsdeactivation states +- if (colObj->getActivationState() == 1) //active ++ if ((colObj->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE) == 0) + { +- if (i & 1) ++ ++ btRigidBody* body=btRigidBody::upcast(colObj); ++ if(body&&body->getMotionState()) + { +- wireColor += btVector3 (1.f,0.f,0.f); ++ btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState(); ++ myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); ++ rot=myMotionState->m_graphicsWorldTrans.getBasis(); } - } + else +- { +- wireColor += btVector3 (.5f,0.f,0.f); ++ { ++ colObj->getWorldTransform().getOpenGLMatrix(m); ++ rot=colObj->getWorldTransform().getBasis(); + } +- } +- if(colObj->getActivationState()==2) //ISLAND_SLEEPING +- { +- if(i&1) ++ btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation ++ if(i&1) wireColor=btVector3(0.f,0.0f,1.f); ++ ///color differently for active, sleeping, wantsdeactivation states ++ if (colObj->getActivationState() == 1) //active + { +- wireColor += btVector3 (0.f,1.f, 0.f); ++ if (i & 1) ++ { ++ wireColor += btVector3 (1.f,0.f,0.f); ++ } ++ else ++ { ++ wireColor += btVector3 (.5f,0.f,0.f); ++ } + } +- else ++ if(colObj->getActivationState()==2) //ISLAND_SLEEPING + { +- wireColor += btVector3 (0.f,0.5f,0.f); ++ if(i&1) ++ { ++ wireColor += btVector3 (0.f,1.f, 0.f); ++ } ++ else ++ { ++ wireColor += btVector3 (0.f,0.5f,0.f); ++ } + } +- } - btVector3 aabbMin(0,0,0),aabbMax(0,0,0); - //m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); -+ btVector3 aabbMin,aabbMax; -+ m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); - - aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); -@@ -1228,10 +1218,17 @@ +- +- aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); +- aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); +-// printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ()); +-// printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ()); +-// m_dynamicsWorld->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1)); ++ btVector3 aabbMin,aabbMax; ++ m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); ++ ++ aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); ++ aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); ++ // printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ()); ++ // printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ()); ++ // m_dynamicsWorld->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1)); + + +- if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe)) +- { +- switch(pass) ++ if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe)) + { +- case 0: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break; +- case 1: m_shapeDrawer->drawShadow(m,m_sundirection*rot,colObj->getCollisionShape(),aabbMin,aabbMax);break; +- case 2: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor*btScalar(0.3),0,aabbMin,aabbMax);break; ++ switch(pass) ++ { ++ case 0: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break; ++ case 1: m_shapeDrawer->drawShadow(m,m_sundirection*rot,colObj->getCollisionShape(),aabbMin,aabbMax);break; ++ case 2: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor*btScalar(0.3),0,aabbMin,aabbMax);break; ++ } + } + } + } } // @@ -1390,7 +723,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem updateCamera(); if (m_dynamicsWorld) -@@ -1334,10 +1331,35 @@ +@@ -1334,10 +1335,35 @@ void DemoApplication::renderme() } @@ -1427,583 +760,10 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { removePickingConstraint(); -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp.rej Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.cpp.rej ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp.rej 1969-12-31 19:00:00.000000000 -0500 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.cpp.rej 2014-05-20 15:58:13.453527009 -0400 -@@ -0,0 +1,569 @@ -+--- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.cpp 2014-05-20 15:37:13.685484285 -0400 -++++ Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.cpp 2014-05-12 12:02:18.297093465 -0400 -+@@ -14,7 +14,7 @@ -+ */ -+ -+ -+-#include "DemoApplication.h" -++#include "tgDemoApplication.h" -+ #include "LinearMath/btIDebugDraw.h" -+ #include "BulletDynamics/Dynamics/btDynamicsWorld.h" -+ -+@@ -33,6 +33,7 @@ -+ #include "LinearMath/btSerializer.h" -+ #include "GLDebugFont.h" -+ -++#include -+ -+ extern bool gDisableDeactivation; -+ int numObjects = 0; -+@@ -55,7 +56,7 @@ -+ #endif // -+ -+ -+-DemoApplication::DemoApplication() -++tgDemoApplication::tgDemoApplication() -+ //see btIDebugDraw.h for modes -+ : -+ m_dynamicsWorld(0), -+@@ -85,6 +86,9 @@ -+ m_stepping(true), -+ m_singleStep(false), -+ m_idle(false), -++exitFlag(false), -++m_autocam(false), -++ -+ -+ m_enableshadows(false), -+ m_sundirection(btVector3(1,-2,1)*1000), -+@@ -101,7 +105,7 @@ -+ -+ -+ -+-DemoApplication::~DemoApplication() -++tgDemoApplication::~tgDemoApplication() -+ { -+ #ifndef BT_NO_PROFILE -+ CProfileManager::Release_Iterator(m_profileIterator); -+@@ -112,17 +116,19 @@ -+ -+ if (m_shapeDrawer) -+ delete m_shapeDrawer; -++ -++ std::cout<< "deleted tgDemoApplication" <enableTexture (m_shapeDrawer->hasTextureEnabled()); -+ delete m_shapeDrawer; -+ m_shapeDrawer = shapeDrawer; -+ } -+ -+-void DemoApplication::myinit(void) -++void tgDemoApplication::myinit(void) -+ { -+ -+ GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) }; -+@@ -158,19 +164,19 @@ -+ } -+ -+ -+-void DemoApplication::setCameraDistance(float dist) -++void tgDemoApplication::setCameraDistance(float dist) -+ { -+ m_cameraDistance = dist; -+ } -+ -+-float DemoApplication::getCameraDistance() -++float tgDemoApplication::getCameraDistance() -+ { -+ return m_cameraDistance; -+ } -+ -+ -+ -+-void DemoApplication::toggleIdle() { -++void tgDemoApplication::toggleIdle() { -+ if (m_idle) { -+ m_idle = false; -+ } -+@@ -182,7 +188,7 @@ -+ -+ -+ -+-void DemoApplication::updateCamera() { -++void tgDemoApplication::updateCamera() { -+ -+ -+ glMatrixMode(GL_PROJECTION); -+@@ -254,30 +260,30 @@ -+ -+ const float STEPSIZE = 5; -+ -+-void DemoApplication::stepLeft() -++void tgDemoApplication::stepLeft() -+ { -+ m_azi -= STEPSIZE; if (m_azi < 0) m_azi += 360; updateCamera(); -+ } -+-void DemoApplication::stepRight() -++void tgDemoApplication::stepRight() -+ { -+ m_azi += STEPSIZE; if (m_azi >= 360) m_azi -= 360; updateCamera(); -+ } -+-void DemoApplication::stepFront() -++void tgDemoApplication::stepFront() -+ { -+ m_ele += STEPSIZE; if (m_ele >= 360) m_ele -= 360; updateCamera(); -+ } -+-void DemoApplication::stepBack() -++void tgDemoApplication::stepBack() -+ { -+ m_ele -= STEPSIZE; if (m_ele < 0) m_ele += 360; updateCamera(); -+ } -+-void DemoApplication::zoomIn() -++void tgDemoApplication::zoomIn() -+ { -+ m_cameraDistance -= btScalar(m_zoomStepSize); updateCamera(); -+ if (m_cameraDistance < btScalar(0.1)) -+ m_cameraDistance = btScalar(0.1); -+ -+ } -+-void DemoApplication::zoomOut() -++void tgDemoApplication::zoomOut() -+ { -+ m_cameraDistance += btScalar(m_zoomStepSize); updateCamera(); -+ -+@@ -292,7 +298,7 @@ -+ -+ -+ -+-void DemoApplication::reshape(int w, int h) -++void tgDemoApplication::reshape(int w, int h) -+ { -+ GLDebugResetFont(w,h); -+ -+@@ -305,7 +311,7 @@ -+ -+ -+ -+-void DemoApplication::keyboardCallback(unsigned char key, int x, int y) -++void tgDemoApplication::keyboardCallback(unsigned char key, int x, int y) -+ { -+ (void)x; -+ (void)y; -+@@ -326,31 +332,13 @@ -+ -+ switch (key) -+ { -+- case 8: -+- { -+- int numObj = getDynamicsWorld()->getNumCollisionObjects(); -+- if (numObj) -+- { -+- btCollisionObject* obj = getDynamicsWorld()->getCollisionObjectArray()[numObj-1]; -+- -+- getDynamicsWorld()->removeCollisionObject(obj); -+- btRigidBody* body = btRigidBody::upcast(obj); -+- if (body && body->getMotionState()) -+- { -+- delete body->getMotionState(); -+- } -+- delete obj; -+- -+- -+- } -+- break; -+- } -+ case 'q' : -+ #ifdef BT_USE_FREEGLUT -+ //return from glutMainLoop(), detect memory leaks etc. -+ glutLeaveMainLoop(); -++ exitFlag = true; -+ #else -+- exit(0); -++ exit(0); -+ #endif -+ break; -+ -+@@ -507,6 +495,12 @@ -+ m_ShootBoxInitialSpeed -= 10.f; -+ break; -+ } -++ -++ case ';': -++ { -++ m_autocam=!m_autocam; -++ break; -++ } -+ -+ default: -+ // std::cout << "unused key : " << key << std::endl; -+@@ -520,7 +514,7 @@ -+ -+ } -+ -+-void DemoApplication::setDebugMode(int mode) -++void tgDemoApplication::setDebugMode(int mode) -+ { -+ m_debugMode = mode; -+ if (getDynamicsWorld() && getDynamicsWorld()->getDebugDrawer()) -+@@ -532,7 +526,7 @@ -+ -+ -+ -+-void DemoApplication::moveAndDisplay() -++void tgDemoApplication::moveAndDisplay() -+ { -+ if (!m_idle) -+ clientMoveAndDisplay(); -+@@ -543,13 +537,13 @@ -+ -+ -+ -+-void DemoApplication::displayCallback() -++void tgDemoApplication::displayCallback() -+ { -+ } -+ -+ #define NUM_SPHERES_ON_DIAGONAL 9 -+ -+-void DemoApplication::setShootBoxShape () -++void tgDemoApplication::setShootBoxShape () -+ { -+ if (!m_shootBoxShape) -+ { -+@@ -559,7 +553,7 @@ -+ } -+ } -+ -+-void DemoApplication::shootBox(const btVector3& destination) -++void tgDemoApplication::shootBox(const btVector3& destination) -+ { -+ -+ if (m_dynamicsWorld) -+@@ -597,11 +591,11 @@ -+ int gPickingConstraintId = 0; -+ btVector3 gOldPickingPos; -+ btVector3 gHitPos(-1,-1,-1); -+-btScalar gOldPickingDist = 0.f; -++float gOldPickingDist = 0.f; -+ btRigidBody* pickedBody = 0;//for deactivation state -+ -+ -+-btVector3 DemoApplication::getRayTo(int x,int y) -++btVector3 tgDemoApplication::getRayTo(int x,int y) -+ { -+ -+ -+@@ -674,7 +668,7 @@ -+ btScalar mousePickClamping = 30.f; -+ -+ -+-void DemoApplication::mouseFunc(int button, int state, int x, int y) -++void tgDemoApplication::mouseFunc(int button, int state, int x, int y) -+ { -+ if (state == 0) -+ { -+@@ -773,14 +767,82 @@ -+ if (rayCallback.hasHit()) -+ { -+ -+- btVector3 pickPos = rayCallback.m_hitPointWorld; -+- -+- pickObject(pickPos, rayCallback.m_collisionObject); -+- -+- gOldPickingPos = rayTo; -+- gHitPos = pickPos; -+ -+- gOldPickingDist = (pickPos-rayFrom).length(); -++ btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); -++ if (body) -++ { -++ //other exclusions? -++ if (!(body->isStaticObject() || body->isKinematicObject())) -++ { -++ pickedBody = body; -++ pickedBody->setActivationState(DISABLE_DEACTIVATION); -++ -++ -++ btVector3 pickPos = rayCallback.m_hitPointWorld; -++ //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); -++ -++ -++ btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; -++ -++ -++ -++ -++ -++ -++ if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0) -++ { -++ btTransform tr; -++ tr.setIdentity(); -++ tr.setOrigin(localPivot); -++ btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false); -++ dof6->setLinearLowerLimit(btVector3(0,0,0)); -++ dof6->setLinearUpperLimit(btVector3(0,0,0)); -++ dof6->setAngularLowerLimit(btVector3(0,0,0)); -++ dof6->setAngularUpperLimit(btVector3(0,0,0)); -++ -++ m_dynamicsWorld->addConstraint(dof6,true); -++ m_pickConstraint = dof6; -++ -++ dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0); -++ dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1); -++ dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2); -++ dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3); -++ dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4); -++ dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5); -++ -++ dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0); -++ dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1); -++ dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2); -++ dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3); -++ dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4); -++ dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5); -++ } else -++ { -++ btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); -++ m_dynamicsWorld->addConstraint(p2p,true); -++ m_pickConstraint = p2p; -++ p2p->m_setting.m_impulseClamp = mousePickClamping; -++ //very weak constraint for picking -++ p2p->m_setting.m_tau = 0.001f; -++/* -++ p2p->setParam(BT_CONSTRAINT_CFM,0.8,0); -++ p2p->setParam(BT_CONSTRAINT_CFM,0.8,1); -++ p2p->setParam(BT_CONSTRAINT_CFM,0.8,2); -++ p2p->setParam(BT_CONSTRAINT_ERP,0.1,0); -++ p2p->setParam(BT_CONSTRAINT_ERP,0.1,1); -++ p2p->setParam(BT_CONSTRAINT_ERP,0.1,2); -++ */ -++ -++ -++ } -++ -++ //save mouse position for dragging -++ gOldPickingPos = rayTo; -++ gHitPos = pickPos; -++ -++ gOldPickingDist = (pickPos-rayFrom).length(); -++ } -++ } -+ } -+ } -+ -+@@ -799,79 +861,7 @@ -+ -+ } -+ -+-void DemoApplication::pickObject(const btVector3& pickPos, const btCollisionObject* hitObj) -+-{ -+- -+- btRigidBody* body = (btRigidBody*)btRigidBody::upcast(hitObj); -+- if (body) -+- { -+- //other exclusions? -+- if (!(body->isStaticObject() || body->isKinematicObject())) -+- { -+- pickedBody = body; -+- pickedBody->setActivationState(DISABLE_DEACTIVATION); -+- -+- -+- //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); -+- -+- -+- btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; -+- -+- if ((m_modifierKeys& BT_ACTIVE_SHIFT)!=0) -+- { -+- btTransform tr; -+- tr.setIdentity(); -+- tr.setOrigin(localPivot); -+- btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false); -+- dof6->setLinearLowerLimit(btVector3(0,0,0)); -+- dof6->setLinearUpperLimit(btVector3(0,0,0)); -+- dof6->setAngularLowerLimit(btVector3(0,0,0)); -+- dof6->setAngularUpperLimit(btVector3(0,0,0)); -+- -+- m_dynamicsWorld->addConstraint(dof6,true); -+- m_pickConstraint = dof6; -+- -+- dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0); -+- dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1); -+- dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2); -+- dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3); -+- dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4); -+- dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5); -+- -+- dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0); -+- dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1); -+- dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2); -+- dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3); -+- dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4); -+- dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5); -+- } else -+- { -+- btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); -+- m_dynamicsWorld->addConstraint(p2p,true); -+- m_pickConstraint = p2p; -+- p2p->m_setting.m_impulseClamp = mousePickClamping; -+- //very weak constraint for picking -+- p2p->m_setting.m_tau = 0.001f; -+- /* -+- p2p->setParam(BT_CONSTRAINT_CFM,0.8,0); -+- p2p->setParam(BT_CONSTRAINT_CFM,0.8,1); -+- p2p->setParam(BT_CONSTRAINT_CFM,0.8,2); -+- p2p->setParam(BT_CONSTRAINT_ERP,0.1,0); -+- p2p->setParam(BT_CONSTRAINT_ERP,0.1,1); -+- p2p->setParam(BT_CONSTRAINT_ERP,0.1,2); -+- */ -+- -+- -+- } -+- -+- //save mouse position for dragging -+- -+- } -+- } -+- -+-} -+- -+-void DemoApplication::removePickingConstraint() -++void tgDemoApplication::removePickingConstraint() -+ { -+ if (m_pickConstraint && m_dynamicsWorld) -+ { -+@@ -885,7 +875,7 @@ -+ } -+ } -+ -+-void DemoApplication::mouseMotionFunc(int x,int y) -++void tgDemoApplication::mouseMotionFunc(int x,int y) -+ { -+ -+ if (m_pickConstraint) -+@@ -1006,7 +996,7 @@ -+ -+ -+ -+-btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape) -++btRigidBody* tgDemoApplication::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape) -+ { -+ btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE)); -+ -+@@ -1039,7 +1029,7 @@ -+ } -+ -+ //See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho -+-void DemoApplication::setOrthographicProjection() -++void tgDemoApplication::setOrthographicProjection() -+ { -+ -+ // switch to projection mode -+@@ -1063,7 +1053,7 @@ -+ -+ } -+ -+-void DemoApplication::resetPerspectiveProjection() -++void tgDemoApplication::resetPerspectiveProjection() -+ { -+ -+ glMatrixMode(GL_PROJECTION); -+@@ -1077,14 +1067,14 @@ -+ -+ extern CProfileIterator * m_profileIterator; -+ -+-void DemoApplication::displayProfileString(int xOffset,int yStart,char* message) -++void tgDemoApplication::displayProfileString(int xOffset,int yStart,char* message) -+ { -+ glRasterPos3f(btScalar(xOffset),btScalar(yStart),btScalar(0)); -+ GLDebugDrawString(xOffset,yStart,message); -+ } -+ -+ -+-void DemoApplication::showProfileInfo(int& xOffset,int& yStart, int yIncr) -++void tgDemoApplication::showProfileInfo(int& xOffset,int& yStart, int yIncr) -+ { -+ #ifndef BT_NO_PROFILE -+ -+@@ -1158,7 +1148,7 @@ -+ -+ -+ // -+-void DemoApplication::renderscene(int pass) -++void tgDemoApplication::renderscene(int pass) -+ { -+ btScalar m[16]; -+ btMatrix3x3 rot;rot.setIdentity(); -+@@ -1205,8 +1195,8 @@ -+ } -+ } -+ -+- btVector3 aabbMin(0,0,0),aabbMax(0,0,0); -+- //m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); -++ btVector3 aabbMin,aabbMax; -++ m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); -+ -+ aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); -+ aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); -+@@ -1228,10 +1218,17 @@ -+ } -+ -+ // -+-void DemoApplication::renderme() -++void tgDemoApplication::renderme() -+ { -+ myinit(); -++ -++ const btVector3 ps = getPs(); -+ -++ if (m_autocam) -++ { -++ m_cameraTargetPosition += (ps - m_cameraTargetPosition) * 0.05; -++ } -++ -+ updateCamera(); -+ -+ if (m_dynamicsWorld) -+@@ -1334,10 +1331,35 @@ -+ -+ } -+ -++btVector3 tgDemoApplication::getPs() -++{ -++ btVector3 ps(0.0, 0.0, 0.0); -++ size_t nps = 0; -++ -++ const btCollisionObjectArray btarr = -++ getDynamicsWorld()->getCollisionObjectArray(); -++ const size_t count = btarr.size(); -++ nps += count; -++ /// @todo Use std::for_each() -++ for (int ib = 0; ib < count; ++ib) -++ { -++ const btRigidBody* const pRigid = -++ static_cast(btarr[ib]); -++ ps += pRigid->getCenterOfMassPosition(); -++ } -++ -++ if (nps != 0) -++ { -++ ps /= static_cast(nps); -++ } -++ -++ return ps; -++} -++ -+ #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" -+ -+ -+-void DemoApplication::clientResetScene() -++void tgDemoApplication::clientResetScene() -+ { -+ removePickingConstraint(); -+ -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.h Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.h ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDemoApplication.h 2014-05-20 16:21:25.837574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgDemoApplication.h 2014-05-12 12:02:18.309093466 -0400 -@@ -13,11 +13,11 @@ +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.h NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.h +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.h 2014-11-10 09:15:18.684913618 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgDemoApplication.h 2014-08-18 20:29:03.002095342 -0700 +@@ -13,11 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ @@ -2018,7 +778,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem #include "GL_ShapeDrawer.h" #include -@@ -38,7 +38,7 @@ +@@ -38,7 +38,7 @@ class btTypedConstraint; @@ -2027,7 +787,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem { protected: void displayProfileString(int xOffset,int yStart,char* message); -@@ -57,9 +57,6 @@ +@@ -57,9 +57,6 @@ protected: virtual void removePickingConstraint(); @@ -2037,7 +797,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem btCollisionShape* m_shootBoxShape; float m_cameraDistance; -@@ -73,6 +70,8 @@ +@@ -73,6 +70,8 @@ protected: int m_mouseOldX; int m_mouseOldY; int m_mouseButtons; @@ -2046,7 +806,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem public: int m_modifierKeys; protected: -@@ -106,11 +105,15 @@ +@@ -106,11 +105,15 @@ protected: btVector3 m_sundirection; btScalar m_defaultContactProcessingThreshold; @@ -2064,7 +824,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem btDynamicsWorld* getDynamicsWorld() { -@@ -118,6 +121,8 @@ +@@ -118,6 +121,8 @@ public: } virtual void initPhysics() = 0; @@ -2073,7 +833,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem virtual void setDrawClusters(bool drawClusters) { -@@ -152,11 +157,6 @@ +@@ -152,11 +157,6 @@ public: { m_azi = azi; } @@ -2085,7 +845,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem void setCameraUp(const btVector3& camUp) { -@@ -208,7 +208,7 @@ +@@ -208,7 +208,7 @@ public: virtual void clientResetScene(); @@ -2094,7 +854,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem virtual void setShootBoxShape (); virtual void shootBox(const btVector3& destination); -@@ -257,10 +257,16 @@ +@@ -257,10 +257,16 @@ public: { m_idle = idle; } @@ -2113,9 +873,9 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgDem +#endif //TG_DEMO_APPLICATION_H -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgGLDebugDrawer.cpp ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp 2014-05-20 16:21:25.841574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgGLDebugDrawer.cpp 2014-05-12 12:02:18.309093466 -0400 +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp 2014-11-10 09:15:18.704913617 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.cpp 2014-08-18 20:29:03.002095342 -0700 @@ -1,22 +1,22 @@ -#include "GLDebugDrawer.h" @@ -2144,7 +904,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLD { glBegin(GL_LINES); glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ()); -@@ -26,12 +26,12 @@ +@@ -26,12 +26,12 @@ void GLDebugDrawer::drawLine(const btVec glEnd(); } @@ -2159,7 +919,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLD { glColor4f (color.getX(), color.getY(), color.getZ(), btScalar(1.0f)); glPushMatrix (); -@@ -69,7 +69,7 @@ +@@ -69,7 +69,7 @@ void GLDebugDrawer::drawSphere (const bt @@ -2168,7 +928,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLD { // if (m_debugMode > 0) { -@@ -84,24 +84,24 @@ +@@ -84,24 +84,24 @@ void GLDebugDrawer::drawTriangle(const b } } @@ -2197,9 +957,9 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLD { { -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h Tensegrity/trunk/NTRT/src/core/bullet/tgGLDebugDrawer.h ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h 2014-05-20 16:21:25.837574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgGLDebugDrawer.h 2014-05-12 12:02:18.289093465 -0400 +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h 2014-11-10 09:15:18.696913618 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGLDebugDrawer.h 2014-08-18 20:29:03.006093341 -0700 @@ -1,18 +1,18 @@ -#ifndef GL_DEBUG_DRAWER_H -#define GL_DEBUG_DRAWER_H @@ -2224,9 +984,9 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGLD virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor); -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgGlutDemoApplication.cpp ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp 2014-05-20 16:21:25.841574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgGlutDemoApplication.cpp 2014-05-12 12:02:18.289093465 -0400 +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp 2014-11-10 09:15:18.716913617 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.cpp 2014-08-18 20:29:03.006093341 -0700 @@ -1,14 +1,14 @@ #ifndef _WINDOWS @@ -2245,7 +1005,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu { m_modifierKeys = 0; if (glutGetModifiers() & GLUT_ACTIVE_ALT) -@@ -21,7 +21,7 @@ +@@ -21,7 +21,7 @@ void GlutDemoApplication::updateModifier m_modifierKeys |= BT_ACTIVE_SHIFT; } @@ -2254,7 +1014,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu { (void)x; (void)y; -@@ -43,21 +43,8 @@ +@@ -43,21 +43,8 @@ void GlutDemoApplication::specialKeyboar case GLUT_KEY_END: { @@ -2278,7 +1038,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu break; } case GLUT_KEY_LEFT : stepLeft(); break; -@@ -76,7 +63,7 @@ +@@ -76,7 +63,7 @@ void GlutDemoApplication::specialKeyboar } @@ -2287,10 +1047,10 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu { glutSwapBuffers(); -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h Tensegrity/trunk/NTRT/src/core/bullet/tgGlutDemoApplication.h ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h 2014-05-20 16:21:25.841574231 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgGlutDemoApplication.h 2014-05-12 12:02:18.301093466 -0400 -@@ -14,12 +14,12 @@ +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h 2014-11-10 09:15:18.708913617 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutDemoApplication.h 2014-08-18 20:29:03.006093341 -0700 +@@ -14,12 +14,12 @@ subject to the following restrictions: */ @@ -2307,17 +1067,17 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu { public: -@@ -32,5 +32,5 @@ +@@ -32,5 +32,5 @@ public: virtual void updateModifierKeys(); }; -#endif //GLUT_DEMO_APPLICATION_H +#endif //TG_GLUT_DEMO_APPLICATION_H -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp Tensegrity/trunk/NTRT/src/core/bullet/tgGlutStuff.cpp ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp 2014-05-20 16:21:25.845574232 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgGlutStuff.cpp 2014-05-12 12:02:18.289093465 -0400 -@@ -15,77 +15,82 @@ +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp 2014-11-10 09:15:19.724913575 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.cpp 2014-08-18 20:29:03.006093341 -0700 +@@ -15,77 +15,82 @@ subject to the following restrictions: #ifndef _WINDOWS @@ -2418,7 +1178,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu glutKeyboardFunc(glutKeyboardCallback); glutKeyboardUpFunc(glutKeyboardUpCallback); -@@ -110,11 +115,13 @@ +@@ -110,11 +115,13 @@ CGLContextObj cgl_context = CGLGetCurren CGLSetParameter(cgl_context, kCGLCPSwapInterval, &swap_interval); #endif @@ -2436,10 +1196,10 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu +#endif #endif //_WINDOWS -diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutStuff.h Tensegrity/trunk/NTRT/src/core/bullet/tgGlutStuff.h ---- SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlutStuff.h 2014-05-20 16:21:25.845574232 -0400 -+++ Tensegrity/trunk/NTRT/src/core/bullet/tgGlutStuff.h 2014-05-12 12:02:18.297093465 -0400 -@@ -40,7 +40,7 @@ +diff -rupN NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.h NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.h +--- NTRTTests/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.h 2014-11-10 09:15:18.720913617 -0800 ++++ NTRTsim/env/build/bullet/Demos/OpenGL_FreeGlut/tgGlutStuff.h 2014-08-18 20:29:03.006093341 -0700 +@@ -40,7 +40,7 @@ subject to the following restrictions: #ifdef _WINDOWS #define BT_ACTIVE_ALT VK_LMENU @@ -2448,7 +1208,7 @@ diff -ruN SimulatorPatch/env/build/bullet-2.82-r2704/Demos/OpenGL_FreeGlut/tgGlu #else #define BT_KEY_K 'k' #define BT_KEY_LEFT GLUT_KEY_LEFT -@@ -67,9 +67,11 @@ +@@ -67,9 +67,11 @@ subject to the following restrictions: diff --git a/bin/setup/setup_bullet.sh b/bin/setup/setup_bullet.sh index 124ac1548..2a27e42bc 100755 --- a/bin/setup/setup_bullet.sh +++ b/bin/setup/setup_bullet.sh @@ -161,7 +161,7 @@ function build_bullet() -DCMAKE_EXE_LINKER_FLAGS="-fPIC" \ -DCMAKE_MODULE_LINKER_FLAGS="-fPIC" \ -DCMAKE_SHARED_LINKER_FLAGS="-fPIC" \ - -DUSE_DOUBLE_PRECISION=OFF \ + -DUSE_DOUBLE_PRECISION=ON \ -DCMAKE_INSTALL_NAME_DIR="$BULLET_INSTALL_PREFIX" || { echo "- ERROR: CMake for Bullet Physics failed."; exit 1; } #If you turn this on, turn it on in inc.CMakeBullet.txt as well for the NTRT build # Additional bullet options: diff --git a/resources/src/learningSpines/TetrahedralComplex/logs/scores.csv b/resources/src/learningSpines/TetrahedralComplex/logs/scores.csv index d3102118d..7ccf49479 100644 --- a/resources/src/learningSpines/TetrahedralComplex/logs/scores.csv +++ b/resources/src/learningSpines/TetrahedralComplex/logs/scores.csv @@ -1,4 +1,4 @@ -188.694,0,0.574355,0.667178,0.92242,0.951502,0.649085,0.370246,0.63157,0.798183,0.252216,0.109682,0.570909,0.289227,0.206585,0.565489,0.226903,0.608789,0.627033,0.874933,0.949421,0.390933,0.331501,0.517563,0.336555,0.656275,0.201108,0.755367,0.301678,0.734099,0.0243267,0.308478,0.825867,0.892307,0.418799,0.505121,0,0.150197,0.6989,0.636736,0.219761,0.260838,0.121364,0.384688,0.847468,0.336644,0.886209,0.622353,0.368257,0.683465,0.629894,0.213689,0.381253,0.112479,0.542324,0.25011,0.385549,0.0569112,0.249051,0.944199,0.890838,0.681835,0.39601,0.0426024,0.563194,0.756648,0.15785,0.152261,0.648003,0.503404,0.517371,0.187282,0.264245,0.94945,0.0949539,0.499047,0.632035,0.501327,0.805586,0.940529,0.838961,0.136846,0.966131,0.615388,0.656599,0.878884,0.0604043,0.467032,0.970744,0.174869,0.0876592,0.110583,0.650787,0.827132,0.190299,0.472693,0.464487,0.730811,0.785389,0.731433,0.512723,0.296963,0.666727,0.669905,0.0683262,0.678516,0.0884876,0.162832,0.221118,0.337972,0.885626,0.364452,0.669965,0.942653,0.688506,0.227925,0.307416,0.966719,0.847357,0.534307,0.0190767,0.956831,0.0718153,0.463076,0.853016,0.0747419,0.252248,0.832607,0.203762,0.865142,0.223195,0.751554,0.823704,0.263856,0.800104,0.725464,0.906961,0.771818,0.965239,0.97043,0.550734,0.0159571,0.549538,0.226366,0.15271,0.303892,0.22255,0.810243,0.30181,0.39315,0.341047,0.247736,0.267545,0.288896,0.822032,0.12065,0.112453,0.945476,0.0719805,0.0451206,0.804337,0.703803,0.637648,0.825483,0.757174,0.526074,0.868302,0.516448,0.721486,0.79417,0.457379,0.83158,0.726853,0.00212634,0.177735,0.634874,0.0636932,0.20401,0.116145,0.644262,0.931133,0.29104,0.716784,0.2831,0.933681,0.926569,0.11613,0.0894762,0.216428,0.762989,0.792675,0.470684,0.138896,0.922645,0.0299032,0.503953,0.506732,0.21569,0.814534,0.974611,0.286567,0.766467 -188.694,0,0.563107,0.771837 +197.632,0,0.574355,0.667178,0.92242,0.951502,0.649085,0.370246,0.63157,0.798183,0.252216,0.109682,0.570909,0.289227,0.206585,0.565489,0.226903,0.608789,0.627033,0.874933,0.949421,0.390933,0.331501,0.517563,0.336555,0.656275,0.201108,0.755367,0.301678,0.734099,0.0243267,0.308478,0.825867,0.892307,0.418799,0.505121,0,0.150197,0.6989,0.636736,0.219761,0.260838,0.121364,0.384688,0.847468,0.336644,0.886209,0.622353,0.368257,0.683465,0.629894,0.213689,0.381253,0.112479,0.542324,0.25011,0.385549,0.0569112,0.249051,0.944199,0.890838,0.681835,0.39601,0.0426024,0.563194,0.756648,0.15785,0.152261,0.648003,0.503404,0.517371,0.187282,0.264245,0.94945,0.0949539,0.499047,0.632035,0.501327,0.805586,0.940529,0.838961,0.136846,0.966131,0.615388,0.656599,0.878884,0.0604043,0.467032,0.970744,0.174869,0.0876592,0.110583,0.650787,0.827132,0.190299,0.472693,0.464487,0.730811,0.785389,0.731433,0.512723,0.296963,0.666727,0.669905,0.0683262,0.678516,0.0884876,0.162832,0.221118,0.337972,0.885626,0.364452,0.669965,0.942653,0.688506,0.227925,0.307416,0.966719,0.847357,0.534307,0.0190767,0.956831,0.0718153,0.463076,0.853016,0.0747419,0.252248,0.832607,0.203762,0.865142,0.223195,0.751554,0.823704,0.263856,0.800104,0.725464,0.906961,0.771818,0.965239,0.97043,0.550734,0.0159571,0.549538,0.226366,0.15271,0.303892,0.22255,0.810243,0.30181,0.39315,0.341047,0.247736,0.267545,0.288896,0.822032,0.12065,0.112453,0.945476,0.0719805,0.0451206,0.804337,0.703803,0.637648,0.825483,0.757174,0.526074,0.868302,0.516448,0.721486,0.79417,0.457379,0.83158,0.726853,0.00212634,0.177735,0.634874,0.0636932,0.20401,0.116145,0.644262,0.931133,0.29104,0.716784,0.2831,0.933681,0.926569,0.11613,0.0894762,0.216428,0.762989,0.792675,0.470684,0.138896,0.922645,0.0299032,0.503953,0.506732,0.21569,0.814534,0.974611,0.286567,0.766467 +197.632,0,0.563107,0.771837 0,0,0.574355,0.667178,0.92242,0.951502,0.649085,0.370246,0.63157,0.798183,0.252216,0.109682,0.570909,0.289227,0.206585,0.565489,0.226903,0.608789,0.627033,0.874933,0.949421,0.390933,0.331501,0.517563,0.336555,0.656275,0.201108,0.755367,0.301678,0.734099,0.0243267,0.308478,0.825867,0.892307,0.418799,0.505121,0,0.150197,0.6989,0.636736,0.219761,0.260838,0.121364,0.384688,0.847468,0.336644,0.886209,0.622353,0.368257,0.683465,0.629894,0.213689,0.381253,0.112479,0.542324,0.25011,0.385549,0.0569112,0.249051,0.944199,0.890838,0.681835,0.39601,0.0426024,0.563194,0.756648,0.15785,0.152261,0.648003,0.503404,0.517371,0.187282,0.264245,0.94945,0.0949539,0.499047,0.632035,0.501327,0.805586,0.940529,0.838961,0.136846,0.966131,0.615388,0.656599,0.878884,0.0604043,0.467032,0.970744,0.174869,0.0876592,0.110583,0.650787,0.827132,0.190299,0.472693,0.464487,0.730811,0.785389,0.731433,0.512723,0.296963,0.666727,0.669905,0.0683262,0.678516,0.0884876,0.162832,0.221118,0.337972,0.885626,0.364452,0.669965,0.942653,0.688506,0.227925,0.307416,0.966719,0.847357,0.534307,0.0190767,0.956831,0.0718153,0.463076,0.853016,0.0747419,0.252248,0.832607,0.203762,0.865142,0.223195,0.751554,0.823704,0.263856,0.800104,0.725464,0.906961,0.771818,0.965239,0.97043,0.550734,0.0159571,0.549538,0.226366,0.15271,0.303892,0.22255,0.810243,0.30181,0.39315,0.341047,0.247736,0.267545,0.288896,0.822032,0.12065,0.112453,0.945476,0.0719805,0.0451206,0.804337,0.703803,0.637648,0.825483,0.757174,0.526074,0.868302,0.516448,0.721486,0.79417,0.457379,0.83158,0.726853,0.00212634,0.177735,0.634874,0.0636932,0.20401,0.116145,0.644262,0.931133,0.29104,0.716784,0.2831,0.933681,0.926569,0.11613,0.0894762,0.216428,0.762989,0.792675,0.470684,0.138896,0.922645,0.0299032,0.503953,0.506732,0.21569,0.814534,0.974611,0.286567,0.766467 0,0,0.563107,0.771837 diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 1792f4aec..4c8db3db1 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -3,6 +3,7 @@ project(core) add_library( ${PROJECT_NAME} SHARED tgWorldBulletPhysicsImpl.cpp + muscleAnchor.cpp Muscle2P.cpp ImpedanceControl.cpp diff --git a/src/core/Muscle2P.cpp b/src/core/Muscle2P.cpp index de95e23a7..2e2834c58 100644 --- a/src/core/Muscle2P.cpp +++ b/src/core/Muscle2P.cpp @@ -25,30 +25,43 @@ // This module #include "Muscle2P.h" +#include "muscleAnchor.h" // The BulletPhysics library #include "BulletDynamics/Dynamics/btRigidBody.h" #include +#include -Muscle2P::Muscle2P(btRigidBody * body1, - btVector3 pos1, - btRigidBody * body2, - btVector3 pos2, - double coefK, - double dampingCoefficient) : +Muscle2P::Muscle2P( const std::vector& anchors, + double coefK, + double dampingCoefficient, + double pretension) : m_velocity(0.0), m_damping(0.0), m_coefK (coefK), -m_dampingCoefficient(dampingCoefficient) - +m_dampingCoefficient(dampingCoefficient), +m_anchors(anchors), +anchor1(anchors.front()), +anchor2(anchors.back()) { - anchor1 = new muscleAnchor(body1, pos1); - anchor2 = new muscleAnchor(body2, pos2); - m_restLength = pos1.distance(pos2); - m_prevLength = m_restLength; + assert(m_anchors.size() >= 2); + assert(coefK > 0.0); + assert(dampingCoefficient >= 0.0); + + btVector3 pos1 = anchor1->getWorldPosition(); + btVector3 pos2 = anchor2->getWorldPosition(); + + m_restLength = pos1.distance(pos2) - pretension / coefK; + + if (m_restLength <= 0.0) + { + throw std::invalid_argument("Pretension causes string to shorten past rest length!"); + } + + m_prevLength = m_restLength; } -btVector3 Muscle2P::calculateAndApplyForce(double dt) +void Muscle2P::calculateAndApplyForce(double dt) { btVector3 force(0.0, 0.0, 0.0); double magnitude = 0.0; @@ -99,8 +112,6 @@ btVector3 Muscle2P::calculateAndApplyForce(double dt) btVector3 point2 = this->anchor2->getRelativePosition(); this->anchor2->attachedBody->activate(); this->anchor2->attachedBody->applyImpulse(-force*dt,point2); - - return force; } void Muscle2P::setRestLength( const double newRestLength) @@ -136,44 +147,25 @@ Muscle2P::~Muscle2P() #if (0) std::cout << "Destroying Muscle2P" << std::endl; #endif - delete anchor1; - delete anchor2; -} - -// todo: make seperate class - -muscleAnchor::muscleAnchor() -{ -} - -muscleAnchor::muscleAnchor(btRigidBody * body, - btVector3 worldPos) : - attachedBody(body), - // Find relative position - // This should give relative position in a default orientation. - attachedRelativeOriginalPosition(attachedBody->getWorldTransform().inverse() * - worldPos), - height(999.0) -{ -} - -muscleAnchor::~muscleAnchor() -{ - // World should delete this - attachedBody = NULL; + + std::size_t n = m_anchors.size(); + + // Make absolutely sure these are deleted, in case we have a poorly timed reset + if (m_anchors[0] != anchor1) + { + delete anchor1; + } -} - -// This returns current position relative to the rigidbody. -btVector3 muscleAnchor::getRelativePosition() -{ - const btTransform tr = attachedBody->getWorldTransform(); - const btVector3 worldPos = tr * attachedRelativeOriginalPosition; - return worldPos-this->attachedBody->getCenterOfMassPosition(); -} + if (m_anchors[n-1] != anchor2) + { + delete anchor2; + } + + for (std::size_t i = 0; i < n; i++) + { + delete m_anchors[i]; + } -btVector3 muscleAnchor::getWorldPosition() -{ - const btTransform tr = attachedBody->getWorldTransform(); - return tr * attachedRelativeOriginalPosition; + + m_anchors.clear(); } diff --git a/src/core/Muscle2P.h b/src/core/Muscle2P.h index 17b5e263c..bf9fa7a8a 100644 --- a/src/core/Muscle2P.h +++ b/src/core/Muscle2P.h @@ -32,6 +32,7 @@ // The C++ Standard Library #include +#include // Forward references class btRigidBody; @@ -39,18 +40,17 @@ class muscleAnchor; class Muscle2P { -public: - Muscle2P(btRigidBody * body1, - btVector3 pos1, - btRigidBody * body2, - btVector3 pos2, - double coefK, - double dampingCoefficient); - +public: + // Alternative constructor + Muscle2P( const std::vector& anchors, + double coefK, + double dampingCoefficient, + double pretension = 0.0); + virtual ~Muscle2P(); // Called by tensegrity class update function for each muscle2p - virtual btVector3 calculateAndApplyForce(double dt); + virtual void calculateAndApplyForce(double dt); void setName(std::string a) { name = a; } @@ -58,7 +58,7 @@ class Muscle2P void setRestLength( const double newRestLength); - const btScalar getActualLength() const; + virtual const btScalar getActualLength() const; const double getTension() const; @@ -81,16 +81,25 @@ class Muscle2P return name; } - muscleAnchor * anchor1; + muscleAnchor * const anchor1; - muscleAnchor * anchor2; + muscleAnchor * const anchor2; + + const std::vector& getAnchors() const + { + return m_anchors; + } std::string name; bool recordHistory; +protected: + + // Wanted to do a set, but need random access iterator to sort + // Needs to be stored here for consistent rendering + std::vector m_anchors; - private: // Necessary for computations double m_restLength; @@ -105,31 +114,9 @@ class Muscle2P const btScalar m_dampingCoefficient; const btScalar m_coefK; - - bool invariant(void) const; -}; -class muscleAnchor -{ -public: - muscleAnchor(); - - muscleAnchor(btRigidBody *body, btVector3 pos); - - ~muscleAnchor(); - - btVector3 getWorldPosition(); - - // Relative to the body - btVector3 getRelativePosition(); - - btRigidBody * attachedBody; - - // Relative to the body when it is first constructed - btVector3 attachedRelativeOriginalPosition; - - btScalar height; - private: + private: + bool invariant(void) const; }; #endif // NTRT_MUSCLE2P_H_ diff --git a/src/core/muscleAnchor.cpp b/src/core/muscleAnchor.cpp new file mode 100644 index 000000000..53ad00ac3 --- /dev/null +++ b/src/core/muscleAnchor.cpp @@ -0,0 +1,307 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file MuscleAnchor.cpp + * @brief Definitions of class MuscleAnchor. + * $Id$ + */ + +#include "muscleAnchor.h" + +// The BulletPhysics library +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" + +// The C++ Standard Library +#include +#include +#include + +// Does the contact normal get updated using the body's rotation? (99% sure its yes) +#define USE_BASIS +// Do we update the contact based on the manifold? - Causes contacts to be missed, doesn't prevent angular energy from accumulating +#define SKIP_CONTACT_UPDATE +//#define VERBOSE + +muscleAnchor::muscleAnchor(btRigidBody * body, + btVector3 worldPos, + btVector3 cn, + bool perm, + bool slide, + btPersistentManifold* m) : + attachedBody(body), + // Find relative position + // This should give relative position in a default orientation. + attachedRelativeOriginalPosition(attachedBody->getWorldTransform().inverse() * + worldPos), +#ifdef USE_BASIS + contactNormal(attachedBody->getWorldTransform().inverse().getBasis() * cn), +#else + contactNormal(cn), +#endif + permanent(perm), + sliding(slide), + force(0.0, 0.0, 0.0), + manifold(m) +{ + assert(body); + + // Ensure we're either not using a manifold, or we currently have the right manifold + assert(manifold == NULL || body == manifold->getBody0() || body == manifold->getBody1()); +} + +muscleAnchor::~muscleAnchor() +{ + // World will delete attached body, bullet owns the manifolds + +} + +// This returns current position relative to the rigidbody. +btVector3 muscleAnchor::getRelativePosition() const +{ + const btTransform tr = attachedBody->getWorldTransform(); + const btVector3 worldPos = tr * attachedRelativeOriginalPosition; + return worldPos-this->attachedBody->getCenterOfMassPosition(); +} + +btVector3 muscleAnchor::getWorldPosition() const +{ + const btTransform tr = attachedBody->getWorldTransform(); + return tr * attachedRelativeOriginalPosition; +} + +bool muscleAnchor::setWorldPosition(btVector3& newPos) +{ + bool ret = false; + + // Only sliding anchors should have their positions changed + if (sliding) + { + /// @todo - this is very similar to getManifoldDistance. Is there a good way to combine them?? + // Figure out which body to use + bool useB = true; + if (manifold->getBody0() != attachedBody) + { + useB = false; + } + + if(useB || manifold->getBody1() == attachedBody) + { + btScalar length = INFINITY; + + int n = manifold->getNumContacts(); + + btVector3 contactPos = getWorldPosition(); + btVector3 newNormal = contactNormal; + btScalar dist = 0.0; + + // Find closest contact point in this manifold + for (int p = 0; p < n; p++) + { + const btManifoldPoint& pt = manifold->getContactPoint(p); + + // Original position picked at beginning + btVector3 pos = useB ? pt.m_positionWorldOnA : pt.m_positionWorldOnB; + + btScalar contactDist = (pos - newPos).length(); + + if (contactDist < length) + { + length = contactDist; + contactPos = pos; + + btScalar directionSign = useB ? btScalar(1.0) : btScalar(-1.0); + +#ifdef USE_BASIS + newNormal = attachedBody->getWorldTransform().inverse().getBasis() * pt.m_normalWorldOnB * directionSign; +#else + newNormal = pt.m_normalWorldOnB * directionSign; +#endif + dist = pt.getDistance(); + +#ifdef VERBOSE + if (n >= 2) + { + std::cout << "Extra contacts!! " << dist << std::endl; + } +#endif + } + + } + + // We've lost this contact for some reason, skip update and delete + //if (!(dist > 0.0 && length < 0.01)) + { + // If contact is sufficiently close, update + if (length < 0.1) + { + // This makes contact handling better in some cases and worse in other + // Better conservation of momentum without it, but contacts tend to exist a little too long + // Just deleting at this stage is better for sliding, but worse for contact with multiple bodies + attachedRelativeOriginalPosition = attachedBody->getWorldTransform().inverse() * + newPos; + + if ((newNormal + contactNormal).length() < 0.5) + { + std::cout<< "Reversed normal" << std::endl; + } + else + { + ret = true; + } + + // Update again here in case we have the original manifold?? + #ifndef SKIP_CONTACT_UPDATE + contactNormal = newNormal; + #endif + } + // Check if the update was bad based on the original position, if not delete + else if ( (getWorldPosition() - contactPos).length() <= 0.1) + { + ret = true; + } + } + } + // Else: neither body is attached, delete + } + else + { + std::cerr << "Tried to update a static anchor" << std::endl; + // This will return as a delete, make sure you check the anchor is not permanent + } + + return ret; +} + +btVector3 muscleAnchor::getContactNormal() const +{ + +#ifdef USE_BASIS + const btTransform tr = attachedBody->getWorldTransform(); + btVector3 newNormal = (tr.getBasis() * contactNormal); + newNormal = newNormal.length() > 0.0 ? newNormal.normalize() : btVector3(0.0, 0.0, 0.0); + //assert(newNormal.length() == 1.0); + return newNormal; +#else + return contactNormal; +#endif + +} + +bool muscleAnchor::updateManifold(btPersistentManifold* m) +{ + bool ret = false; + // Does the new manifold actually affect the attached body + if (m && (m->getBody0() == attachedBody || m->getBody1() == attachedBody )) + { + std::pair manifoldValues = getManifoldDistance(m); + btScalar newDist = manifoldValues.first; + // If the original manifold is NULL, just use the new one + if (!manifold) + { + //manifold = m; + ret = true; + } + // Use new manifold + else if (getManifoldDistance(manifold).first >= newDist) + { + //manifold = m; + ret = true; + } + + // If we updated, ensure the new contact normal is good + if(ret) + { + btVector3 newNormal = manifoldValues.second; + if ((newNormal + contactNormal).length() < 0.5) + { + std::cout <<"Reversed normal during anchor update" << std::endl; + ret = false; + } + else + { + manifold = m; + } + #ifndef SKIP_CONTACT_UPDATE + // Updating here appears to break conservation of momentum + //contactNormal = newNormal; + #endif + } + } + + return ret; +} + +std::pair muscleAnchor::getManifoldDistance(btPersistentManifold* m) const +{ + bool useB = true; + + btScalar length = INFINITY; + btVector3 newNormal = contactNormal; + + if (m->getBody0() != attachedBody) + { + useB = false; + } + if(useB || m->getBody1() == attachedBody) + { + + int n = m->getNumContacts(); + + btVector3 contactPos = getWorldPosition(); + btScalar dist = 0.0; + for (int p = 0; p < n; p++) + { + const btManifoldPoint& pt = m->getContactPoint(p); + + // Original position picked at beginning + btVector3 pos = useB ? pt.m_positionWorldOnA : pt.m_positionWorldOnB; + + btScalar contactDist = (pos - getWorldPosition()).length(); + + if (contactDist < length) + { + length = contactDist; + contactPos = pos; + + btScalar directionSign = useB ? btScalar(1.0) : btScalar(-1.0); + + if (length < 0.1) + { + #ifdef USE_BASIS + newNormal = attachedBody->getWorldTransform().inverse().getBasis() * pt.m_normalWorldOnB * directionSign; + #else + newNormal = pt.m_normalWorldOnB * directionSign; + #endif + } + + dist = pt.getDistance(); +#ifdef VERBOSE + if (n >= 2) + { + std::cout << "Extra contacts!! " << p << " length " << length << " dist: " << dist << std::endl; + } +#endif + } + } + } + + return std::make_pair (length, newNormal); +} diff --git a/src/core/muscleAnchor.h b/src/core/muscleAnchor.h new file mode 100644 index 000000000..3e2dc245f --- /dev/null +++ b/src/core/muscleAnchor.h @@ -0,0 +1,198 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef NTRT_MUSCLE_ANCHOR_H_ +#define NTRT_MUSCLE_ANCHOR_H_ + +/** + * @file MuscleAnchor.h + * @brief Definitions of class MuscleAnchor. + * @author Brian Mirletz and Atil Iscen + * @date November 2014 + * $Id$ + */ + +// The Bullet Physics library +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include +#include //std::pair + +// Forward References +class btRigidBody; +class btPersistentManifold; +class MuscleNP; + +/** + * A class that allows Muscle2P and MuscleNP to attach to btRigidBodies + * Anchors track a specific point on a body as that body translates + * and rotates. They can either be 'non-sliding' which typically means + * a pin jointed anchor (and are typically permanent), or sliding, + * which means they track a specific contact point within a + * btPersistentManifold. + */ +class muscleAnchor +{ +public: + // MuscleNP needs to scale the forces + friend class MuscleNP; + + /** + * The only constructor. At a minimum requires a body and a position + * on that body to track. Sliding anchors require additional data + * @param[in] body - a pointer to the btRigidBody this is attached to + * @param[in] pos - The position in world coordinates where this attaches + * @param[in] cn - A btVector3 that specifies the direction of contact + * Only requried for sliding anchors + * @param[in] perm - Whether or not this anchor can be deleted in + * the middle of a simulation + * @param[in] slide - Whether this represents a pin joint or a sliding contact + * @param[in] m - a btPersistenManifold that is used to track contacts + */ + muscleAnchor(btRigidBody *body, + btVector3 pos, + btVector3 cn = btVector3(0.0, 0.0, 0.0), + bool perm = true, + bool slide = false, + btPersistentManifold* m = NULL); + + /** + * Destructor, nothing to delete + */ + ~muscleAnchor(); + + /** + * Return the current position of the anchor in world coordinates + * Uses attachedRelativeOriginalPosition and the attachedBody's + * btTransform + */ + btVector3 getWorldPosition() const; + + /** + * Update attachedRelativeOriginalPosition based on the sliding + * of the string. This also checks if the new sliding position + * is still on the body. + * @return bool returns if this point is actually on the body. The + * body should be deleted if this returns false + */ + bool setWorldPosition(btVector3& newPos); + + /** + * Get the position of the point in body coordinaates + * @return a btVector3 in body coordinates + */ + btVector3 getRelativePosition() const; + + /** + * Return an up to date contact normal based on the rigid + * body's btTransform + * @return the contact normal, accounting for any rotation from + * when the body was first contacted. + */ + btVector3 getContactNormal() const; + + /** + * Update our manifold pointer. This memory is often reassigned + * so the new manifold is accepted if our old manifold no longer + * contains our rigid body. It is also accepted if its contact point + * is closer than our manifold's + * @return a bool that is true if the new manifold was accepted + */ + bool updateManifold(btPersistentManifold* m); + + /** + * Return a pointer to our btPersistentManifold (typically so + * something can compare getManifoldDistance with a new manifold) + */ + btPersistentManifold* getManifold() const + { + return manifold; + } + + /** + * @return a const reference to the force we just applied (or are + * about to apply) + */ + const btVector3& getForce() const + { + return force; + } + + /** + * The rigid body we affect + * Address should never be changed, body is not const + * @todo Create applyForce functions so this doesn't have to + * be exposed + */ + btRigidBody * const attachedBody; + + /** + * A boolean value indicating whether this a temporary or permanent contact + * if permanent do not delete it until teardown!! + */ + const bool permanent; + + /** + * How the force is applied to the rigid body. True applies along the + * contact normal, false is applied towards the next anchor. + * Application depends on other classes + * @todo Do we want an internal apply force function? May simplify things (prevent Muscles from needing to include rigid bodies??) + */ + const bool sliding; + + /** + * A pair of the distance between the current world position and + * this manifolds contact point, as well as the contact normal + * of this manifold + * @return distance to the contact, contact normal + */ + std::pair getManifoldDistance(btPersistentManifold* m) const; + +private: + + + /** + * The relative world position, stored by multiplying by + * the inverse transform of the initial orientation + */ + btVector3 attachedRelativeOriginalPosition; + + /** + * For sliding anchors: the normal between the relevent two + * btCollisionObjects. Used in determining validity of anchors and + * applying forces to rigid bodies + */ + btVector3 contactNormal; + + /** + * The manifold that generated this body if it is a sliding contact + * NULL if its a pin joint + * Not const, bullet owns this, and we update it as best we can + */ + btPersistentManifold* manifold; + + /** + * Store force so we can normalize it appropreately. Set and + * accessed directly by MuscleNP + */ + btVector3 force; + +}; + +#endif //NTRT_MUSCLE_ANCHOR_H_ diff --git a/src/core/terrain/tgBulletGround.cpp b/src/core/terrain/tgBulletGround.cpp index d8341b022..e13a1c9bf 100644 --- a/src/core/terrain/tgBulletGround.cpp +++ b/src/core/terrain/tgBulletGround.cpp @@ -32,6 +32,11 @@ // The C++ Standard Library #include +tgBulletGround::tgBulletGround() : +pGroundShape(NULL), +tgGround() +{ } + tgBulletGround::~tgBulletGround() { delete pGroundShape; diff --git a/src/core/terrain/tgBulletGround.h b/src/core/terrain/tgBulletGround.h index be1e21697..51dc328ce 100644 --- a/src/core/terrain/tgBulletGround.h +++ b/src/core/terrain/tgBulletGround.h @@ -44,8 +44,7 @@ class tgBulletGround : public tgGround * The only constructor. The base class initializes nothing. * @param[in] config configuration POD */ - tgBulletGround() : tgGround() - { } + tgBulletGround(); /** Clean up the implementation. Deletes the collision object */ virtual ~tgBulletGround(); diff --git a/src/core/terrain/tgEmptyGround.cpp b/src/core/terrain/tgEmptyGround.cpp index fc33515b9..6f70164e0 100644 --- a/src/core/terrain/tgEmptyGround.cpp +++ b/src/core/terrain/tgEmptyGround.cpp @@ -32,7 +32,8 @@ // The C++ Standard Library #include -tgEmptyGround::tgEmptyGround() +tgEmptyGround::tgEmptyGround() : +tgBulletGround() { } diff --git a/src/core/terrain/tgHillyGround.h b/src/core/terrain/tgHillyGround.h index c8c6eb67b..d50995171 100644 --- a/src/core/terrain/tgHillyGround.h +++ b/src/core/terrain/tgHillyGround.h @@ -51,7 +51,7 @@ class tgHillyGround : public tgBulletGround btVector3 origin = btVector3(0.0, 0.0, 0.0), size_t nx = 50, size_t ny = 50, - double margin = 0.5, + double margin = 0.05, double triangleSize = 5.0, double waveHeight = 5.0, double offset = 0.5); diff --git a/src/core/tgBaseString.cpp b/src/core/tgBaseString.cpp index af0754674..ca72a5f2c 100644 --- a/src/core/tgBaseString.cpp +++ b/src/core/tgBaseString.cpp @@ -35,22 +35,24 @@ using namespace std; tgBaseString::Config::Config(double s, double d, + double p, bool h, - double rot, double mf, double tVel, double mxAcc, double mnAL, - double mnRL) : + double mnRL, + double rot) : stiffness(s), damping(d), + pretension(p), hist(h), - rotation(rot), maxTens(mf), targetVelocity(tVel), maxAcc(mxAcc), minActualLength(mnAL), - minRestLength(mnRL) + minRestLength(mnRL), + rotation(rot) { ///@todo is this the right place for this, or the constructor of this class? if (s < 0.0) diff --git a/src/core/tgBaseString.h b/src/core/tgBaseString.h index ada85d6d5..fb5d71cd3 100644 --- a/src/core/tgBaseString.h +++ b/src/core/tgBaseString.h @@ -53,13 +53,14 @@ class tgBaseString : public tgModel */ Config( double s = 1000.0, double d = 10.0, + double p = 0.0, bool h = false, - double rot = 0, double mf = 1000.0, double tVel = 100.0, double mxAcc = 10000.0, double mnAL = 0.1, - double mnRL = 0.1); + double mnRL = 0.1, + double rot = 0); /** * Scale parameters that depend on the length of the simulation. @@ -84,22 +85,21 @@ class tgBaseString : public tgModel */ double damping; + /** + * Specifies the amount of tension (in units of force) in the + * string at construction. Adjusts the rest length, + * Must be low enough (based on stiffness) to + * leave the rest length non-negitive + */ + double pretension; + // History Parameters /** * Specifies whether data such as length and tension will be stored * in deque objects. Useful for computing the energy of a trial. */ bool hist; - - // Construction Parameters - /** - * Specifies the rotation around the face of the object its attached - * to. Any value will work, but +/- PI is the most meaningful. - * Units are radians. - * @todo Is this meaningful for non-rod shapes? - */ - double rotation; - + // Motor model parameters /** * The parameters that affect how the string can be controlled @@ -143,7 +143,16 @@ class tgBaseString : public tgModel * Units are length * Must be nonnegative */ - double minRestLength; + double minRestLength; + + // Construction Parameters + /** + * Specifies the rotation around the face of the object its attached + * to. Any value will work, but +/- PI is the most meaningful. + * Units are radians. + * @todo Is this meaningful for non-rod shapes? + */ + double rotation; }; /** Encapsulate the history members. */ diff --git a/src/core/tgBulletRenderer.cpp b/src/core/tgBulletRenderer.cpp index f88173cdb..a8ce7ff61 100644 --- a/src/core/tgBulletRenderer.cpp +++ b/src/core/tgBulletRenderer.cpp @@ -27,10 +27,16 @@ #include "tgBulletRenderer.h" // This application #include "Muscle2P.h" +#include "muscleAnchor.h" #include "tgBulletUtil.h" #include "tgLinearString.h" #include "tgWorld.h" #include "tgWorldBulletPhysicsImpl.h" + +#include "tgCast.h" + +#include "LinearMath/btQuickprof.h" + // OpenGL_FreeGlut (patched Bullet) #include "tgGLDebugDrawer.h" // The Bullet Physics library @@ -45,11 +51,17 @@ tgBulletRenderer::tgBulletRenderer(tgWorld& world) : m_world(world) void tgBulletRenderer::render(const tgRod& rod) const { +#ifndef BT_NO_PROFILE + BT_PROFILE("tgBulletRenderer::renderRod"); +#endif //BT_NO_PROFILE // render the rod (change color, etc. if we want) } void tgBulletRenderer::render(const tgLinearString& linString) const { +#ifndef BT_NO_PROFILE + BT_PROFILE("tgBulletRenderer::renderString"); +#endif //BT_NO_PROFILE // Fetch the btDynamicsWorld btDynamicsWorld& dynamicsWorld = tgBulletUtil::worldToDynamicsWorld(m_world); @@ -58,29 +70,35 @@ void tgBulletRenderer::render(const tgLinearString& linString) const const Muscle2P* const pMuscle = linString.getMuscle(); - if (pDrawer && pMuscle) + if(pDrawer && pMuscle) { - - const btVector3 lineFrom = - pMuscle->anchor1->getWorldPosition(); - const btVector3 lineTo = - pMuscle->anchor2->getWorldPosition(); - // Should this be normalized?? - const double stretch = - linString.getCurrentLength() - pMuscle->getRestLength(); - const btVector3 color = - (stretch < 0.0) ? - btVector3(0.0, 0.0, 1.0) : - btVector3(0.5 + stretch / 3.0, - 0.5 - stretch / 2.0, - 0.0); - pDrawer->drawLine(lineFrom, lineTo, color); - } + const std::vector& anchors = pMuscle->getAnchors(); + std::size_t n = anchors.size() - 1; + for (std::size_t i = 0; i < n; i++) + { + const btVector3 lineFrom = + anchors[i]->getWorldPosition(); + const btVector3 lineTo = + anchors[i+1]->getWorldPosition(); + // Should this be normalized?? + const double stretch = + linString.getCurrentLength() - pMuscle->getRestLength(); + const btVector3 color = + (stretch < 0.0) ? + btVector3(0.0, 0.0, 1.0) : + btVector3(0.5 + stretch / 3.0, + 0.5 - stretch / 2.0, + 0.0); + pDrawer->drawLine(lineFrom, lineTo, color); + } + } } void tgBulletRenderer::render(const tgModel& model) const { - +#ifndef BT_NO_PROFILE + BT_PROFILE("tgBulletRenderer::renderModel"); +#endif //BT_NO_PROFILE /** * Render the markers of the model using spheres. */ diff --git a/src/core/tgBulletUtil.cpp b/src/core/tgBulletUtil.cpp index 3c05d8238..2e8a897d8 100644 --- a/src/core/tgBulletUtil.cpp +++ b/src/core/tgBulletUtil.cpp @@ -61,7 +61,7 @@ btRigidBody* tgBulletUtil::createRigidBody(btDynamicsWorld* dynamicsWorld, // This is defined in DemoApplication as BT_LARGE_FLOAT 1e30 if using // double precision, 1e18.f if using single - float defaultContactProcessingThreshold = 1e18; // @TODO: What should this be? + double defaultContactProcessingThreshold = 1.0e30; // @TODO: What should this be? btRigidBody* body = new btRigidBody(cInfo); body->setContactProcessingThreshold(defaultContactProcessingThreshold); diff --git a/src/core/tgLinearString.cpp b/src/core/tgLinearString.cpp index 53265a0df..bdb1e672e 100644 --- a/src/core/tgLinearString.cpp +++ b/src/core/tgLinearString.cpp @@ -28,6 +28,9 @@ #include "tgLinearString.h" #include "tgModelVisitor.h" #include "tgWorld.h" +// The Bullet Physics Library +#include "LinearMath/btQuickprof.h" + // The C++ Standard Library #include #include // For history @@ -97,6 +100,9 @@ void tgLinearString::teardown() void tgLinearString::step(double dt) { +#ifndef BT_NO_PROFILE + BT_PROFILE("tgLinearString::step"); +#endif //BT_NO_PROFILE if (dt <= 0.0) { throw std::invalid_argument("dt is not positive."); @@ -113,6 +119,9 @@ void tgLinearString::step(double dt) void tgLinearString::onVisit(const tgModelVisitor& r) const { +#ifndef BT_NO_PROFILE + BT_PROFILE("tgLinearString::onVisit"); +#endif //BT_NO_PROFILE r.render(*this); } diff --git a/src/core/tgSimulation.cpp b/src/core/tgSimulation.cpp index 13bb8adeb..5fd9bb1b8 100644 --- a/src/core/tgSimulation.cpp +++ b/src/core/tgSimulation.cpp @@ -30,6 +30,9 @@ #include "tgSimView.h" #include "tgSimViewGraphics.h" #include "tgWorld.h" +// The Bullet Physics Library +#include "LinearMath/btQuickprof.h" + // The C++ Standard Library #include @@ -75,6 +78,9 @@ void tgSimulation::addModel(tgModel* pModel) void tgSimulation::onVisit(const tgModelVisitor& r) const { +#ifndef BT_NO_PROFILE + BT_PROFILE("tgSimulation::onVisit"); +#endif //BT_NO_PROFILE // Removed sending the visitor to the world since it wasn't used // Write a worldVisitor if its necessary for (int i = 0; i < m_models.size(); i++) { @@ -105,6 +111,8 @@ tgWorld& tgSimulation::getWorld() const void tgSimulation::step(double dt) const { +// Trying to profile here creates trouble for tgLinearString - this is outside of the profile loop + if (dt <= 0) { throw std::invalid_argument("dt for step is not positive"); diff --git a/src/core/tgWorldBulletPhysicsImpl.cpp b/src/core/tgWorldBulletPhysicsImpl.cpp index f6f32d0e3..1bb69dd3e 100644 --- a/src/core/tgWorldBulletPhysicsImpl.cpp +++ b/src/core/tgWorldBulletPhysicsImpl.cpp @@ -39,6 +39,7 @@ #include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btDynamicsWorld.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" @@ -48,6 +49,11 @@ #include "LinearMath/btScalar.h" #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" +#include "LinearMath/btQuickprof.h" + +// Ghost objects +#include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #define MCLP_SOLVER @@ -70,6 +76,7 @@ class IntermediateBuildProducts corner1 (-worldSize,-worldSize, -worldSize), corner2 (worldSize, worldSize, worldSize), dispatcher(&collisionConfiguration), + ghostCallback(), #ifndef MLCP_SOLVER #if (1) // More acc broadphase - remeber the comma (consider doing ifndef) broadphase(corner1, corner2, 16384) @@ -80,11 +87,13 @@ class IntermediateBuildProducts #endif //MLCP_SOLVER { + broadphase.getOverlappingPairCache()->setInternalGhostPairCallback(&ghostCallback); } const btVector3 corner1; const btVector3 corner2; btSoftBodyRigidBodyCollisionConfiguration collisionConfiguration; btCollisionDispatcher dispatcher; + btGhostPairCallback ghostCallback; #if (0) // Default broadphase btDbvtBroadphase broadphase; #else @@ -99,6 +108,7 @@ class IntermediateBuildProducts #else btSequentialImpulseConstraintSolver solver; #endif + }; tgWorldBulletPhysicsImpl::tgWorldBulletPhysicsImpl(const tgWorld::Config& config, @@ -117,9 +127,20 @@ tgWorldBulletPhysicsImpl::tgWorldBulletPhysicsImpl(const tgWorld::Config& config m_pDynamicsWorld->addRigidBody(ground->getGroundRigidBody()); } - #if (1) /// @todo This is a line from the old BasicLearningApp.cpp that we're not using. Investigate further + /* + * These are lines from the old BasicLearningApp.cpp that we aren't using. + * http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo + */ + #if (0) + // Split impulse does not appear to apply to MLCP solver m_pDynamicsWorld->getSolverInfo().m_splitImpulse = true; + m_pDynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = -0.02; + + // Default is 10 - increases runtime but decreases odds of penetration + // Makes tetraspine sine waves more accurate and static test less accurate + m_pDynamicsWorld->getSolverInfo().m_numIterations = 20; #endif + // Postcondition assert(invariant()); } @@ -194,6 +215,10 @@ void tgWorldBulletPhysicsImpl::step(double dt) void tgWorldBulletPhysicsImpl::addCollisionShape(btCollisionShape* pShape) { +#ifndef BT_NO_PROFILE + BT_PROFILE("addCollisionShape"); +#endif //BT_NO_PROFILE + if (pShape) { m_collisionShapes.push_back(pShape); @@ -203,6 +228,31 @@ void tgWorldBulletPhysicsImpl::addCollisionShape(btCollisionShape* pShape) assert(invariant()); } +void tgWorldBulletPhysicsImpl::deleteCollisionShape(btCollisionShape* pShape) +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("deleteCollisionShape"); +#endif //BT_NO_PROFILE + + if (pShape) + { + btCompoundShape* cShape = tgCast::cast(pShape); + if (cShape) + { + std::size_t n = cShape->getNumChildShapes(); + for( std::size_t i = 0; i < n; i++) + { + deleteCollisionShape(cShape->getChildShape(i)); + } + } + m_collisionShapes.remove(pShape); + delete pShape; + } + + // Postcondition + assert(invariant()); +} + bool tgWorldBulletPhysicsImpl::invariant() const { return (m_pDynamicsWorld != 0); diff --git a/src/core/tgWorldBulletPhysicsImpl.h b/src/core/tgWorldBulletPhysicsImpl.h index 66c038cea..560d89c25 100644 --- a/src/core/tgWorldBulletPhysicsImpl.h +++ b/src/core/tgWorldBulletPhysicsImpl.h @@ -32,6 +32,8 @@ #include "tgWorldImpl.h" #include "LinearMath/btAlignedObjectArray.h" + + // Forward declarations class btCollisionShape; class btTypedConstraint; @@ -86,7 +88,13 @@ class tgWorldBulletPhysicsImpl : public tgWorldImpl * @param[in] pShape a pointer to a btCollisionShape; do nothing if NULL */ void addCollisionShape(btCollisionShape* pShape); - + + /** + * Immediately delete a collision shape to avoid leaking memory during a rial + * @param[in] pShape a pointer to a btCollisionShape; do nothing if NULL + */ + void deleteCollisionShape(btCollisionShape* pShape); + /** * Add a btTypedConstraint to a collection for deletion upon * destruction. Also add to the physics. @@ -129,7 +137,7 @@ class tgWorldBulletPhysicsImpl : public tgWorldImpl btDynamicsWorld* m_pDynamicsWorld; /* - * A vector of collision shapes for easy reference. Does not affect + * A btAlignedObjectArray of collision shapes for easy reference. Does not affect * physics or rendering unles the shape is placed into the dynamics * world. Bullet encourages reuse of collision shapes when possible * for efficiency. diff --git a/src/dev/CMakeLists.txt b/src/dev/CMakeLists.txt index 1621850a9..cd2ecf49c 100644 --- a/src/dev/CMakeLists.txt +++ b/src/dev/CMakeLists.txt @@ -11,6 +11,7 @@ subdirs( kmorse apsabelhaus SpineHardwareProject + muscleNP jbruce ) diff --git a/src/dev/apsabelhaus/tgDataLoggerRedesign/T6Model_tgDLR.cpp b/src/dev/apsabelhaus/tgDataLoggerRedesign/T6Model_tgDLR.cpp index 5b132edaf..e0d88e6f0 100644 --- a/src/dev/apsabelhaus/tgDataLoggerRedesign/T6Model_tgDLR.cpp +++ b/src/dev/apsabelhaus/tgDataLoggerRedesign/T6Model_tgDLR.cpp @@ -63,7 +63,8 @@ namespace double friction; double rollFriction; double restitution; - double rotation; + double pretension; + bool history; double maxTens; double targetVelocity; double maxAcc; @@ -78,7 +79,8 @@ namespace 1.0, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) - 0, // rotation + 0.0, // pretension (force) + false, // history (boolean) 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -172,7 +174,7 @@ void T6Model_tgDLR::setup(tgWorld& world) c.rollFriction, c.restitution); // TO-DO: check out this new config, what exactly are we passing as False? - tgLinearString::Config muscleConfig(c.stiffness, c.damping, false, c.rotation, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.history, c.maxTens, c.targetVelocity, c.maxAcc); diff --git a/src/dev/atil/SuperBallModel.cpp b/src/dev/atil/SuperBallModel.cpp index b5b5b1840..5eff03960 100644 --- a/src/dev/atil/SuperBallModel.cpp +++ b/src/dev/atil/SuperBallModel.cpp @@ -64,7 +64,8 @@ namespace double friction; double rollFriction; double restitution; - double rotation; + double pretension; + bool history; double maxTens; double targetVelocity; double maxAcc; @@ -80,6 +81,7 @@ namespace 0.01, // rollFriction (unitless) 0.2, // restitution (?) 0, // rotation + false, // history 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -267,8 +269,8 @@ void SuperBallModel::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.rotation, - c.maxTens, c.targetVelocity, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, + c.history, c.maxTens, c.targetVelocity, c.maxAcc); // Start creating the structure diff --git a/src/dev/btietz/CMakeLists.txt b/src/dev/btietz/CMakeLists.txt index 3b30350f2..b36edb766 100644 --- a/src/dev/btietz/CMakeLists.txt +++ b/src/dev/btietz/CMakeLists.txt @@ -17,6 +17,7 @@ add_library(${PROJECT_NAME} SHARED ) subdirs( + Corde boxFeet caterpillar RBTests @@ -25,4 +26,8 @@ subdirs( hardwareLearning hardwareSineWaves handTunedSineWaves + muscleNPTests + tetraCollisions + muscleNPEnergy + TCContact ) diff --git a/src/dev/btietz/Corde/AppCordeTest.cpp b/src/dev/btietz/Corde/AppCordeTest.cpp new file mode 100644 index 000000000..cd64028f4 --- /dev/null +++ b/src/dev/btietz/Corde/AppCordeTest.cpp @@ -0,0 +1,94 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file AppCordeTest.cpp + * @brief Contains the definition function main() for testing the Corde + * string model + * @author Brian Mirletz + * $Id$ + */ + +// This application +#include "CordeModel.h" +// This library +#include "core/tgModel.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "tgcreator/tgUtil.h" +// The Bullet Physics Library +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +// The C++ Standard Library +#include + +/** + * The entry point. + * @param[in] argc the number of command-line arguments + * @param[in] argv argv[0] is the executable name + * @return 0 + */ +int main(int argc, char** argv) +{ + +#if (1) // X Pos + btVector3 startPos(0.0, 0.0, 0.0); + btVector3 endPos (10.0, 0.0, 0.0); + + // Setup for neither bending nor rotation + btQuaternion startRot( 0, sqrt(2)/2.0, 0, sqrt(2)/2.0); + btQuaternion endRot = startRot; +#else + btVector3 startPos(0.0, 0.0, 0.0); + btVector3 endPos (0.0, 0.0, 10.0); + + // Setup for neither bending nor rotation + btQuaternion startRot( 0, 0, 0, 1); + btQuaternion endRot = startRot; +#endif + // Values for Rope from Spillman's paper + const std::size_t resolution = 10; + const double radius = 0.01; + const double density = 1300; + const double youngMod = 0.5; + const double shearMod = 0.5; + const double stretchMod = 20.0; + const double springConst = 100.0 * pow(10, 3); + const double gammaT = 10.0 * pow(10, -6); + const double gammaR = 1.0 * pow(10, -6); + CordeModel::Config config(resolution, radius, density, youngMod, shearMod, + stretchMod, springConst, gammaT, gammaR); + + CordeModel testString(startPos, endPos, startRot, endRot, config); + + double t = 0.0; + double dt = 0.0001; + for (int i = 0; i < 10000; i++) + { + testString.step(dt); + t += dt; + } + #ifdef BT_USE_DOUBLE_PRECISION + std::cout << "Double precision" << std::endl; + #else + std::cout << "Single Precision" << std::endl; + #endif + + return 0; +} diff --git a/src/dev/btietz/Corde/AppLineInsertionCheck.cpp b/src/dev/btietz/Corde/AppLineInsertionCheck.cpp new file mode 100644 index 000000000..64a2177e0 --- /dev/null +++ b/src/dev/btietz/Corde/AppLineInsertionCheck.cpp @@ -0,0 +1,105 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file AppLineInsertionCheck.cpp + * @brief Simple tests for determining the order of points in space + * @author Brian Mirletz + * $Id$ + */ + +#include "tgcreator/tgUtil.h" + +// The Bullet Physics Library +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +// The C++ Standard Library +#include +#include + +/** + * The entry point. + * @param[in] argc the number of command-line arguments + * @param[in] argv argv[0] is the executable name + * @return 0 + */ +int main(int argc, char** argv) +{ + btVector3 point1(atof(argv[1]), atof(argv[2]), atof(argv[3])); + btVector3 point2(atof(argv[4]), atof(argv[5]), atof(argv[6])); + + btVector3 point3(atof(argv[7]), atof(argv[8]), atof(argv[9])); +#if (1) + btVector3 normal(atof(argv[10]), atof(argv[11]), atof(argv[12])); +#else + btVector3 point4(0.0, 0.0, 0.0); +#endif + std::cout << point1 << " " << point2 << " " << point3 << std::endl; + + std::cout << "new point " << (point3 - point1).dot(point2) << std::endl; + std::cout << "old point " << (point3 - point1).dot(point3) << std::endl; + + btVector3 lineA = point3 - point2; + btVector3 lineB = point1 - point2; + + normal.normalize(); + +#if (1) + btVector3 tangentDir =( (lineB - lineA).cross(normal)).normalize(); + btVector3 tangentMove = (lineB + lineA).dot(tangentDir) * tangentDir / 2.0; + btVector3 newPos = point2 + tangentMove; + + std::cout << "tangentDir " << tangentDir << std::endl; + std::cout << "tangentMove " << tangentMove << std::endl; + std::cout << "newPos " << newPos << std::endl; +#else + btQuaternion q1 = tgUtil::getQuaternionBetween(lineA, normal); + btQuaternion q2 = tgUtil::getQuaternionBetween(normal, lineB); + btQuaternion q3 = tgUtil::getQuaternionBetween(lineB, lineA); + + btVector3 lineACopy = lineA; + btVector3 lineBCopy = lineB; + btVector3 ab = lineA + lineB; + btVector3 abNorm = (lineACopy.normalize() + lineBCopy.normalize()).normalize(); + + // Project normal into AB plane + btVector3 AN = lineA.dot(normal) * lineACopy.normalize(); + btVector3 BN = lineB.dot(normal) * lineBCopy.normalize(); + btVector3 normalProjection = abNorm.dot(normal) * abNorm; + + normalProjection.normalize(); + + std::cout << "lineA Norm: " << lineACopy << std::endl; + std::cout << "lineB Norm: " << lineBCopy << std::endl; + std::cout << "Normal: " << normal << std::endl; + std::cout << "Projection: " << normalProjection << std::endl; + std::cout << "Angle AN: " << lineA.angle(normalProjection) << std::endl; + std::cout << "Angle BN " << lineB.angle(normalProjection) << std::endl; + std::cout << "Sum AN + BN " << lineA.angle(normalProjection) + lineB.angle(normalProjection) << std::endl; + std::cout << "Angle AB " << lineA.angle(lineB) << std::endl; + + std::cout << "Angle normal w/ x axis " << btVector3(1.0, 0.0, 0.0).angle(normal) << std::endl; + std::cout << "Angle lineA w/ x axis " << btVector3(1.0, 0.0, 0.0).angle(lineA) << std::endl; + std::cout << "Angle lineB w/ x axis " << btVector3(1.0, 0.0, 0.0).angle(lineB) << std::endl; + + std::cout << "Traditional: " << lineA.dot(normal) << " " << lineB.dot(normal) << std::endl; + +#endif + return 0; +} diff --git a/src/dev/btietz/Corde/CMakeLists.txt b/src/dev/btietz/Corde/CMakeLists.txt new file mode 100644 index 000000000..f3d5c8118 --- /dev/null +++ b/src/dev/btietz/Corde/CMakeLists.txt @@ -0,0 +1,19 @@ +link_directories(${LIB_DIR}) + +link_libraries(btietz + tgcreator + util + sensors + core + terrain + tgOpenGLSupport) + + +add_executable(AppCordeTest + CordeModel.cpp + AppCordeTest.cpp +) + +add_executable(AppLineInsertionCheck + AppLineInsertionCheck.cpp +) diff --git a/src/dev/btietz/Corde/CordeModel.cpp b/src/dev/btietz/Corde/CordeModel.cpp new file mode 100644 index 000000000..eee00553a --- /dev/null +++ b/src/dev/btietz/Corde/CordeModel.cpp @@ -0,0 +1,579 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file CordeModel.cpp + * @brief Defines structure for the Corde softbody String Model + * @author Brian Mirletz + * $Id$ + */ + +// This module +#include "CordeModel.h" + +// This library +#include "tgcreator/tgUtil.h" + +// The C++ Standard Library +#include + +CordeModel::Config::Config(const std::size_t res, + const double r, const double d, + const double ym, const double shm, + const double stm, const double csc, + const double gt, const double gr) : + resolution(res), + radius(r), + density(d), + YoungMod(ym), + ShearMod(shm), + StretchMod(stm), + ConsSpringConst(csc), + gammaT(gt), + gammaR(gr) +{ + if (r <= 0.0) + { + throw std::invalid_argument("Corde string radius is not positive."); + } + else if (d <= 0.0) + { + throw std::invalid_argument("Corde String density is not positive."); + } + else if (ym < 0.0) + { + throw std::invalid_argument("String Young's Modulus is negative."); + } + else if (shm < 0.0) + { + throw std::invalid_argument("Shear Modulus is negative."); + } + else if (stm < 0.0) + { + throw std::invalid_argument("Stretch Modulus is negative."); + } + else if (csc < 0.0) + { + throw std::invalid_argument("Spring Constant is negative."); + } + else if (gt < 0.0) + { + throw std::invalid_argument("Damping Constant (position) is negative."); + } + else if (gr < 0.0) + { + throw std::invalid_argument("Damping Constant (rotation) is negative."); + } +} + +CordeModel::CordeModel(btVector3 pos1, btVector3 pos2, btQuaternion quat1, btQuaternion quat2, CordeModel::Config& Config) : +m_config(Config), + simTime(0.0) +{ + computeConstants(); + + btVector3 rodLength(pos2 - pos1); + btVector3 unitLength( rodLength / ((double) m_config.resolution - 1) ); + btVector3 massPos(pos1); + + double unitMass = m_config.density * M_PI * pow( m_config.radius, 2) * unitLength.length(); + + CordePositionElement* currentPoint = new CordePositionElement(massPos, unitMass); + + m_massPoints.push_back(currentPoint); + + std::cout << massPos << std::endl; + // Setup mass elements + for (std::size_t i = 1; i < m_config.resolution; i++) + { + + massPos += unitLength; + currentPoint = new CordePositionElement(massPos, unitMass); + m_massPoints.push_back(currentPoint); + // Introduce stretch + linkLengths.push_back(unitLength.length() * 1.0); + std::cout << massPos << " " << unitMass << std::endl; + } + + CordeQuaternionElement* currentAngle = new CordeQuaternionElement(quat1); + m_centerlines.push_back(currentAngle); + + std::size_t n = m_config.resolution - 1; + for (std::size_t i = 1; i < n; i++) + { + currentAngle = new CordeQuaternionElement(quat1.slerp(quat2, (double) i / (double) n) ); + m_centerlines.push_back(currentAngle); + std::cout << currentAngle->q << std::endl; + quaternionShapes.push_back(unitLength.length()); + } + + assert(invariant()); +} + +CordeModel::~CordeModel() +{ + for (std::size_t i = 0; i < m_massPoints.size(); i++) + { + delete m_massPoints[i]; + } + for (std::size_t i = 0; i < m_centerlines.size(); i++) + { + delete m_centerlines[i]; + } +} + +void CordeModel::step (btScalar dt) +{ + if (dt <= 0.0) + { + throw std::invalid_argument("Timestep is not positive."); + } + + stepPrerequisites(); + computeInternalForces(); + unconstrainedMotion(dt); + simTime += dt; + if (simTime >= .01) + { + size_t n = m_massPoints.size(); + for (std::size_t i = 0; i < n; i++) + { + std::cout << "Position " << i << " " << m_massPoints[i]->pos << std::endl + << "Force " << i << " " << m_massPoints[i]->force << std::endl; + if (i < n - 1) + { + std::cout << "Quaternion " << i << " " << m_centerlines[i]->q << std::endl + << "Qdot " << i << " " << m_centerlines[i]->qdot << std::endl + << "Force " << i << " " << m_centerlines[i]->tprime << std::endl + << "Torque " << i << " " << m_centerlines[i]->torques << std::endl; + } + } + simTime = 0.0; + } + + assert(invariant()); +} + +void CordeModel::computeConstants() +{ + assert(computedStiffness.empty()); + + const double pir2 = M_PI * pow(m_config.radius, 2); + + computedStiffness.push_back( m_config.StretchMod * pir2); + computedStiffness.push_back( m_config.YoungMod * pir2 / 4.0); + computedStiffness.push_back( m_config.YoungMod * pir2 / 4.0); + computedStiffness.push_back( m_config.ShearMod * pir2 / 2.0); + + /* Could probably do this in constructor directly, but easier + * here since pir2 is already computed + */ + computedInertia.setValue(m_config.density * pir2 / 4.0, + m_config.density * pir2 / 4.0, + m_config.density * pir2 / 2.0); + + // Can assume if one element is zero, all elements are zero and we've screwed up + // Should pass automatically based on exceptions in config constructor + assert(!computedInertia.fuzzyZero()); + + inverseInertia.setValue(1.0/computedInertia[0], + 1.0/computedInertia[1], + 1.0/computedInertia[2]); + +} + +/** + * @todo consider making a reset function for the elements + */ +void CordeModel::stepPrerequisites() +{ + std::size_t n = m_massPoints.size(); + for (std::size_t i = 0; i < n - 1; i++) + { + CordePositionElement* r_0 = m_massPoints[i]; + r_0->force.setZero(); + } + + n = m_centerlines.size(); + for (std::size_t i = 0; i < n - 1; i++) + { + CordeQuaternionElement* q_0 = m_centerlines[i]; + q_0->tprime = btQuaternion(0.0, 0.0, 0.0, 0.0); + q_0->torques.setZero(); + } +} + +void CordeModel::computeInternalForces() +{ + std::size_t n = m_massPoints.size() - 1; + + // Update position elements + for (std::size_t i = 0; i < n; i++) + { + CordePositionElement* r_0 = m_massPoints[i]; + CordePositionElement* r_1 = m_massPoints[i + 1]; + + CordeQuaternionElement* quat_0 = m_centerlines[i]; + + // Get position elements in standard variable names + const btScalar x1 = r_0->pos[0]; + const btScalar y1 = r_0->pos[1]; + const btScalar z1 = r_0->pos[2]; + + const btScalar x2 = r_1->pos[0]; + const btScalar y2 = r_1->pos[1]; + const btScalar z2 = r_1->pos[2]; + + // Same for quaternion elements + const btScalar q11 = quat_0->q[0]; + const btScalar q12 = quat_0->q[1]; + const btScalar q13 = quat_0->q[2]; + const btScalar q14 = quat_0->q[3]; + + // Setup common factors + const btVector3 posDiff = r_0->pos - r_1->pos; + const btVector3 velDiff = r_0->vel - r_1->vel; + const btScalar posNorm = posDiff.length(); + const btScalar posNorm_2 = posDiff.length2(); + const btVector3 director( (2.0 * (q11 * q13 + q12 * q14)), + (2.0 * (q12 * q13 - q11 * q14)), + ( -1.0 * q11 * q11 - q12 * q12 + q13 * q13 + q14 * q14)); + + // Sum Forces, have to split it out into components due to + // derivatives of energy quantaties + + // Spring common + const btScalar spring_common = computedStiffness[0] * + (linkLengths[i] - posNorm) / (linkLengths[i] * posNorm); + + const btScalar diss_common = m_config.gammaT * + posNorm_2 * posDiff.dot(velDiff) / pow (linkLengths[i] , 5); + + /* Quaternion Constraint X */ + const btScalar quat_cons_x = m_config.ConsSpringConst * linkLengths[i] * + ( director[2] * (x1 - x2) * (z1 - z2) - director[0] * ( pow( posDiff[1], 2) + pow( posDiff[2], 2) ) + + director[1] * (x1 - x2) * (y1 - y2) ) / ( pow (posNorm, 3) ); + + /* Quaternion Constraint Y */ + const btScalar quat_cons_y = m_config.ConsSpringConst * linkLengths[i] * + ( -1.0 * director[2] * (y1 - y2) * (z1 - z2) + director[1] * ( pow( posDiff[0], 2) + pow( posDiff[2], 2) ) + - director[0] * (x1 - x2) * (z1 - z2) ) / ( pow (posNorm, 3) ); + + /* Quaternion Constraint Z */ + const btScalar quat_cons_z = m_config.ConsSpringConst * linkLengths[i] * + ( -1.0 * director[0] * (y1 - y2) * (z1 - z2) + director[2] * ( pow( posDiff[0], 2) + pow( posDiff[1], 2) ) + - director[1] * (x1 - x2) * (z1 - z2) ) / ( pow (posNorm, 3) ); + + r_0->force[0] += -1.0 * (x1 - x2) * (spring_common + diss_common); + + r_1->force[0] += (x1 - x2) * (spring_common + diss_common); + + /* Apply Y forces */ + r_0->force[1] += -1.0 * (y1 - y2) * (spring_common + diss_common); + + r_1->force[1] += (y1 - y2) * (spring_common + diss_common); + + /* Apply Z Forces */ + r_0->force[2] += -1.0 * (z1 - z2) * (spring_common + diss_common); + + r_1->force[2] += (z1 - z2) * (spring_common + diss_common); + + /* Apply constraint equation with boundry conditions */ + if (i == 0) + { + r_1->force[0] += quat_cons_x; + + + r_1->force[1] += quat_cons_y; + + + r_1->force[2] += quat_cons_z; + } + else if (i == n - 1) + { + r_0->force[0] -= quat_cons_x; + + r_0->force[1] -= quat_cons_y; + + r_0->force[2] -= quat_cons_z; + } + else + { + r_0->force[0] -= quat_cons_x; + r_1->force[0] += quat_cons_x; + + r_0->force[1] -= quat_cons_y; + r_1->force[1] += quat_cons_y; + + r_0->force[2] -= quat_cons_z; + r_1->force[2] += quat_cons_z; + } + +#if (0) // Original derivation + /* Torques resulting from quaternion alignment constraints */ + quat_0->tprime[0] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q11 * quat_0->q.length2() + (q13 * posDiff[0] - + q14 * posDiff[1] - q11 * posDiff[2]) / posNorm); + + quat_0->tprime[1] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q12 * quat_0->q.length2() + (q14 * posDiff[0] + + q13 * posDiff[1] - q12 * posDiff[2]) / posNorm); + + quat_0->tprime[2] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q13 * quat_0->q.length2() + (q11 * posDiff[0] + + q12 * posDiff[1] + q13 * posDiff[2]) / posNorm); + + quat_0->tprime[3] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q14 * quat_0->q.length2() + (q12 * posDiff[0] - + q11 * posDiff[1] + q14 * posDiff[2]) / posNorm); +#else // quat_0->q.length2() should always be 1, but sometimes numerical precision renders it slightly greater + // The simulation is much more stable if we just assume its one. + quat_0->tprime[0] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q11 + (q13 * posDiff[0] - + q14 * posDiff[1] - q11 * posDiff[2]) / posNorm); + + quat_0->tprime[1] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q12 + (q14 * posDiff[0] + + q13 * posDiff[1] - q12 * posDiff[2]) / posNorm); + + quat_0->tprime[2] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q13 + (q11 * posDiff[0] + + q12 * posDiff[1] + q13 * posDiff[2]) / posNorm); + + quat_0->tprime[3] += 2.0 * m_config.ConsSpringConst * linkLengths[i] + * ( q14 + (q12 * posDiff[0] - + q11 * posDiff[1] + q14 * posDiff[2]) / posNorm); +#endif + } + + n = m_centerlines.size() - 1; + + // Update quaternion elements + for (std::size_t i = 0; i < n; i++) + { + CordeQuaternionElement* quat_0 = m_centerlines[i]; + CordeQuaternionElement* quat_1 = m_centerlines[i + 1]; + + /* Setup Variables */ + const btScalar q11 = quat_0->q[0]; + const btScalar q12 = quat_0->q[1]; + const btScalar q13 = quat_0->q[2]; + const btScalar q14 = quat_0->q[3]; + + const btScalar q21 = quat_1->q[0]; + const btScalar q22 = quat_1->q[1]; + const btScalar q23 = quat_1->q[2]; + const btScalar q24 = quat_1->q[3]; + + const btScalar qdot11 = quat_0->qdot[0]; + const btScalar qdot12 = quat_0->qdot[1]; + const btScalar qdot13 = quat_0->qdot[2]; + const btScalar qdot14 = quat_0->qdot[3]; + + const btScalar qdot21 = quat_1->qdot[0]; + const btScalar qdot22 = quat_1->qdot[1]; + const btScalar qdot23 = quat_1->qdot[2]; + const btScalar qdot24 = quat_1->qdot[3]; + + const btScalar k1 = computedStiffness[1]; + const btScalar k2 = computedStiffness[2]; + const btScalar k3 = computedStiffness[3]; + + /* I apologize for the mess below - the derivatives involved + * here do not leave a lot of common factors. If you see + * any nice vector operations I missed, implement them and/or + * let me know! _Brian + */ + + /* Bending and torsional stiffness */ + const btScalar stiffness_common = 4.0 / quaternionShapes[i] * + pow(quaternionShapes[i] - 1.0, 2); + + const btScalar q11_stiffness = stiffness_common * + (k1 * q24 * (q11 * q24 + q12 * q23 - q13 * q22 - q14 * q21) + + k2 * q23 * (q11 * q23 - q12 * q24 - q13 * q21 + q14 * q22) + + k3 * q22 * (q11 * q22 - q12 * q21 + q13 * q24 - q14 * q23)); + + const btScalar q12_stiffness = stiffness_common * + (k1 * q23 * (q12 * q23 + q11 * q24 - q13 * q22 - q14 * q21) + + k2 * q24 * (q12 * q24 - q11 * q23 + q13 * q21 - q14 * q22) + + k3 * q21 * (q12 * q21 - q11 * q22 - q13 * q24 + q14 * q23)); + + const btScalar q13_stiffness = stiffness_common * + (k1 * q22 * (q13 * q22 - q11 * q24 - q12 * q23 + q14 * q21) + + k2 * q21 * (q13 * q21 - q11 * q23 + q12 * q24 - q14 * q22) + + k3 * q24 * (q13 * q24 + q11 * q22 - q12 * q21 - q14 * q23)); + + const btScalar q14_stiffness = stiffness_common * + (k1 * q21 * (q14 * q21 - q11 * q24 - q12 * q23 + q13 * q22) + + k2 * q22 * (q14 * q22 + q11 * q23 - q12 * q24 - q13 * q21) + + k3 * q23 * (q14 * q23 - q11 * q22 + q12 * q21 - q13 * q24)); + + const btScalar q21_stiffness = stiffness_common * + (k1 * q14 * (q14 * q21 - q11 * q24 - q12 * q23 + q13 * q22) + + k2 * q13 * (q13 * q21 - q11 * q23 + q12 * q24 - q14 * q22) + + k3 * q12 * (q12 * q21 - q11 * q22 + q14 * q23 - q13 * q24)); + + const btScalar q22_stiffness = stiffness_common * + (k1 * q13 * (q13 * q22 - q11 * q24 - q12 * q23 + q14 * q21) + + k2 * q14 * (q14 * q22 + q11 * q23 - q12 * q24 - q13 * q21) + + k3 * q11 * (q11 * q22 - q12 * q21 + q13 * q24 - q14 * q23)); + + const btScalar q23_stiffness = stiffness_common * + (k1 * q12 * (q12 * q23 + q11 * q24 - q13 * q22 - q14 * q21) + + k2 * q11 * (q11 * q23 - q13 * q21 - q12 * q24 + q14 * q22) + + k3 * q14 * (q14 * q23 - q11 * q22 + q12 * q21 - q13 * q24)); + + const btScalar q24_stiffness = stiffness_common * + (k1 * q11 * (q11 * q24 + q12 * q23 - q13 * q22 - q14 * q21) + + k2 * q12 * (q12 * q24 - q11 * q23 + q13 * q21 - q14 * q22) + + k3 * q13 * (q13 * q24 + q11 * q22 - q12 * q21 - q14 * q23)); + + /* Torsional Damping */ + const btScalar damping_common = 4.0 * m_config.gammaR / quaternionShapes[i]; + + const btScalar q11_damping = damping_common * + (q12 * (q12 * qdot11 - q11 * qdot12 + q21 * qdot22 - q22 * qdot21 - q23 * qdot24 + q24 * qdot23) + + q13 * (q13 * qdot11 - q11 * qdot13 + q21 * qdot23 + q22 * qdot24 - q23 * qdot21 - q24 * qdot22) + + q14 * (q14 * qdot11 - q11 * qdot14 + q21 * qdot24 - q22 * qdot23 + q23 * qdot22 - q24 * qdot21)); + + const btScalar q12_damping = damping_common * + (q11 * (q11 * qdot12 - q12 * qdot11 - q21 * qdot22 + q22 * qdot21 + q23 * qdot24 - q24 * qdot23) + + q13 * (q13 * qdot12 - q13 * qdot13 - q21 * qdot24 + q22 * qdot23 - q23 * qdot22 + q24 * qdot21) + + q14 * (q14 * qdot12 - q14 * qdot14 + q21 * qdot23 + q22 * qdot24 - q23 * qdot21 - q24 * qdot22)); + + const btScalar q13_damping = damping_common * + (q11 * (q11 * qdot13 - q13 * qdot11 - q21 * qdot23 - q22 * qdot24 + q23 * qdot21 + q24 * qdot22) + + q12 * (q12 * qdot13 - q13 * qdot12 + q21 * qdot24 - q22 * qdot23 + q23 * qdot22 - q24 * qdot21) + + q14 * (q14 * qdot13 - q13 * qdot14 - q21 * qdot22 + q22 * qdot21 + q23 * qdot24 - q24 * qdot23)); + + const btScalar q14_damping = damping_common * + (q11 * (q11 * qdot14 - q14 * qdot11 - q21 * qdot24 + q22 * qdot23 - q23 * qdot22 + q24 * qdot21) + + q12 * (q12 * qdot14 - q14 * qdot12 - q21 * qdot23 - q22 * qdot24 + q23 * qdot21 + q24 * qdot22) + + q13 * (q13 * qdot14 - q14 * qdot13 + q21 * qdot22 - q22 * qdot21 - q23 * qdot24 + q24 * qdot23)); + + const btScalar q21_damping = damping_common * + (q22 * (q22 * qdot21 + q11 * qdot12 - q12 * qdot11 - q13 * qdot14 + q14 * qdot13 - q21 * qdot22) + + q23 * (q23 * qdot21 + q11 * qdot13 + q12 * qdot14 - q13 * qdot11 - q14 * qdot12 - q21 * qdot23) + + q24 * (q24 * qdot21 + q11 * qdot14 - q12 * qdot13 + q13 * qdot12 - q14 * qdot11 - q21 * qdot24)); + + const btScalar q22_damping = damping_common * + (q21 * (q21 * qdot22 - q11 * qdot12 + q12 * qdot11 + q13 * qdot14 - q14 * qdot13 - q22 * qdot21) + + q23 * (q23 * qdot22 - q11 * qdot14 + q12 * qdot13 - q13 * qdot12 + q14 * qdot11 - q22 * qdot23) + + q24 * (q24 * qdot22 + q11 * qdot13 + q12 * qdot14 - q13 * qdot11 - q14 * qdot12 - q22 * qdot24)); + + const btScalar q23_damping = damping_common * + (q21 * (q21 * qdot23 - q11 * qdot13 + q13 * qdot11 - q12 * qdot14 + q14 * qdot12 - q23 * qdot21) + + q22 * (q22 * qdot23 + q11 * qdot14 - q12 * qdot13 + q13 * qdot12 - q14 * qdot11 - q22 * qdot22) + + q24 * (q24 * qdot23 - q11 * qdot12 + q12 * qdot11 + q13 * qdot14 - q14 * qdot13 - q23 * qdot24)); + + const btScalar q24_damping = damping_common * + (q21 * (q21 * qdot24 - q11 * qdot14 + q12 * qdot13 - q13 * qdot12 + q14 * qdot11 - q24 * qdot21) + + q22 * (q21 * qdot24 - q11 * qdot13 - q12 * qdot14 + q13 * qdot11 + q14 * qdot12 - q24 * qdot22) + + q23 * (q23 * qdot24 + q11 * qdot12 - q12 * qdot11 - q13 * qdot14 + q14 * qdot13 - q24 * qdot23)); + + /* Apply torques */ /// @todo double check the sign convention. Looks good numerically.q + quat_0->tprime[0] += q11_stiffness + q11_damping; + + quat_1->tprime[0] += q21_stiffness + q21_damping; + + quat_0->tprime[1] += q12_stiffness + q12_damping; + + quat_1->tprime[1] += q22_stiffness + q22_damping; + + quat_0->tprime[2] += q13_stiffness + q13_damping; + + quat_1->tprime[2] += q23_stiffness + q23_damping; + + quat_0->tprime[3] += q14_stiffness + q14_damping; + + quat_1->tprime[3] += q24_stiffness + q24_damping; + + } +} + +void CordeModel::unconstrainedMotion(double dt) +{ + for (std::size_t i = 0; i < m_massPoints.size(); i++) + { + CordePositionElement* p_0 = m_massPoints[i]; + // Velocity update - semi-implicit Euler + p_0->vel += dt / p_0->mass * p_0->force; + // Position update, uses v(t + dt) + p_0->pos += dt * p_0->vel; + } + for (std::size_t i = 0; i < m_centerlines.size(); i++) + { + /* Transpose quaternion torques into Euclidean torques */ + CordeQuaternionElement* quat_0 = m_centerlines[i]; + quat_0->transposeTorques(); + + const btVector3 omega = quat_0->omega; + // Since I is diagonal, we can use elementwise multiplication of vectors + quat_0->omega += inverseInertia * (quat_0->torques - + omega.cross(computedInertia * omega)) * dt; + quat_0->updateQDot(); + quat_0->q = (quat_0->qdot*dt + quat_0->q).normalize(); + } +} + +CordeModel::CordePositionElement::CordePositionElement(btVector3 p1, double m) : + pos(p1), + vel(0.0, 0.0, 0.0), + force(0.0, 0.0, 0.0), + mass(m) +{ + if (m < 0.0) + { + throw std::invalid_argument("Mass is negative."); + } +} + +CordeModel::CordeQuaternionElement::CordeQuaternionElement(btQuaternion q1) : + q(q1.normalize()), + qdot(0.0, 0.0, 0.0, 0.0), + tprime(0.0, 0.0, 0.0, 0.0), + torques(0.0, 0.0, 0.0), + omega(0.0, 0.0, 0.0) +{ + +} + +void CordeModel::CordeQuaternionElement::transposeTorques() +{ + torques[0] += 1.0/2.0 * (q[0] * tprime[2] - q[2] * tprime[0] - q[1] * tprime[3] + q[3] * tprime[1]); + torques[1] += 1.0/2.0 * (q[1] * tprime[0] - q[0] * tprime[1] - q[2] * tprime[3] + q[3] * tprime[2]); + torques[2] += 1.0/2.0 * (q[0] * tprime[0] + q[1] * tprime[1] + q[2] * tprime[2] + q[3] * tprime[3]); +} + +void CordeModel::CordeQuaternionElement::updateQDot() +{ + qdot[0] = 1.0/2.0 * (q[0] * omega[2] + q[1] * omega[1] - q[2] * omega[0]); + qdot[1] = 1.0/2.0 * (q[1] * omega[2] - q[0] * omega[1] + q[3] * omega[0]); + qdot[2] = 1.0/2.0 * (q[0] * omega[0] + q[2] * omega[2] + q[3] * omega[1]); + qdot[3] = 1.0/2.0 * (q[3] * omega[2] - q[2] * omega[1] - q[1] * omega[0]); +} + +/// Checks lengths of vectors. @todo add additional invariants +bool CordeModel::invariant() +{ + return (m_massPoints.size() == m_centerlines.size() + 1) + && (m_centerlines.size() == linkLengths.size()) + && (linkLengths.size() == quaternionShapes.size() + 1) + && (computedStiffness.size() == 4); +} diff --git a/src/dev/btietz/Corde/CordeModel.h b/src/dev/btietz/Corde/CordeModel.h new file mode 100644 index 000000000..ee10b527b --- /dev/null +++ b/src/dev/btietz/Corde/CordeModel.h @@ -0,0 +1,167 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +//#define BT_USE_DOUBLE_PRECISION + +#ifndef CORDE_MODEL +#define CORDE_MODEL + +/** + * @file CordeModel.h + * @brief Defines structure for the Corde softbody String Model + * @author Brian Mirletz + * $Id$ + */ + +// Bullet Linear Algebra +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" + +// The C++ Standard Library +#include + +class CordeModel +{ +public: + struct Config + { + Config(const std::size_t res, + const double r, const double d, + const double ym, const double shm, + const double stm, const double csc, + const double gt, const double gr); + + const std::size_t resolution; + const double radius; + const double density; + const double YoungMod; + const double ShearMod; + const double StretchMod; + const double ConsSpringConst; + /** + * For really short segments (< .001 length) consider decreasing + * these further or changing length to cubic (currently ^5) + */ + const double gammaT; + const double gammaR; + }; + + /** + * A constructor which assumes uniformally distributed mass + * points and rotation + * pos1 and pos2 specify the start and end points of the rod. + * quat1 and quat2 need to be computed based on the torsion in the rod. + * Note that if there is neither bending nor torsion one can say quat1 = quat2 + * = btQuaternion((pos2 - pos1).normalize, 0) (axis-angle constructor) + * @todo develop a constructor that can handle more complex shapes + * i.e. wrapped around a motor. This one maxes out at 1 - eps rotations + */ + CordeModel(btVector3 pos1, btVector3 pos2, btQuaternion quat1, btQuaternion quat2, CordeModel::Config& Config); + + ~CordeModel(); + + void step (btScalar dt); + +private: + void computeConstants(); + + void stepPrerequisites(); + + void computeInternalForces(); + + void unconstrainedMotion(double dt); + + /** + * Holds all of the data for one of the mass elements of the string + */ + struct CordePositionElement + { + /** + * Sets pos to p1, mass to m, everything else to zero + * Assumes rod is at rest on start + */ + CordePositionElement(btVector3 p1, double m); + + btVector3 pos; + btVector3 vel; + btVector3 force; + double mass; + }; + + /** + * Holds all of the data for the centerline quaternions of the string + */ + struct CordeQuaternionElement + { + /** + * Sets q to q1.normalized(), everything else to zero + */ + CordeQuaternionElement(btQuaternion q1); + + void transposeTorques(); + /** + * Must be called after transpose torques and omega is updated + */ + void updateQDot(); + + btQuaternion q; + btQuaternion qdot; + /** + * Just a 4x1 vector, but easier to store this way. + */ + btQuaternion tprime; + btVector3 torques; + btVector3 omega; + }; + + CordeModel::Config m_config; + + std::vector m_massPoints; + std::vector m_centerlines; + /** + * Should have length equal to m_massPoints.size()-1 + */ + std::vector linkLengths; + /** + * Should have length equal to m_Centerlines.size()-1 + */ + std::vector quaternionShapes; + + /** + * Computed based on the values in config. Should have length 4 + * 0: linear stiffness used by mass models + * 1 - 3: bending and torsion stiffnesses + * @todo can this be const? + */ + std::vector computedStiffness; + + /** + * Computed based on the values in config. Should have length 3 + * Assuming products of inertia are negligible as in the paper + */ + btVector3 computedInertia; + btVector3 inverseInertia; + + bool invariant(); + + double simTime; +}; + + +#endif // CORDE_MODEL diff --git a/src/dev/btietz/TCContact/.gitignore b/src/dev/btietz/TCContact/.gitignore new file mode 100644 index 000000000..d2127d056 --- /dev/null +++ b/src/dev/btietz/TCContact/.gitignore @@ -0,0 +1 @@ +*.ini diff --git a/src/dev/btietz/TCContact/AppFlemonsSpineContact.cpp b/src/dev/btietz/TCContact/AppFlemonsSpineContact.cpp new file mode 100644 index 000000000..efabc871e --- /dev/null +++ b/src/dev/btietz/TCContact/AppFlemonsSpineContact.cpp @@ -0,0 +1,134 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file AppFlemonsSpineContact.cpp + * @brief Contains the definition function main() for the Flemons Spine Contact + * application. + * @author Brian Mirletz + * @copyright Copyright (C) 2014 NASA Ames Research Center + * $Id$ + */ + +// This application +#include "FlemonsSpineModelContact.h" +#include "examples/learningSpines/BaseSpineCPGControl.h" +// This library +#include "core/tgModel.h" +#include "core/tgSimView.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "core/terrain/tgHillyGround.h" +// The C++ Standard Library +#include + +/** + * The entry point. + * @param[in] argc the number of command-line arguments + * @param[in] argv argv[0] is the executable name; argv[1], if supplied, is the + * suffix for the controller + * @return 0 + */ +int main(int argc, char** argv) +{ + std::cout << "AppFlemonsSpineContact" << std::endl; + + // First create the world + const tgWorld::Config config(981); // gravity, cm/sec^2 +#if (1) + btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0); + btScalar friction = 0.5; + btScalar restitution = 0.0; + btVector3 size = btVector3(500.0, 1.5, 500.0); + btVector3 origin = btVector3(0.0, 0.0, 0.0); + size_t nx = 50; + size_t ny = 50; + double margin = 0.5; + double triangleSize = 7.5; + double waveHeight = 5.0; + double offset = 0.0; + tgHillyGround::Config groundConfig(eulerAngles, friction, restitution, + size, origin, nx, ny, margin, triangleSize, + waveHeight, offset); + + tgHillyGround* ground = new tgHillyGround(groundConfig); + + tgWorld world(config, ground); +#else + tgWorld world(config); +#endif + + // Second create the view + const double stepSize = 1.0/500.0; // Seconds + const double renderRate = 1.0/60.0; // Seconds + tgSimViewGraphics view(world, stepSize, renderRate); + + // Third create the simulation + tgSimulation simulation(view); + + // Fourth create the models with their controllers and add the models to the + // simulation + const int segments = 12; + FlemonsSpineModelContact* myModel = + new FlemonsSpineModelContact(segments); + + /* Required for setting up learning file input/output. */ + const std::string suffix((argc > 1) ? argv[1] : "default"); + + const int segmentSpan = 3; + const int numMuscles = 8; + const int numParams = 2; + const int segNumber = 6; // For learning results + const double controlTime = .001; + const double lowPhase = -1 * M_PI; + const double highPhase = M_PI; + const double lowAmplitude = -30.0; + const double highAmplitude = 30.0; + BaseSpineCPGControl::Config control_config(segmentSpan, numMuscles, numMuscles, numParams, segNumber, controlTime, + lowAmplitude, highAmplitude, lowPhase, highPhase); + BaseSpineCPGControl* const myControl = + new BaseSpineCPGControl(control_config, suffix, "learningSpines/TetrahedralComplex/"); + myModel->attach(myControl); + + simulation.addModel(myModel); + + int i = 0; + while (i < 3000) + { + simulation.run(30000); + #ifdef BT_USE_DOUBLE_PRECISION + std::cout << "Double precision" << std::endl; + #else + std::cout << "Single Precision" << std::endl; + #endif + simulation.reset(); + i++; + } + + /// @todo Does the model assume ownership of the controller? + /** No - a single controller could be attached to multiple subjects + * However, having this here causes a segfault, since there is a call + * to onTeardown() when the simulation is deleted + */ + #if (0) + delete myControl; + #endif + //Teardown is handled by delete, so that should be automatic + return 0; +} diff --git a/src/dev/btietz/TCContact/CMakeLists.txt b/src/dev/btietz/TCContact/CMakeLists.txt new file mode 100644 index 000000000..ecafcba43 --- /dev/null +++ b/src/dev/btietz/TCContact/CMakeLists.txt @@ -0,0 +1,19 @@ +link_directories(${LIB_DIR}) + +link_libraries(learningSpines + sensors + tgcreator + MuscleNP + core + util + terrain + Adapters + Configuration + AnnealEvolution + tgOpenGLSupport) + +add_executable(AppFlemonsSpineContact + FlemonsSpineModelContact.cpp + AppFlemonsSpineContact.cpp + +) diff --git a/src/dev/btietz/TCContact/FlemonsSpineModelContact.cpp b/src/dev/btietz/TCContact/FlemonsSpineModelContact.cpp new file mode 100644 index 000000000..c08e14341 --- /dev/null +++ b/src/dev/btietz/TCContact/FlemonsSpineModelContact.cpp @@ -0,0 +1,192 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file FlemonsSpineModelContact.cpp + * @brief Implementing the tetrahedral complex spine inspired by Tom Flemons + * @author Brian Mirletz + * @date November 2014 + * @version 1.0.0 + * $Id$ + */ + +// This module +#include "FlemonsSpineModelContact.h" +// This library +#include "core/tgCast.h" +#include "core/tgLinearString.h" +#include "core/tgString.h" +#include "dev/muscleNP/MuscleNP.h" +#include "dev/muscleNP/tgMultiPointStringInfo.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgLinearStringInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +#include "tgcreator/tgUtil.h" +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include // std::fill +#include +#include +#include + +FlemonsSpineModelContact::FlemonsSpineModelContact(int segments) : + BaseSpineModelLearning(segments) +{ +} + +FlemonsSpineModelContact::~FlemonsSpineModelContact() +{ +} + +void FlemonsSpineModelContact::setup(tgWorld& world) +{ + // 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 + + // Rod and Muscle configuration + // Note: This needs to be high enough or things fly apart... + const double density = 4.2/300.0; + const double radius = 0.5; + const double friction = 0.5; + const double rollFriction = 0.0; + const double restitution = 0.0; + const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution); + + tgLinearString::Config muscleConfig(1000, 10, 0.0, false, 7000, 12, 4000); + + // Calculations for the flemons spine model + double v_size = 10.0; + + // Create the tetrahedra + tgStructure tetra; + + tetra.addNode(0.0, 0.0, 0.0); // center + tetra.addNode( v_size, v_size, v_size); // front + tetra.addNode( v_size, -v_size, -v_size); // right + tetra.addNode(-v_size, v_size, -v_size); // back + tetra.addNode(-v_size, -v_size, v_size); // left + + tetra.addPair(0, 1, "front rod"); + tetra.addPair(0, 2, "right rod"); + tetra.addPair(0, 3, "back rod"); + tetra.addPair(0, 4, "left rod"); + + // Move the first one so we can create a longer snake. + // Or you could move the snake at the end, up to you. + tetra.move(btVector3(0.0,15.0,100.0)); + + // Create our snake segments + tgStructure snake; + /// @todo: there seems to be an issue with Muscle2P connections if the front + /// of a tetra is inside the next one. + btVector3 offset(0.0, 0.0, -v_size * 1.15); + for (std::size_t i = 0; i < m_segments; i++) + { + /// @todo: the snake is a temporary variable -- + /// will its destructor be called? + /// If not, where do we delete its children? + tgStructure* const p = new tgStructure(tetra); + p->addTags(tgString("segment num", i + 1)); + p->move((i + 1.0) * offset); + snake.addChild(p); // Add a child to the snake + } + //conditionally compile for debugging +#if (1) + // Add muscles that connect the segments + // Tag the muscles with their segment numbers so CPGs can find + // them. + std::vector children = snake.getChildren(); + for (std::size_t i = 1; i < children.size(); i++) + { + tgNodes n0 = children[i - 1]->getNodes(); + tgNodes n1 = children[i]->getNodes(); + + snake.addPair(n0[1], n1[1], + tgString("outer front muscle seg", i - 1) + tgString(" seg", i)); + snake.addPair(n0[2], n1[2], + tgString("outer right muscle seg", i - 1) + tgString(" seg", i)); + snake.addPair(n0[3], n1[3], + tgString("outer back muscle seg", i - 1) + tgString(" seg", i)); + snake.addPair(n0[4], n1[4], + tgString("outer top muscle seg", i - 1) + tgString(" seg", i)); + + snake.addPair(n0[2], n1[1], + tgString("inner front muscle seg", i - 1) + tgString(" seg", i)); + snake.addPair(n0[2], n1[4], + tgString("inner right muscle seg", i - 1) + tgString(" seg", i)); + snake.addPair(n0[3], n1[1], + tgString("inner left muscle seg", i - 1) + tgString(" seg", i)); + snake.addPair(n0[3], n1[4], + tgString("inner back muscle seg", i - 1) + tgString(" seg", i)); + + } +#endif + // Create the build spec that uses tags to turn the structure into a real model + tgBuildSpec spec; + spec.addBuilder("rod", new tgRodInfo(rodConfig)); + +#if (1) + spec.addBuilder("muscle", new tgMultiPointStringInfo(muscleConfig)); +#else + spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); +#endif + + // Create your structureInfo + tgStructureInfo structureInfo(snake, spec); + + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + + // Setup vectors for control + m_allMuscles = tgCast::filter (getDescendants()); + + m_allSegments = this->find ("segment"); + +#if (0) + // Debug printing + std::cout << "StructureInfo:" << std::endl; + std::cout << structureInfo << std::endl; + + std::cout << "Model: " << std::endl; + std::cout << *this << std::endl; +#endif + children.clear(); + + // Actually setup the children, notify controller + BaseSpineModelLearning::setup(world); +} + +void FlemonsSpineModelContact::teardown() +{ + + BaseSpineModelLearning::teardown(); + +} + +void FlemonsSpineModelContact::step(double dt) +{ + /* CPG update occurs in the controller so that we can decouple it + * from the physics update + */ + + BaseSpineModelLearning::step(dt); // Step any children +} diff --git a/src/dev/btietz/TCContact/FlemonsSpineModelContact.h b/src/dev/btietz/TCContact/FlemonsSpineModelContact.h new file mode 100644 index 000000000..b80506108 --- /dev/null +++ b/src/dev/btietz/TCContact/FlemonsSpineModelContact.h @@ -0,0 +1,58 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef FLEMONS_SPINE_MODEL_CONTACT_H +#define FLEMONS_SPINE_MODEL_CONTACT_H + +/** + * @file FlemonsSpineModelContact.h + * @brief Implementing the tetrahedral complex spine inspired by Tom Flemons + * @author Brian Mirletz + * @date November 2014 + * @version 1.0.0 + * $Id$ + */ + +#include "examples/learningSpines/BaseSpineModelLearning.h" + +class tgWorld; +class tgStructureInfo; +class tgLinearString; + +/** + * This class implements the tetrahedral complex tensegrity spine + * based on the work of Tom Flemons + * This is similar to the version in examples, but uses muscles with contact dynamics + */ +class FlemonsSpineModelContact : public BaseSpineModelLearning +{ +public: + + FlemonsSpineModelContact(int segments); + + virtual ~FlemonsSpineModelContact(); + + virtual void setup(tgWorld& world); + + virtual void teardown(); + + virtual void step(double dt); + +}; + +#endif // FLEMONS_SPINE_MODEL_H diff --git a/src/dev/btietz/TetraSpineStatic/AppTetraSpineStatic.cpp b/src/dev/btietz/TetraSpineStatic/AppTetraSpineStatic.cpp index 9b2901f14..66bf97939 100644 --- a/src/dev/btietz/TetraSpineStatic/AppTetraSpineStatic.cpp +++ b/src/dev/btietz/TetraSpineStatic/AppTetraSpineStatic.cpp @@ -58,7 +58,7 @@ int main(int argc, char** argv) // Second create the view const double stepSize = 1.0/2000.0; // Seconds const double renderRate = 1.0/60.0; // Seconds - tgSimViewGraphics view(world, stepSize, renderRate); + tgSimView view(world, stepSize, renderRate); // Third create the simulation tgSimulation simulation(view); diff --git a/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel.cpp b/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel.cpp index f373b96bf..627cd4b85 100644 --- a/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel.cpp +++ b/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel.cpp @@ -280,8 +280,8 @@ void TetraSpineStaticModel::setup(tgWorld& world) spec.addBuilder("PCB num2", new tgSphereInfo(PCB_2_Config)); // Two different string configs - tgLinearString::Config muscleConfig(210.15 * 50.0, 10, false, 0, 7000, 7.0, 9500); - tgLinearString::Config muscleConfig2(210.15, 2, false, 0, 7000, 7.0, 9500); + tgLinearString::Config muscleConfig(210.15 * 50.0, 10, 0.0, false, 7000, 7.0, 9500); + tgLinearString::Config muscleConfig2(210.15, 2, 0.0, false, 7000, 7.0, 9500); spec.addBuilder("top muscle", new tgLinearStringInfo(muscleConfig)); spec.addBuilder("left muscle", new tgLinearStringInfo(muscleConfig2)); spec.addBuilder("right muscle", new tgLinearStringInfo(muscleConfig2)); diff --git a/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel_hf.cpp b/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel_hf.cpp index 047b40318..c69a53659 100644 --- a/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel_hf.cpp +++ b/src/dev/btietz/TetraSpineStatic/TetraSpineStaticModel_hf.cpp @@ -280,8 +280,8 @@ void TetraSpineStaticModel_hf::setup(tgWorld& world) spec.addBuilder("PCB num2", new tgSphereInfo(PCB_2_Config)); // Two different string configs - tgLinearString::Config muscleConfig(229.16 * 2.0, 20, false, 0, 5000, 7.0, 9500, 10.0, 10.0); - tgLinearString::Config muscleConfig2(229.16, 20, false, 0, 5000, 7.0, 9500, 10.0, 10.0); + tgLinearString::Config muscleConfig(229.16 * 2.0, 20, 0.0, false, 5000, 7.0, 9500, 10.0, 10.0); + tgLinearString::Config muscleConfig2(229.16, 20, 0.0, false, 5000, 7.0, 9500, 10.0, 10.0); spec.addBuilder("top muscle", new tgLinearStringInfo(muscleConfig)); spec.addBuilder("left muscle", new tgLinearStringInfo(muscleConfig2)); spec.addBuilder("right muscle", new tgLinearStringInfo(muscleConfig2)); diff --git a/src/dev/btietz/caterpillar/CaterpillarModel.cpp b/src/dev/btietz/caterpillar/CaterpillarModel.cpp index bf6a8c598..486a8b84a 100644 --- a/src/dev/btietz/caterpillar/CaterpillarModel.cpp +++ b/src/dev/btietz/caterpillar/CaterpillarModel.cpp @@ -63,8 +63,8 @@ namespace double friction; double rollFriction; double restitution; + double pretension; bool hist; - double rotation; double maxTens; double targetVelocity; double maxAcc; @@ -79,8 +79,8 @@ namespace 0.99, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) + 0.0, // pretension 0, // History logging (boolean) - 0, // rotation 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -295,7 +295,7 @@ void CaterpillarModel::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.hist, c.rotation, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); diff --git a/src/dev/btietz/handTunedSineWaves/AppTetraSpineHT.cpp b/src/dev/btietz/handTunedSineWaves/AppTetraSpineHT.cpp index 47ff36018..472770ee5 100644 --- a/src/dev/btietz/handTunedSineWaves/AppTetraSpineHT.cpp +++ b/src/dev/btietz/handTunedSineWaves/AppTetraSpineHT.cpp @@ -56,7 +56,7 @@ int main(int argc, char** argv) // Second create the view const double stepSize = 1.0/4000.0; // Seconds const double renderRate = 1.0/60.0; // Seconds - tgSimViewGraphics view(world, stepSize, renderRate); + tgSimView view(world, stepSize, renderRate); // Third create the simulation tgSimulation simulation(view); diff --git a/src/dev/btietz/muscleNPEnergy/AppMuscleNPCons.cpp b/src/dev/btietz/muscleNPEnergy/AppMuscleNPCons.cpp new file mode 100644 index 000000000..386c38312 --- /dev/null +++ b/src/dev/btietz/muscleNPEnergy/AppMuscleNPCons.cpp @@ -0,0 +1,84 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ +/** +* @file AppMuscleNPCons.cpp +* @brief Contains the definition function main() for testing tconservation +* of energy for MuscleNP +* @author Brian Mirletz +* $Id$ +*/ +// This application +#include "MuscleNPCons.h" +// This library +#include "core/terrain/tgBoxGround.h" +#include "core/terrain/tgEmptyGround.h" +#include "core/tgModel.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "tgcreator/tgUtil.h" + +// The Bullet Physics Library +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +// The C++ Standard Library +#include +/** +* The entry point. +* @param[in] argc the number of command-line arguments +* @param[in] argv argv[0] is the executable name +* @return 0 +*/ +int main(int argc, char** argv) +{ + + std::cout << "AppMuscleNPTest" << std::endl; + + // First create the ground and world. Specify ground rotation in radians + tgEmptyGround* ground = new tgEmptyGround(); + + const tgWorld::Config config(0.0); // gravity, cm/sec^2 + tgWorld world(config, ground); + + // Second create the view + const double timestep_physics = 1.0/500.0; // seconds + const double timestep_graphics = 1.f/120.f; // seconds + tgSimViewGraphics view(world, timestep_physics, timestep_graphics); + + // Third create the simulation + tgSimulation simulation(view); + + // Fourth create the models with their controllers and add the models to the + // simulation + MuscleNPCons* const myModel = new MuscleNPCons(); + // Add the model to the world + simulation.addModel(myModel); + + double energyStart = myModel->getEnergy(); + btVector3 momentumStart = myModel->getMomentum(); + btVector3 velocityStart = myModel->getVelocityOfBody(2); + + simulation.run(4000); + simulation.reset(); + + double energyEnd = myModel->getEnergy(); + btVector3 momentumEnd = myModel->getMomentum(); + btVector3 velocityEnd = myModel->getVelocityOfBody(2); + + return 0; +} diff --git a/src/dev/btietz/muscleNPEnergy/CMakeLists.txt b/src/dev/btietz/muscleNPEnergy/CMakeLists.txt new file mode 100644 index 000000000..d7d2951d9 --- /dev/null +++ b/src/dev/btietz/muscleNPEnergy/CMakeLists.txt @@ -0,0 +1,9 @@ +link_libraries(tgcreator MuscleNP core) + +add_library(MuscleNPCons SHARED + MuscleNPCons.cpp) + +add_executable(AppMuscleNPCons + MuscleNPCons.cpp + AppMuscleNPCons.cpp +) diff --git a/src/dev/btietz/muscleNPEnergy/MuscleNPCons.cpp b/src/dev/btietz/muscleNPEnergy/MuscleNPCons.cpp new file mode 100644 index 000000000..f2d2cb679 --- /dev/null +++ b/src/dev/btietz/muscleNPEnergy/MuscleNPCons.cpp @@ -0,0 +1,227 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ + +#include "MuscleNPCons.h" +#include "core/tgModelVisitor.h" +#include "core/tgBulletUtil.h" +#include "core/tgWorld.h" +#include "core/tgLinearString.h" +#include "core/tgBaseString.h" +#include "core/tgRod.h" +#include "core/tgBox.h" +#include "core/tgBaseRigid.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgLinearStringInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgBoxInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +#include "dev/muscleNP/tgMultiPointStringInfo.h" + +#include "core/Muscle2P.cpp" +#include "core/muscleAnchor.cpp" + +// The Bullet Physics Library +#include "BulletDynamics/Dynamics/btRigidBody.h" + +MuscleNPCons::MuscleNPCons() +{ +} + +MuscleNPCons::~MuscleNPCons() +{ +} + +void MuscleNPCons::setup(tgWorld& world) +{ + + + const double rodDensity = 1; // Note: This needs to be high enough or things fly apart... + const double rodRadius = 0.25; + const tgRod::Config rodConfig(rodRadius, rodDensity); + const tgRod::Config rodConfig2(rodRadius, 0.0); + const tgBox::Config boxConfig(rodRadius, rodRadius, rodDensity); + + tgStructure s; + + s.addNode(-2, 2, 0); + s.addNode(0, 2, 0); + s.addNode(0, 2, 0); + s.addNode(0, 4, 0); + s.addNode(0, 12, 0); + s.addNode(0, 14, 0); + + + // Free rod to which we will apply motion + s.addNode(-1, 6, 5); + s.addNode(1, 6, 5); + + //s.addPair(0, 1, "rod"); + s.addPair(2, 3, "box"); + s.addPair(4, 5, "box"); + + s.addPair(6, 7, "rod"); + + //s.addPair(1, 2, "muscle"); + s.addPair(3, 4, "muscle"); + s.move(btVector3(0, 0, 0)); + + + // Move the structure so it doesn't start in the ground + s.move(btVector3(0, 0, 0)); + s.addRotation(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 1.0, 0.0), M_PI/4); + tgBaseString::Config muscleConfig(1000, 0, 0.0, false, 600000000); + + // Create the build spec that uses tags to turn the structure into a real model + tgBuildSpec spec; + + spec.addBuilder("rod2", new tgRodInfo(rodConfig2)); + spec.addBuilder("box", new tgBoxInfo(boxConfig)); + spec.addBuilder("rod", new tgRodInfo(rodConfig)); +#if (1) + spec.addBuilder("muscle", new tgMultiPointStringInfo(muscleConfig)); +#else + spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); +#endif + // Create your structureInfo + tgStructureInfo structureInfo(s, spec); + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + // We could now use tgCast::filter or similar to pull out the + // models (e.g. muscles) that we want to control. + allRods.clear(); + allMuscles.clear(); + allMuscles = tgCast::filter (getDescendants()); + allRods = tgCast::filter (getDescendants()); + + btRigidBody* body = allRods[0]->getPRigidBody(); + btRigidBody* body2 = allRods[1]->getPRigidBody(); + + // Apply initial impulse + btVector3 impulse(0.5, 0.0, 0.5); + body->applyCentralImpulse(impulse); + body2->applyCentralImpulse(impulse); + + btRigidBody* body3 = allRods[2]->getPRigidBody(); + + std::cout << body3->getCenterOfMassTransform () << std::endl; + + notifySetup(); + totalTime = 0.0; + tgModel::setup(world); +} + +void MuscleNPCons::teardown() +{ + tgModel::teardown(); +} + +void MuscleNPCons::step(double dt) +{ + totalTime += dt; + + tgModel::step(dt); + + btVector3 vCom(0, 0, 0); + btScalar mass = 0; + btScalar energy = 0; + for (std::size_t i = 0; i < allRods.size(); i++) + { + tgBaseRigid& ri = *(allRods[i]); + btRigidBody* body = ri.getPRigidBody(); + btVector3 localVel = body->getLinearVelocity(); + vCom += localVel * ri.mass(); + energy += localVel.length2() * ri.mass();// + body->getAngularVelocity().length2() ; + mass += ri.mass(); + } + + btVector3 forceSum(0.0, 0.0, 0.0); + + const std::vector& anchorList = allMuscles[0]->getMuscle()->getAnchors(); + int n = anchorList.size(); + for (std::size_t i = 0; i < n; i++) + { + forceSum += anchorList[i]->getForce(); + } + +#if (1) + std::cout << "Momentum " << vCom << std::endl; + std::cout << "Energy " << energy << std::endl; + std::cout << "Other Momentum " << getMomentum() << std::endl; + std::cout << "Force sum " << forceSum << std::endl; + std::cout << "Length " << allMuscles[0]->getCurrentLength(); + std::cout << " Dist " << (anchorList[0]->getWorldPosition() - anchorList[n-1]->getWorldPosition()).length() << std::endl; + std::cout << "Anchors: " << n << std::endl; + + if (energy > 20){ + std::cout << "Here!" << std::endl; + } +#endif +} + +/** +* Call tgModelVisitor::render() on self and all descendants. +* @param[in,out] r a reference to a tgModelVisitor +*/ +void MuscleNPCons::onVisit(const tgModelVisitor& r) const +{ + r.render(*this); + tgModel::onVisit(r); +} + +double MuscleNPCons::getEnergy() const +{ + double energy = 0; + btScalar mass = 0; + for (std::size_t i = 0; i < allRods.size(); i++) + { + tgBaseRigid* ri = allRods[i]; + btRigidBody* body = ri->getPRigidBody(); + btVector3 localVel = body->getLinearVelocity(); + energy += localVel.length2() * ri->mass();// + body->getAngularVelocity().length2() ; + mass += ri->mass(); + } + + return energy; +} + +btVector3 MuscleNPCons::getMomentum() const +{ + btVector3 vCom(0, 0, 0); + btScalar mass = 0; + + for (std::size_t i = 0; i < allRods.size(); i++) + { + tgBaseRigid* ri = allRods[i]; + btRigidBody* body = ri->getPRigidBody(); + btVector3 localVel = body->getLinearVelocity(); + vCom += localVel * ri->mass(); + mass += ri->mass(); + } + + return vCom; +} + +btVector3 MuscleNPCons::getVelocityOfBody(int body_num) const +{ + assert(body_num < allRods.size() && body_num >= 0); + tgBaseRigid* ri = allRods[body_num]; + btRigidBody* body = ri->getPRigidBody(); + + return body->getLinearVelocity(); +} diff --git a/src/dev/btietz/muscleNPEnergy/MuscleNPCons.h b/src/dev/btietz/muscleNPEnergy/MuscleNPCons.h new file mode 100644 index 000000000..01239c637 --- /dev/null +++ b/src/dev/btietz/muscleNPEnergy/MuscleNPCons.h @@ -0,0 +1,110 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ + +#ifndef MUSCLE_NP_CONS +#define MUSCLE_NP_CONS + +/** +* @file MuscleNPCons.h +* @brief For testing MuscleNP contacts +* @author Brian Mirletz +* @version 1.0.0 +* $Id$ +*/ + +// This library +#include "core/tgModel.h" +#include "core/tgSubject.h" + +// The Bullet Physics Library +#include "LinearMath/btVector3.h" + +// The C++ Standard Library +#include + +// Forward declarations +class tgLinearString; +class tgModelVisitor; +class tgWorld; +class tgBaseRigid; + +/** +* A class that constructs a three bar tensegrity prism using the tools +* in tgcreator. +*/ +class MuscleNPCons : public tgSubject, public tgModel +{ +public: + /** + * The only constructor. Configuration parameters are within the + * .cpp file in this case, not passed in. + */ + MuscleNPCons(); + /** + * Destructor. Deletes controllers, if any were added during setup. + * Teardown handles everything else. + */ + virtual ~MuscleNPCons(); + /** + * Create the model. Place the rods and strings into the world + * that is passed into the simulation. This is triggered + * automatically when the model is added to the simulation, when + * tgModel::setup(world) is called (if this model is a child), + * and when reset is called. Also notifies controllers of setup. + * @param[in] world - the world we're building into + */ + virtual void setup(tgWorld& world); + /** + * Undoes setup. Deletes child models. Called automatically on + * reset and end of simulation. Notifies controllers of teardown + */ + virtual void teardown(); + /** + * Step the model, its children. Notifies controllers of step. + * @param[in] dt, the timestep. Must be positive. + */ + virtual void step(double dt); + /** + * Receives a tgModelVisitor and dispatches itself into the + * visitor's "render" function. This model will go to the default + * tgModel function, which does nothing. + * @param[in] r - a tgModelVisitor which will pass this model back + * to itself + */ + virtual void onVisit(const tgModelVisitor& r) const; + + /** + * Sums the square of the velocity times the mass + * @todo account for rotational energy as well, and energy in the string + */ + virtual double getEnergy() const; + + /** + * Returns a btVector3 of the current center of mass velocity (momentum) + */ + virtual btVector3 getMomentum() const; + + virtual btVector3 getVelocityOfBody(int body_num) const; + +private: + double totalTime; + std::vector allMuscles; + std::vector allRods; + +}; +#endif // SIMPLE_CORDE_TENSEGRITY_H diff --git a/src/dev/btietz/muscleNPTests/AppsimpleMuscleNP.cpp b/src/dev/btietz/muscleNPTests/AppsimpleMuscleNP.cpp new file mode 100644 index 000000000..e69bc56a2 --- /dev/null +++ b/src/dev/btietz/muscleNPTests/AppsimpleMuscleNP.cpp @@ -0,0 +1,83 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ +/** +* @file AppCordeTest.cpp +* @brief Contains the definition function main() for testing the Corde +* string model +* @author Brian Mirletz +* $Id$ +*/ +// This application +#include "simpleMuscleNP.h" +// This library +#include "core/terrain/tgBoxGround.h" +#include "core/terrain/tgEmptyGround.h" +#include "core/tgModel.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "tgcreator/tgUtil.h" + +// The Bullet Physics Library +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +// The C++ Standard Library +#include +/** +* The entry point. +* @param[in] argc the number of command-line arguments +* @param[in] argv argv[0] is the executable name +* @return 0 +*/ +int main(int argc, char** argv) +{ + + std::cout << "AppMuscleNPTest" << std::endl; + + // First create the ground and world. Specify ground rotation in radians + const double yaw = 0.0; + const double pitch = 0.0; + const double roll = 0.0; + const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll)); + + // the world will delete this +#if (0) + tgBoxGround* ground = new tgBoxGround(groundConfig); +#else + tgEmptyGround* ground = new tgEmptyGround(); +#endif + const tgWorld::Config config(9.81); // gravity, cm/sec^2 + tgWorld world(config, ground); + + // Second create the view + const double timestep_physics = 1.0/1000.0; // seconds + const double timestep_graphics = 1.f/60.f; // seconds + tgSimViewGraphics view(world, timestep_physics, timestep_graphics); + + // Third create the simulation + tgSimulation simulation(view); + + + // Fourth create the models with their controllers and add the models to the + // simulation + simpleMuscleNP* const myModel = new simpleMuscleNP(); + // Add the model to the world + simulation.addModel(myModel); + simulation.run(); + return 0; +} diff --git a/src/dev/btietz/muscleNPTests/CMakeLists.txt b/src/dev/btietz/muscleNPTests/CMakeLists.txt new file mode 100644 index 000000000..1c501cebe --- /dev/null +++ b/src/dev/btietz/muscleNPTests/CMakeLists.txt @@ -0,0 +1,6 @@ +link_libraries(tgcreator MuscleNP core) + +add_executable(AppSimpleMuscleNP + simpleMuscleNP.cpp + AppsimpleMuscleNP.cpp +) diff --git a/src/dev/btietz/muscleNPTests/simpleMuscleNP.cpp b/src/dev/btietz/muscleNPTests/simpleMuscleNP.cpp new file mode 100644 index 000000000..9415845aa --- /dev/null +++ b/src/dev/btietz/muscleNPTests/simpleMuscleNP.cpp @@ -0,0 +1,151 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ + +#include "simpleMuscleNP.h" +#include "core/tgModelVisitor.h" +#include "core/tgBulletUtil.h" +#include "core/tgWorld.h" +#include "core/tgLinearString.h" +#include "core/tgBaseString.h" +#include "core/tgRod.h" +#include "core/tgBox.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgLinearStringInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgBoxInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +#include "dev/muscleNP/tgMultiPointStringInfo.h" +simpleMuscleNP::simpleMuscleNP() +{ +} + +simpleMuscleNP::~simpleMuscleNP() +{ +} + +void simpleMuscleNP::setup(tgWorld& world) +{ + + + const double rodDensity = 1; // Note: This needs to be high enough or things fly apart... + const double rodRadius = 0.25; + const tgRod::Config rodConfig(rodRadius, rodDensity); + const tgRod::Config rodConfig2(rodRadius, 0.0); + const tgBox::Config boxConfig(rodRadius, rodRadius, 0.0); + + tgStructure s; + +#if (0) + s.addNode(-2, 2.1, 0); + s.addNode(0, 2.1, 0); + s.addNode(10, 2.1, 0); + s.addNode(12, 2.1, 0); + s.addNode(22, 2.1, 0); + s.addNode(24, 2.1, 0); + + s.addPair(0, 1, "rod2"); + s.addPair(2, 3, "rod2"); + s.addPair(4, 5, "rod2"); + + s.addPair(1, 2, "muscle"); + s.addPair(3, 4, "muscle"); + s.move(btVector3(0, 0, 0)); +#else + s.addNode(0, 5, 2); + s.addNode(0, 5, 0); + s.addNode(10, 5, 2); + s.addNode(10, 5, 0); + s.addNode(5, 2, -5); + s.addNode(5, 2, 5); + + s.addPair(0, 1, "rod"); + s.addPair(2, 3, "rod"); + s.addPair(4, 5, "rod2"); + + s.addPair(0, 2, "muscle"); + s.addPair(1, 3, "muscle"); + //s.addPair(3, 4, "muscle"); + s.move(btVector3(0, 0, 0)); +#endif + + + // Move the structure so it doesn't start in the ground + s.move(btVector3(0, 0, 0)); + //s.addRotation(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 1.0, 0.0), M_PI_2); + tgBaseString::Config muscleConfig(1000, 10, 0.0, false, 600000000); + + // Create the build spec that uses tags to turn the structure into a real model + tgBuildSpec spec; + spec.addBuilder("rod", new tgRodInfo(rodConfig)); + spec.addBuilder("rod2", new tgRodInfo(rodConfig2)); + spec.addBuilder("box", new tgBoxInfo(boxConfig)); +#if (1) + spec.addBuilder("muscle", new tgMultiPointStringInfo(muscleConfig)); +#else + spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); +#endif + // Create your structureInfo + tgStructureInfo structureInfo(s, spec); + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + // We could now use tgCast::filter or similar to pull out the + // models (e.g. muscles) that we want to control. + allMuscles = tgCast::filter (getDescendants()); + allRods = tgCast::filter (getDescendants()); + + notifySetup(); + totalTime = 0.0; + tgModel::setup(world); +} + +void simpleMuscleNP::teardown() +{ + tgModel::teardown(); +} + +void simpleMuscleNP::step(double dt) +{ + totalTime += dt; + + //allMuscles[0]->setRestLength(11, dt); +// allMuscles[1]->setRestLength(11, dt); + + btVector3 com(0, 0, 0); + btScalar mass = 0; + btScalar energy = 0; + for (std::size_t i = 0; i < allRods.size(); i++) + { + tgRod& ri = *(allRods[i]); + com += ri.centerOfMass() * ri.mass(); + mass += ri.mass(); + } + + //std::cout << allMuscles[0]->getTension() << std::endl; + tgModel::step(dt); +} + +/** +* Call tgModelVisitor::render() on self and all descendants. +* @param[in,out] r a reference to a tgModelVisitor +*/ +void simpleMuscleNP::onVisit(const tgModelVisitor& r) const +{ + r.render(*this); + tgModel::onVisit(r); +} diff --git a/src/dev/btietz/muscleNPTests/simpleMuscleNP.h b/src/dev/btietz/muscleNPTests/simpleMuscleNP.h new file mode 100644 index 000000000..95d9a38f3 --- /dev/null +++ b/src/dev/btietz/muscleNPTests/simpleMuscleNP.h @@ -0,0 +1,93 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ + +#ifndef SIMPLE_MUSCLE_NP_H +#define SIMPLE_MUSCLE_NP_H + +/** +* @file simpleMuscleNP.h +* @brief For testing MuscleNP contacts +* @author Brian Mirletz +* @version 1.0.0 +* $Id$ +*/ + +// This library +#include "core/tgModel.h" +#include "core/tgSubject.h" +// The C++ Standard Library +#include + +// Forward declarations +class tgLinearString; +class tgModelVisitor; +class tgWorld; +class tgRod; + +/** +* A class that constructs a three bar tensegrity prism using the tools +* in tgcreator. +*/ +class simpleMuscleNP : public tgSubject, public tgModel +{ +public: + /** + * The only constructor. Configuration parameters are within the + * .cpp file in this case, not passed in. + */ + simpleMuscleNP(); + /** + * Destructor. Deletes controllers, if any were added during setup. + * Teardown handles everything else. + */ + virtual ~simpleMuscleNP(); + /** + * Create the model. Place the rods and strings into the world + * that is passed into the simulation. This is triggered + * automatically when the model is added to the simulation, when + * tgModel::setup(world) is called (if this model is a child), + * and when reset is called. Also notifies controllers of setup. + * @param[in] world - the world we're building into + */ + virtual void setup(tgWorld& world); + /** + * Undoes setup. Deletes child models. Called automatically on + * reset and end of simulation. Notifies controllers of teardown + */ + virtual void teardown(); + /** + * Step the model, its children. Notifies controllers of step. + * @param[in] dt, the timestep. Must be positive. + */ + virtual void step(double dt); + /** + * Receives a tgModelVisitor and dispatches itself into the + * visitor's "render" function. This model will go to the default + * tgModel function, which does nothing. + * @param[in] r - a tgModelVisitor which will pass this model back + * to itself + */ + virtual void onVisit(const tgModelVisitor& r) const; + +private: + double totalTime; + std::vector allMuscles; + std::vector allRods; + +}; +#endif // SIMPLE_CORDE_TENSEGRITY_H diff --git a/src/dev/btietz/sphereFeet/AppSUPERSphereball.cpp b/src/dev/btietz/sphereFeet/AppSUPERSphereball.cpp index 3cdc5dda0..3d3aa84d7 100644 --- a/src/dev/btietz/sphereFeet/AppSUPERSphereball.cpp +++ b/src/dev/btietz/sphereFeet/AppSUPERSphereball.cpp @@ -25,6 +25,7 @@ // This application #include "T6SphereModel.h" +#include "examples/SUPERball/T6TensionController.h" // This library #include "core/terrain/tgBoxGround.h" #include "core/tgModel.h" @@ -50,7 +51,7 @@ int main(int argc, char** argv) // Determine the angle of the ground in radians. All 0 is flat const double yaw = 0.0; - const double pitch = M_PI/15.0; + const double pitch = 0.0; const double roll = 0.0; const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll)); // the world will delete this @@ -63,7 +64,7 @@ int main(int argc, char** argv) tgWorld world(config, ground); // Second create the view - const double timestep_physics = 0.0001; // Seconds + const double timestep_physics = 0.001; // Seconds const double timestep_graphics = 1.f/60.f; // Seconds tgSimViewGraphics view(world, timestep_physics, timestep_graphics); @@ -87,7 +88,7 @@ int main(int argc, char** argv) // Set the tension of the controller units of kg * length / s^2 // So 10000 units at this scale is 1000 N - // T6TensionController* const pTC = new T6TensionController(10000); + //T6TensionController* const pTC = new T6TensionController(100); //myModel->attach(pTC); simulation.addModel(myModel); diff --git a/src/dev/btietz/sphereFeet/CMakeLists.txt b/src/dev/btietz/sphereFeet/CMakeLists.txt index f7eebaddf..2de7af1ec 100644 --- a/src/dev/btietz/sphereFeet/CMakeLists.txt +++ b/src/dev/btietz/sphereFeet/CMakeLists.txt @@ -1,4 +1,4 @@ -link_libraries(tgcreator core) +link_libraries(tgcreator MuscleNP core) add_executable(AppSuperSphereBall T6SphereModel.cpp diff --git a/src/dev/btietz/sphereFeet/T6SphereModel.cpp b/src/dev/btietz/sphereFeet/T6SphereModel.cpp index e0cc06c96..f9685fe10 100644 --- a/src/dev/btietz/sphereFeet/T6SphereModel.cpp +++ b/src/dev/btietz/sphereFeet/T6SphereModel.cpp @@ -34,6 +34,7 @@ #include "tgcreator/tgSphereInfo.h" #include "tgcreator/tgStructure.h" #include "tgcreator/tgStructureInfo.h" +#include "dev/muscleNP/tgMultiPointStringInfo.h" // The Bullet Physics library #include "LinearMath/btVector3.h" // The C++ Standard Library @@ -64,7 +65,8 @@ namespace double friction; double rollFriction; double restitution; - double rotation; + double pretension; + bool history; double maxTens; double targetVelocity; double maxAcc; @@ -79,7 +81,8 @@ namespace 0.99, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) - 0, // rotation + 0.0, // pretension (force) + false, // history (boolean) 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -163,7 +166,7 @@ void T6SphereModel::addMuscles(tgStructure& s) s.addPair(6, 11, "muscle"); s.addPair(7, 8, "muscle"); - s.addPair(7, 9, "muscle"); + s.addPair(7, 9, "muscleN"); } @@ -173,11 +176,11 @@ void T6SphereModel::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.rotation, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.history, c.maxTens, c.targetVelocity, c.maxAcc); - const tgSphere::Config sphereConfig(0.5, 0.5); + const tgSphere::Config sphereConfig(0.5, 0.25); const tgSphere::Config sphereConfig2(0.5, 2.5); @@ -198,9 +201,10 @@ void T6SphereModel::setup(tgWorld& world) // Create the build spec that uses tags to turn the structure into a real model tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); + spec.addBuilder("muscle", new tgMultiPointStringInfo(muscleConfig)); //spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); - spec.addBuilder("sphere1", new tgSphereInfo(sphereConfig)); - spec.addBuilder("sphere2", new tgSphereInfo(sphereConfig2)); + //spec.addBuilder("sphere1", new tgSphereInfo(sphereConfig)); + //spec.addBuilder("sphere2", new tgSphereInfo(sphereConfig)); // Create your structureInfo tgStructureInfo structureInfo(s, spec); diff --git a/src/dev/btietz/tetraCollisions/AppTetraSpineCol.cpp b/src/dev/btietz/tetraCollisions/AppTetraSpineCol.cpp new file mode 100644 index 000000000..57e610187 --- /dev/null +++ b/src/dev/btietz/tetraCollisions/AppTetraSpineCol.cpp @@ -0,0 +1,130 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file AppTetraSpineCol.cpp + * @brief Contains the definition function main() for the Tetra Spine Static + * application. + * @author Brian Mirletz + * @copyright Copyright (C) 2014 NASA Ames Research Center + * $Id$ + */ + +// This application +#include "TetraSpineCollisions.h" +#include "colSpineSine.h" +#include "Wall.h" +// This library +#include "core/tgModel.h" +#include "core/tgSimView.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "core/terrain/tgHillyGround.h" +#include "examples/learningSpines/tgCPGLogger.h" +// The C++ Standard Library +#include + +/** + * The entry point. + * @param[in] argc the number of command-line arguments + * @param[in] argv argv[0] is the executable name; argv[1], if supplied, is the + * suffix for the controller + * @return 0 + */ +int main(int argc, char** argv) +{ + std::cout << "AppTetraSpineHT" << std::endl; + + // First create the world + const tgWorld::Config config(981); // gravity, cm/sec^2 + + ; +#if (1) + btVector3 eulerAngles = btVector3(0.0, 0.0, 0.0); + btScalar friction = 0.5; + btScalar restitution = 0.0; + btVector3 size = btVector3(500.0, 1.5, 500.0); + btVector3 origin = btVector3(0.0, 0.0, 0.0); + size_t nx = 50; + size_t ny = 50; + double margin = 0.2; + double triangleSize = 12; + double waveHeight = 5.0; + double offset = 0.0; + tgHillyGround::Config groundConfig(eulerAngles, friction, restitution, + size, origin, nx, ny, margin, triangleSize, + waveHeight, offset); + + tgHillyGround* ground = new tgHillyGround(groundConfig); + + tgWorld world(config, ground); +#else + tgWorld world(config); +#endif + // Second create the view + const double stepSize = 1.0/500.0; // Seconds + const double renderRate = 1.0/60.0; // Seconds + tgSimViewGraphics view(world, stepSize, renderRate); + + // Third create the simulation + tgSimulation simulation(view); + + // Fourth create the models with their controllers and add the models to the + // simulation + const int segments = 6; + TetraSpineCollisions* myModel = + new TetraSpineCollisions(segments); + + colSpineSine* const myControl = + new colSpineSine(); + + myModel->attach(myControl); + /* + tgCPGLogger* const myLogger = + new tgCPGLogger("logs/CPGValues.txt"); + + myControl->attach(myLogger); + */ + + // Add obstacles + btVector3 wallOrigin(0.0, 0.0, 50.0); + Wall* myWall = new Wall(wallOrigin); + + simulation.addModel(myModel); + simulation.addModel(myWall); + + int i = 0; + while (i < 1) + { + simulation.run(240000); + //simulation.reset(); + i++; + } + + /// @todo Does the model assume ownership of the controller? + /** No - a single controller could be attached to multiple subjects + * However, having this here causes a segfault, since there is a call + * to onTeardown() when the simulation is deleted + */ + #if (0) + delete myControl; + #endif + //Teardown is handled by delete, so that should be automatic + return 0; +} diff --git a/src/dev/btietz/tetraCollisions/CMakeLists.txt b/src/dev/btietz/tetraCollisions/CMakeLists.txt new file mode 100644 index 000000000..6277c73b5 --- /dev/null +++ b/src/dev/btietz/tetraCollisions/CMakeLists.txt @@ -0,0 +1,27 @@ +link_directories(${LIB_DIR}) + + +link_libraries(tetraSpineLearningSine + tetraSpineHardware + learningSpines + sensors + tgcreator + MuscleNP + core + util + terrain + Adapters + Configuration + AnnealEvolution + tgOpenGLSupport) + +add_executable(AppTetraSpineCol + TetraSpineCollisions.cpp + colSpineSine.cpp + Wall.cpp + AppTetraSpineCol.cpp +) + +target_link_libraries(AppTetraSpineCol ${ENV_LIB_DIR}/libjsoncpp.a FileHelpers) +configure_file("controlVars.json" "controlVars.json" COPYONLY) + diff --git a/src/dev/btietz/tetraCollisions/TetraSpineCollisions.cpp b/src/dev/btietz/tetraCollisions/TetraSpineCollisions.cpp new file mode 100644 index 000000000..7d1dd4384 --- /dev/null +++ b/src/dev/btietz/tetraCollisions/TetraSpineCollisions.cpp @@ -0,0 +1,321 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +// This module +#include "TetraSpineCollisions.h" +// This library +#include "core/tgCast.h" +#include "core/tgLinearString.h" +#include "core/tgString.h" +#include "core/tgSphere.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgLinearStringInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgSphereInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +#include "tgcreator/tgUtil.h" + +#include "dev/muscleNP/tgMultiPointStringInfo.h" + +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include +#include + +/** + * @file TetraSpineCollisions.cpp + * @brief Tetraspine, configured for learning in the NTRT simulator + * @author Brian Tietz + * @date May 2014 + * @version 1.0.0 + * $Id$ + */ + +TetraSpineCollisions::TetraSpineCollisions(size_t segments) : + BaseSpineModelLearning(segments) +{ +} + +TetraSpineCollisions::~TetraSpineCollisions() +{ +} +namespace +{ + void addNodes(tgStructure& tetra, double edge, double height) + { + // Front segment + // right + tetra.addNode(-edge / 2.0, 0, 0, "base"); + // left + tetra.addNode( edge / 2.0, 0, 0, "base"); + // top + tetra.addNode(0, height, 0, "base"); + // front + tetra.addNode(0, edge / 2.0 * tan(M_PI / 6.0), 2.50 * 5.0, "tip"); + + // Get the next two nodes from existing nodes: + tgNodes oldNodes = tetra.getNodes(); + tgNode nn1 = (oldNodes[0] + oldNodes[2])/2.0; // 4 mid + nn1.addTags("PCB"); + tetra.addNode(nn1); + tgNode nn2 = (oldNodes[1] + oldNodes[2])/2.0; // 5 mid + nn2.addTags("PCB"); + tetra.addNode(nn2); + + std::cout << (oldNodes[3] - oldNodes[2]).length() << std::endl; + std::cout << (oldNodes[3] - oldNodes[1]).length() << std::endl; + std::cout << (oldNodes[3] - oldNodes[0]).length() << std::endl; + + } + + void addPairs(tgStructure& tetra) + { + tetra.addPair(0, 1, "back bottom rod"); + tetra.addPair(0, 4, "back rightBottom rod"); + tetra.addPair(4, 2, "back rightTop rod"); + tetra.addPair(0, 3, "front right rod"); + tetra.addPair(1, 5, "back leftBottom rod"); + tetra.addPair(5, 2, "back leftTop rod"); + tetra.addPair(1, 3, "front left rod"); + tetra.addPair(2, 3, "front top rod"); + } + + + void addSegments(tgStructure& snake, const tgStructure& tetra, double edge, + size_t segmentCount) + { + const btVector3 offset(0, 0, -2.30 * 5.0); + for (size_t i = 0; i < segmentCount; ++i) + { + /// @todo: the snake is a temporary variable -- will its destructor be called? + /// If not, where do we delete its children? + tgStructure* const t = new tgStructure(tetra); + t->addTags(tgString("segment num", i )); // Use num0, num1, num2 as a tag!! + + t->move((i + 1) * offset); + + // Add a child to the snake + snake.addChild(t); + + } + } + + // Add muscles that connect the segments + void addMuscles(tgStructure& snake) + { + const std::vector children = snake.getChildren(); + for (size_t i = 1; i < children.size(); ++i) + { + tgNodes n0 = children[i-1]->getNodes(); + tgNodes n1 = children[i ]->getNodes(); + + snake.addPair(n0[0], n1[0], tgString("outer right muscle seg", i)); + snake.addPair(n0[1], n1[1], tgString("outer left muscle seg", i)); + snake.addPair(n0[2], n1[2], tgString("outer top muscle seg", i)); + + snake.addPair(n0[0], n1[3], tgString("inner right muscle seg", i)); + snake.addPair(n0[1], n1[3], tgString("inner left muscle seg", i)); + snake.addPair(n0[2], n1[3], tgString("inner top muscle seg", i)); + } + } + + void mapMuscles(TetraSpineCollisions::MuscleMap& muscleMap, + tgModel& model) + { + // Note that tags don't need to match exactly, we could create + // supersets if we wanted to + muscleMap["inner left"] = model.find("inner left muscle"); + muscleMap["inner right"] = model.find("inner right muscle"); + muscleMap["inner top"] = model.find("inner top muscle"); + muscleMap["outer left"] = model.find("outer left muscle"); + muscleMap["outer right"] = model.find("outer right muscle"); + muscleMap["outer top"] = model.find("outer top muscle"); + } + + void addMarkers(tgStructure& structure, TetraSpineCollisions& model) + { + + const std::vector children = structure.getChildren(); + tgNodes n0 = children[0]->getNodes(); + + // TODO: consider using the segments vector here + btRigidBody* firstBody = model.getAllRigids()[0]->getPRigidBody(); + + std::vector myRigids = model.getAllRigids(); +#if (0) + for (int i =0; i < myRigids.size(); i++) + { + std::cout << myRigids[i]->mass() << " " <getPRigidBody() << std::endl; + } +#endif + + abstractMarker marker1(firstBody, n0[3] - firstBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0); + + model.addMarker(marker1); + + tgNodes n1 = children[1]->getNodes(); + + btRigidBody* secondBody = model.getAllRigids()[15]->getPRigidBody(); + + abstractMarker marker2(secondBody, n1[3] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0); + + model.addMarker(marker2); + + abstractMarker marker3(secondBody, n1[1] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0); + + model.addMarker(marker3); + + abstractMarker marker4(secondBody, n1[0] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0); + + model.addMarker(marker4); + + tgNodes n2 = children[2]->getNodes(); + + btRigidBody* thirdBody = model.getAllRigids()[29]->getPRigidBody(); + + abstractMarker marker5(thirdBody, n2[3] - thirdBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0); + + model.addMarker(marker5); + } + + void trace(const tgStructureInfo& structureInfo, tgModel& model) + { + std::cout << "StructureInfo:" << std::endl + << structureInfo << std::endl + << "Model: " << std::endl + << model << std::endl; + // Showing the find function + const std::vector outerMuscles = + model.find("outer"); + for (size_t i = 0; i < outerMuscles.size(); ++i) + { + const tgLinearString* const pMuscle = outerMuscles[i]; + assert(pMuscle != NULL); + std::cout << "Outer muscle: " << *pMuscle << std::endl; + } + } + +} // namespace + +// 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) +void TetraSpineCollisions::setup(tgWorld& world) +{ + const double edge = 3.8 * 5.0; + const double height = tgUtil::round(std::sqrt(3.0)/2 * edge); + std::cout << "edge: " << edge << "; height: " << height << std::endl; + + // Create the tetrahedra + tgStructure tetra; + addNodes(tetra, edge, height); + addPairs(tetra); + + // Move the first one so we can create a longer snake. + // Or you could move the snake at the end, up to you. + tetra.move(btVector3(0.0, 8.0, 10.0)); + + // Create our snake segments + tgStructure snake; + addSegments(snake, tetra, edge, m_segments); + addMuscles(snake); + + // Create the build spec that uses tags to turn the structure into a real model + // Note: This needs to be high enough or things fly apart... + + + // Params for In Won + const double radius = 0.635 / 2.0; + const double sphereRadius = 0.635 / 2.0; + const double density = .0201 / (pow(radius, 2) * M_PI * edge); // Mass divided by volume... should there be a way to set this automatically?? + const double friction = 0.5; + const tgRod::Config rodConfig(radius, density, friction); + tgBuildSpec spec; + spec.addBuilder("rod", new tgRodInfo(rodConfig)); + + // 1000 is so the units below can be in grams + const double sphereVolume1 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3); + const double sphereVolume2 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3); + + const double baseCornerMidD = 180.0 / sphereVolume1; + const tgSphere::Config baseCornerMidConfig(sphereRadius, baseCornerMidD, friction); + spec.addBuilder("base", new tgSphereInfo(baseCornerMidConfig)); + + const double tipCornerMidD = 120.0 / sphereVolume1; + const tgSphere::Config tipCornerMidConfig(sphereRadius, tipCornerMidD, friction); + spec.addBuilder("tip", new tgSphereInfo(tipCornerMidConfig)); + + const double PCBD = 70.0 / sphereVolume2; + const tgSphere::Config PCB_1_Config(radius, PCBD, friction); + spec.addBuilder("PCB", new tgSphereInfo(PCB_1_Config)); + + + // Two different string configs + tgLinearString::Config muscleConfig(229.16 * 2.0, 20, 0.0, false, 500, 7.0, 9500, 0.1, 0.1); + tgLinearString::Config muscleConfig2(229.16, 20, 0.0, false, 500, 7.0, 9500, 0.1, 0.1); +#if (1) + spec.addBuilder("top muscle", new tgMultiPointStringInfo(muscleConfig)); + spec.addBuilder("left muscle", new tgMultiPointStringInfo(muscleConfig2)); + spec.addBuilder("right muscle", new tgMultiPointStringInfo(muscleConfig2)); +#else + spec.addBuilder("top muscle", new tgLinearStringInfo(muscleConfig)); + spec.addBuilder("left muscle", new tgLinearStringInfo(muscleConfig2)); + spec.addBuilder("right muscle", new tgLinearStringInfo(muscleConfig2)); +#endif + // Create your structureInfo + tgStructureInfo structureInfo(snake, spec); + + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + + // We could now use tgCast::filter or similar to pull out the models (e.g. muscles) + // that we want to control. + m_allMuscles = this->find ("muscle"); + m_allSegments = this->find ("segment"); + mapMuscles(m_muscleMap, *this); + + addMarkers(snake, *this); + + #if (0) + trace(structureInfo, *this); + #endif + + // Actually setup the children + BaseSpineModelLearning::setup(world); +} + +void TetraSpineCollisions::teardown() +{ + + BaseSpineModelLearning::teardown(); + +} + +void TetraSpineCollisions::step(double dt) +{ + if (dt < 0.0) + { + throw std::invalid_argument("dt is not positive"); + } + else + { + // Step any children, notify observers + BaseSpineModelLearning::step(dt); + } +} diff --git a/src/dev/btietz/tetraCollisions/TetraSpineCollisions.h b/src/dev/btietz/tetraCollisions/TetraSpineCollisions.h new file mode 100644 index 000000000..5fd3f1332 --- /dev/null +++ b/src/dev/btietz/tetraCollisions/TetraSpineCollisions.h @@ -0,0 +1,60 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef TETRA_SPINE_COLLISIONS +#define TETRA_SPINE_COLLISIONS + +/** + * @file TetraSpineCollisions.h + * @brief Middle segment of In Won's robot reconfigured with MuscleNP and more segments + * @author Brian Mirletz + * @date September 2014 + * @version 1.0.0 + * $Id$ + */ + +// This library +#include "examples/learningSpines/BaseSpineModelLearning.h" + +// Forward Declarations +class tgWorld; + + +/** + * Basically the same structure as NestedStructureTestModel + * just with different parameters and learning capabilities. + * This is an simplification of the model used in the 2015 ICRA paper + */ +class TetraSpineCollisions: public BaseSpineModelLearning +{ +public: + + + TetraSpineCollisions(size_t segments); + + virtual ~TetraSpineCollisions(); + + virtual void setup(tgWorld& world); + + virtual void teardown(); + + virtual void step(const double dt); + +}; + +#endif diff --git a/src/dev/btietz/tetraCollisions/Wall.cpp b/src/dev/btietz/tetraCollisions/Wall.cpp new file mode 100644 index 000000000..3a47bb700 --- /dev/null +++ b/src/dev/btietz/tetraCollisions/Wall.cpp @@ -0,0 +1,148 @@ +/* + * Copyright © 2014, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. + */ + +/** + * @file Wall.cpp + * @brief Contains the implementation of class Wall. + * $Id$ + */ + +// This module +#include "Wall.h" +// This library +#include "core/tgBox.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgBoxInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +#include "tgcreator/tgNode.h" +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include +#include + +namespace +{ + // similarly, frictional parameters are for the tgRod objects. + const struct Config + { + double width; + double height; + double density; + double friction; + double rollFriction; + double restitution; + } c = + { + 5.0, // width (dm?) + 1.0, // height (dm?) + 0.0, // density (kg / length^3) + 1.0, // friction (unitless) + 0.01, // rollFriction (unitless) + 0.2, // restitution (?) + }; +} // namespace + +Wall::Wall() : tgModel() { + origin = btVector3(0,0,0); +} + +Wall::Wall(btVector3 center) : tgModel() { + origin = btVector3(center.getX(), center.getY(), center.getZ()); +} + +Wall::~Wall() {} + +void Wall::setup(tgWorld& world) { + const tgBox::Config boxConfig(c.width, c.height, c.density, c.friction, c.rollFriction, c.restitution); + + // Start creating the structure + tgStructure s; + addNodes(s); + + // Create the build spec that uses tags to turn the structure into a real model + tgBuildSpec spec; + spec.addBuilder("box", new tgBoxInfo(boxConfig)); + + // Create your structureInfo + tgStructureInfo structureInfo(s, spec); + + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + + // call the onSetup methods of all observed things e.g. controllers + notifySetup(); + + // Actually setup the children + tgModel::setup(world); +} + +void Wall::step(double dt) { + // Precondition + if (dt <= 0.0) { + throw std::invalid_argument("dt is not positive"); + } + else { //Notify observers (controllers) of the step so that they can take action + notifyStep(dt); + tgModel::step(dt); // Step any children + } +} + +void Wall::onVisit(tgModelVisitor& r) { + tgModel::onVisit(r); +} + +void Wall::teardown() { + notifyTeardown(); + tgModel::teardown(); +} + +// Nodes: center points of opposing faces of rectangles +void Wall::addNodes(tgStructure& s) { + const int nBoxes = 1; + + // Accumulating rotation on boxes + btVector3 rotationPoint = origin; + btVector3 rotationAxis = btVector3(0, 1, 0); // y-axis + double rotationAngle = 0.0; + + addBoxNodes(); + + for(int i=0;i + +// Forward declarations +class tgLinearString; +class tgModelVisitor; +class tgStructure; +class tgWorld; +class tgNode; + +/** + * Class that creates box "models" that act as crater walls + */ +class Wall : public tgSubject, public tgModel +{ + public: + + /** + * Default constructor. Sets center point of crater to (0,0,0) + */ + Wall(); + + /** + * Origin constructor. Sets center point to input param 'origin'. + * @param[in] origin - the center point of the Wall object + */ + Wall(btVector3 origin); + + /** + * Destructor. Deletes controllers, if any were added during setup. + * Teardown handles everything else. + */ + virtual ~Wall(); + + /** + * Create the model. + * @param[in] world - the world we're building into + */ + virtual void setup(tgWorld& world); + + /** + * Step the model, its children. Notifies controllers of step. + * @param[in] dt, the timestep. Must be positive. + */ + virtual void step(double dt); + + /** + * Receives a tgModelVisitor and dispatches itself into the + * visitor's "render" function. This model will go to the default + * tgModel function, which does nothing. + * @param[in] r - a tgModelVisitor which will pass this model back + * to itself + */ + virtual void onVisit(tgModelVisitor& r); + + /** + * Undoes setup. Deletes child models. Called automatically on + * reset and end of simulation. Notifies controllers of teardown + */ + void teardown(); + + private: + + /** + * A function called during setup that determines the positions of + * the nodes (center points of opposing box faces) + * based on construction parameters. + * @param[in] s: the tgStructure that we're building into + */ + void addNodes(tgStructure& s); + + /** + * Determines the box nodes (center points of opposing box faces + * Adds nodes to 'nodes' vector + */ + void addBoxNodes(); + + std::vector nodes; + btVector3 origin; +}; + +#endif // TETRA_COLLISIONS_WALL diff --git a/src/dev/btietz/tetraCollisions/colSpineSine.cpp b/src/dev/btietz/tetraCollisions/colSpineSine.cpp new file mode 100644 index 000000000..23b169fac --- /dev/null +++ b/src/dev/btietz/tetraCollisions/colSpineSine.cpp @@ -0,0 +1,233 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file colSpinesine.cpp + * @brief Controller for TetraSpineLearningModel + * @author Brian Mirletz + * @date November 2014 + * @version 1.0.0 + * $Id$ + */ + +#include "colSpineSine.h" + +#include + + +// Should include tgString, but compiler complains since its been +// included from TetraSpineLearningModel. Perhaps we should move things +// to a cpp over there +#include "core/tgLinearString.h" +#include "core/ImpedanceControl.h" + +#include "learning/AnnealEvolution/AnnealEvolution.h" +#include "learning/Configuration/configuration.h" + +#include "dev/btietz/hardwareSineWaves/tgSineStringControl.h" + +// JSON Serialization +#include "helpers/FileHelpers.h" +#include + +// The C++ Standard Library +#include + +//#define LOGGING + +/** + * Defining the adapters here assumes the controller is around and + * attached for the lifecycle of the learning runs. I.E. that the setup + * and teardown functions are used for tgModel + */ +colSpineSine::colSpineSine() : +m_dataObserver("logs/TCData") +{ +} + +void colSpineSine::onSetup(BaseSpineModelLearning& subject) +{ + + setupWaves(subject); + +#ifdef LOGGING // Conditional compile for data logging + m_dataObserver.onSetup(subject); +#endif + + + m_updateTime = 0.0; +} + +void colSpineSine::onStep(BaseSpineModelLearning& subject, double dt) +{ + /// Basically nothing to do. Sine controllers will take care of themselves + m_updateTime += dt; + if (m_updateTime >= m_controlTime) + { + +#ifdef LOGGING // Conditional compile for data logging + m_dataObserver.onStep(subject, m_updateTime); +#endif + m_updateTime = 0; + } +} + +void colSpineSine::onTeardown(BaseSpineModelLearning& subject) +{ + + for(size_t i = 0; i < m_sineControllers.size(); i++) + { + delete m_sineControllers[i]; + } + m_sineControllers.clear(); +} + +void colSpineSine::setupWaves(BaseSpineModelLearning& subject) +{ + std::vector allMuscles = subject.getAllMuscles(); + + double tension; + double kPosition; + double kVelocity; + double controlLength; + + double amplitude; + double phase; + double phaseOffset; + double offset; + + Json::Value root; // will contains the root value after parsing. + Json::Reader reader; + + bool parsingSuccessful = reader.parse( FileHelpers::getFileString("controlVars.json"), root ); + if ( !parsingSuccessful ) + { + // report to the user the failure and their locations in the document. + std::cout << "Failed to parse configuration\n" + << reader.getFormattedErrorMessages(); + /// @todo should this throw an exception instead?? + throw std::invalid_argument("Bad JSON filename"); + } + + m_controlTime = root.get("updateFrequency", "UTF-8").asDouble(); + double frequency = root.get("cpg_frequency", "UTF-8").asDouble(); + double bodywaves = root.get("body_waves", "UTF-8").asDouble() * 2.0 * M_PI / (allMuscles.size() / 6.0); + + for (std::size_t i = 0; i < allMuscles.size(); i++) + { + if (allMuscles[i]->hasTag("inner top")) + { + tension = 100.0; + kPosition = 300.0; + + controlLength = allMuscles[i]->getStartLength(); + + amplitude = root.get("in_top_amp_a", "UTF-8").asDouble(); + /// @todo get top, left, right offset, add bodywaves back in. + phase = i * bodywaves + root.get("in_top_offset", "UTF-8").asDouble(); + kVelocity = 50.0; + + } + else if (allMuscles[i]->hasTag("outer top")) + { + tension = 200.0; + kPosition = 200.0; + kVelocity = 100.0; + + + amplitude = root.get("out_top_amp_a", "UTF-8").asDouble(); + phase = i * bodywaves + root.get("out_top_offset", "UTF-8").asDouble(); + controlLength = allMuscles[i]->getStartLength(); + + } + else if (allMuscles[i]->hasTag("inner left")) + { + tension = 100.0; + kPosition = 300.0; + kVelocity = 50.0; + controlLength = allMuscles[i]->getStartLength(); + + amplitude = root.get("in_bottom_amp_a", "UTF-8").asDouble(); + phase = i * bodywaves + root.get("in_left_offset", "UTF-8").asDouble(); + + } + else if (allMuscles[i]->hasTag("outer left")) + { + tension = 50.0; + kPosition = 200.0; + kVelocity = 100.0; + controlLength = allMuscles[i]->getStartLength(); + + amplitude = root.get("out_bottom_amp_a", "UTF-8").asDouble(); + phase = i * bodywaves + root.get("out_left_offset", "UTF-8").asDouble(); + + //2 * bodyWaves * M_PI * i / (segments) + phaseOffsets[phase] + + } + else if (allMuscles[i]->hasTag("inner right")) + { + tension = 100.0; + kPosition = 300.0; + kVelocity = 50.0; + controlLength = allMuscles[i]->getStartLength(); + + amplitude = root.get("in_bottom_amp_a", "UTF-8").asDouble(); + phase = i * bodywaves + root.get("in_right_offset", "UTF-8").asDouble(); + + } + else if (allMuscles[i]->hasTag("outer right")) + { + tension = 50.0; + kPosition = 200.0; + kVelocity = 100.0; + controlLength = allMuscles[i]->getStartLength(); + + amplitude = root.get("out_bottom_amp_a", "UTF-8").asDouble(); + phase = i * bodywaves + root.get("out_right_offset", "UTF-8").asDouble(); + + //2 * bodyWaves * M_PI * i / (segments) + phaseOffsets[phase] + + } + else + { + throw std::runtime_error("Missing tags!"); + } + + ImpedanceControl* p_ipc = new ImpedanceControl( tension, + kPosition, + kVelocity); + + offset = 0.0; + + tgSineStringControl* pStringControl = new tgSineStringControl(m_controlTime, + p_ipc, + amplitude, + frequency, + phase, + offset, + controlLength); + + + allMuscles[i]->attach(pStringControl); + m_sineControllers.push_back(pStringControl); + } + + assert(m_sineControllers.size() == allMuscles.size()); + +} + diff --git a/src/dev/btietz/tetraCollisions/colSpineSine.h b/src/dev/btietz/tetraCollisions/colSpineSine.h new file mode 100644 index 000000000..35bd2bc8e --- /dev/null +++ b/src/dev/btietz/tetraCollisions/colSpineSine.h @@ -0,0 +1,69 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef COL_SPINE_SINE_H +#define COL_SPINE_SINE_H + +/** + * @file colSpineSine.h + * @brief Controller for TetraSpineCollisions + * @author Brian Mirletz + * @date November 2014 + * @version 1.0.0 + * $Id$ + */ + +#include "examples/learningSpines/BaseSpineCPGControl.h" +#include "core/tgObserver.h" + +#include "sensors/tgDataObserver.h" + +class tgSineStringControl; + +/** + * Inherits from BaseSpineCPGControl, and overrides setupCPGs so + * different muscle groups can have different ImpedanceControl parameters + * Modified from htSpineSine to accomidate additional segments + */ +class colSpineSine : public tgObserver +{ +public: + colSpineSine(); + + ~colSpineSine() {} + + virtual void onSetup(BaseSpineModelLearning& subject); + + virtual void onStep(BaseSpineModelLearning& subject, double dt); + + virtual void onTeardown(BaseSpineModelLearning& subject); + +protected: + + virtual void setupWaves(BaseSpineModelLearning& subject); + + std::vector m_sineControllers; + + tgDataObserver m_dataObserver; + + double m_updateTime; + + double m_controlTime; +}; + +#endif // HT_SPINE_SINE_H diff --git a/src/dev/btietz/tetraCollisions/controlVars.json b/src/dev/btietz/tetraCollisions/controlVars.json new file mode 100644 index 000000000..78f57242a --- /dev/null +++ b/src/dev/btietz/tetraCollisions/controlVars.json @@ -0,0 +1,15 @@ +{ + "in_top_amp_a" : -30.0, + "in_bottom_amp_a" : -30.0, + "out_top_amp_a" : 100.0, + "out_bottom_amp_a" : 100.0, + "cpg_frequency" : 0.5, + "body_waves" : 1, + "in_top_offset" : 1.57, + "in_left_offset" : 0.0, + "in_right_offset" : 0.0, + "out_top_offset" : 1.57, + "out_left_offset" : 0.0, + "out_right_offset" : 0.0, + "updateFrequency" : 0.001 +} diff --git a/src/dev/jbruce/internalFriction/T6Model.cpp b/src/dev/jbruce/internalFriction/T6Model.cpp index 49ece3323..95ac18d11 100644 --- a/src/dev/jbruce/internalFriction/T6Model.cpp +++ b/src/dev/jbruce/internalFriction/T6Model.cpp @@ -66,8 +66,8 @@ namespace double friction; double rollFriction; double restitution; + double pretension; bool hist; - double rotation; double maxTens; double targetVelocity; double maxAcc; @@ -83,8 +83,8 @@ namespace 0.99, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) + 0.0, // pretension true, // History logging (boolean) - 0, // rotation 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -177,11 +177,11 @@ void T6Model::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig_passive(c.stiffness_passive, c.damping, c.hist, c.rotation, + tgLinearString::Config muscleConfig_passive(c.stiffness_passive, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); - tgLinearString::Config muscleConfig_active(c.stiffness_active, c.damping, c.hist, c.rotation, + tgLinearString::Config muscleConfig_active(c.stiffness_active, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); diff --git a/src/dev/jbruce/sineWaveEscapeController/T6Model.cpp b/src/dev/jbruce/sineWaveEscapeController/T6Model.cpp index d1ebe0c41..a80dc1535 100644 --- a/src/dev/jbruce/sineWaveEscapeController/T6Model.cpp +++ b/src/dev/jbruce/sineWaveEscapeController/T6Model.cpp @@ -80,8 +80,8 @@ namespace double friction; double rollFriction; double restitution; + double pretension; bool hist; - double rotation; double maxTens; double targetVelocity; double maxAcc; @@ -97,8 +97,8 @@ namespace 0.99, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) + 0.0, // pretension (force) true, // History logging (boolean) - 0, // rotation 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -203,11 +203,11 @@ void T6Model::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig_passive(c.stiffness_passive, c.damping, c.hist, c.rotation, + tgLinearString::Config muscleConfig_passive(c.stiffness_passive, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); - tgLinearString::Config muscleConfig_active(c.stiffness_active, c.damping, c.hist, c.rotation, + tgLinearString::Config muscleConfig_active(c.stiffness_active, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); diff --git a/src/dev/jbruce/twoSpringSUPERball/T6Model.cpp b/src/dev/jbruce/twoSpringSUPERball/T6Model.cpp index fa3f300a5..3d40759aa 100644 --- a/src/dev/jbruce/twoSpringSUPERball/T6Model.cpp +++ b/src/dev/jbruce/twoSpringSUPERball/T6Model.cpp @@ -66,8 +66,8 @@ namespace double friction; double rollFriction; double restitution; + double pretension; bool hist; - double rotation; double maxTens; double targetVelocity; double maxAcc; @@ -83,8 +83,8 @@ namespace 0.99, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) + 0.0, // pretension (force) 0, // History logging (boolean) - 0, // rotation 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -177,11 +177,11 @@ void T6Model::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig_passive(c.stiffness_passive, c.damping, 0, c.rotation, + tgLinearString::Config muscleConfig_passive(c.stiffness_passive, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); - tgLinearString::Config muscleConfig_active(c.stiffness_active, c.damping, 0, c.rotation, + tgLinearString::Config muscleConfig_active(c.stiffness_active, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); diff --git a/src/dev/kmorse/T6Model.cpp b/src/dev/kmorse/T6Model.cpp index e0b47bf40..fc9a10369 100644 --- a/src/dev/kmorse/T6Model.cpp +++ b/src/dev/kmorse/T6Model.cpp @@ -67,7 +67,8 @@ namespace double friction; double rollFriction; double restitution; - double rotation; + double pretension; + bool history; double maxTens; double targetVelocity; double maxAcc; @@ -87,7 +88,8 @@ namespace 1.0, // friction (unitless) 0.1, // rollFriction (unitless) 0.0, // restitution (?) - 0, // rotation + 0.0, // pretension (force) + false, // history 1000000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -205,11 +207,11 @@ void T6Model::setup(tgWorld& world) const tgRod::Config payConfig(c.radius_pay, c.density_pay, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.rotation, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.history, c.maxTens, c.targetVelocity, c.maxAcc); - tgLinearString::Config muscleInConfig(c.stiffness_in, c.damping_in, c.rotation, + tgLinearString::Config muscleInConfig(c.stiffness_in, c.damping_in, c.pretension, c.history, c.maxTens, c.targetVelocity, c.maxAcc); diff --git a/src/dev/muscleNP/CMakeLists.txt b/src/dev/muscleNP/CMakeLists.txt new file mode 100644 index 000000000..c313fa0a9 --- /dev/null +++ b/src/dev/muscleNP/CMakeLists.txt @@ -0,0 +1,16 @@ +Project(MuscleNP) + +link_libraries(tgcreator + core + tgOpenGLSupport) + +add_library(${PROJECT_NAME} SHARED + + MuscleNP.cpp + tgGhostInfo.cpp + tgGhostModel.cpp + tgMultiPointStringInfo.cpp + ) + +target_link_libraries(${PROJECT_NAME} tgcreator core tgOpenGLSupport) + diff --git a/src/dev/muscleNP/MuscleNP.cpp b/src/dev/muscleNP/MuscleNP.cpp new file mode 100644 index 000000000..c716284b6 --- /dev/null +++ b/src/dev/muscleNP/MuscleNP.cpp @@ -0,0 +1,874 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file MuscleNP.cpp + * @brief Definition of a massless cable with contact dynamics + * @author Brian Mirletz + * @date November 2014 + * $Id$ + */ + +// This object +#include "MuscleNP.h" + +// NTRT +#include "tgcreator/tgUtil.h" +#include "core/muscleAnchor.h" +#include "core/tgCast.h" +#include "core/tgBulletUtil.h" +#include "core/tgWorld.h" +#include "core/tgWorldBulletPhysicsImpl.h" + +// The Bullet Physics library +#include "btBulletDynamicsCommon.h" +#include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletDynamics/Dynamics/btDynamicsWorld.h" +#include "LinearMath/btDefaultMotionState.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btQuickprof.h" + +// The C++ Standard Library +#include +#include // abs +#include + +//#define VERBOSE + +MuscleNP::MuscleNP(btPairCachingGhostObject* ghostObject, + tgWorld& world, + const std::vector& anchors, + double coefK, + double dampingCoefficient, + double pretension, + double thickness, + double resolution) : +Muscle2P (anchors, coefK, dampingCoefficient, pretension), +m_ghostObject(ghostObject), +m_world(world), +m_thickness(thickness), +m_resolution(resolution) +{ + +} + +MuscleNP::~MuscleNP() +{ + btDynamicsWorld& m_dynamicsWorld = tgBulletUtil::worldToDynamicsWorld(m_world); + m_dynamicsWorld.removeCollisionObject(m_ghostObject); + + btCollisionShape* shape = m_ghostObject->getCollisionShape(); + deleteCollisionShape(shape); + delete m_ghostObject; +} + +const btScalar MuscleNP::getActualLength() const +{ + btScalar length = 0; + + std::size_t n = m_anchors.size() - 1; + for (std::size_t i = 0; i < n; i++) + { + length += (m_anchors[i]->getWorldPosition() - m_anchors[i+1]->getWorldPosition()).length(); + } + + return length; +} + +void MuscleNP::calculateAndApplyForce(double dt) +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("calculateAndApplyForce"); +#endif //BT_NO_PROFILE + + updateManifolds(); + + pruneAnchors(); + + updateAnchorList(); + + // See if the new anchors change anything + pruneAnchors(); + + m_forceTotals = btVector3(0.0, 0.0, 0.0); + m_forceScales = btVector3(1.0, 1.0, 1.0); + + const double tension = getTension(); + const double currLength = getActualLength(); + + const double deltaStretch = currLength - m_prevLength; + m_velocity = deltaStretch / dt; + + m_damping = m_dampingCoefficient * m_velocity; + + if (btFabs(tension) * 1.0 < btFabs(m_damping)) + { + m_damping = + (m_damping > 0.0 ? tension * 1.0 : -tension * 1.0); + } + + const double magnitude = tension + m_damping; + + // Apply forces + std::size_t n = m_anchors.size(); + + for (std::size_t i = 0; i < n; i++) + { + btVector3 force = btVector3 (0.0, 0.0, 0.0); + if (i == 0) + { + btVector3 direction = m_anchors[i + 1]->getWorldPosition() - m_anchors[i]->getWorldPosition(); + force = direction.normalize() * magnitude; + } + // Will likely only be true for the last anchor + else if (m_anchors[i]->sliding == false) + { + btVector3 direction = m_anchors[i]->getWorldPosition() - m_anchors[i - 1]->getWorldPosition(); + force = -direction.normalize() * magnitude; + } + else if (i < n - 1) + { + // Already normalized + btVector3 direction = m_anchors[i]->getContactNormal(); + + // Get normal to string + btVector3 back = m_anchors[i - 1]->getWorldPosition(); + btVector3 current = m_anchors[i]->getWorldPosition(); + btVector3 forward = m_anchors[i + 1]->getWorldPosition(); + + + btVector3 first = (current - forward); + btVector3 second = (current - back); + + btVector3 forceDir = (first.normalize() + second.normalize() ).normalize(); + + // Apply dot of contact normal with string's normal + force = (tension * direction).dot(forceDir) * forceDir; + + // Only care about scaling sliding forces + m_forceTotals += force; + } + else + { + throw std::runtime_error("MuscleNP: First or last anchor is a sliding constraint!!"); + } + + m_anchors[i]->force = force; + + } + + btVector3 maxForce = (anchor1->force + anchor2->force); + + for (std::size_t i = 0; i < 3; i++) + { + if (m_forceTotals[i] != maxForce[i] && m_forceTotals[i] != 0.0) + { + m_forceScales[i] = btFabs(maxForce[i] / m_forceTotals[i]); + } + else if (m_forceTotals[i] == 0.0) + { + m_forceScales[i] = 1.0; + } + } + +#ifdef VERBOSE + std::cout << "Force Scaling " << m_forceScales << std::endl; +#endif + + for (std::size_t i = 0; i < n; i++) + { + btRigidBody* body = m_anchors[i]->attachedBody; + + btVector3 contactPoint = m_anchors[i]->getRelativePosition(); + body->activate(); + + // Scale the force of the sliding anchors + if (m_anchors[i]->sliding) + { + // Elementwise multiply + m_anchors[i]->force *= m_forceScales; + } + + btVector3 impulse = m_anchors[i]->force* dt; + + body->applyImpulse(impulse, contactPoint); + } + + // Finished calculating, so can store things + m_prevLength = currLength; + + // Do this last so the ghost object gets populated with collisions before it is deleted + updateCollisionObject(); +} + +void MuscleNP::updateManifolds() +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("updateAnchorList"); +#endif //BT_NO_PROFILE + + // Copy this vector so we can remove as necessary + + btManifoldArray m_manifoldArray; + btVector3 m_touchingNormal; + + btBroadphaseInterface* const m_overlappingPairCache = tgBulletUtil::worldToDynamicsWorld(m_world).getBroadphase(); + + // Only caches the pairs, they don't have a lot of useful information + btBroadphasePairArray& pairArray = m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray(); + int numPairs = pairArray.size(); + + std::vector rejectedAnchors; + + for (int i = 0; i < numPairs; i++) + { + m_manifoldArray.clear(); + + const btBroadphasePair& pair = pairArray[i]; + + // The real broadphase's pair cache has the useful info + btBroadphasePair* collisionPair = m_overlappingPairCache->getOverlappingPairCache()->findPair(pair.m_pProxy0,pair.m_pProxy1); + + btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + + if (collisionPair->m_algorithm) + collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); + + for (int j = 0; j < m_manifoldArray.size(); j++) + { + btPersistentManifold* manifold = m_manifoldArray[j]; + btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); + + for (int p=0; p < manifold->getNumContacts(); p++) + { + const btManifoldPoint& pt = manifold->getContactPoint(p); + + btScalar dist = pt.getDistance(); + + if (dist < 0.0) + { + + m_touchingNormal = pt.m_normalWorldOnB * directionSign; + + btVector3 pos = directionSign < 0 ? pt.m_positionWorldOnB : pt.m_positionWorldOnA; + + btRigidBody* rb = NULL; + + //std::cout << pos << " " << manifold << " " << manifold->m_index1a << std::endl; + + if (manifold->getBody0() == m_ghostObject) + { + if (manifold->getBody1() == obj1) + rb = btRigidBody::upcast(obj1); + else if (manifold->getBody1() == obj0) + rb = btRigidBody::upcast(obj0); + else + { + throw std::runtime_error("Can't find the right object!!"); + } + } + else + { + if (manifold->getBody0() == obj0) + rb = btRigidBody::upcast(obj0); + else if (manifold->getBody0() == obj1) + rb = btRigidBody::upcast(obj1); + else + { + throw std::runtime_error("Can't find the right object!!"); + } + } + + if(rb) + { + + int anchorPos = findNearestPastAnchor(pos); + assert(anchorPos < (int)(m_anchors.size() - 1)); + + // -1 means findNearestPastAnchor failed + if (anchorPos >= 0) + { + // Not permanent, sliding contact + muscleAnchor* const newAnchor = new muscleAnchor(rb, pos, m_touchingNormal, false, true, manifold); + + + muscleAnchor* backAnchor = m_anchors[anchorPos]; + muscleAnchor* forwardAnchor = m_anchors[anchorPos + 1]; + + btVector3 pos0 = backAnchor->getWorldPosition(); + btVector3 pos2 = forwardAnchor->getWorldPosition(); + + btVector3 lineA = (pos2 - pos); + btVector3 lineB = (pos0 - pos); + + btScalar lengthA = lineA.length(); + btScalar lengthB = lineB.length(); + + btScalar mDistB = backAnchor->getManifoldDistance(newAnchor->getManifold()).first; + btScalar mDistA = forwardAnchor->getManifoldDistance(newAnchor->getManifold()).first; + + //std::cout << "Update Manifolds " << newAnchor->getManifold() << std::endl; + + bool del = false; + + if (lengthB <= m_resolution && rb == backAnchor->attachedBody && mDistB < mDistA) + { + if(backAnchor->updateManifold(manifold)) + del = true; + //std::cout << "UpdateB " << mDistB << std::endl; + } + if (lengthA <= m_resolution && rb == forwardAnchor->attachedBody && mDistA < mDistB) + { + if (forwardAnchor->updateManifold(manifold)) + del = true; + //std::cout << "UpdateA " << mDistA << std::endl; + } + + if (del) + { + /// @todo further examination of whether the anchors should be deleted here + delete newAnchor; + } + else + { + + m_newAnchors.push_back(newAnchor); + } // If anchor passes distance tests + } // If we could find the anchor's position + } // If body is a rigid body + } // If distance less than 0.0 + } // For number of contacts + } // For number of manifolds + } // For pairs of objects + + +} + +void MuscleNP::updateAnchorList() +{ + int numContacts = 2; + + while (m_newAnchors.size() > 0) + { + // Not permanent, sliding contact + muscleAnchor* const newAnchor = m_newAnchors[0]; + m_newAnchors.erase(m_newAnchors.begin()); + + btVector3 pos1 = newAnchor->getWorldPosition(); + + int anchorPos = findNearestPastAnchor(pos1); + + assert(anchorPos < (int) (m_anchors.size() - 1)); + + // -1 means findNearestPastAnchor failed + if (anchorPos >= 0) + { + + muscleAnchor* backAnchor = m_anchors[anchorPos]; + muscleAnchor* forwardAnchor = m_anchors[anchorPos + 1]; + + btVector3 pos0 = backAnchor->getWorldPosition(); + btVector3 pos2 = forwardAnchor->getWorldPosition(); + + btVector3 lineA = (pos2 - pos1); + btVector3 lineB = (pos0 - pos1); + + btScalar lengthA = lineA.length(); + btScalar lengthB = lineB.length(); + + btVector3 contactNormal = newAnchor->getContactNormal(); + + btScalar normalValue1 = (lineA).dot( newAnchor->getContactNormal()); + btScalar normalValue2 = (lineB).dot( newAnchor->getContactNormal()); + + bool del = false; + + btScalar mDistB = backAnchor->getManifoldDistance(newAnchor->getManifold()).first; + btScalar mDistA = forwardAnchor->getManifoldDistance(newAnchor->getManifold()).first; + + //std::cout << "Update anchor list " << newAnchor->getManifold() << std::endl; + + // These may have changed, so check again + if (lengthB <= m_resolution && newAnchor->attachedBody == backAnchor->attachedBody && mDistB < mDistA) + { + if(backAnchor->updateManifold(newAnchor->getManifold())) + { + del = true; + //std::cout << "UpdateB " << mDistB << std::endl; + } + } + if (lengthA <= m_resolution && newAnchor->attachedBody == forwardAnchor->attachedBody && mDistA < mDistB) + { + if(forwardAnchor->updateManifold(newAnchor->getManifold())) + del = true; + //std::cout << "UpdateA " << mDistA << std::endl; + } + + if (del) + { + delete newAnchor; + } + else if(normalValue1 < 0.0 || normalValue2 < 0.0) + { + delete newAnchor; + } + else + { + + m_anchorIt = m_anchors.begin() + anchorPos + 1; + + m_anchorIt = m_anchors.insert(m_anchorIt, newAnchor); + + numContacts++; + } + } + else + { + delete newAnchor; + } + } + + //std::cout << "contacts " << numContacts << " unprunedAnchors " << m_anchors.size(); + + //std::cout << " prunedAnchors " << m_anchors.size() << std::endl; + +} + +void MuscleNP::pruneAnchors() +{ + int numPruned = 0; + int passes = 0; + std::size_t i; + + // Attempt to eliminate points that would cause the string to push + while (numPruned > 0 || passes <= 3) + { + #ifndef BT_NO_PROFILE + BT_PROFILE("pruneAnchors"); + #endif //BT_NO_PROFILE + numPruned = 0; + i = 1; + while (i < m_anchors.size() - 1) + { + bool keep = true; //m_anchors[i]->updateContactNormal(); + + numPruned = 0; + btVector3 back = m_anchors[i - 1]->getWorldPosition(); + btVector3 current = m_anchors[i]->getWorldPosition(); + btVector3 forward = m_anchors[i + 1]->getWorldPosition(); + + btVector3 lineA = (forward - current); + btVector3 lineB = (back - current); + + + btVector3 contactNormal = m_anchors[i]->getContactNormal(); + + if (!m_anchors[i]->permanent) + { + btVector3 tangentDir = ( (lineB - lineA).cross(contactNormal)).normalize(); + btScalar tangentDot = (lineB + lineA).dot(tangentDir); + btVector3 tangentMove = (lineB + lineA).dot(tangentDir) * tangentDir / 2.0; + btVector3 newPos = current + tangentMove; + // Check if new position is on body + if (!keep || !m_anchors[i]->setWorldPosition(newPos)) + { + deleteAnchor(i); + numPruned++; + } + } + else + { + i++; + } + + if (numPruned == 0 && !m_anchors[i]->permanent) + { + btScalar normalValue1; + btScalar normalValue2; + + // Get new values + + back = m_anchors[i - 1]->getWorldPosition(); + current = m_anchors[i]->getWorldPosition(); + forward = m_anchors[i + 1]->getWorldPosition(); + + lineA = (forward - current); + lineB = (back - current); + + contactNormal = m_anchors[i]->getContactNormal(); + + + if (lineA.length() < m_resolution / 2.0 || lineB.length() < m_resolution / 2.0) + { + // Arbitrary value that deletes the nodes + normalValue1 = -1.0; + normalValue2 = -1.0; + } + else + { + //lineA.normalize(); + //lineB.normalize(); + //std::cout << "Normals " << std::btFabs(line.dot( m_anchors[i]->contactNormal)) << std::endl; + + normalValue1 = (lineA).dot(contactNormal); + normalValue2 = (lineB).dot(contactNormal); + } + if ((normalValue1 < 0.0) || (normalValue2 < 0.0)) + { + #ifdef VERBOSE + std::cout << "Erased normal: " << normalValue1 << " " << normalValue2 << " "; + #endif + if (deleteAnchor(i)) + { + numPruned++; + } + else + { + // Permanent anchor, move on + i++; + } + } + else + { + i++; + } + } + + } + if (numPruned == 0) + { + passes++; + } + else + { + passes = 0; + } + } + + //std::cout << " Good Normal " << m_anchors.size(); + +#ifdef VERBOSE + std::size_t n = m_anchors.size(); + for (i = 0; i < n; i++) + { + std::cout << m_anchors[i]->getWorldPosition() << std::endl; + } +#endif + +} + +// This works ok at the moment. Need an effective way of determining if the rope is under an object +void MuscleNP::updateCollisionObject() +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("updateCollisionObject"); +#endif //BT_NO_PROFILE + + btDispatcher* m_dispatcher = tgBulletUtil::worldToDynamicsWorld(m_world).getDispatcher(); + btBroadphaseInterface* const m_overlappingPairCache = tgBulletUtil::worldToDynamicsWorld(m_world).getBroadphase(); + + // Clear the existing child shapes + btCompoundShape* m_compoundShape = tgCast::cast (m_ghostObject->getCollisionShape()); + clearCompoundShape(m_compoundShape); + + btVector3 maxes(anchor2->getWorldPosition()); + btVector3 mins(anchor1->getWorldPosition()); + + std::size_t n = m_anchors.size(); + + for (std::size_t i = 0; i < n; i++) + { + btVector3 worldPos = m_anchors[i]->getWorldPosition(); + for (std::size_t j = 0; j < 3; j++) + { + if (worldPos[j] > maxes[j]) + { + maxes[j] = worldPos[j]; + } + if (worldPos[j] < mins[j]) + { + mins[j] = worldPos[j]; + } + } + } + btVector3 center = (maxes + mins)/2.0; + + btVector3 from = anchor1->getWorldPosition(); + btVector3 to = anchor2->getWorldPosition(); + + for (std::size_t i = 0; i < n-1; i++) + { + btVector3 pos1 = m_anchors[i]->getWorldPosition(); + btVector3 pos2 = m_anchors[i+1]->getWorldPosition(); + + // Children handles the orientation data + btTransform t = tgUtil::getTransform(pos2, pos1); + t.setOrigin(t.getOrigin() - center); + + btScalar length = (pos2 - pos1).length() / 2.0; + + /// @todo - seriously examine box vs cylinder shapes + btCylinderShape* box = new btCylinderShape(btVector3(m_thickness, length, m_thickness)); + + m_compoundShape->addChildShape(t, box); + } + // Default margin is 0.04, so larger than default thickness. Behavior is better with larger margin + //m_compoundShape->setMargin(m_thickness); + + btTransform transform; + transform.setOrigin(center); + transform.setRotation(btQuaternion::getIdentity()); + + m_ghostObject->setCollisionShape (m_compoundShape); + m_ghostObject->setWorldTransform(transform); + + // Delete the existing contacts in bullet to prevent sticking - may exacerbate problems with rotations + m_overlappingPairCache->getOverlappingPairCache()->cleanProxyFromPairs(m_ghostObject->getBroadphaseHandle(),m_dispatcher); +} + +void MuscleNP::deleteCollisionShape(btCollisionShape* pShape) +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("deleteCollisionShape"); +#endif //BT_NO_PROFILE + + if (pShape) + { + btCompoundShape* cShape = tgCast::cast(pShape); + if (cShape) + { + std::size_t n = cShape->getNumChildShapes(); + for( std::size_t i = 0; i < n; i++) + { + deleteCollisionShape(cShape->getChildShape(i)); + } + } + + delete pShape; + } +} + +void MuscleNP::clearCompoundShape(btCompoundShape* pShape) +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("clearCompoundShape"); +#endif //BT_NO_PROFILE + + if (pShape) + { + while (pShape->getNumChildShapes() > 0) + { + btCollisionShape* pCShape = pShape->getChildShape(0); + deleteCollisionShape(pCShape); + pShape->removeChildShapeByIndex(0); + } + } + +} + +bool MuscleNP::deleteAnchor(int i) +{ +#ifndef BT_NO_PROFILE + BT_PROFILE("deleteAnchor"); +#endif //BT_NO_PROFILE + assert(i < m_anchors.size() && i >= 0); + + if (m_anchors[i]->permanent != true) + { + delete m_anchors[i]; + m_anchors.erase(m_anchors.begin() + i); + return true; + } + else + { + return false; + } +} + +int MuscleNP::findNearestPastAnchor(btVector3& pos) +{ + + std::size_t i = 0; + std::size_t n = m_anchors.size() - 1; + assert (n >= 1); + + /// @todo Find a way to make this bidirectional. If its actually closer to anchor2 you may want to integrate backwards + /// Also deal with the situation that its between anchors 1 and 2 in distance. How do you consider 3D space?? + // Start by determining the "correct" position + btScalar startDist = (pos - m_anchors[i]->getWorldPosition()).length(); + btScalar dist = startDist; + + while (dist <= startDist && i < n) + { + i++; + btVector3 anchorPos = m_anchors[i]->getWorldPosition(); + dist = (pos - anchorPos).length(); + if (dist < startDist) + { + startDist = dist; + } + } + + // Check if we hit both break conditions at the same time + if (dist > startDist) + { + i--; + } + + if (i == n) + { + i--; + } + + if (i == 0) + { + // Do nothing + } + else if (n > 1) + { + // Know we've got 3 anchors, so we need to compare along the line + muscleAnchor* a0 = m_anchors[i - 1]; + muscleAnchor* an = m_anchors[i + 1]; + + MuscleNP::anchorCompare m_acTemp(a0, an); + + btVector3 current = m_anchors[i]->getWorldPosition(); + + m_acTemp.comparePoints(pos, current) ? i-- : i+=0; + + assert((m_anchors[i]->getWorldPosition() - pos).length() <= (a0->getWorldPosition() - pos).length()); + } + + // Check to make sure it's actually in this line + muscleAnchor* a0 = m_anchors[i]; + muscleAnchor* an = m_anchors[i + 1]; + + btVector3 current = a0->getWorldPosition(); + MuscleNP::anchorCompare m_acTemp(a0, an); + if( m_acTemp.comparePoints(current, pos)) + { + // Success! do nothing, move on + } + else + { + //std::cout << "iterating backwards!" << std::endl; + // Start over, iterate from the back, see if its better + btScalar endDist = (pos - m_anchors[n]->getWorldPosition()).length(); + btScalar dist2 = endDist; + + int j = n; + + while (dist2 <= endDist && j > 0) + { + j--; + btVector3 anchorPos = m_anchors[j]->getWorldPosition(); + dist2 = (pos - anchorPos).length(); + if (dist2 < endDist) + { + endDist = dist2; + } + } + + if (j == n) + { + j--; + } + + if (j == 0) + { + // Do nothing + } + else if (n > 1) + { + // Know we've got 3 anchors, so we need to compare along the line + muscleAnchor* a0 = m_anchors[j - 1]; + muscleAnchor* an = m_anchors[j + 1]; + + MuscleNP::anchorCompare m_acTemp(a0, an); + + btVector3 current = m_anchors[j]->getWorldPosition(); + + m_acTemp.comparePoints(pos, current) ? j-- : j+=0; + + // This assert doesn't work due to iteration order. Is there a comparable assert? + //assert((m_anchors[j]->getWorldPosition() - pos).length() <= (a0->getWorldPosition() - pos).length()); + } + + // Check to make sure it's actually in this line + muscleAnchor* a0 = m_anchors[j]; + muscleAnchor* an = m_anchors[j + 1]; + + btVector3 current = a0->getWorldPosition(); + MuscleNP::anchorCompare m_acTemp(a0, an); + if( m_acTemp.comparePoints(current, pos)) + { + // Success! Set i to j and return + i = j; + } + else + { + if ( i !=j ) + { + std::cout << "Error in iteration order First try: " << i << " Second Try: " << j << std::endl; + //throw std::runtime_error("Neither the front nor back iterations worked!"); + } + + // Return failure + return -1; + } + } + + muscleAnchor* prevAnchor = m_anchors[i]; + assert (prevAnchor); + + return i; + +} + +MuscleNP::anchorCompare::anchorCompare(const muscleAnchor* m1, const muscleAnchor* m2) : +ma1(m1), +ma2(m2) +{ + +} + +bool MuscleNP::anchorCompare::operator() (const muscleAnchor* lhs, const muscleAnchor* rhs) const +{ + btVector3 pt2 = lhs->getWorldPosition(); + btVector3 pt3 = rhs->getWorldPosition(); + + return comparePoints(pt2, pt3); +} + +bool MuscleNP::anchorCompare::comparePoints(btVector3& pt2, btVector3& pt3) const +{ + // @todo make sure these are good anchors. Assert? + btVector3 pt1 = ma1->getWorldPosition(); + btVector3 ptN = ma2->getWorldPosition(); + + btScalar lhDot = (ptN - pt1).dot(pt2); + btScalar rhDot = (ptN - pt1).dot(pt3); + + return lhDot < rhDot; + +} diff --git a/src/dev/muscleNP/MuscleNP.h b/src/dev/muscleNP/MuscleNP.h new file mode 100644 index 000000000..be35ea931 --- /dev/null +++ b/src/dev/muscleNP/MuscleNP.h @@ -0,0 +1,261 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef NTRT_MUSCLENP_H_ +#define NTRT_MUSCLENP_H_ + +/** + * @file MuscleNP.h + * @brief Definition of a massless cable with contact dynamics + * @author Brian Mirletz + * @date November 2014 + * $Id$ + */ + +// NTRT +#include "core/Muscle2P.h" +// The Bullet Physics library +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +// The C++ Standard Library + +#include + +// Forward references +class tgWorld; +class muscleAnchor; +class btRigidBody; +class btCollisionShape; +class btCompoundShape; +class btPairCachingGhostObject; +class btDynamicsWorld; + +/** + * An extension of Muscle2P that places a ghostObject into the bullet world + * and then uses that object to generate collision dynamics. The + * ghost object is updated every time step to account for the new + * shape of the string. Anchors track their associated btPersistentManifold + * to determine whether they are still in contact with the rigid object. The string + * is massless, but collision handling should conserve momentum (see unit tests) + * + * This could eventually be merged with Corde for a massive, low node + * string. Merging with Corde could also address some of the current + * issues with rotational energy + */ +class MuscleNP : public Muscle2P +{ +public: + + /** + * The only constructor. Requires a number of parameters typically + * provided by the builder tools, in this case tgMultiPointStringInfo + * @param[in] ghostObject - the btPairCashingGhostObject that this interacts with + * @param[in] world - the tgWorld - we need to keep access to the bullet + * dynamics world's broadphase so that we can determine collisions and contact points + * @param[in] anchors - a reference to a list of anchors. Will be passed to Muscle2P + * and form the initial (likely permanent) anchor list. Size must be 2 or greater + * (enforced in Muscle2P) + * @param[in] coefK - The stiffness of this cable. Units are + * mass / sec^2, sets a member variable of Muscle2P + * @param[in] dampingCofefficient - The damping coefficient of the force application equation. + * Units are mass / sec, sets a member variable of Muscle2P + * @param[in] The pretension on the string, units are force. Must be small enough + * (based on stiffness) such that rest length > 0; + * @param[in] thickness, the radius of the cylinder used for the btCollisionObject + * @param[in] resolution, the spatial resultion used to prune new contacts. + * also affects runtime (lower corresponds to longer runtime) + */ + MuscleNP(btPairCachingGhostObject* ghostObject, + tgWorld& world, + const std::vector& anchors, + double coefK, + double dampingCoefficient, + double pretension = 0.0, + double thickness = 0.001, + double resolution = 0.1); + /** + * The destructor. Removes the ghost object from the world, + * deletes its collision shape, and then deletes the object. + * Muscle2P ensures all anchors are deleted + */ + virtual ~MuscleNP(); + + /** + * @return a btScalar of the string's actual length - the sum of the + * lengths between the anchors. + */ + virtual const btScalar getActualLength() const; + + /** + * The "update" function for MuscleNP. In addition to calculating and + * applying the force, this calls: + * updateManifolds() + * pruneAnchors() + * updateAnchorList() + * pruneAnchors() + * THEN it calculates and applies the forces (calculating permanent anchors + * and sliding contacts slightly differently) and distributes them so momentum + * is conserved + * finally updateCollisionObject() + * @todo change this to update(dt) or similar, since that's the role its serving + */ + virtual void calculateAndApplyForce(double dt); + +private: + + /** + * Function object to compare the spatial position of two anchors, + * given two other anchors of interest. Used to place new anchors + */ + struct anchorCompare + { + anchorCompare(const muscleAnchor* m1, const muscleAnchor* m2); + + /** + * Calls comparePoints on anchors lhs and rhs + */ + bool operator() (const muscleAnchor* lhs, const muscleAnchor* rhs) const; + + /** + * Sees which of the points is further along the line between ma1 + * and ma2 + */ + bool comparePoints(btVector3& pt2, btVector3& pt3) const; + + private: + const muscleAnchor* const ma1; + const muscleAnchor* const ma2; + }; + + /** + * Iterate through the pairs of objects to find the contact positions + * Contacts are then compared with existing anchors, and either the + * btPersistantManifold of the anchors is updated, or the point is + * stored in m_newAnchors as a new anchor + */ + void updateManifolds(); + + /** + * Iterates through the list of new anchors created by updateManifolds + * and checks whether new anchors are in the same position as + * existing anchors, or if they would 'push' against the colliding + * object. Valid anchors are added to m_anchors. + */ + void updateAnchorList(); + + /** + * First minimizes the length of the string in allowed directions + * and deletes anchors with out of date manifolds. Then deletes + * anchors that are too close to surrounding anchors or where + * normals would push. + */ + void pruneAnchors(); + + /** + * Uses m_anchors to update the collision shape of the m_ghostObject + * Also resets the broadphase's pairCache after collision object + * is changed. + */ + void updateCollisionObject(); + + /** + * Deletes a collision shape and it's child shapes + * @param[in] pShape the btCollisionShape to be deleted + */ + void deleteCollisionShape(btCollisionShape* pShape); + + /** + * Deletes the child shapes of the collision object, leaving + * an empty container + * @param[in] pShape the shape to be emptied + */ + void clearCompoundShape(btCompoundShape* pShape); + + /** + * Determine if the anchor at i is permanent, if not, delete it + * and remove it from m_anchors. + * @param[in] i the index of the anchor to be deleted + * @return true if the anchor has been deleted + */ + bool deleteAnchor(int i); + + /** + * Find the anchor closest to this position in space, then return + * the index of the anchor that would immediately proceed it + * once it is inserted into m_anchors. Used by both updateManifolds() + * and updateAnchorList() + * @param[in] the position of the contact or anchor in question + * @return the index of the relevant anchor + * @todo Introduce more flexibility to this function for contacts + * between the two anchors that are not along a line + */ + int findNearestPastAnchor(btVector3& pos); + + /** + * An iterator over a list of muscleAnchors. Used to insert new + * anchors during updateAnchorList() + */ + std::vector::iterator m_anchorIt; + + /** + * The total force experienced by the sliding anchors + */ + btVector3 m_forceTotals; + + /** + * The scaling factor between the sliding and permanent anchors that + * (usually) ensures momentum is conserved + */ + btVector3 m_forceScales; + + /** + * Temporary storage for anchors between updateManifolds() and + * updateAnchorList() + */ + std::vector m_newAnchors; + + /** + * A reference to the dynamics world so that we can track the + * contact points in the broadphase's pairCache and remove + * the ghostObject from the collision world + */ + tgWorld& m_world; + +protected: + + /** + * A btPairCachingGhostObject that exists in the dynamics world + * and provides us information about what it collides with. + * We own this. + */ + btPairCachingGhostObject* m_ghostObject; + + /** + * The radius of the ghost object + * Units of length + */ + const double m_thickness; + + /** + * The spatial resolution used in collision detection + * Units of length + */ + const double m_resolution; +}; + +#endif // NTRT_MUSCLENP_H_ diff --git a/src/dev/muscleNP/tgGhostInfo.cpp b/src/dev/muscleNP/tgGhostInfo.cpp new file mode 100644 index 000000000..4a45809bc --- /dev/null +++ b/src/dev/muscleNP/tgGhostInfo.cpp @@ -0,0 +1,125 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file tgGhostInfo.cpp + * @brief Contains the definitions of members of class tgGhostInfo + * @author Brian Mirletz + * $Id$ + */ + +// This module +#include "tgGhostInfo.h" +// This application +#include "tgGhostModel.h" +#include "tgcreator/tgNode.h" +#include "tgcreator/tgNodes.h" +#include "tgcreator/tgPair.h" +#include "tgcreator/tgPairs.h" +// The NTRT Core Libary +#include "core/tgBulletUtil.h" +#include "core/tgTagSearch.h" +#include "tgcreator/tgUtil.h" +#include "core/tgBulletUtil.h" +#include "core/tgWorld.h" + + +// The Bullet Physics library +#include "btBulletDynamicsCommon.h" +#include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "BulletDynamics/Dynamics/btDynamicsWorld.h" + +tgGhostInfo::tgGhostInfo(const tgBox::Config& config) : + tgBoxInfo(config) +{} + +tgGhostInfo::tgGhostInfo(const tgBox::Config& config, tgTags tags) : + tgBoxInfo(config, tags) +{} + +tgGhostInfo::tgGhostInfo(const tgBox::Config& config, const tgPair& pair) : + tgBoxInfo(config, pair) +{} + +tgGhostInfo::tgGhostInfo(const tgBox::Config& config, tgTags tags, const tgPair& pair) : + tgBoxInfo(config, tags, pair) +{} + +tgRigidInfo* tgGhostInfo::createRigidInfo(const tgPair& pair) +{ + return new tgGhostInfo(getConfig(), pair); +} + + +/// @todo This is the key class to override +void tgGhostInfo::initRigidBody(tgWorld& world) +{ + if(!getCollisionObject()) { + + // we want to do this based on group instead the rigid itself; otherwise we throw away autocompounding. + tgRigidInfo* rigid = getRigidInfoGroup(); + + // If we're not using autocompounding, use the rigid body itself. + // NOTE: This means that auto-compounding can be silently skipped, which means that your parts may not be joined correctly. Do we want that? + if(rigid == 0) { + rigid = this; + } + + if (rigid->getCollisionObject() == NULL) // Init only if it doesn't have a btRigidBody (has already been initialized) + { + + btDynamicsWorld& m_dynamicsWorld = tgBulletUtil::worldToDynamicsWorld(world); + + btTransform transform = rigid->getTransform(); + btCollisionShape* shape = rigid->getCollisionShape(world); + + // Dynamics world will own this + btPairCachingGhostObject* ghostObject = new btPairCachingGhostObject(); + + ghostObject->setCollisionShape (shape); + ghostObject->setWorldTransform(transform); + ghostObject->setCollisionFlags (btCollisionObject::CF_NO_CONTACT_RESPONSE); + + // @todo look up what the second and third arguments of this are + m_dynamicsWorld.addCollisionObject(ghostObject,btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::StaticFilter|btBroadphaseProxy::DefaultFilter); + + rigid->setCollisionObject(ghostObject); + } + + } +} + +tgModel* tgGhostInfo::createModel(tgWorld& world) +{ + // @todo: handle tags -> model + // @todo: check to make sure the rigidBody has been built + // @todo: Do we need to use the group here? + + // Just in case it hasn't been done already... + initRigidBody(world); + + #if (0) + std::cout << "creating box with tags " << getTags() << std::endl; + #endif + + btPairCachingGhostObject* ghostObject = tgCast::cast (getCollisionObject()); + + tgGhostModel* slimer = new tgGhostModel(ghostObject, getTags()); + + return slimer; +} diff --git a/src/dev/muscleNP/tgGhostInfo.h b/src/dev/muscleNP/tgGhostInfo.h new file mode 100644 index 000000000..e54790221 --- /dev/null +++ b/src/dev/muscleNP/tgGhostInfo.h @@ -0,0 +1,135 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef TG_GHOST_INFO_H +#define TG_GHOST_INFO_H + +/** + * @file tgGhostInfo.h + * @brief Definition of abstract class tgGhostInfo + * @author Brian Mirletz + * @date October 2014 + * $Id$ + */ + +// The C++ Standard Library +#include +// This library +#include "core/tgTaggable.h" +#include "core/tgModel.h" +#include "tgcreator/tgRigidInfo.h" +#include "tgcreator/tgBoxInfo.h" +//Bullet Physics +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" + +// Forward references +class tgCompoundRigidInfo; +class tgNode; +class tgNodes; +class tgPair; +class tgPairs; +class tgTagSearch; +class tgWorld; + +class btRigidBody; +class btCollisionShape; +class btTransform; + +/** + * Builds off of tgRigidInfo and tgBoxInfo, but creates ghost objects instead of + * rigid bodies. Needs to be in its own seperate structure, so + * these don't get accidentally compounded with rigid objects. + * Used by MuscleNP and tgMultiPointStringInfo + */ +class tgGhostInfo : public tgBoxInfo { +public: + + /** + * Construct a tgBoxInfo with just a config. The pair must be filled in + * later, or factory methods can be used to create instances with + * pairs. + */ + tgGhostInfo(const tgBox::Config& config); + + /** + * Construct a tgBoxInfo with just a config and tags. The pair must + * be filled in later, or factory methods can be used to create instances + * with pairs. + */ + tgGhostInfo(const tgBox::Config& config, tgTags tags); + + /** + * Construct a tgBoxInfo from its endpoints, radius and density. + * @param[in] from one endpoint + * @param[in] to the other endpoint + * @param[in] config contains the radius and density + * @todo: make sure that tgPairs returns references to the vectors... + */ + tgGhostInfo(const tgBox::Config& config, const tgPair& pair); + + /** + * Construct a tgBoxInfo from its endpoints, radius and density. + * @param[in] from one endpoint + * @param[in] to the other endpoint + * @param[in] config contains the radius and density + * @todo: make sure that tgPairs returns references to the vectors... + */ + tgGhostInfo(const tgBox::Config& config, tgTags tags, const tgPair& pair); + + /** + * World will destroy the rigid body + */ + virtual ~tgGhostInfo() {} + + /** + * Create a tgRigidInfo* from a tgPair + */ + tgRigidInfo* createRigidInfo(const tgPair& pair); + + virtual void initRigidBody(tgWorld& world); + + virtual tgModel* createModel(tgWorld& world); + +#if (0) // Default to box's com so we can do compounding + /** + * Return the ghost object's mass + * Ghost objects have no mass + * @return the mass of the ghost object + */ + virtual double getMass() const + { + return 0; + } +#endif +}; + +/** + * Overload operator<<() to handle a tgBoxInfo. + * @param[in,out] os an ostream + * @param[in] q a tgBoxInfo + * @return os + */ +inline std::ostream& operator<<(std::ostream& os, const tgGhostInfo& casper) +{ + os << "tgGhostInfo(" << casper.getFrom() << ", " << casper.getTo() <<")"; + return os; +} + + +#endif diff --git a/src/dev/muscleNP/tgGhostModel.cpp b/src/dev/muscleNP/tgGhostModel.cpp new file mode 100644 index 000000000..34de8d685 --- /dev/null +++ b/src/dev/muscleNP/tgGhostModel.cpp @@ -0,0 +1,71 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file tgGhostModel.cpp + * @brief Contains the definitions of members of class tgRod + * @author Brian Mirletz and Ryan Adams + * $Id$ + */ + +// This module +#include "tgGhostModel.h" +#include "core/tgModelVisitor.h" +// The Bullet Physics library +#include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "btBulletDynamicsCommon.h" +// The C++ Standard Library +#include +#include + +tgGhostModel::tgGhostModel(btPairCachingGhostObject* pCollisionObject, + const tgTags& tags) : + tgModel(tags), + m_pCollisionObject(pCollisionObject) +{ + if (pCollisionObject == NULL) + { + throw std::invalid_argument("Pointer to ghostObject is NULL"); + } + + // Postcondition + assert(invariant()); + assert(m_pCollisionObject == pCollisionObject); +} + +tgGhostModel::~tgGhostModel() { } + +void tgGhostModel::onVisit(const tgModelVisitor& v) const +{ + v.render(*this); + +} + +void tgGhostModel::teardown() +{ + // This does not appear to be called + + // Postcondition + // This does not preserve the invariant +} + +bool tgGhostModel::invariant() const +{ + return + (m_pCollisionObject != NULL); +} diff --git a/src/dev/muscleNP/tgGhostModel.h b/src/dev/muscleNP/tgGhostModel.h new file mode 100644 index 000000000..0ccdb48cf --- /dev/null +++ b/src/dev/muscleNP/tgGhostModel.h @@ -0,0 +1,81 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef TG_GHOST_MODEL_H +#define TG_GHOST_MODEL_H + +/** + * @file tgGhostModel.h + * @brief Like tgBaseRigid, but uses btPairCachingGhostObject instead + * @author Brian Mirletz + * @date October 2014 + * $Id$ + */ + +// This application +#include "core/tgModel.h" +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include + +// Forward declarations +class btPairCachingGhostObject; + +/** + * Holds a pointer to a btPairCachingGhostObject through the build process + * It could be used to keep additional functions away from MuscleNP in the future, + * by holding pointers to the dispatcher and similar + */ +class tgGhostModel : public tgModel +{ +public: + + tgGhostModel(btPairCachingGhostObject* pCollisionObject, + const tgTags& tags); + + /** A class with a virtual memeber function requires a virtual destructor. */ + virtual ~tgGhostModel(); + + virtual void teardown(); + + virtual void onVisit(const tgModelVisitor& v) const; + + /** + * Getter for rigid body + */ + virtual btPairCachingGhostObject* getPGhostObject() + { + return m_pCollisionObject; + } + +private: + + /** Integrity predicate. */ + bool invariant() const; + +protected: + + /** + * The Bullet Physics implementation of the collision object. + */ + btPairCachingGhostObject* m_pCollisionObject; + +}; + +#endif diff --git a/src/dev/muscleNP/tgMultiPointStringInfo.cpp b/src/dev/muscleNP/tgMultiPointStringInfo.cpp new file mode 100644 index 000000000..3a3df5c68 --- /dev/null +++ b/src/dev/muscleNP/tgMultiPointStringInfo.cpp @@ -0,0 +1,141 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file tgMultiPointStringInfo.cpp + * @brief Implementation of class tgLinearStringInfo + * @author Brian Mirletz and Ryan Adams + * @date October 2014 + * $Id$ + */ + +#include "tgMultiPointStringInfo.h" + +#include "MuscleNP.h" + +#include "core/tgBulletUtil.h" +#include "core/muscleAnchor.h" + +#include "tgcreator/tgNode.h" + + +// The Bullet Physics Library +#include "btBulletDynamicsCommon.h" +#include "BulletCollision/CollisionDispatch/btGhostObject.h" + +tgMultiPointStringInfo::tgMultiPointStringInfo(const tgLinearString::Config& config) : +m_config(config), +tgConnectorInfo() +{} + +tgMultiPointStringInfo::tgMultiPointStringInfo(const tgLinearString::Config& config, tgTags tags) : +m_config(config), +tgConnectorInfo(tags) +{} + +tgMultiPointStringInfo::tgMultiPointStringInfo(const tgLinearString::Config& config, const tgPair& pair) : +m_config(config), +tgConnectorInfo(pair) +{} + + +tgConnectorInfo* tgMultiPointStringInfo::createConnectorInfo(const tgPair& pair) +{ + return new tgMultiPointStringInfo(m_config, pair); +} + +void tgMultiPointStringInfo::initConnector(tgWorld& world) +{ + // Note: MuscleNP holds pointers to things in the world, but it doesn't actually have any in-world representation. + m_muscleNP = createMuscleNP(world); +} + +tgModel* tgMultiPointStringInfo::createModel(tgWorld& world) +{ + // Don't have to do anything in the world for a MuscleNP... + // @todo: set the name based on joined tags, or remove name from the model... + //std::cout << "tgMultiPointStringInfo::createModel" << std::endl; + + // ensure connector has been initialized + assert(m_muscleNP); + return new tgLinearString(m_muscleNP, getTags(), m_config); +} + +double tgMultiPointStringInfo::getMass() +{ + // @todo: calculate a mass? MuscleNP doesn't have mass... + return 0; +} + + +MuscleNP* tgMultiPointStringInfo::createMuscleNP(tgWorld& world) +{ + //std::cout << "tgLinearStringInfo::createMuscleNP()" << std::endl; + + //std::cout << " getFromRigidInfo(): " << getFromRigidInfo() << std::endl; + //std::cout << " getFromRigidInfo(): " << getFromRigidInfo()->getRigidInfoGroup() << std::endl; + + // @todo: need to check somewhere that the rigid bodies have been set... + btRigidBody* fromBody = getFromRigidBody(); + btRigidBody* toBody = getToRigidBody(); + + tgNode from = getFromRigidInfo()->getConnectionPoint(getFrom(), getTo(), m_config.rotation); + tgNode to = getToRigidInfo()->getConnectionPoint(getTo(), getFrom(), m_config.rotation); + + std::vector anchorList; + + muscleAnchor* anchor1 = new muscleAnchor(fromBody, from); + anchorList.push_back(anchor1); + + muscleAnchor* anchor2 = new muscleAnchor(toBody, to); + anchorList.push_back(anchor2); + + /// @todo generalize this to n anchors. May take a new info class + + btTransform transform = tgUtil::getTransform(from, to); + + btCompoundShape* m_compoundShape = new btCompoundShape(&world); + + btTransform t = transform; + btVector3 origin(0.0, 0.0, 0.0); + t.setOrigin(origin); + + // @todo import this! Only the first two params matter + btScalar radius = 0.001; + btScalar length = (from - to).length() / 2.0; + btBoxShape* box = new btBoxShape(btVector3(radius, length, radius)); + + m_compoundShape->addChildShape(t, box); + + btPairCachingGhostObject* m_ghostObject = new btPairCachingGhostObject(); + + // Don't double rotate + transform.setRotation(btQuaternion::getIdentity()); + + m_ghostObject->setCollisionShape (m_compoundShape); + m_ghostObject->setWorldTransform(transform); + m_ghostObject->setCollisionFlags (btCollisionObject::CF_NO_CONTACT_RESPONSE); + + // Add ghost object to world + // @todo MuscleNP handles deleting from world - should it handle adding too? + btDynamicsWorld& m_dynamicsWorld = tgBulletUtil::worldToDynamicsWorld(world); + m_dynamicsWorld.addCollisionObject(m_ghostObject,btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::StaticFilter|btBroadphaseProxy::DefaultFilter); + + return new MuscleNP(m_ghostObject, world, anchorList, m_config.stiffness, m_config.damping, m_config.pretension); +} + diff --git a/src/dev/muscleNP/tgMultiPointStringInfo.h b/src/dev/muscleNP/tgMultiPointStringInfo.h new file mode 100644 index 000000000..956d98214 --- /dev/null +++ b/src/dev/muscleNP/tgMultiPointStringInfo.h @@ -0,0 +1,93 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file tgMultiPointStringInfo.h + * @brief Definition of class tgMultiPointStringInfo + * @author Brian Mirletz and Ryan Adams + * @date October 2014 + * $Id$ + */ + +#ifndef TG_MULTIPOINT_STRING_INFO_H +#define TG_MULTIPOINT_STRING_INFO_H + +#include "tgcreator/tgConnectorInfo.h" + +#include "tgcreator/tgRigidInfo.h" + +#include + +#include "core/tgLinearString.h" +#include "core/tgTags.h" + +class MuscleNP; + +class tgMultiPointStringInfo : public tgConnectorInfo +{ +public: + + /** + * Construct a tgMultiPointStringInfo with just a config. The pair must be filled in + * later, or factory methods can be used to create instances with + * pairs. + */ + tgMultiPointStringInfo(const tgLinearString::Config& config); + + /** + * Construct a tgMultiPointStringInfo with just a config and tags. The pair must + * be filled in later, or factory methods can be used to create instances + * with pairs. + */ + tgMultiPointStringInfo(const tgLinearString::Config& config, tgTags tags); + + /** + * Construct a tgMultiPointStringInfo from its endpoints, radius and density. + * @param[in] from one endpoint + * @param[in] to the other endpoint + * @param[in] config contains the radius and density + * @todo: make sure that tgPairs returns references to the vectors... + */ + tgMultiPointStringInfo(const tgLinearString::Config& config, const tgPair& pair); + + + virtual ~tgMultiPointStringInfo() {} + + /** + * Create a tgConnectorInfo* from a tgPair + */ + virtual tgConnectorInfo* createConnectorInfo(const tgPair& pair); + + void initConnector(tgWorld& world); + + virtual tgModel* createModel(tgWorld& world); + + double getMass(); + +private: + + MuscleNP* createMuscleNP(tgWorld& world); + +private: + + tgLinearString::Config m_config; + MuscleNP* m_muscleNP; +}; + + +#endif diff --git a/src/dev/steve/Escape_T6/Escape_T6Model.cpp b/src/dev/steve/Escape_T6/Escape_T6Model.cpp index 567c74888..b6c9e011b 100644 --- a/src/dev/steve/Escape_T6/Escape_T6Model.cpp +++ b/src/dev/steve/Escape_T6/Escape_T6Model.cpp @@ -58,6 +58,7 @@ namespace double stiffness; bool history; double damping; + double pretension; double rod_length; double rod_space; double friction; @@ -74,6 +75,7 @@ namespace 3000.0, // stiffness (kg / sec^2) true, // history (record?) 200.0, // damping (kg / sec) + 0.0, // pretension (force) 15.0, // rod_length (length) 7.5, // rod_space (length) 1.0, // friction (unitless) @@ -124,7 +126,7 @@ void Escape_T6Model::setup(tgWorld& world) { c.rollFriction, c.restitution); tgLinearString::Config muscleConfig(c.stiffness, c.damping, - c.history, c.rotation, c.maxTens, c.targetVelocity, + c.pretension, c.history, c.maxTens, c.targetVelocity, c.maxAcc); // Start creating the structure diff --git a/src/dev/steve/T12SuperBallPayload/T12SuperBallPayload.cpp b/src/dev/steve/T12SuperBallPayload/T12SuperBallPayload.cpp index f9642e16b..595015812 100644 --- a/src/dev/steve/T12SuperBallPayload/T12SuperBallPayload.cpp +++ b/src/dev/steve/T12SuperBallPayload/T12SuperBallPayload.cpp @@ -62,7 +62,8 @@ namespace double friction; double rollFriction; double restitution; - double rotation; + double pretension; + bool history; double maxTens; double targetVelocity; double maxAcc; @@ -77,7 +78,8 @@ namespace 1.0, // friction (unitless) 0.01, // rollFriction (unitless) 0.2, // restitution (?) - 0, // rotation + 0, // pretension (force) + false, // history (boolean) 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -241,8 +243,8 @@ void T12SuperBallPayload::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.rotation, - c.maxTens, c.targetVelocity, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, + c.history, c.maxTens, c.targetVelocity, c.maxAcc); // Start creating the structure diff --git a/src/dev/tests/RBStringTest.cpp b/src/dev/tests/RBStringTest.cpp index eca012181..7d82f1b28 100644 --- a/src/dev/tests/RBStringTest.cpp +++ b/src/dev/tests/RBStringTest.cpp @@ -60,10 +60,11 @@ void RBStringTest::setup(tgWorld& world) // todo - figure out a way to change only the one parameter with less code const double stiffness = m_config.m_stringConfig.stiffness; const double damping = m_config.m_stringConfig.damping; - tgLinearString::Config muscleConfig1(stiffness, damping, false, -M_PI / 2.0); - tgLinearString::Config muscleConfig2(stiffness, damping, false, M_PI / 2.0); - tgLinearString::Config muscleConfig3(stiffness, damping, false, M_PI); - tgLinearString::Config muscleConfig4(stiffness, damping, false, 0); + const double pretension = 0.0; + tgLinearString::Config muscleConfig1(stiffness, damping, pretension, false, 1000.0, 100.0, 10000.0, 0.1, 0.1, -M_PI / 2.0); + tgLinearString::Config muscleConfig2(stiffness, damping, pretension, false, 1000.0, 100.0, 10000.0, 0.1, 0.1, M_PI / 2.0); + tgLinearString::Config muscleConfig3(stiffness, damping, pretension, false, 1000.0, 100.0, 10000.0, 0.1, 0.1, M_PI); + tgLinearString::Config muscleConfig4(stiffness, damping, pretension, false, 1000.0, 100.0, 10000.0, 0.1, 0.1, 0); // Calculations for the flemons spine model double v_size = m_config.m_minTotalLength / (double) m_config.m_segments; diff --git a/src/examples/3_prism_no_control/AppPrismModel.cpp b/src/examples/3_prism_no_control/AppPrismModel.cpp new file mode 100644 index 000000000..18155d6e8 --- /dev/null +++ b/src/examples/3_prism_no_control/AppPrismModel.cpp @@ -0,0 +1,80 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file AppPrismModel.cpp + * @brief Contains the definition function main() for the Three strut + * tensegrity prism example application + * @author Brian Tietz + * $Id$ + */ + +// This application +#include "PrismModel.h" +// This library +#include "core/terrain/tgBoxGround.h" +#include "core/tgModel.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +// Bullet Physics +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include + +/** + * The entry point. + * @param[in] argc the number of command-line arguments + * @param[in] argv argv[0] is the executable name + * @return 0 + */ +int main(int argc, char** argv) +{ + std::cout << "AppPrismModelTest" << std::endl; + + // First create the ground and world. Specify ground rotation in radians + const double yaw = 0.0; + const double pitch = 0.0; + const double roll = 0.0; + const tgBoxGround::Config groundConfig(btVector3(yaw, pitch, roll)); + // the world will delete this + tgBoxGround* ground = new tgBoxGround(groundConfig); + + const tgWorld::Config config(981); // gravity, cm/sec^2 + tgWorld world(config, ground); + + // Second create the view + const double timestep_physics = 0.001; // seconds + const double timestep_graphics = 1.f/60.f; // seconds + tgSimViewGraphics view(world, timestep_physics, timestep_graphics); + + // Third create the simulation + tgSimulation simulation(view); + + // Fourth create the models with their controllers and add the models to the + // simulation + PrismModel* const myModel = new PrismModel(); + + // Add the model to the world + simulation.addModel(myModel); + + simulation.run(); + + //Teardown is handled by delete, so that should be automatic + return 0; +} diff --git a/src/examples/3_prism_no_control/CMakeLists.txt b/src/examples/3_prism_no_control/CMakeLists.txt new file mode 100644 index 000000000..ba663b6c3 --- /dev/null +++ b/src/examples/3_prism_no_control/CMakeLists.txt @@ -0,0 +1,15 @@ +link_directories(${LIB_DIR}) + +link_libraries(controllers + tgcreator + util + sensors + core + terrain + tgOpenGLSupport) + +add_executable(AppPrismModel_nc + PrismModel.cpp + AppPrismModel.cpp +) + diff --git a/src/examples/3_prism_no_control/PrismModel.cpp b/src/examples/3_prism_no_control/PrismModel.cpp new file mode 100644 index 000000000..33ed3b819 --- /dev/null +++ b/src/examples/3_prism_no_control/PrismModel.cpp @@ -0,0 +1,200 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +/** + * @file PrismModel.cpp + * @brief Contains the definition of the members of the class PrismModel. + * $Id$ + */ + +// This module +#include "PrismModel.h" +// This library +#include "core/tgLinearString.h" +#include "core/tgRod.h" +#include "tgcreator/tgBuildSpec.h" +#include "tgcreator/tgLinearStringInfo.h" +#include "tgcreator/tgRodInfo.h" +#include "tgcreator/tgStructure.h" +#include "tgcreator/tgStructureInfo.h" +// The Bullet Physics library +#include "LinearMath/btVector3.h" +// The C++ Standard Library +#include + +/** + * Anonomous namespace so we don't have to declare the config in + * the header. + */ +namespace +{ + /** + * Configuration parameters so they're easily accessable. + * All parameters must be positive. + */ + const struct Config + { + double density; + double radius; + double stiffness; + double damping; + double pretension; + double triangle_length; + double triangle_height; + double prism_height; + } c = + { + 0.2, // density (mass / length^3) + 0.31, // radius (length) + 1000.0, // stiffness (mass / sec^2) + 10.0, // damping (mass / sec) + 500.0, // pretension (mass * length / sec^2) + 10.0, // triangle_length (length) + 10.0, // triangle_height (length) + 20.0, // prism_height (length) + }; +} // namespace + +PrismModel::PrismModel() : +tgModel() +{ +} + +PrismModel::~PrismModel() +{ +} + +void PrismModel::addNodes(tgStructure& s, + double edge, + double width, + double height) +{ + // bottom right + s.addNode(-edge / 2.0, 0, 0); // 1 + // bottom left + s.addNode( edge / 2.0, 0, 0); // 2 + // bottom front + s.addNode(0, 0, width); // 3 + // top right + s.addNode(-edge / 2.0, height, 0); // 4 + // top left + s.addNode( edge / 2.0, height, 0); // 5 + // top front + s.addNode(0, height, width); // 6 +} + +void PrismModel::addRods(tgStructure& s) +{ + s.addPair( 0, 4, "rod"); + s.addPair( 1, 5, "rod"); + s.addPair( 2, 3, "rod"); +} + +void PrismModel::addMuscles(tgStructure& s) +{ + // Bottom Triangle + s.addPair(0, 1, "muscle"); + s.addPair(1, 2, "muscle"); + s.addPair(2, 0, "muscle"); + + // Top + s.addPair(3, 4, "muscle"); + s.addPair(4, 5, "muscle"); + s.addPair(5, 3, "muscle"); + + //Edges + s.addPair(0, 3, "muscle"); + s.addPair(1, 4, "muscle"); + s.addPair(2, 5, "muscle"); +} + +void PrismModel::setup(tgWorld& world) +{ + // Define the configurations of the rods and strings + // Note that pretension is defined for this string + const tgRod::Config rodConfig(c.radius, c.density); + const tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension); + + // Create a structure that will hold the details of this model + tgStructure s; + + // Add nodes to the structure + addNodes(s, c.triangle_length, c.triangle_height, c.prism_height); + + // Add rods to the structure + addRods(s); + + // Add muscles to the structure + addMuscles(s); + + // Move the structure so it doesn't start in the ground + s.move(btVector3(0, 10, 0)); + + // Create the build spec that uses tags to turn the structure into a real model + tgBuildSpec spec; + spec.addBuilder("rod", new tgRodInfo(rodConfig)); + spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); + + // Create your structureInfo + tgStructureInfo structureInfo(s, spec); + + // Use the structureInfo to build ourselves + structureInfo.buildInto(*this, world); + + // We could now use tgCast::filter or similar to pull out the + // models (e.g. muscles) that we want to control. + allMuscles = tgCast::filter (getDescendants()); + + // Notify controllers that setup has finished. + notifySetup(); + + // Actually setup the children + tgModel::setup(world); +} + +void PrismModel::step(double dt) +{ + // Precondition + if (dt <= 0.0) + { + throw std::invalid_argument("dt is not positive"); + } + else + { + // Notify observers (controllers) of the step so that they can take action + notifyStep(dt); + tgModel::step(dt); // Step any children + } +} + +void PrismModel::onVisit(tgModelVisitor& r) +{ + // Example: m_rod->getRigidBody()->dosomething()... + tgModel::onVisit(r); +} + +const std::vector& PrismModel::getAllMuscles() const +{ + return allMuscles; +} + +void PrismModel::teardown() +{ + notifyTeardown(); + tgModel::teardown(); +} diff --git a/src/examples/3_prism_no_control/PrismModel.h b/src/examples/3_prism_no_control/PrismModel.h new file mode 100644 index 000000000..5287767bb --- /dev/null +++ b/src/examples/3_prism_no_control/PrismModel.h @@ -0,0 +1,146 @@ +/* + * Copyright © 2012, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * All rights reserved. + * + * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed + * under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0. + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific language + * governing permissions and limitations under the License. +*/ + +#ifndef PRISM_MODEL_H +#define PRISM_MODEL_H + +/** + * @file PrismModel.h + * @brief Defines a 3 strut 9 string tensegrity model + * @author Brian Mirletz + * @version 1.1.0 + * $Id$ + */ + +// This library +#include "core/tgModel.h" +#include "core/tgSubject.h" +// The C++ Standard Library +#include + +// Forward declarations +class tgLinearString; +class tgModelVisitor; +class PretensionController; +class tgStructure; +class tgWorld; + +/** + * A class that constructs a three bar tensegrity prism using the tools + * in tgcreator. This iteration avoids using a controller and instead + * uses the new (to v1.1) ability to define pretension in a + * tgLinearString's constructor + */ +class PrismModel : public tgSubject, public tgModel +{ +public: + + /** + * The only constructor. Configuration parameters are within the + * .cpp file in this case, not passed in. + */ + PrismModel(); + + /** + * Destructor. Deletes controllers, if any were added during setup. + * Teardown handles everything else. + */ + virtual ~PrismModel(); + + /** + * Create the model. Place the rods and strings into the world + * that is passed into the simulation. This is triggered + * automatically when the model is added to the simulation, when + * tgModel::setup(world) is called (if this model is a child), + * and when reset is called. Also notifies controllers of setup. + * @param[in] world - the world we're building into + */ + virtual void setup(tgWorld& world); + + /** + * Undoes setup. Deletes child models. Called automatically on + * reset and end of simulation. Notifies controllers of teardown + */ + virtual void teardown(); + + /** + * Step the model, its children. Notifies controllers of step. + * @param[in] dt, the timestep. Must be positive. + */ + virtual void step(double dt); + + /** + * Receives a tgModelVisitor and dispatches itself into the + * visitor's "render" function. This model will go to the default + * tgModel function, which does nothing. + * @param[in] r - a tgModelVisitor which will pass this model back + * to itself + */ + virtual void onVisit(tgModelVisitor& r); + + /** + * Return a vector of all muscles for the controllers to work with. + * @return A vector of all of the muscles + */ + const std::vector& getAllMuscles() const; + +private: + + /** + * A function called during setup that determines the positions of + * the nodes based on construction parameters. Rewrite this function + * for your own models + * @param[in] s: A tgStructure that we're building into + * @param[in] edge: the X distance of the base points + * @param[in] width: the Z distance of the base triangle + * @param[in] height: the Y distance along the axis of the prism + */ + static void addNodes(tgStructure& s, + double edge, + double width, + double height); + + /** + * A function called during setup that creates rods from the + * relevant nodes. Rewrite this function for your own models. + * @param[in] s A tgStructure that we're building into + */ + static void addRods(tgStructure& s); + + /** + * A function called during setup that creates muscles (Strings) from + * the relevant nodes. Rewrite this function for your own models. + * @param[in] s A tgStructure that we're building into + */ + static void addMuscles(tgStructure& s); + +private: + /** + * A controller that is attached to all of the strings which applies + * pretension to the muscles. + */ + PretensionController* m_pStringController; + + /** + * A list of all of the muscles. Will be empty until most of the way + * through setup when it is filled using tgModel's find methods + */ + std::vector allMuscles; +}; + +#endif // Prism_MODEL_H diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index a9d787308..6b59dfa96 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -4,6 +4,7 @@ Project(examples) subdirs( 3_prism 3_prism_serialize + 3_prism_no_control NestedTetrahedrons SUPERball learningSpines diff --git a/src/examples/SUPERball/T6Model.cpp b/src/examples/SUPERball/T6Model.cpp index 2d7311ac2..bcb750416 100644 --- a/src/examples/SUPERball/T6Model.cpp +++ b/src/examples/SUPERball/T6Model.cpp @@ -62,8 +62,8 @@ namespace double friction; double rollFriction; double restitution; + double pretension; bool hist; - double rotation; double maxTens; double targetVelocity; double maxAcc; @@ -78,8 +78,8 @@ namespace 0.99, // friction (unitless) 0.01, // rollFriction (unitless) 0.0, // restitution (?) + 0, // pretension 0, // History logging (boolean) - 0, // rotation 100000, // maxTens 10000, // targetVelocity 20000 // maxAcc @@ -173,7 +173,7 @@ void T6Model::setup(tgWorld& world) const tgRod::Config rodConfig(c.radius, c.density, c.friction, c.rollFriction, c.restitution); - tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.hist, c.rotation, + tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.pretension, c.hist, c.maxTens, c.targetVelocity, c.maxAcc); @@ -240,5 +240,6 @@ const std::vector& T6Model::getAllMuscles() const void T6Model::teardown() { + notifyTeardown(); tgModel::teardown(); } diff --git a/src/examples/learningSpines/OctahedralComplex/FlemonsSpineModelLearningCL.cpp b/src/examples/learningSpines/OctahedralComplex/FlemonsSpineModelLearningCL.cpp index 189da204d..fdbeb4ba0 100644 --- a/src/examples/learningSpines/OctahedralComplex/FlemonsSpineModelLearningCL.cpp +++ b/src/examples/learningSpines/OctahedralComplex/FlemonsSpineModelLearningCL.cpp @@ -72,8 +72,9 @@ void FlemonsSpineModelLearningCL::setup(tgWorld& world) const double stiffness = 1000.0; const double damping = .01*stiffness; + const double pretension = 0.0; //const int segments = 8; - const tgLinearString::Config stringConfig(stiffness, damping, false, 0, 7000, 24, 10000); + const tgLinearString::Config stringConfig(stiffness, damping, pretension, false, 7000, 24, 10000); //const tgRBString::Config rbConfig(segments, rodConfig2, stringConfig, 10.0); tgLinearString::Config muscleConfig(2000, 20); diff --git a/src/examples/learningSpines/TetraSpine/TetraSpineLearningModel.cpp b/src/examples/learningSpines/TetraSpine/TetraSpineLearningModel.cpp index c171a5f34..5625b810b 100644 --- a/src/examples/learningSpines/TetraSpine/TetraSpineLearningModel.cpp +++ b/src/examples/learningSpines/TetraSpine/TetraSpineLearningModel.cpp @@ -177,7 +177,7 @@ void TetraSpineLearningModel::setup(tgWorld& world) tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); - tgLinearString::Config muscleConfig(1000, 100, false, 0, 7000, 24, 10000); + tgLinearString::Config muscleConfig(1000, 100, 0, false, 7000, 24, 10000); spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); #else // Params for In Won const double density = .00311; diff --git a/src/examples/learningSpines/TetrahedralComplex/AppFlemonsSpineLearning.cpp b/src/examples/learningSpines/TetrahedralComplex/AppFlemonsSpineLearning.cpp index 0f0d3e64e..77690bf87 100644 --- a/src/examples/learningSpines/TetrahedralComplex/AppFlemonsSpineLearning.cpp +++ b/src/examples/learningSpines/TetrahedralComplex/AppFlemonsSpineLearning.cpp @@ -90,6 +90,11 @@ int main(int argc, char** argv) while (i < 3000) { simulation.run(30000); + #ifdef BT_USE_DOUBLE_PRECISION + std::cout << "Double precision" << std::endl; + #else + std::cout << "Single Precision" << std::endl; + #endif simulation.reset(); i++; } diff --git a/src/examples/learningSpines/TetrahedralComplex/FlemonsSpineModelLearning.cpp b/src/examples/learningSpines/TetrahedralComplex/FlemonsSpineModelLearning.cpp index ba1de8e38..12c753d0c 100644 --- a/src/examples/learningSpines/TetrahedralComplex/FlemonsSpineModelLearning.cpp +++ b/src/examples/learningSpines/TetrahedralComplex/FlemonsSpineModelLearning.cpp @@ -69,7 +69,7 @@ void FlemonsSpineModelLearning::setup(tgWorld& world) const double restitution = 0.0; const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution); - tgLinearString::Config muscleConfig(1000, 10, false, 0, 7000, 12, 4000); + tgLinearString::Config muscleConfig(1000, 10, 0.0, false, 7000, 12, 4000); // Calculations for the flemons spine model double v_size = 10.0; diff --git a/src/examples/learningSpines/ribDemo/RibModel.cpp b/src/examples/learningSpines/ribDemo/RibModel.cpp index 4d01bb88e..676058ca6 100644 --- a/src/examples/learningSpines/ribDemo/RibModel.cpp +++ b/src/examples/learningSpines/ribDemo/RibModel.cpp @@ -254,11 +254,12 @@ void RibModel::setup(tgWorld& world) tgBuildSpec spec; spec.addBuilder("rod", new tgRodInfo(rodConfig)); + const double pretension = 0.0; const bool hist = false; - tgLinearString::Config muscleConfig(500, 5, hist); + tgLinearString::Config muscleConfig(500, 5, pretension, hist); spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig)); - tgLinearString::Config muscleConfigAct(1000, 10, hist, 0, 7000, 24, 10000); + tgLinearString::Config muscleConfigAct(1000, 10, pretension, hist, 7000, 24, 10000); spec.addBuilder("muscleAct", new tgLinearStringInfo(muscleConfigAct)); #if (0) // Compliant Rib Attachments diff --git a/src/examples/learningSpines/tgCPGStringControl.cpp b/src/examples/learningSpines/tgCPGStringControl.cpp index d31bbce1c..537cb2f80 100644 --- a/src/examples/learningSpines/tgCPGStringControl.cpp +++ b/src/examples/learningSpines/tgCPGStringControl.cpp @@ -19,6 +19,7 @@ #include "tgCPGStringControl.h" #include "core/Muscle2P.h" +#include "core/muscleAnchor.h" #include "core/ImpedanceControl.h" #include "util/CPGEquations.h" diff --git a/src/inc.CMakeBullet.txt b/src/inc.CMakeBullet.txt index 124b11983..8abc6f8df 100644 --- a/src/inc.CMakeBullet.txt +++ b/src/inc.CMakeBullet.txt @@ -28,9 +28,11 @@ include_directories ( link_directories(${LIB_DIR} ${OPENGL_LIB} ${OPENGL_FG_LIB}) OPTION(USE_GLUT "Use Glut" ON) + # If you turn this on, turn it on in setup_bullet.sh as well and # re-build your env directory (line 191 as of 6-24-14) -OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF) +OPTION(USE_DOUBLE_PRECISION "Use double precision" ON) + FIND_PACKAGE(OpenGL) IF (OPENGL_FOUND) diff --git a/src/tgcreator/CMakeLists.txt b/src/tgcreator/CMakeLists.txt index 819d17a86..be1522bf3 100644 --- a/src/tgcreator/CMakeLists.txt +++ b/src/tgcreator/CMakeLists.txt @@ -18,4 +18,4 @@ add_library( ${PROJECT_NAME} SHARED link_directories(${LIB_DIR}) -target_link_libraries(${PROJECT_NAME} core util tgOpenGLSupport) +target_link_libraries(${PROJECT_NAME} core tgOpenGLSupport) diff --git a/src/tgcreator/tgBoxInfo.cpp b/src/tgcreator/tgBoxInfo.cpp index 6add27302..84dd016a3 100644 --- a/src/tgcreator/tgBoxInfo.cpp +++ b/src/tgcreator/tgBoxInfo.cpp @@ -73,10 +73,10 @@ tgRigidInfo* tgBoxInfo::createRigidInfo(const tgPair& pair) void tgBoxInfo::initRigidBody(tgWorld& world) { tgRigidInfo::initRigidBody(world); - assert(m_rigidBody != NULL); - m_rigidBody->setFriction(m_config.friction); - m_rigidBody->setRollingFriction(m_config.rollFriction); - m_rigidBody->setRestitution(m_config.restitution); + assert(m_collisionObject != NULL); + getRigidBody()->setFriction(m_config.friction); + getRigidBody()->setRollingFriction(m_config.rollFriction); + getRigidBody()->setRestitution(m_config.restitution); } tgModel* tgBoxInfo::createModel(tgWorld& world) diff --git a/src/tgcreator/tgBoxInfo.h b/src/tgcreator/tgBoxInfo.h index 7c090b122..895dc19e5 100644 --- a/src/tgcreator/tgBoxInfo.h +++ b/src/tgcreator/tgBoxInfo.h @@ -98,7 +98,7 @@ class tgBoxInfo : public tgRigidInfo { */ virtual void initRigidBody(tgWorld& world); - tgModel* createModel(tgWorld& world); + virtual tgModel* createModel(tgWorld& world); /** * Return a const reference to the container of the radius and density. @@ -197,7 +197,7 @@ class tgBoxInfo : public tgRigidInfo { */ virtual bool containsNode(const btVector3& nodeVector) const { - return (getFrom() == nodeVector) || (getTo() == nodeVector); + return ((getFrom() - nodeVector).fuzzyZero() || (getTo() - nodeVector).fuzzyZero()); } /** diff --git a/src/tgcreator/tgCompoundRigidInfo.cpp b/src/tgcreator/tgCompoundRigidInfo.cpp index ea088283b..6472f4361 100644 --- a/src/tgcreator/tgCompoundRigidInfo.cpp +++ b/src/tgcreator/tgCompoundRigidInfo.cpp @@ -25,7 +25,7 @@ */ #include "tgCompoundRigidInfo.h" -tgCompoundRigidInfo::tgCompoundRigidInfo() : m_compoundShape(NULL), m_rigidBody(NULL) +tgCompoundRigidInfo::tgCompoundRigidInfo() : m_compoundShape(NULL), tgRigidInfo() { } @@ -62,7 +62,7 @@ btCompoundShape* tgCompoundRigidInfo::createCompoundShape(tgWorld& world) const { if (m_compoundShape == 0) { - // Deallocated in the destructor + // Deallocated by the world implementation m_compoundShape = new btCompoundShape(&world); const btVector3 com = getCenterOfMass(); @@ -73,12 +73,12 @@ btCompoundShape* tgCompoundRigidInfo::createCompoundShape(tgWorld& world) const btTransform t = rigid->getTransform(); t.setOrigin(t.getOrigin() - com); m_compoundShape->addChildShape(t, rigid->getCollisionShape(world)); - } - + } // Add the collision shape to the array so we can delete it later tgWorldBulletPhysicsImpl& bulletWorld = (tgWorldBulletPhysicsImpl&)world.implementation(); bulletWorld.addCollisionShape(m_compoundShape); + } return m_compoundShape; } @@ -110,16 +110,39 @@ double tgCompoundRigidInfo::getMass() const } return mass; } + +btRigidBody* tgCompoundRigidInfo::getRigidBody() +{ + btRigidBody* rb = tgCast::cast(m_collisionObject); + return rb; +} + +const btRigidBody* tgCompoundRigidInfo::getRigidBody() const +{ + btRigidBody* rb = tgCast::cast(m_collisionObject); + return rb; +} void tgCompoundRigidInfo::setRigidBody(btRigidBody* const rigidBody) { - m_rigidBody = rigidBody; + m_collisionObject = rigidBody; // Set the rigid body for all components /// @todo Use std::for_each() for (int ii = 0; ii < m_rigids.size(); ii++) { m_rigids[ii]->setRigidBody(rigidBody); } } + +void tgCompoundRigidInfo::setCollisionObject(btCollisionObject* collisionObject) +{ + m_collisionObject = collisionObject; + // Set the rigid body for all components + /// @todo Use std::for_each() + for (int ii = 0; ii < m_rigids.size(); ii++) { + m_rigids[ii]->setCollisionObject(collisionObject); + } +} + std::set tgCompoundRigidInfo::getLeafRigids() { diff --git a/src/tgcreator/tgCompoundRigidInfo.h b/src/tgcreator/tgCompoundRigidInfo.h index bb15c04a3..8e23835c6 100644 --- a/src/tgcreator/tgCompoundRigidInfo.h +++ b/src/tgcreator/tgCompoundRigidInfo.h @@ -110,27 +110,44 @@ class tgCompoundRigidInfo : public tgRigidInfo * Return a pointer to the corresponding btRigidBody. * @return a pointer to the corresponding btRigidBody */ - virtual btRigidBody* getRigidBody() - { - return m_rigidBody; - } + virtual btRigidBody* getRigidBody(); /** * Return a const pointer to the corresponding btRigidBody. * @return a pointer to the corresponding btRigidBody */ - virtual const btRigidBody* getRigidBody() const - { - return m_rigidBody; - } + virtual const btRigidBody* getRigidBody() const; /** * Set the corresponding btRigidBody. * @param[in,out] a pointer to a btRigidBody - * @todo tgCompoundRigidInfo is infected by Bullet Physics */ virtual void setRigidBody(btRigidBody* const rigidBody); + /** + * Return a pointer to the collisionObject without upcasting + * @return a pointer to the corresponding btCollisionObject + */ + virtual btCollisionObject* getCollisionObject() + { + return m_collisionObject; + } + + /** + * Return a pointer to the collisionObject without upcasting + * @return a pointer to the corresponding btCollisionObject + */ + virtual const btCollisionObject* getCollisionObject() const + { + return m_collisionObject; + } + + /** + * Set the collision object to a new collision object + * @return a pointer to the corresponding btCollisionObject + */ + virtual void setCollisionObject(btCollisionObject* collisionObject); + /** * By default this should look for the object that has the closest * connectionPoint to the destination or something. @@ -198,11 +215,6 @@ class tgCompoundRigidInfo : public tgRigidInfo */ mutable btCompoundShape * m_compoundShape; - /** - * The btRididBody that represents this object to Bullet. - */ - btRigidBody * m_rigidBody; - }; diff --git a/src/tgcreator/tgLinearStringInfo.cpp b/src/tgcreator/tgLinearStringInfo.cpp index 36065d240..205f816d3 100644 --- a/src/tgcreator/tgLinearStringInfo.cpp +++ b/src/tgcreator/tgLinearStringInfo.cpp @@ -27,6 +27,7 @@ #include "tgLinearStringInfo.h" #include "core/Muscle2P.h" +#include "core/muscleAnchor.h" tgLinearStringInfo::tgLinearStringInfo(const tgLinearString::Config& config) : m_config(config), @@ -86,7 +87,15 @@ Muscle2P* tgLinearStringInfo::createMuscle2P() btVector3 from = getFromRigidInfo()->getConnectionPoint(getFrom(), getTo(), m_config.rotation); btVector3 to = getToRigidInfo()->getConnectionPoint(getTo(), getFrom(), m_config.rotation); - - return new Muscle2P(fromBody, from, toBody, to, m_config.stiffness, m_config.damping); + + std::vector anchorList; + + muscleAnchor* anchor1 = new muscleAnchor(fromBody, from); + anchorList.push_back(anchor1); + + muscleAnchor* anchor2 = new muscleAnchor(toBody, to); + anchorList.push_back(anchor2); + + return new Muscle2P(anchorList, m_config.stiffness, m_config.damping, m_config.pretension); } diff --git a/src/tgcreator/tgRigidAutoCompound.cpp b/src/tgcreator/tgRigidAutoCompound.cpp index d6c5d8e95..e813cf80f 100644 --- a/src/tgcreator/tgRigidAutoCompound.cpp +++ b/src/tgcreator/tgRigidAutoCompound.cpp @@ -66,9 +66,9 @@ std::vector< tgRigidInfo* > tgRigidAutoCompound::execute() { // @todo: we probably don't need this any more -- this will be taken care of in the tgRigidInfo => tgModel step // @todo: NOTE: we need to have a way to check to see if a rigid has already been instantiated -- maybe just check get -void tgRigidAutoCompound::setRigidBodyForGroup(btRigidBody* body, std::deque& group) { +void tgRigidAutoCompound::setRigidBodyForGroup(btCollisionObject* body, std::deque& group) { for(int i = 0; i < group.size(); i++) { - group[i]->setRigidBody(body); + group[i]->setCollisionObject(body); } } diff --git a/src/tgcreator/tgRigidAutoCompound.h b/src/tgcreator/tgRigidAutoCompound.h index ab5fca433..f4a3c71c0 100644 --- a/src/tgcreator/tgRigidAutoCompound.h +++ b/src/tgcreator/tgRigidAutoCompound.h @@ -31,6 +31,7 @@ #include class tgRigidInfo; +class btCollisionObject; class btRigidBody; /** @@ -58,7 +59,7 @@ class tgRigidAutoCompound { // @todo: we probably don't need this any more -- this will be taken care of in the tgRigidInfo => tgModel step // @todo: NOTE: we need to have a way to check to see if a rigid has already been instantiated -- maybe just check get - void setRigidBodyForGroup(btRigidBody* body, std::deque& group); + void setRigidBodyForGroup(btCollisionObject* body, std::deque& group); void setRigidInfoForGroup(tgRigidInfo* rigidInfo, std::deque& group); diff --git a/src/tgcreator/tgRigidInfo.cpp b/src/tgcreator/tgRigidInfo.cpp index 21efa5d25..7281e1b87 100644 --- a/src/tgcreator/tgRigidInfo.cpp +++ b/src/tgcreator/tgRigidInfo.cpp @@ -34,8 +34,14 @@ #include "core/tgBulletUtil.h" #include "core/tgTagSearch.h" #include "tgUtil.h" +#include "core/tgBulletUtil.h" #include "core/tgWorld.h" + +// The Bullet Physics library +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" + tgRigidInfo* tgRigidInfo::createRigidInfo(const tgNode& node, const tgTagSearch& tagSearch) { // Our subclasses may not be able to create rigidInfos based on nodes. Also, the @@ -114,6 +120,17 @@ void tgRigidInfo::initRigidBody(tgWorld& world) } } } + +btRigidBody* tgRigidInfo::getRigidBody() +{ + btRigidBody* body = tgCast::cast(m_collisionObject); + return body; +} + +const btRigidBody* tgRigidInfo::getRigidBody() const { + const btRigidBody* body = tgCast::cast(m_collisionObject); + return body; +} bool tgRigidInfo::sharesNodesWith(const tgRigidInfo& other) const { diff --git a/src/tgcreator/tgRigidInfo.h b/src/tgcreator/tgRigidInfo.h index 55addb986..b3e0d826e 100644 --- a/src/tgcreator/tgRigidInfo.h +++ b/src/tgcreator/tgRigidInfo.h @@ -27,29 +27,27 @@ * $Id$ */ -// The Bullet Physics library -#include "btBulletDynamicsCommon.h" -#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" // The C++ Standard Library #include // This library - -// @todo: move these includes to tgRigidInfo.cpp #include "core/tgTaggable.h" -#include "core/tgBulletUtil.h" -#include "core/tgWorld.h" #include "core/tgModel.h" +//Bullet Physics +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" // Forward references class tgCompoundRigidInfo; -class tgTaggable; class tgNode; class tgNodes; class tgPair; class tgPairs; class tgTagSearch; +class tgWorld; -#include "tgUtil.h" // Testing/debugging only +class btRigidBody; +class btCollisionShape; +class btTransform; /** * A collector for keeping track of all of the necessary components of a @@ -79,21 +77,21 @@ class tgRigidInfo : public tgTaggable { tgRigidInfo() : m_collisionShape(NULL), m_rigidInfoGroup(NULL), - m_rigidBody(NULL), + m_collisionObject(NULL), tgTaggable() {} tgRigidInfo(tgTags tags) : m_collisionShape(NULL), m_rigidInfoGroup(NULL), - m_rigidBody(NULL), + m_collisionObject(NULL), tgTaggable(tags) {} tgRigidInfo(const std::string& space_separated_tags) : m_collisionShape(NULL), m_rigidInfoGroup(NULL), - m_rigidBody(NULL), + m_collisionObject(NULL), tgTaggable(space_separated_tags) {} @@ -170,18 +168,13 @@ class tgRigidInfo : public tgTaggable { * Return a pointer to the corresponding btRigidBody. * @return a pointer to the corresponding btRigidBody */ - virtual btRigidBody* getRigidBody() - { - return m_rigidBody; - } + virtual btRigidBody* getRigidBody(); /** * Return a const pointer to the corresponding btRigidBody. * @return a pointer to the corresponding btRigidBody */ - virtual const btRigidBody* getRigidBody() const { - return m_rigidBody; - } + virtual const btRigidBody* getRigidBody() const; /** * Set the corresponding btRigidBody. @@ -189,9 +182,37 @@ class tgRigidInfo : public tgTaggable { */ virtual void setRigidBody(btRigidBody* rigidBody) { - /// @todo Does this leak any previous value of m_rigidBody? - m_rigidBody = rigidBody; + /// @todo Does this leak any previous value of m_collisionObject? + m_collisionObject = rigidBody; } + + /** + * Return a pointer to the collisionObject without upcasting + * @return a pointer to the corresponding btCollisionObject + */ + virtual btCollisionObject* getCollisionObject() + { + return m_collisionObject; + } + + /** + * Return a pointer to the collisionObject without upcasting + * @return a pointer to the corresponding btCollisionObject + */ + virtual const btCollisionObject* getCollisionObject() const + { + return m_collisionObject; + } + + /** + * Set the collision object to a new collision object + * @return a pointer to the corresponding btCollisionObject + */ + virtual void setCollisionObject(btCollisionObject* collisionObject) + { + /// @todo Does this leak any previous value of m_collisionObject? + m_collisionObject = collisionObject; + } /** * Return a btTransform. @@ -323,9 +344,10 @@ class tgRigidInfo : public tgTaggable { mutable tgRigidInfo* m_rigidInfoGroup; /** - * A pointer to the corresponding btRigidBody. + * A pointer to the corresponding btCollisionObject. + * Typically a btRigidBody, but can also be a btGhostObject */ - mutable btRigidBody* m_rigidBody; + mutable btCollisionObject* m_collisionObject; }; diff --git a/src/tgcreator/tgRodInfo.cpp b/src/tgcreator/tgRodInfo.cpp index 21a059a01..b8743111f 100644 --- a/src/tgcreator/tgRodInfo.cpp +++ b/src/tgcreator/tgRodInfo.cpp @@ -72,10 +72,10 @@ tgRigidInfo* tgRodInfo::createRigidInfo(const tgPair& pair) void tgRodInfo::initRigidBody(tgWorld& world) { tgRigidInfo::initRigidBody(world); - assert(m_rigidBody != NULL); - m_rigidBody->setFriction(m_config.friction); - m_rigidBody->setRollingFriction(m_config.rollFriction); - m_rigidBody->setRestitution(m_config.restitution); + assert(m_collisionObject != NULL); + getRigidBody()->setFriction(m_config.friction); + getRigidBody()->setRollingFriction(m_config.rollFriction); + getRigidBody()->setRestitution(m_config.restitution); } tgModel* tgRodInfo::createModel(tgWorld& world) diff --git a/src/tgcreator/tgRodInfo.h b/src/tgcreator/tgRodInfo.h index e836700af..cc1d534bc 100644 --- a/src/tgcreator/tgRodInfo.h +++ b/src/tgcreator/tgRodInfo.h @@ -197,7 +197,7 @@ class tgRodInfo : public tgRigidInfo { */ virtual bool containsNode(const btVector3& nodeVector) const { - return (getFrom() == nodeVector) || (getTo() == nodeVector); + return ((getFrom() - nodeVector).fuzzyZero() || (getTo() - nodeVector).fuzzyZero()); } /** diff --git a/src/tgcreator/tgSphereInfo.cpp b/src/tgcreator/tgSphereInfo.cpp index a30acf83c..2ea348762 100644 --- a/src/tgcreator/tgSphereInfo.cpp +++ b/src/tgcreator/tgSphereInfo.cpp @@ -73,10 +73,10 @@ tgRigidInfo* tgSphereInfo::createRigidInfo(const tgNode& node) void tgSphereInfo::initRigidBody(tgWorld& world) { tgRigidInfo::initRigidBody(world); - assert(m_rigidBody != NULL); - m_rigidBody->setFriction(m_config.friction); - m_rigidBody->setRollingFriction(m_config.rollFriction); - m_rigidBody->setRestitution(m_config.restitution); + assert(m_collisionObject != NULL); + getRigidBody()->setFriction(m_config.friction); + getRigidBody()->setRollingFriction(m_config.rollFriction); + getRigidBody()->setRestitution(m_config.restitution); } tgModel* tgSphereInfo::createModel(tgWorld& world) @@ -148,7 +148,7 @@ tgSphereInfo::getConnectionPoint(const btVector3& referencePoint, const btVector3 startPoint = (getNode()); // Vector from reference point to destination point const btVector3 refToDest = - (referencePoint - destinationPoint).normalize(); + (destinationPoint - referencePoint).normalize(); // Project along the radius to the destination point diff --git a/src/tgcreator/tgStructureInfo.cpp b/src/tgcreator/tgStructureInfo.cpp index a3bba2054..750b6a05a 100644 --- a/src/tgcreator/tgStructureInfo.cpp +++ b/src/tgcreator/tgStructureInfo.cpp @@ -31,6 +31,8 @@ #include "tgConnectorInfo.h" #include "tgRigidAutoCompound.h" #include "tgStructure.h" +#include "core/tgWorld.h" +#include "core/tgModel.h" // The C++ Standard Library #include diff --git a/src/tgcreator/tgUtil.h b/src/tgcreator/tgUtil.h index 44d3b071a..484d6eb93 100644 --- a/src/tgcreator/tgUtil.h +++ b/src/tgcreator/tgUtil.h @@ -28,7 +28,7 @@ * $Id$ */ -#include "btBulletDynamicsCommon.h" +#include "btBulletDynamicsCommon.h" // stream operations for collision shapes #include "LinearMath/btQuaternion.h" #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" @@ -96,8 +96,11 @@ class tgUtil btTransform t = btTransform(); t.setIdentity(); t.setOrigin(origin); - t.setRotation(getQuaternionBetween(startOrientation, - getVector(start, end))); + // If they for some reason gave us the same vector, keep identity + + t.setRotation(getQuaternionBetween(startOrientation, + getVector(start, end))); + return t; } @@ -125,7 +128,7 @@ class tgUtil * Get a vector that points from a to b. * @param[in] from a btVector3 * @param[in] to a btVector3 - * @return the vector difference to - from + * @return the vector difference to - fromn */ static btVector3 getVector(const btVector3& from, const btVector3& to) @@ -188,6 +191,7 @@ class tgUtil * @param[in] b a btVector3, passed by value * @return a quaternion that, if applied, would rotate vector a to align * with vector b + * @todo get some sensible value if a or b = (0, 0, 0). See getTransform */ static btQuaternion getQuaternionBetween(btVector3 a, btVector3 b) { @@ -196,18 +200,26 @@ class tgUtil // The return value btQuaternion result; - + // Account for equal vectors (can't calculate c in this case) if (almostEqual(a, b)) { result = btQuaternion::getIdentity(); } else if (almostEqual(a, -b)) { // Account for opposing vectors (can't calculate c in // this case either) - // What to do here? - const btVector3 arb = - a + getArbitraryNonParallelVector(a); - const btVector3 c = (a.cross(arb)).normalize(); - result = btQuaternion(c, M_PI).normalize();; + btVector3 xAxis(1.0, 0.0, 0.0); + if (a.dot(xAxis) == 0.0) + { + // Gets around bad btTransforms with an up vector + result = btQuaternion (-1.0, 0.0, 0.0, 0.0); + } + else + { + const btVector3 arb = + a + getArbitraryNonParallelVector(a); + const btVector3 c = (a.cross(arb)).normalize(); + result = btQuaternion(c, M_PI).normalize(); + } } else { // Create a vector normal to both a and b const btVector3 c = (a.cross(b)).normalize(); @@ -275,7 +287,11 @@ class tgUtil { addRotation(v, fixedPoint, rotation.getAxis(), rotation.getAngle()); } - + + /** + * @todo write a function that returns the normal of a vector without overwriting the original vector + * line btVector3's .normalize() functions + */ /** * Convert radians to degrees. diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2df5e5eee..21a8bcfb1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -14,4 +14,5 @@ include_directories(${ENV_INC_DIR}) link_directories(${ENV_LIB_DIR}) subdirs( - helpers) + helpers + tgcreator) diff --git a/test/tgcreator/CMakeLists.txt b/test/tgcreator/CMakeLists.txt new file mode 100644 index 000000000..b3f3ee62a --- /dev/null +++ b/test/tgcreator/CMakeLists.txt @@ -0,0 +1,27 @@ +project(tgcreator) + +SET(OPENGL_LIB ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL) +SET(OPENGL_FG_LIB ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL_FreeGlut) +SET(SRC_DIR ${PROJECT_SOURCE_DIR}/../../src) +SET(NTRT_BUILD_DIR ${PROJECT_SOURCE_DIR}/../../build) + +include_directories(${CMAKE_CURRENT_BINARY_DIR} + ${ENV_INC_DIR} + ${BULLET_PHYSICS_SOURCE_DIR}/src + ${ENV_INC_DIR}/bullet + ${ENV_INC_DIR}/boost + ${ENV_INC_DIR}/tensegrity + ${SRC_DIR} + ${OPENGL_LIB} + ${OPENGL_FG_LIB}) + +# openGL libs required for core +link_directories(${ENV_LIB_DIR} ${OPENGL_LIB} ${OPENGL_FG_LIB} ${NTRT_BUILD_DIR}) + + +add_executable(tgUtil_test + tgUtil_test.cpp) + +# libcore gets us enough links to bullet to get the btAlignedAllocator +target_link_libraries(tgUtil_test ${ENV_LIB_DIR}/libgtest.a pthread + ${NTRT_BUILD_DIR}/core/libcore.so ) diff --git a/test/tgcreator/tgUtil_test.cpp b/test/tgcreator/tgUtil_test.cpp new file mode 100644 index 000000000..80a91f292 --- /dev/null +++ b/test/tgcreator/tgUtil_test.cpp @@ -0,0 +1,101 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License for the specific language +* governing permissions and limitations under the License. +*/ + +/** +* @file tgUtil_test.cpp +* @brief Contains a test of the vector and quaternion manipulations in +* tgUtil +* $Id$ +*/ + +// This application +#include "tgcreator/tgUtil.h" +// The Bullet Physics Library +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btMatrix3x3.h" +// The C++ Standard Library +#include +#include +// Google Test +#include "gtest/gtest.h" + + +using namespace std; + +namespace { + + // The fixture for testing class FileHelpers. + class tgUtilTest : public ::testing::Test { + protected: + // You can remove any or all of the following functions if its body + // is empty. + + tgUtilTest() { + + } + + virtual ~tgUtilTest() { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + virtual void SetUp() { + // Code here will be called immediately after the constructor (right + // before each test). + } + + virtual void TearDown() { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Objects declared here can be used by all tests in the test case for FileHelpers. + }; + + TEST_F(tgUtilTest, testQuaternion) { + + // This is a key test since we define btTransforms against the up axis + btVector3 up(0.0, 1.0, 0.0); + btVector3 down(0.0, -1.0, 0.0); + + btQuaternion testQuaternion = tgUtil::getQuaternionBetween(down, up); + + btVector3 result = down.rotate(testQuaternion.getAxis(), testQuaternion.getAngle()); + + // Just comparing the vectors results in a floating point residual, which causes the test to fail + EXPECT_TRUE((up - result).fuzzyZero()); + + // Test another arbitrary opposite vector + btVector3 start(1.0, -1.0, 2.0); + btVector3 end (-1.0, 1.0, -2.0); + + testQuaternion = tgUtil::getQuaternionBetween(start, end); + + result = end.rotate(testQuaternion.getAxis(), testQuaternion.getAngle()); + + EXPECT_TRUE((start - result).fuzzyZero()); + } + +} // namespace + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_integration/CMakeLists.txt b/test_integration/CMakeLists.txt index beb8ccdfc..1cb6ee727 100644 --- a/test_integration/CMakeLists.txt +++ b/test_integration/CMakeLists.txt @@ -21,10 +21,12 @@ include_directories(${ENV_INC_DIR} ${ENV_INC_DIR}/boost ${ENV_INC_DIR}/tensegrity ${PROJECT_SOURCE_DIR} + ${SRC_DIR} ${OPENGL_LIB} ${OPENGL_FG_LIB} "/usr/include/glib-2.0") link_directories(${ENV_LIB_DIR} ${OPENGL_LIB} ${OPENGL_FG_LIB}) subdirs( + #MuscleNP - Segmentation fault as of 11/26/14 SpineTests) diff --git a/test_integration/MuscleNP/CMakeLists.txt b/test_integration/MuscleNP/CMakeLists.txt new file mode 100644 index 000000000..82cf5f85d --- /dev/null +++ b/test_integration/MuscleNP/CMakeLists.txt @@ -0,0 +1,15 @@ +link_directories(${ENV_LIB_DIR} ${NTRT_BUILD_DIR}) + +link_libraries( tgOpenGLSupport + ) + +add_executable(MuscleNP_test + MuscleNP_test.cpp) + +target_link_libraries(MuscleNP_test ${ENV_LIB_DIR}/libgtest.a pthread + ${NTRT_BUILD_DIR}/core/libcore.so + ${NTRT_BUILD_DIR}/core/terrain/libterrain.so + ${NTRT_BUILD_DIR}/tgcreator/libtgcreator.so + ${NTRT_BUILD_DIR}/dev/muscleNP/libMuscleNP.so + ${NTRT_BUILD_DIR}/dev/btietz/muscleNPEnergy/libMuscleNPCons.so + ) diff --git a/test_integration/MuscleNP/MuscleNP_test.cpp b/test_integration/MuscleNP/MuscleNP_test.cpp new file mode 100644 index 000000000..a763548c0 --- /dev/null +++ b/test_integration/MuscleNP/MuscleNP_test.cpp @@ -0,0 +1,118 @@ +/* +* Copyright © 2012, United States Government, as represented by the +* Administrator of the National Aeronautics and Space Administration. +* All rights reserved. +* +* The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed +* under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* http://www.apache.org/licenses/LICENSE-2.0. +* +* Unless required by applicable law or agreed to in writing, +* software distributed under the License is distributed on an +* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, +* either express or implied. See the License 197.632for the specific language +* governing permissions and limitations under the License. +*/ + +/** +* @file MuscleNP_test.cpp +* @brief Contains a test of conservation of momentum, energy, contact for MuscleNP +* $Id$ +*/ + +// This application +#include "dev/btietz/muscleNPEnergy/MuscleNPCons.h" +// This library +#include "core/tgModel.h" +#include "core/tgSimView.h" +#include "core/tgSimViewGraphics.h" +#include "core/tgSimulation.h" +#include "core/tgWorld.h" +#include "core/terrain/tgEmptyGround.h" + +#include "LinearMath/btVector3.h" + +// The C++ Standard Library +#include +#include +// Google Test +#include "gtest/gtest.h" + + +using namespace std; + +namespace { + + // The fixture for testing class FileHelpers. + class MuscleNPTest : public ::testing::Test { + protected: + // You can remove any or all of the following functions if its body + // is empty. + + MuscleNPTest() { + + } + + virtual ~MuscleNPTest() { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + virtual void SetUp() { + // Code here will be called immediately after the constructor (right + // before each test). + } + + virtual void TearDown() { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Objects declared here can be used by all tests in the test case for FileHelpers. + }; + + TEST_F(MuscleNPTest, MuscleNPMomentum) { + + // First create the world + const tgWorld::Config config(0.0); // gravity, cm/sec^2 + tgEmptyGround* ground = new tgEmptyGround(); + tgWorld world(config, ground); + + // Second create the view + const double stepSize = 1.0/1000.0; // Seconds + const double renderRate = 1.0/60.0; // Seconds + tgSimView view(world, stepSize, renderRate); + + // Third create the simulation + tgSimulation simulation(view); + + // Fourth create the models with their controllers and add the models to the + // simulation + MuscleNPCons* myModel = new MuscleNPCons(); + + simulation.addModel(myModel); + + double energyStart = myModel->getEnergy(); + btVector3 momentumStart = myModel->getMomentum(); + btVector3 velocityStart = myModel->getVelocityOfBody(2); + + simulation.run(10000); + + double energyEnd = myModel->getEnergy(); + btVector3 momentumEnd = myModel->getMomentum(); + btVector3 velocityEnd = myModel->getVelocityOfBody(2); + + EXPECT_EQ(momentumStart, momentumEnd); + + + } + +} // namespace + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_integration/SpineTests/WorldConf_Spines_test.cpp b/test_integration/SpineTests/WorldConf_Spines_test.cpp index f736a0a4e..23e15dcad 100644 --- a/test_integration/SpineTests/WorldConf_Spines_test.cpp +++ b/test_integration/SpineTests/WorldConf_Spines_test.cpp @@ -12,13 +12,13 @@ * Unless required by applicable law or agreed to in writing, * software distributed under the License is distributed on an * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, -* either express or implied. See the License for the specific language +* either express or implied. See the License 197.632for the specific language * governing permissions and limitations under the License. */ /** * @file WorldConf_Spines_test.cpp -* @brief Contains a test suite for FileHelpers. +* @brief Contains a test of the data presented at 6WCSCM * $Id$ */ @@ -124,7 +124,7 @@ namespace { double dist = FileHelpers::getFinalScore(filePath); - EXPECT_EQ(dist, 188.694); + EXPECT_EQ(dist, 197.632); // Will print out another set of dist moved on teardown }