Add preliminary PhysX 4.0 backend for PyBullet
Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py) Fix related to TinyRenderer object transforms not updating when using collision filtering
This commit is contained in:
erwincoumans
15
examples/SharedMemory/physx/PhysXC_API.cpp
Normal file
15
examples/SharedMemory/physx/PhysXC_API.cpp
Normal file
@@ -0,0 +1,15 @@
|
||||
#ifdef BT_ENABLE_PHYSX
|
||||
#include "PhysXC_API.h"
|
||||
#include "PhysXServerCommandProcessor.h"
|
||||
#include "PhysXClient.h"
|
||||
|
||||
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX()
|
||||
{
|
||||
PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor;
|
||||
|
||||
PhysXClient* direct = new PhysXClient(sdk, true);
|
||||
bool connected;
|
||||
connected = direct->connect();
|
||||
return (b3PhysicsClientHandle)direct;
|
||||
}
|
||||
#endif //BT_ENABLE_PHYSX
|
||||
20
examples/SharedMemory/physx/PhysXC_API.h
Normal file
20
examples/SharedMemory/physx/PhysXC_API.h
Normal file
@@ -0,0 +1,20 @@
|
||||
#ifndef PHYSX_C_API_H
|
||||
#define PHYSX_C_API_H
|
||||
|
||||
#ifdef BT_ENABLE_PHYSX
|
||||
|
||||
#include "../PhysicsClientC_API.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX();
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //BT_ENABLE_PHYSX
|
||||
#endif //PHYSX_C_API_H
|
||||
1409
examples/SharedMemory/physx/PhysXClient.cpp
Normal file
1409
examples/SharedMemory/physx/PhysXClient.cpp
Normal file
File diff suppressed because it is too large
Load Diff
135
examples/SharedMemory/physx/PhysXClient.h
Normal file
135
examples/SharedMemory/physx/PhysXClient.h
Normal file
@@ -0,0 +1,135 @@
|
||||
#ifndef PHYSX_CLIENT_H
|
||||
#define PHYSX_CLIENT_H
|
||||
|
||||
#include "../PhysicsClient.h"
|
||||
|
||||
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
|
||||
class PhysXClient : public PhysicsClient
|
||||
{
|
||||
protected:
|
||||
struct PhysXDirectInternalData* m_data;
|
||||
|
||||
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
bool processCamera(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
|
||||
|
||||
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
|
||||
|
||||
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
|
||||
|
||||
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
|
||||
|
||||
void resetData();
|
||||
|
||||
void removeCachedBody(int bodyUniqueId);
|
||||
|
||||
public:
|
||||
PhysXClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
|
||||
|
||||
virtual ~PhysXClient();
|
||||
|
||||
// return true if connection succesfull, can also check 'isConnected'
|
||||
//it is OK to pass a null pointer for the gui helper
|
||||
virtual bool connect();
|
||||
|
||||
////todo: rename to 'disconnect'
|
||||
virtual void disconnectSharedMemory();
|
||||
|
||||
virtual bool isConnected() const;
|
||||
|
||||
// return non-null if there is a status, nullptr otherwise
|
||||
virtual const SharedMemoryStatus* processServerStatus();
|
||||
|
||||
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
|
||||
|
||||
virtual bool canSubmitCommand() const;
|
||||
|
||||
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
|
||||
|
||||
virtual int getNumBodies() const;
|
||||
|
||||
virtual int getBodyUniqueId(int serialIndex) const;
|
||||
|
||||
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
|
||||
|
||||
virtual int getNumJoints(int bodyIndex) const;
|
||||
|
||||
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
|
||||
|
||||
virtual int getNumUserConstraints() const;
|
||||
|
||||
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
|
||||
|
||||
virtual int getUserConstraintId(int serialIndex) const;
|
||||
|
||||
///todo: move this out of the
|
||||
virtual void setSharedMemoryKey(int key);
|
||||
|
||||
void uploadBulletFileToSharedMemory(const char* data, int len);
|
||||
|
||||
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
|
||||
|
||||
virtual int getNumDebugLines() const;
|
||||
|
||||
virtual const float* getDebugLinesFrom() const;
|
||||
virtual const float* getDebugLinesTo() const;
|
||||
virtual const float* getDebugLinesColor() const;
|
||||
|
||||
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
|
||||
|
||||
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
|
||||
|
||||
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
|
||||
|
||||
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
|
||||
|
||||
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
|
||||
|
||||
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
|
||||
|
||||
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);
|
||||
|
||||
virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData);
|
||||
|
||||
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
|
||||
|
||||
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
|
||||
|
||||
//the following APIs are for internal use for visualization:
|
||||
virtual bool connect(struct GUIHelperInterface* guiHelper);
|
||||
virtual void renderScene();
|
||||
virtual void debugDraw(int debugDrawMode);
|
||||
|
||||
virtual void setTimeOut(double timeOutInSeconds);
|
||||
virtual double getTimeOut() const;
|
||||
|
||||
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
virtual int getNumUserData(int bodyUniqueId) const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
|
||||
{
|
||||
*keyOut = 0;
|
||||
*userDataIdOut = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
virtual void pushProfileTiming(const char* timingName);
|
||||
virtual void popProfileTiming();
|
||||
};
|
||||
|
||||
#endif //PHYSX_CLIENT_H
|
||||
1639
examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
Normal file
1639
examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
Normal file
File diff suppressed because it is too large
Load Diff
47
examples/SharedMemory/physx/PhysXServerCommandProcessor.h
Normal file
47
examples/SharedMemory/physx/PhysXServerCommandProcessor.h
Normal file
@@ -0,0 +1,47 @@
|
||||
#ifndef PHYSX_SERVER_COMMAND_PROCESSOR_H
|
||||
#define PHYSX_SERVER_COMMAND_PROCESSOR_H
|
||||
|
||||
#include "../PhysicsCommandProcessorInterface.h"
|
||||
|
||||
class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface
|
||||
{
|
||||
struct PhysXServerCommandProcessorInternalData* m_data;
|
||||
|
||||
bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
void resetSimulation();
|
||||
bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
public:
|
||||
PhysXServerCommandProcessor();
|
||||
|
||||
virtual ~PhysXServerCommandProcessor();
|
||||
|
||||
virtual bool connect();
|
||||
|
||||
virtual void disconnect();
|
||||
|
||||
virtual bool isConnected() const;
|
||||
|
||||
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
virtual void renderScene(int renderFlags) {}
|
||||
virtual void physicsDebugDraw(int debugDrawFlags) {}
|
||||
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) {}
|
||||
virtual void setTimeOut(double timeOutInSeconds) {}
|
||||
|
||||
virtual void reportNotifications() {}
|
||||
};
|
||||
|
||||
#endif //PHYSX_SERVER_COMMAND_PROCESSOR_H
|
||||
1434
examples/SharedMemory/physx/PhysXUrdfImporter.cpp
Normal file
1434
examples/SharedMemory/physx/PhysXUrdfImporter.cpp
Normal file
File diff suppressed because it is too large
Load Diff
116
examples/SharedMemory/physx/PhysXUrdfImporter.h
Normal file
116
examples/SharedMemory/physx/PhysXUrdfImporter.h
Normal file
@@ -0,0 +1,116 @@
|
||||
#ifndef PHYSX_URDF_IMPORTER_H
|
||||
#define PHYSX_URDF_IMPORTER_H
|
||||
|
||||
#include "../../Importers/ImportURDFDemo/URDFImporterInterface.h"
|
||||
#include "../../Importers/ImportURDFDemo/UrdfRenderingInterface.h"
|
||||
|
||||
struct PhysXURDFTexture
|
||||
{
|
||||
int m_width;
|
||||
int m_height;
|
||||
unsigned char* textureData1;
|
||||
bool m_isCached;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
///PhysXURDFImporter can deal with URDF and SDF files
|
||||
class PhysXURDFImporter : public URDFImporterInterface
|
||||
{
|
||||
struct PhysXURDFInternalData* m_data;
|
||||
|
||||
public:
|
||||
PhysXURDFImporter(struct CommonFileIOInterface* fileIO=0,double globalScaling=1, int flags=0);
|
||||
|
||||
virtual ~PhysXURDFImporter();
|
||||
|
||||
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
|
||||
|
||||
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
|
||||
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
|
||||
virtual int getNumModels() const;
|
||||
virtual void activateModel(int modelIndex);
|
||||
virtual void setBodyUniqueId(int bodyId);
|
||||
virtual int getBodyUniqueId() const;
|
||||
const char* getPathPrefix();
|
||||
|
||||
void printTree(); //for debugging
|
||||
|
||||
virtual int getRootLinkIndex() const;
|
||||
|
||||
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
|
||||
|
||||
virtual std::string getBodyName() const;
|
||||
|
||||
virtual std::string getLinkName(int linkIndex) const;
|
||||
|
||||
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
|
||||
|
||||
virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const;
|
||||
|
||||
virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
|
||||
|
||||
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const;
|
||||
|
||||
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
|
||||
|
||||
virtual std::string getJointName(int linkIndex) const;
|
||||
|
||||
virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
||||
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
|
||||
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
|
||||
|
||||
|
||||
|
||||
virtual int convertLinkVisualShapes3(
|
||||
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
||||
const struct UrdfLink* linkPtr, const UrdfModel* model,
|
||||
int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
|
||||
class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
|
||||
|
||||
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||
|
||||
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const;
|
||||
|
||||
virtual const struct UrdfModel* getUrdfModel() const;
|
||||
|
||||
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
|
||||
|
||||
virtual int getNumAllocatedCollisionShapes() const;
|
||||
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
|
||||
|
||||
virtual int getNumAllocatedMeshInterfaces() const;
|
||||
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
|
||||
|
||||
virtual int getNumAllocatedTextures() const;
|
||||
virtual int getAllocatedTexture(int index) const;
|
||||
|
||||
virtual void setEnableTinyRenderer(bool enable);
|
||||
//void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct PhysXURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
|
||||
|
||||
|
||||
int getNumPhysXLinks() const;
|
||||
|
||||
};
|
||||
|
||||
#endif //PHYSX_URDF_IMPORTER_H
|
||||
8
examples/SharedMemory/physx/PhysXUserData.h
Normal file
8
examples/SharedMemory/physx/PhysXUserData.h
Normal file
@@ -0,0 +1,8 @@
|
||||
#ifndef PHYSX_USER_DATA_H
|
||||
#define PHYSX_USER_DATA_H
|
||||
|
||||
struct MyPhysXUserData
|
||||
{
|
||||
int m_graphicsUniqueId;
|
||||
};
|
||||
#endif //PHYSX_USER_DATA_H
|
||||
908
examples/SharedMemory/physx/URDF2PhysX.cpp
Normal file
908
examples/SharedMemory/physx/URDF2PhysX.cpp
Normal file
@@ -0,0 +1,908 @@
|
||||
|
||||
#include "URDF2PhysX.h"
|
||||
|
||||
#include "PhysXUrdfImporter.h"
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
#include "PxArticulationReducedCoordinate.h"
|
||||
#include "PxArticulationJointReducedCoordinate.h"
|
||||
#include "PxRigidActorExt.h"
|
||||
#include "PxArticulation.h"
|
||||
#include "PxRigidBodyExt.h"
|
||||
#include "PxRigidBody.h"
|
||||
#include "LinearMath/btThreads.h"
|
||||
#include "PxRigidActorExt.h"
|
||||
#include "PxArticulationBase.h"
|
||||
#include "PxArticulationLink.h"
|
||||
#include "PxMaterial.h"
|
||||
#include "PxCooking.h"
|
||||
#include "PxScene.h"
|
||||
#include "PxAggregate.h"
|
||||
|
||||
#include "PhysXUserData.h"
|
||||
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||
|
||||
#include "Importers/ImportURDFDemo/UrdfParser.h"
|
||||
|
||||
|
||||
|
||||
|
||||
struct URDF2PhysXCachedData
|
||||
{
|
||||
URDF2PhysXCachedData()
|
||||
: m_currentMultiBodyLinkIndex(-1),
|
||||
m_articulation(0),
|
||||
m_totalNumJoints1(0)
|
||||
{
|
||||
}
|
||||
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
||||
|
||||
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
||||
btAlignedObjectArray<class physx::PxArticulationLink*> m_urdfLink2physxLink;
|
||||
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
||||
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
physx::PxArticulationReducedCoordinate* m_articulation;
|
||||
|
||||
btAlignedObjectArray<physx::PxTransform> m_linkTransWorldSpace;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndex;
|
||||
btAlignedObjectArray<int> m_parentUrdfLinkIndex;
|
||||
btAlignedObjectArray<physx::PxReal> m_linkMasses;
|
||||
btAlignedObjectArray<physx::PxArticulationJointType::Enum> m_jointTypes;
|
||||
|
||||
btAlignedObjectArray<physx::PxTransform> m_parentLocalPoses;
|
||||
btAlignedObjectArray<physx::PxTransform> m_childLocalPoses;
|
||||
btAlignedObjectArray<UrdfGeomTypes> m_geomTypes;
|
||||
btAlignedObjectArray<physx::PxVec3> m_geomDimensions;
|
||||
btAlignedObjectArray<physx::PxVec3> m_linkMaterials;
|
||||
btAlignedObjectArray<physx::PxTransform> m_geomLocalPoses;
|
||||
|
||||
|
||||
|
||||
//this will be initialized in the constructor
|
||||
int m_totalNumJoints1;
|
||||
int getParentUrdfIndex(int linkIndex) const
|
||||
{
|
||||
return m_urdfLinkParentIndices[linkIndex];
|
||||
}
|
||||
int getMbIndexFromUrdfIndex(int urdfIndex) const
|
||||
{
|
||||
if (urdfIndex == -2)
|
||||
return -2;
|
||||
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
|
||||
}
|
||||
|
||||
void registerMultiBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& localInertialFrame)
|
||||
{
|
||||
m_urdfLink2physxLink[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
|
||||
class physx::PxArticulationLink* getPhysxLinkFromLink(int urdfLinkIndex)
|
||||
{
|
||||
return m_urdfLink2physxLink[urdfLinkIndex];
|
||||
}
|
||||
|
||||
void registerRigidBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
||||
{
|
||||
btAssert(m_urdfLink2physxLink[urdfLinkIndex] == 0);
|
||||
|
||||
m_urdfLink2physxLink[urdfLinkIndex] = body;
|
||||
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
||||
{
|
||||
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
|
||||
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
|
||||
for (int i = 0; i < childIndices.size(); i++)
|
||||
{
|
||||
ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int linkIndex)
|
||||
{
|
||||
btAlignedObjectArray<int> childIndices;
|
||||
u2b.getLinkChildIndices(linkIndex, childIndices);
|
||||
cache.m_totalNumJoints1 += childIndices.size();
|
||||
for (int i = 0; i < childIndices.size(); i++)
|
||||
{
|
||||
int childIndex = childIndices[i];
|
||||
ComputeTotalNumberOfJoints(u2b, cache, childIndex);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int flags)
|
||||
{
|
||||
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
||||
cache.m_totalNumJoints1 = 0;
|
||||
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
if (rootLinkIndex >= 0)
|
||||
{
|
||||
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
|
||||
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
|
||||
|
||||
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLink2physxLink.resize(numTotalLinksIncludingBase);
|
||||
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
|
||||
|
||||
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
|
||||
|
||||
bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0;
|
||||
if (maintainLinkOrder)
|
||||
{
|
||||
URDF2PhysXCachedData cache2 = cache;
|
||||
|
||||
ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
|
||||
|
||||
for (int j = 0; j<numTotalLinksIncludingBase; j++)
|
||||
{
|
||||
cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
|
||||
cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
struct ArticulationCreationInterface
|
||||
{
|
||||
physx::PxFoundation* m_foundation;
|
||||
physx::PxPhysics* m_physics;
|
||||
physx::PxCooking* m_cooking;
|
||||
physx::PxScene* m_scene;
|
||||
CommonFileIOInterface* m_fileIO;
|
||||
|
||||
b3AlignedObjectArray<int> m_mb2urdfLink;
|
||||
void addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
||||
{
|
||||
if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
|
||||
{
|
||||
m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
|
||||
}
|
||||
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
||||
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
||||
}
|
||||
};
|
||||
|
||||
static btVector4 colors4[4] =
|
||||
{
|
||||
btVector4(1, 0, 0, 1),
|
||||
btVector4(0, 1, 0, 1),
|
||||
btVector4(0, 1, 1, 1),
|
||||
btVector4(1, 1, 0, 1),
|
||||
};
|
||||
|
||||
static btVector4 selectColor4()
|
||||
{
|
||||
static btSpinMutex sMutex;
|
||||
sMutex.lock();
|
||||
static int curColor = 0;
|
||||
btVector4 color = colors4[curColor];
|
||||
curColor++;
|
||||
curColor &= 3;
|
||||
sMutex.unlock();
|
||||
return color;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
static physx::PxConvexMesh* createPhysXConvex(physx::PxU32 numVerts, const physx::PxVec3* verts, ArticulationCreationInterface& creation)
|
||||
{
|
||||
physx::PxCookingParams params = creation.m_cooking->getParams();
|
||||
|
||||
// Use the new (default) PxConvexMeshCookingType::eQUICKHULL
|
||||
params.convexMeshCookingType = physx::PxConvexMeshCookingType::eQUICKHULL;
|
||||
|
||||
// If the gaussMapLimit is chosen higher than the number of output vertices, no gauss map is added to the convex mesh data (here 256).
|
||||
// If the gaussMapLimit is chosen lower than the number of output vertices, a gauss map is added to the convex mesh data (here 16).
|
||||
int gaussMapLimit = 256;
|
||||
|
||||
params.gaussMapLimit = gaussMapLimit;
|
||||
creation.m_cooking->setParams(params);
|
||||
|
||||
// Setup the convex mesh descriptor
|
||||
physx::PxConvexMeshDesc desc;
|
||||
|
||||
// We provide points only, therefore the PxConvexFlag::eCOMPUTE_CONVEX flag must be specified
|
||||
desc.points.data = verts;
|
||||
desc.points.count = numVerts;
|
||||
desc.points.stride = sizeof(physx::PxVec3);
|
||||
desc.flags = physx::PxConvexFlag::eCOMPUTE_CONVEX;
|
||||
|
||||
physx::PxU32 meshSize = 0;
|
||||
|
||||
// Directly insert mesh into PhysX
|
||||
physx::PxConvexMesh* convex = creation.m_cooking->createConvexMesh(desc, creation.m_physics->getPhysicsInsertionCallback());
|
||||
PX_ASSERT(convex);
|
||||
|
||||
return convex;
|
||||
}
|
||||
|
||||
|
||||
int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, ArticulationCreationInterface& creation, int urdfLinkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
||||
physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxArticulationLink* linkPtr)
|
||||
{
|
||||
int numShapes = 0;
|
||||
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
|
||||
//static friction, dynamic frictoin, restitution
|
||||
cache.m_linkMaterials.push_back(physx::PxVec3(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution));
|
||||
physx::PxMaterial* material = creation.m_physics->createMaterial(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution);
|
||||
|
||||
const UrdfLink* link = u2b.getUrdfLink(urdfLinkIndex);//.convertLinkCollisionShapes m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
|
||||
btAssert(linkPtr);
|
||||
if (linkPtr)
|
||||
{
|
||||
|
||||
for (int v = 0; v < link->m_collisionArray.size(); v++)
|
||||
{
|
||||
const UrdfCollision& col = link->m_collisionArray[v];
|
||||
const UrdfCollision* collision = &col;
|
||||
btTransform childTrans = col.m_linkLocalFrame;
|
||||
btTransform localTrans = localInertiaFrame.inverse()*childTrans;
|
||||
physx::PxTransform tr;
|
||||
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
|
||||
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
|
||||
|
||||
physx::PxShape* shape = 0;
|
||||
cache.m_geomTypes.push_back(col.m_geometry.m_type);
|
||||
|
||||
|
||||
switch (col.m_geometry.m_type)
|
||||
{
|
||||
case URDF_GEOM_PLANE:
|
||||
{
|
||||
btVector3 planeNormal = col.m_geometry.m_planeNormal;
|
||||
btScalar planeConstant = 0; //not available?
|
||||
//PxPlane(1, 0, 0, 0).
|
||||
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material);
|
||||
//todo: compensate for plane axis
|
||||
//physx::PxTransform localPose = tr*physx::PxTransform
|
||||
//shape->setLocalPose(localPose);
|
||||
numShapes++;
|
||||
break;
|
||||
}
|
||||
case URDF_GEOM_CAPSULE:
|
||||
{
|
||||
btScalar radius = collision->m_geometry.m_capsuleRadius;
|
||||
btScalar height = collision->m_geometry.m_capsuleHeight;
|
||||
|
||||
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material);
|
||||
|
||||
btTransform childTrans = col.m_linkLocalFrame;
|
||||
btTransform x2z;
|
||||
x2z.setIdentity();
|
||||
x2z.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
|
||||
btTransform localTrans = localInertiaFrame.inverse()*childTrans*x2z;
|
||||
physx::PxTransform tr;
|
||||
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
|
||||
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
|
||||
|
||||
shape->setLocalPose(tr);
|
||||
cache.m_geomLocalPoses.push_back(tr);
|
||||
numShapes++;
|
||||
//btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height);
|
||||
//shape = capsuleShape;
|
||||
//shape->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_CYLINDER:
|
||||
{
|
||||
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
|
||||
btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight;
|
||||
cache.m_geomDimensions.push_back(physx::PxVec3(cylRadius, cylHalfLength,0));
|
||||
//if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
|
||||
//{
|
||||
// btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength);
|
||||
// btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
|
||||
// shape = cylZShape;
|
||||
//}
|
||||
//else
|
||||
{
|
||||
btAlignedObjectArray<physx::PxVec3> vertices;
|
||||
|
||||
int numSteps = 32;
|
||||
for (int i = 0; i < numSteps; i++)
|
||||
{
|
||||
physx::PxVec3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength);
|
||||
vertices.push_back(vert);
|
||||
vert[2] = -cylHalfLength;
|
||||
vertices.push_back(vert);
|
||||
}
|
||||
|
||||
physx::PxConvexMesh* convexMesh = createPhysXConvex(vertices.size(), &vertices[0], creation);
|
||||
|
||||
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr,
|
||||
physx::PxConvexMeshGeometry(convexMesh), *material);
|
||||
|
||||
shape->setLocalPose(tr);
|
||||
cache.m_geomLocalPoses.push_back(tr);
|
||||
numShapes++;
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
btVector3 extents = collision->m_geometry.m_boxSize;
|
||||
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxBoxGeometry(extents[0] * 0.5, extents[1] * 0.5, extents[2] * 0.5), *material);
|
||||
cache.m_geomDimensions.push_back(physx::PxVec3(extents[0]*0.5, extents[1] * 0.5, extents[2] * 0.5));
|
||||
shape->setLocalPose(tr);
|
||||
cache.m_geomLocalPoses.push_back(tr);
|
||||
numShapes++;
|
||||
break;
|
||||
}
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
btScalar radius = collision->m_geometry.m_sphereRadius;
|
||||
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxSphereGeometry(radius), *material);
|
||||
cache.m_geomDimensions.push_back(physx::PxVec3(radius,0,0));
|
||||
shape->setLocalPose(tr);
|
||||
cache.m_geomLocalPoses.push_back(tr);
|
||||
numShapes++;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
if (shape)
|
||||
{
|
||||
//see https://github.com/NVIDIAGameWorks/PhysX/issues/21
|
||||
physx::PxReal contactOffset = shape->getContactOffset();
|
||||
physx::PxReal restOffset = shape->getContactOffset();
|
||||
|
||||
//shape->setContactOffset(physx::PxReal(.03));
|
||||
//shape->setRestOffset(physx::PxReal(.01)); //
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
return numShapes;
|
||||
}
|
||||
|
||||
btTransform ConvertURDF2PhysXInternal(
|
||||
const PhysXURDFImporter& u2b,
|
||||
ArticulationCreationInterface& creation,
|
||||
URDF2PhysXCachedData& cache, int urdfLinkIndex,
|
||||
const btTransform& parentTransformInWorldSpace,
|
||||
bool createMultiBody, const char* pathPrefix,
|
||||
int flags,
|
||||
UrdfVisualShapeCache2* cachedLinkGraphicsShapesIn,
|
||||
UrdfVisualShapeCache2* cachedLinkGraphicsShapesOut,
|
||||
bool recursive)
|
||||
{
|
||||
B3_PROFILE("ConvertURDF2PhysXInternal");
|
||||
//b3Printf("start converting/extracting data from URDF interface\n");
|
||||
|
||||
btTransform linkTransformInWorldSpace;
|
||||
linkTransformInWorldSpace.setIdentity();
|
||||
|
||||
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
||||
|
||||
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
||||
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
||||
|
||||
|
||||
|
||||
//b3Printf("mb link index = %d\n",mbLinkIndex);
|
||||
|
||||
btTransform parentLocalInertialFrame;
|
||||
parentLocalInertialFrame.setIdentity();
|
||||
btScalar parentMass(1);
|
||||
btVector3 parentLocalInertiaDiagonal(1, 1, 1);
|
||||
|
||||
if (urdfParentIndex == -2)
|
||||
{
|
||||
//b3Printf("root link has no parent\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
|
||||
//b3Printf("mb parent index = %d\n",mbParentIndex);
|
||||
//parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
||||
u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
|
||||
}
|
||||
|
||||
btScalar mass = 0;
|
||||
btTransform localInertialFrame;
|
||||
localInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0, 0, 0);
|
||||
u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
|
||||
|
||||
btTransform parent2joint;
|
||||
parent2joint.setIdentity();
|
||||
|
||||
int jointType;
|
||||
btVector3 jointAxisInJointSpace;
|
||||
btScalar jointLowerLimit;
|
||||
btScalar jointUpperLimit;
|
||||
btScalar jointDamping;
|
||||
btScalar jointFriction;
|
||||
btScalar jointMaxForce;
|
||||
btScalar jointMaxVelocity;
|
||||
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
|
||||
|
||||
btTransform axis2Reference;
|
||||
axis2Reference.setIdentity();
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
case URDFContinuousJoint:
|
||||
case URDFPrismaticJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
//rotate from revolute 'axis' to standard X axis
|
||||
btVector3 refAxis(1, 0, 0);
|
||||
btVector3 axis = jointAxisInJointSpace;
|
||||
//btQuaternion axis2ReferenceRot(btVector3(0, 1, 0), SIMD_HALF_PI);// = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
|
||||
btQuaternion axis2ReferenceRot = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
|
||||
axis2Reference.setRotation(axis2ReferenceRot);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
parent2joint = parent2joint*axis2Reference;
|
||||
//localInertialFrame = axis2Reference.inverse()*localInertialFrame;
|
||||
|
||||
std::string linkName = u2b.getLinkName(urdfLinkIndex);
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
{
|
||||
parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (flags & CUF_USE_MJCF)
|
||||
{
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
|
||||
}
|
||||
else
|
||||
{
|
||||
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int graphicsIndex;
|
||||
{
|
||||
B3_PROFILE("convertLinkVisualShapes");
|
||||
|
||||
|
||||
|
||||
graphicsIndex = u2b.convertLinkVisualShapes3(urdfLinkIndex, pathPrefix, localInertialFrame, u2b.getUrdfLink(urdfLinkIndex), u2b.getUrdfModel(), -1, u2b.getBodyUniqueId(), creation.m_fileIO);
|
||||
|
||||
|
||||
#if 0
|
||||
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
|
||||
{
|
||||
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
|
||||
UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
|
||||
u2b.setLinkColor2(urdfLinkIndex, matColor);
|
||||
}
|
||||
else
|
||||
{
|
||||
graphicsIndex =
|
||||
if (cachedLinkGraphicsShapesOut)
|
||||
{
|
||||
cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex);
|
||||
UrdfMaterialColor matColor;
|
||||
u2b.getLinkColor2(urdfLinkIndex, matColor);
|
||||
cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if (1)
|
||||
{
|
||||
UrdfMaterialColor matColor;
|
||||
btVector4 color2 = selectColor4();
|
||||
btVector3 specular(0.5, 0.5, 0.5);
|
||||
if (u2b.getLinkColor2(urdfLinkIndex, matColor))
|
||||
{
|
||||
color2 = matColor.m_rgbaColor;
|
||||
specular = matColor.m_specularColor;
|
||||
}
|
||||
|
||||
/*
|
||||
if (visual->material.get())
|
||||
{
|
||||
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
||||
}
|
||||
*/
|
||||
if (mass)
|
||||
{
|
||||
if (!(flags & CUF_USE_URDF_INERTIA))
|
||||
{
|
||||
//b3Assert(0);
|
||||
//compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
|
||||
btAssert(localInertiaDiagonal[0] < 1e10);
|
||||
btAssert(localInertiaDiagonal[1] < 1e10);
|
||||
btAssert(localInertiaDiagonal[2] < 1e10);
|
||||
}
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
||||
//temporary inertia scaling until we load inertia from URDF
|
||||
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
|
||||
{
|
||||
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
|
||||
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
|
||||
|
||||
physx::PxArticulationLink* linkPtr = 0;
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
physx::PxTransform tr;
|
||||
tr.p = physx::PxVec3(linkTransformInWorldSpace.getOrigin().x(), linkTransformInWorldSpace.getOrigin().y(), linkTransformInWorldSpace.getOrigin().z());
|
||||
tr.q = physx::PxQuat(linkTransformInWorldSpace.getRotation().x(), linkTransformInWorldSpace.getRotation().y(), linkTransformInWorldSpace.getRotation().z(), linkTransformInWorldSpace.getRotation().w());
|
||||
|
||||
cache.m_linkTransWorldSpace.push_back(tr);
|
||||
cache.m_urdfLinkIndex.push_back(urdfLinkIndex);
|
||||
cache.m_parentUrdfLinkIndex.push_back(urdfParentIndex);
|
||||
cache.m_linkMasses.push_back(mass);
|
||||
|
||||
if (cache.m_articulation == 0)
|
||||
{
|
||||
|
||||
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
|
||||
|
||||
cache.m_articulation = creation.m_physics->createArticulationReducedCoordinate();
|
||||
|
||||
if (isFixedBase)
|
||||
{
|
||||
cache.m_articulation->setArticulationFlags(physx::PxArticulationFlag::eFIX_BASE);
|
||||
}
|
||||
|
||||
|
||||
|
||||
physx::PxArticulationLink* base = cache.m_articulation->createLink(NULL,tr);
|
||||
|
||||
linkPtr = base;
|
||||
|
||||
|
||||
//physx::PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial);
|
||||
physx::PxRigidBody& body = *base;
|
||||
|
||||
//Now create the slider and fixed joints...
|
||||
|
||||
cache.m_articulation->setSolverIterationCounts(32);//todo: API?
|
||||
|
||||
cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED);
|
||||
cache.m_parentLocalPoses.push_back(physx::PxTransform());
|
||||
cache.m_childLocalPoses.push_back(physx::PxTransform());
|
||||
|
||||
// Stabilization can create artefacts on jointed objects so we just disable it
|
||||
//cache.m_articulation->setStabilizationThreshold(0.0f);
|
||||
//cache.m_articulation->setMaxProjectionIterations(16);
|
||||
//cache.m_articulation->setSeparationTolerance(0.001f);
|
||||
|
||||
#if 0
|
||||
int totalNumJoints = cache.m_totalNumJoints1;
|
||||
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
|
||||
if (flags & CUF_GLOBAL_VELOCITIES_MB)
|
||||
{
|
||||
cache.m_bulletMultiBody->useGlobalVelocities(true);
|
||||
}
|
||||
if (flags & CUF_USE_MJCF)
|
||||
{
|
||||
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
physx::PxArticulationLink* parentLink = cache.getPhysxLinkFromLink(urdfParentIndex);
|
||||
linkPtr = cache.m_articulation->createLink(parentLink, tr);
|
||||
|
||||
physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(linkPtr->getInboundJoint());
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
joint->setJointType(physx::PxArticulationJointType::eFIX);
|
||||
break;
|
||||
}
|
||||
case URDFSphericalJoint:
|
||||
{
|
||||
joint->setJointType(physx::PxArticulationJointType::eSPHERICAL);
|
||||
joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
|
||||
joint->setMotion(physx::PxArticulationAxis::eSWING2, physx::PxArticulationMotion::eFREE);
|
||||
joint->setMotion(physx::PxArticulationAxis::eSWING1, physx::PxArticulationMotion::eFREE);
|
||||
break;
|
||||
}
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
joint->setJointType(physx::PxArticulationJointType::eREVOLUTE);
|
||||
joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
|
||||
|
||||
break;
|
||||
}
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
joint->setJointType(physx::PxArticulationJointType::ePRISMATIC);
|
||||
joint->setMotion(physx::PxArticulationAxis::eX, physx::PxArticulationMotion::eFREE);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
joint->setJointType(physx::PxArticulationJointType::eFIX);
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
|
||||
cache.m_jointTypes.push_back(joint->getJointType());
|
||||
btTransform offsetInA, offsetInB;
|
||||
|
||||
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
|
||||
offsetInB = (axis2Reference.inverse()*localInertialFrame).inverse();
|
||||
|
||||
physx::PxTransform parentPose(physx::PxVec3(offsetInA.getOrigin()[0], offsetInA.getOrigin()[1], offsetInA.getOrigin()[2]),
|
||||
physx::PxQuat(offsetInA.getRotation()[0], offsetInA.getRotation()[1], offsetInA.getRotation()[2], offsetInA.getRotation()[3]));
|
||||
|
||||
physx::PxTransform childPose(physx::PxVec3(offsetInB.getOrigin()[0], offsetInB.getOrigin()[1], offsetInB.getOrigin()[2]),
|
||||
physx::PxQuat(offsetInB.getRotation()[0], offsetInB.getRotation()[1], offsetInB.getRotation()[2], offsetInB.getRotation()[3]));
|
||||
|
||||
cache.m_parentLocalPoses.push_back(parentPose);
|
||||
cache.m_childLocalPoses.push_back(childPose);
|
||||
|
||||
joint->setParentPose(parentPose);
|
||||
joint->setChildPose(childPose);
|
||||
|
||||
}
|
||||
cache.registerMultiBody(urdfLinkIndex, linkPtr, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
|
||||
if (linkPtr)
|
||||
{
|
||||
//todo: mem leaks
|
||||
MyPhysXUserData* userData = new MyPhysXUserData();
|
||||
userData->m_graphicsUniqueId = graphicsIndex;
|
||||
linkPtr->userData = userData;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//create collision shapes
|
||||
|
||||
//physx::PxRigidActorExt::createExclusiveShape
|
||||
convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
|
||||
|
||||
|
||||
physx::PxRigidBodyExt::updateMassAndInertia(*linkPtr, mass);
|
||||
|
||||
|
||||
//base->setMass(massOut);
|
||||
//base->setMassSpaceInertiaTensor(diagTensor);
|
||||
//base->setCMassLocalPose(PxTransform(com, orient));
|
||||
|
||||
|
||||
//create a joint if necessary
|
||||
if (hasParentJoint)
|
||||
{
|
||||
btTransform offsetInA, offsetInB;
|
||||
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
|
||||
offsetInB = localInertialFrame.inverse();
|
||||
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
||||
|
||||
bool disableParentCollision = true;
|
||||
|
||||
if (createMultiBody && cache.m_articulation)
|
||||
{
|
||||
#if 0
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
}
|
||||
}//was if 'shape/compountShape'
|
||||
|
||||
btAlignedObjectArray<int> urdfChildIndices;
|
||||
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
|
||||
|
||||
int numChildren = urdfChildIndices.size();
|
||||
|
||||
if (recursive)
|
||||
{
|
||||
for (int i = 0; i < numChildren; i++)
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
|
||||
}
|
||||
}
|
||||
return linkTransformInWorldSpace;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO)
|
||||
{
|
||||
URDF2PhysXCachedData cache;
|
||||
InitURDF2BulletCache(u2p, cache, flags);
|
||||
int urdfLinkIndex = u2p.getRootLinkIndex();
|
||||
int rootIndex = u2p.getRootLinkIndex();
|
||||
B3_PROFILE("ConvertURDF2Bullet");
|
||||
|
||||
UrdfVisualShapeCache2 cachedLinkGraphicsShapesOut;
|
||||
UrdfVisualShapeCache2 cachedLinkGraphicsShapes;
|
||||
|
||||
ArticulationCreationInterface creation;
|
||||
creation.m_foundation = foundation;
|
||||
creation.m_physics = physics;
|
||||
creation.m_cooking = cooking;
|
||||
creation.m_scene = scene;
|
||||
creation.m_fileIO = fileIO;
|
||||
|
||||
bool createMultiBody = true;
|
||||
|
||||
bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER) == 0;
|
||||
|
||||
if (recursive)
|
||||
{
|
||||
ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createMultiBody, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
#if 0
|
||||
btAlignedObjectArray<btTransform> parentTransforms;
|
||||
if (urdfLinkIndex >= parentTransforms.size())
|
||||
{
|
||||
parentTransforms.resize(urdfLinkIndex + 1);
|
||||
}
|
||||
parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
|
||||
btAlignedObjectArray<childParentIndex> allIndices;
|
||||
|
||||
GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
|
||||
allIndices.quickSort(MyIntCompareFunc);
|
||||
|
||||
for (int i = 0; i < allIndices.size(); i++)
|
||||
{
|
||||
int urdfLinkIndex = allIndices[i].m_index;
|
||||
int parentIndex = allIndices[i].m_parentIndex;
|
||||
btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
|
||||
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
||||
if ((urdfLinkIndex + 1) >= parentTransforms.size())
|
||||
{
|
||||
parentTransforms.resize(urdfLinkIndex + 1);
|
||||
}
|
||||
parentTransforms[urdfLinkIndex] = tr;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
#if 0
|
||||
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
|
||||
{
|
||||
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
if (scene && cache.m_articulation)
|
||||
{
|
||||
#ifdef DEBUG_ARTICULATIONS
|
||||
printf("\n-----------------\n");
|
||||
printf("m_linkTransWorldSpace\n");
|
||||
for (int i = 0; i < cache.m_linkTransWorldSpace.size(); i++)
|
||||
{
|
||||
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
||||
cache.m_linkTransWorldSpace[i].p.x, cache.m_linkTransWorldSpace[i].p.y, cache.m_linkTransWorldSpace[i].p.z,
|
||||
cache.m_linkTransWorldSpace[i].q.x, cache.m_linkTransWorldSpace[i].q.y, cache.m_linkTransWorldSpace[i].q.z, cache.m_linkTransWorldSpace[i].q.w);
|
||||
}
|
||||
printf("m_parentLocalPoses\n");
|
||||
for (int i = 0; i < cache.m_parentLocalPoses.size(); i++)
|
||||
{
|
||||
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
||||
cache.m_parentLocalPoses[i].p.x, cache.m_parentLocalPoses[i].p.y, cache.m_parentLocalPoses[i].p.z,
|
||||
cache.m_parentLocalPoses[i].q.x, cache.m_parentLocalPoses[i].q.y, cache.m_parentLocalPoses[i].q.z, cache.m_parentLocalPoses[i].q.w);
|
||||
}
|
||||
|
||||
printf("m_childLocalPoses\n");
|
||||
for (int i = 0; i < cache.m_childLocalPoses.size(); i++)
|
||||
{
|
||||
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
||||
cache.m_childLocalPoses[i].p.x, cache.m_childLocalPoses[i].p.y, cache.m_childLocalPoses[i].p.z,
|
||||
cache.m_childLocalPoses[i].q.x, cache.m_childLocalPoses[i].q.y, cache.m_childLocalPoses[i].q.z, cache.m_childLocalPoses[i].q.w);
|
||||
}
|
||||
|
||||
printf("m_geomDimensions\n");
|
||||
for (int i = 0; i < cache.m_geomDimensions.size(); i++)
|
||||
{
|
||||
printf("PxVec3(%f,%f,%f),\n",
|
||||
cache.m_geomDimensions[i].x, cache.m_geomDimensions[i].y, cache.m_geomDimensions[i].z);
|
||||
}
|
||||
|
||||
printf("m_geomLocalPoses\n");
|
||||
for (int i = 0; i < cache.m_geomLocalPoses.size(); i++)
|
||||
{
|
||||
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
||||
cache.m_geomLocalPoses[i].p.x, cache.m_geomLocalPoses[i].p.y, cache.m_geomLocalPoses[i].p.z,
|
||||
cache.m_geomLocalPoses[i].q.x, cache.m_geomLocalPoses[i].q.y, cache.m_geomLocalPoses[i].q.z, cache.m_geomLocalPoses[i].q.w);
|
||||
}
|
||||
|
||||
printf("m_linkMaterials\n");
|
||||
for (int i = 0; i < cache.m_linkMaterials.size(); i++)
|
||||
{
|
||||
printf("PxVec3(%f,%f,%f),\n",
|
||||
cache.m_linkMaterials[i].x, cache.m_linkMaterials[i].y, cache.m_linkMaterials[i].z);
|
||||
}
|
||||
#endif //DEBUG_ARTICULATIONS
|
||||
|
||||
//see also https://github.com/NVIDIAGameWorks/PhysX/issues/43
|
||||
if ((flags & CUF_USE_SELF_COLLISION) == 0)
|
||||
{
|
||||
physx::PxU32 nbActors = cache.m_articulation->getNbLinks();; // Max number of actors expected in the aggregate
|
||||
bool selfCollisions = false;
|
||||
physx::PxAggregate* aggregate = physics->createAggregate(nbActors, selfCollisions);
|
||||
aggregate->addArticulation(*cache.m_articulation);
|
||||
scene->addAggregate(*aggregate);
|
||||
}
|
||||
else
|
||||
{
|
||||
scene->addArticulation(*cache.m_articulation);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return cache.m_articulation;
|
||||
}
|
||||
25
examples/SharedMemory/physx/URDF2PhysX.h
Normal file
25
examples/SharedMemory/physx/URDF2PhysX.h
Normal file
@@ -0,0 +1,25 @@
|
||||
#ifndef URDF2PHYSX_H
|
||||
#define URDF2PHYSX_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Importers/ImportURDFDemo/URDFJointTypes.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
class PxFoundation;
|
||||
class PxPhysics;
|
||||
class PxDefaultCpuDispatcher;
|
||||
class PxScene;
|
||||
class PxCooking;
|
||||
class PxArticulationReducedCoordinate;
|
||||
};
|
||||
|
||||
struct UrdfVisualShapeCache2
|
||||
{
|
||||
b3AlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
|
||||
b3AlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
|
||||
};
|
||||
|
||||
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const class btTransform& rootTransformInWorldSpace,struct CommonFileIOInterface* fileIO);
|
||||
|
||||
#endif //URDF2PHYSX_H
|
||||
Reference in New Issue
Block a user