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;
break;
case Joint::CONTINUOUS:
jointType = URDF2Bullet::FloatingJoint;
jointType = URDF2Bullet::ContinuousJoint;
break;
default:
{
@@ -1429,8 +1429,8 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
int numJoints = (*robot).m_numJoints;
static bool useFeatherstone = false;
bool useUrdfInterfaceClass = false;
static bool useFeatherstone = true;
bool useUrdfInterfaceClass = true;
{
URDF2BulletMappings mappings;
@@ -1446,7 +1446,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
URDF2BulletConfig config;
config.m_createMultiBody = useFeatherstone;
//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

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_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
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("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
btScalar parentMass(1);
btVector3 parentLocalInertiaDiagonal(1,1,1);
if (urdfParentIndex==-2)
{
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("mb parent index = %d\n",mbParentIndex);
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);
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
btTransform parent2joint;
@@ -240,7 +249,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
static bool toggle=false;
//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
offsetInA = localInertialFrame.inverse()*parent2joint;
offsetInA = parentLocalInertialFrame.inverse()*parent2joint;
offsetInB.setIdentity();
//offsetInB = visual_frame.inverse();
@@ -428,6 +437,7 @@ void ConvertURDF2Bullet(const URDF2Bullet& u2b, URDF2BulletCachedData& cache, in
default:
{
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);
if (mbParentIndex>=0) //???? double-check +/- 1
if (mbLinkIndex>=0) //???? double-check +/- 1
{
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
} else

5
Demos3/ImportURDFDemo/URDF2Bullet.h
View File

@@ -1,6 +1,7 @@
#ifndef _URDF2BULLET_H
#define _URDF2BULLET_H
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include <string>
class btVector3;
class btTransform;
@@ -31,6 +32,7 @@ struct URDF2BulletCachedData
btAlignedObjectArray<int> m_urdfLinkParentIndices;
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
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)
{
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
@@ -65,6 +67,7 @@ struct URDF2BulletCachedData
btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0);
m_urdfLink2rigidBodies[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
/*
struct URDF_LinkInformation