From b14afba3505aa316d8eda4fd452ecf58d695e968 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 22 Jun 2015 15:30:57 -0700 Subject: [PATCH] more work-in-progress on joint-torque sensor sample and server/client shared memory API --- examples/MultiBody/TestJointTorqueSetup.cpp | 45 ++- examples/SharedMemory/PhysicsClient.cpp | 298 ++++++++++-------- examples/SharedMemory/PhysicsServer.cpp | 6 +- examples/SharedMemory/SharedMemoryCommands.h | 39 ++- .../Featherstone/btMultiBody.cpp | 6 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 9 + 6 files changed, 260 insertions(+), 143 deletions(-) diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index bd5b0e10a..baa39e535 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -6,7 +6,7 @@ #include "../CommonInterfaces/CommonMultiBodyBase.h" - +btScalar radius(0.2); struct TestJointTorqueSetup : public CommonMultiBodyBase { @@ -78,17 +78,21 @@ void TestJointTorqueSetup::initPhysics() //create a static ground object if (1) { - btVector3 groundHalfExtents(20,20,20); + btVector3 groundHalfExtents(1,1,0.2); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); - btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-1.5; + btVector3 groundOrigin(-0.4f, 3.f, 0.f); + btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); + groundOrigin[upAxis] -=.5; + groundOrigin[2]-=0.6; start.setOrigin(groundOrigin); + btRigidBody* body = createRigidBody(0,start,box); + body->setFriction(0); btVector4 color = colors[curColor]; curColor++; curColor&=3; @@ -99,7 +103,7 @@ void TestJointTorqueSetup::initPhysics() bool floating = false; bool damping = true; bool gyro = false; - int numLinks = 1; + int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; @@ -161,11 +165,25 @@ void TestJointTorqueSetup::initPhysics() { //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + if (i==0) + { pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + } else + { + btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + + pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, + btQuaternion(0.f, 0.f, 0.f, 1.f), + parentComToCurrentPivot, + currentPivotToCurrentCom, false); + } //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); @@ -255,7 +273,9 @@ void TestJointTorqueSetup::initPhysics() //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); + + tr.setRotation(orn); col->setWorldTransform(tr); bool isDynamic = (baseMass > 0 && floating); @@ -290,8 +310,17 @@ void TestJointTorqueSetup::initPhysics() // float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + btCollisionShape* shape =0; + + if (i==0) + { + shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); + } else + { + + shape = new btSphereShape(radius); + } - btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); m_guiHelper->createCollisionShapeGraphicsObject(shape); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); @@ -326,7 +355,7 @@ void TestJointTorqueSetup::initPhysics() void TestJointTorqueSetup::stepSimulation(float deltaTime) { //m_multiBody->addLinkForce(0,btVector3(100,100,100)); - if (0)//m_once) + if (1)//m_once) { m_once=false; m_multiBody->addJointTorque(0, 10.0); diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 7ba0e22bc..f4869046f 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -10,6 +10,7 @@ class PhysicsClient : public SharedMemoryCommon { +protected: SharedMemoryInterface* m_sharedMemory; SharedMemoryExampleData* m_testBlock1; int m_counter; @@ -19,7 +20,8 @@ class PhysicsClient : public SharedMemoryCommon bool m_serverLoadUrdfOK; bool m_waitingForServer; - + void processServerCommands(); + void createClientCommand(); public: @@ -99,7 +101,7 @@ m_wantsTermination(false), m_serverLoadUrdfOK(false), m_waitingForServer(false) { - b3Printf("Started PhysicsClient"); + b3Printf("Started PhysicsClient\n"); #ifdef _WIN32 m_sharedMemory = new Win32SharedMemoryClient(); #else @@ -109,7 +111,7 @@ m_waitingForServer(false) PhysicsClient::~PhysicsClient() { - b3Printf("~PhysicsClient"); + b3Printf("~PhysicsClient\n"); m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); delete m_sharedMemory; } @@ -117,45 +119,61 @@ PhysicsClient::~PhysicsClient() void PhysicsClient::initPhysics() { + if (m_guiHelper && m_guiHelper->getParameterInterface()) { - bool isTrigger = false; - ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger); - button.m_callback = MyCallback; - button.m_userPointer = this; - m_guiHelper->getParameterInterface()->registerButtonParameter(button); - } + { + bool isTrigger = false; + ButtonParams button("Load URDF",CMD_LOAD_URDF, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } - { - bool isTrigger = false; - ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); - button.m_callback = MyCallback; - button.m_userPointer = this; - m_guiHelper->getParameterInterface()->registerButtonParameter(button); - } + { + bool isTrigger = false; + ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + { + bool isTrigger = false; + ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + + { + bool isTrigger = false; + ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + + { + bool isTrigger = false; + ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger); + button.m_callback = MyCallback; + button.m_userPointer = this; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + } else { - bool isTrigger = false; - ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); - button.m_callback = MyCallback; - button.m_userPointer = this; - m_guiHelper->getParameterInterface()->registerButtonParameter(button); - } - /* - { - bool isTrigger = false; - ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); - button.m_callback = MyCallback; - button.m_userPointer = this; - m_guiHelper->getParameterInterface()->registerButtonParameter(button); - } - */ - { - bool isTrigger = false; - ButtonParams button("Shut Down",CMD_SHUTDOWN, isTrigger); - button.m_callback = MyCallback; - button.m_userPointer = this; - m_guiHelper->getParameterInterface()->registerButtonParameter(button); - } + m_userCommandRequests.push_back(CMD_LOAD_URDF); + m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); + //m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE); + m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); + //m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK); + //m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE); + //m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY); + m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION); + m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE); + m_userCommandRequests.push_back(CMD_SHUTDOWN); + + } m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); @@ -164,106 +182,119 @@ void PhysicsClient::initPhysics() // btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER); if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER) { - b3Error("Error: please start server before client"); + b3Error("Error: please start server before client\n"); m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_testBlock1 = 0; } else { - b3Printf("Shared Memory status is OK"); + b3Printf("Shared Memory status is OK\n"); } - } - + } else + { + m_wantsTermination = true; + } } -void PhysicsClient::stepSimulation(float deltaTime) +void PhysicsClient::processServerCommands() { - - - if (m_testBlock1) - { - //check progress and submit further commands - //we ignore overflow right now - - if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands) - { - btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1); - - const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; - - //consume the command - switch (serverCmd.m_type) - { + btAssert(m_testBlock1); - case CMD_URDF_LOADING_COMPLETED: + if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands) + { + btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1); + + const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; + + //consume the command + switch (serverCmd.m_type) + { + + case CMD_URDF_LOADING_COMPLETED: + { + m_serverLoadUrdfOK = true; + b3Printf("Server loading the URDF OK\n"); + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } + case CMD_URDF_LOADING_FAILED: + { + b3Printf("Server failed loading the URDF...\n"); + m_serverLoadUrdfOK = false; + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { - m_serverLoadUrdfOK = true; - b3Printf("Server loading the URDF OK"); + b3Printf("Received actual state\n"); + + int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ; + int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU; + b3Printf("size Q = %d, size U = %d\n", numQ,numU); + char msg[1024]; + + sprintf(msg,"Q=["); + + for (int i=0;im_actualStateQ[i]); + } else + { + sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]); + } + } + sprintf(msg,"%s]",msg); + + b3Printf(msg); + b3Printf("\n"); break; } - case CMD_STEP_FORWARD_SIMULATION_COMPLETED: - { - break; - } - case CMD_URDF_LOADING_FAILED: - { - b3Printf("Server failed loading the URDF..."); - m_serverLoadUrdfOK = false; - break; - } - case CMD_ACTUAL_STATE_UPDATE_COMPLETED: - { - b3Printf("Received actual state"); - - int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ; - int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU; - b3Printf("size Q = %d, size U = %d\n", numQ,numU); - char msg[1024]; - - sprintf(msg,"Q=["); - - for (int i=0;im_actualStateQ[i]); - } else - { - sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]); - } - } - sprintf(msg,"%s]",msg); - - b3Printf(msg); - - break; - } - default: - { - b3Error("Unknown server command"); - btAssert(0); - } - }; - - - m_testBlock1->m_numProcessedServerCommands++; - - if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands) + default: { - m_waitingForServer = false; - } else - { - m_waitingForServer = true; + b3Error("Unknown server command\n"); + btAssert(0); } - } - - if (!m_waitingForServer) + }; + + + m_testBlock1->m_numProcessedServerCommands++; + //we don't have more than 1 command outstanding (in total, either server or client) + btAssert(m_testBlock1->m_numProcessedServerCommands == m_testBlock1->m_numServerCommands); + + if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands) + { + m_waitingForServer = false; + } else + { + m_waitingForServer = true; + } + } +} + +void PhysicsClient::createClientCommand() +{ + if (!m_waitingForServer) { //process outstanding requests if (m_userCommandRequests.size()) { b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); int command = m_userCommandRequests[0]; - m_userCommandRequests.remove(command); + + //don't use 'remove' because it will re-order the commands + //m_userCommandRequests.remove(command); + //pop_front + for (int i=1;im_clientCommands[0].m_urdfArguments.m_useMultiBody = true; m_testBlock1->m_numClientCommands++; - b3Printf("Client created CMD_LOAD_URDF"); - m_waitingForServer = true; + b3Printf("Client created CMD_LOAD_URDF\n"); } else { - b3Warning("Server already loaded URDF, no client command submitted"); + b3Warning("Server already loaded URDF, no client command submitted\n"); } break; } @@ -288,13 +318,13 @@ void PhysicsClient::stepSimulation(float deltaTime) { if (m_serverLoadUrdfOK) { - b3Printf("Requesting actual state"); + b3Printf("Requesting actual state\n"); m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE; m_testBlock1->m_numClientCommands++; } else { - b3Warning("No URDF loaded"); + b3Warning("No URDF loaded\n"); } break; } @@ -307,10 +337,9 @@ void PhysicsClient::stepSimulation(float deltaTime) m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; m_testBlock1->m_numClientCommands++; b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++); - m_waitingForServer = true; } else { - b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted"); + b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n"); } break; } @@ -319,20 +348,33 @@ void PhysicsClient::stepSimulation(float deltaTime) m_wantsTermination = true; m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; m_testBlock1->m_numClientCommands++; - m_waitingForServer = true; m_serverLoadUrdfOK = false; b3Printf("client created CMD_SHUTDOWN\n"); break; } default: { - b3Error("unknown command requested"); + b3Error("unknown command requested\n"); } } } } - } +} +void PhysicsClient::stepSimulation(float deltaTime) +{ + + btAssert(m_testBlock1); + + if (m_testBlock1) + { + processServerCommands(); + + if (!m_waitingForServer) + { + createClientCommand(); + } + } } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index f252438c2..6a7b65b55 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -62,8 +62,10 @@ m_wantsShutdown(false) void PhysicsServer::releaseSharedMemory() { + b3Printf("releaseSharedMemory1\n"); if (m_testBlock1) { + b3Printf("m_testBlock1\n"); m_testBlock1->m_magicId = 0; b3Printf("magic id = %d\n",m_testBlock1->m_magicId); btAssert(m_sharedMemory); @@ -71,7 +73,7 @@ void PhysicsServer::releaseSharedMemory() } if (m_sharedMemory) { - + b3Printf("m_sharedMemory\n"); delete m_sharedMemory; m_sharedMemory = 0; m_testBlock1 = 0; @@ -343,6 +345,8 @@ void PhysicsServer::stepSimulation(float deltaTime) } if (wantsShutdown) { + b3Printf("releaseSharedMemory!\n"); + m_wantsShutdown = true; releaseSharedMemory(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 320a322fe..6c80570ba 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -4,19 +4,29 @@ //this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc) -enum SharedMemoryServerCommand{ +enum SharedMemoryServerCommand +{ CMD_URDF_LOADING_COMPLETED, CMD_URDF_LOADING_FAILED, + CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, + CMD_RIGID_BODY_CREATION_COMPLETED, + CMD_SET_JOINT_FEEDBACK_COMPLETED, CMD_ACTUAL_STATE_UPDATE_COMPLETED, -// CMD_DESIRED_STATE_RECEIVED_COMPLETED, + CMD_DESIRED_STATE_RECEIVED_COMPLETED, CMD_STEP_FORWARD_SIMULATION_COMPLETED, CMD_MAX_SERVER_COMMANDS }; -enum SharedMemoryClientCommand{ +enum SharedMemoryClientCommand +{ CMD_LOAD_URDF, + CMD_CREATE_BOX_COLLISION_SHAPE, + CMD_DELETE_BOX_COLLISION_SHAPE, + CMD_CREATE_RIGID_BODY, + CMD_DELETE_RIGID_BODY, + CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback + CMD_SEND_DESIRED_STATE, CMD_REQUEST_ACTUAL_STATE, -// CMD_SEND_DESIRED_STATE, CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE CMD_SHUTDOWN, CMD_MAX_CLIENT_COMMANDS @@ -35,6 +45,13 @@ struct UrdfArgs bool m_useFixedBase; }; +struct SetJointFeedbackArgs +{ + int m_bodyUniqueId; + int m_linkId; + bool m_isEnabled; +}; + struct SendDesiredStateArgs { int m_bodyUniqueId; @@ -45,6 +62,19 @@ struct RequestActualStateArgs int m_bodyUniqueId; }; +struct CreateBoxShapeArgs +{ + double m_halfExtents[3]; +}; + +struct CreateRigidBodyArgs +{ + int m_shapeId; + double m_initialPosition[3]; + double m_initialOrientation[3]; +}; + + struct SendActualStateArgs { int m_bodyUniqueId; @@ -53,6 +83,7 @@ struct SendActualStateArgs }; + struct StepSimulationArgs { double m_deltaTimeInSeconds; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 7225dfecf..ade1eb0ff 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1027,8 +1027,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, m_internalNeedsJointFeedback = true; m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1]; - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[i+1].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + //m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + //m_links[i].m_jointFeedback->m_reactionForces.m_topVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index ffc66d9c2..aa7cc2141 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -858,6 +858,15 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); getDebugDrawer()->drawLine(from,to,color); } + if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed) + { + btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); + + btVector4 color(0,0,0,1);//1,1,1); + btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); + btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); + getDebugDrawer()->drawLine(from,to,color); + } if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) { btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);