Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add some minor fixes for URDF2Bullet to make it work (needs more testing)

Browse Source
This commit is contained in:
erwincoumans
2015-03-18 15:12:27 -07:00
parent b35527ad5c
commit 3d19aec7fa
3 changed files with 20 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -123,7 +123,7 @@ public:
jointType = URDF2Bullet::PlanarJoint; jointType = URDF2Bullet::PlanarJoint;
break; break;
case Joint::CONTINUOUS: case Joint::CONTINUOUS:
jointType = URDF2Bullet::FloatingJoint; jointType = URDF2Bullet::ContinuousJoint;
break; break;
default: default:
{ {
@@ -1429,8 +1429,8 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
int numJoints = (*robot).m_numJoints; int numJoints = (*robot).m_numJoints;
static bool useFeatherstone = false; static bool useFeatherstone = true;
bool useUrdfInterfaceClass = false; bool useUrdfInterfaceClass = true;
{ {
URDF2BulletMappings mappings; URDF2BulletMappings mappings;
@@ -1446,7 +1446,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{ {
URDF2BulletConfig config; URDF2BulletConfig config;
config.m_createMultiBody = useFeatherstone;
//ConvertURDF2Bullet(URDF2Bullet& u2b, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,URDF2BulletConfig& config, const char* pathPrefix) //ConvertURDF2Bullet(URDF2Bullet& u2b, int linkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,URDF2BulletConfig& config, const char* pathPrefix)
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user

14
Demos3/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -94,6 +94,7 @@ void InitURDF2BulletCache(const URDF2Bullet& u2b, URDF2BulletCachedData& cache)
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase); cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase); cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase); cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1 cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
ComputeParentIndices(u2b,cache,rootLinkIndex,-2); ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
@@ -119,6 +120,11 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex); printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
printf("mb link index = %d\n",mbLinkIndex); printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
btScalar parentMass(1);
btVector3 parentLocalInertiaDiagonal(1,1,1);
if (urdfParentIndex==-2) if (urdfParentIndex==-2)
{ {
printf("root link has no parent\n"); printf("root link has no parent\n");
@@ -127,6 +133,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
printf("urdf parent index = %d\n",urdfParentIndex); printf("urdf parent index = %d\n",urdfParentIndex);
printf("mb parent index = %d\n",mbParentIndex); printf("mb parent index = %d\n",mbParentIndex);
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
} }
@@ -136,6 +143,8 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
btVector3 localInertiaDiagonal(0,0,0); btVector3 localInertiaDiagonal(0,0,0);
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
btTransform parent2joint; btTransform parent2joint;
@@ -240,7 +249,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
static bool toggle=false; static bool toggle=false;
//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint; //offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
offsetInA = localInertialFrame.inverse()*parent2joint; offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
offsetInB.setIdentity(); offsetInB.setIdentity();
//offsetInB = visual_frame.inverse(); //offsetInB = visual_frame.inverse();
@@ -428,6 +437,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
default: default:
{ {
printf("Error: unsupported joint type in URDF (%d)\n", jointType); printf("Error: unsupported joint type in URDF (%d)\n", jointType);
btAssert(0);
} }
} }
@@ -470,7 +480,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
col->setFriction(friction); col->setFriction(friction);
if (mbParentIndex>=0) //???? double-check +/- 1 if (mbLinkIndex>=0) //???? double-check +/- 1
{ {
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
} else } else

5
Demos3/ImportURDFDemo/URDF2Bullet.h
View File

@@ -1,6 +1,7 @@
#ifndef _URDF2BULLET_H #ifndef _URDF2BULLET_H
#define _URDF2BULLET_H #define _URDF2BULLET_H
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include <string> #include <string>
class btVector3; class btVector3;
class btTransform; class btTransform;
@@ -31,6 +32,7 @@ struct URDF2BulletCachedData
btAlignedObjectArray<int> m_urdfLinkParentIndices; btAlignedObjectArray<int> m_urdfLinkParentIndices;
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices; btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies; btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
int m_currentMultiBodyLinkIndex; int m_currentMultiBodyLinkIndex;
@@ -52,7 +54,7 @@ struct URDF2BulletCachedData
void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame)
{ {
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
} }
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex) class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
@@ -65,6 +67,7 @@ struct URDF2BulletCachedData
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0); btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
m_urdfLink2rigidBodies[urdfLinkIndex] = body; m_urdfLink2rigidBodies[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
/* /*
struct URDF_LinkInformation struct URDF_LinkInformation