mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
Merge pull request #1780 from erwincoumans/master
PyBullet: preliminary DART and MuJoCo backend code (mostly empty placeholders)
This commit is contained in:
erwincoumans
GitHub
commit
d6aae09290
@ -5,7 +5,7 @@
|
||||
<worldbody>
|
||||
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
|
||||
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
|
||||
<body pos="0 0 1">
|
||||
<body name="capsule" pos="0 0 1">
|
||||
<joint type="free"/>
|
||||
<geom name="aux_1_geom" size="0.05 0.1" type="capsule"/>
|
||||
</body>
|
||||
|
||||
@ -180,10 +180,19 @@ bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma
|
||||
data = (unsigned char*) &clientCmd.m_type;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
if (clientCmd.m_type == CMD_REQUEST_VR_EVENTS_DATA)
|
||||
{
|
||||
sz = 3 * sizeof(int) + sizeof(smUint64_t) + 16;
|
||||
data = (unsigned char*)&clientCmd;
|
||||
}
|
||||
else
|
||||
{
|
||||
sz = sizeof(SharedMemoryCommand);
|
||||
data = (unsigned char*)&clientCmd;
|
||||
}
|
||||
}
|
||||
|
||||
m_data->m_tcpSocket.Send((const uint8 *)data,sz);
|
||||
|
||||
|
||||
@ -2719,6 +2719,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
|
||||
SaveWorldObjectData sd;
|
||||
sd.m_fileName = fileName;
|
||||
|
||||
int currentOpenGLTextureIndex = 0;
|
||||
|
||||
for (int m =0; m<u2b.getNumModels();m++)
|
||||
{
|
||||
@ -2885,6 +2886,44 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int startShapeIndex = 0;
|
||||
|
||||
if (m_data->m_pluginManager.getRenderInterface())
|
||||
{
|
||||
int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(bodyUniqueId);
|
||||
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
|
||||
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
|
||||
b3VisualShapeData tmpShape;
|
||||
|
||||
int remain = totalNumVisualShapes - startShapeIndex;
|
||||
int shapeIndex = startShapeIndex;
|
||||
|
||||
int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId,shapeIndex,&tmpShape);
|
||||
if (success)
|
||||
{
|
||||
if (tmpShape.m_tinyRendererTextureId>=0)
|
||||
{
|
||||
int openglTextureUniqueId = -1;
|
||||
|
||||
//find companion opengl texture unique id and create a 'textureUid'
|
||||
if (currentOpenGLTextureIndex<u2b.getNumAllocatedTextures())
|
||||
{
|
||||
openglTextureUniqueId = u2b.getAllocatedTexture(currentOpenGLTextureIndex);
|
||||
currentOpenGLTextureIndex++;
|
||||
}
|
||||
|
||||
int texHandle = m_data->m_textureHandles.allocHandle();
|
||||
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
|
||||
if(texH)
|
||||
{
|
||||
texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId;
|
||||
texH->m_openglTextureId = openglTextureUniqueId;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -2895,6 +2934,14 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
int texHandle = m_data->m_textureHandles.allocHandle();
|
||||
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
|
||||
if(texH)
|
||||
{
|
||||
texH->m_tinyRendererTextureId = -1;
|
||||
texH->m_openglTextureId = -1;
|
||||
*/
|
||||
|
||||
for (int i=0;i<u2b.getNumAllocatedMeshInterfaces();i++)
|
||||
{
|
||||
@ -9284,6 +9331,25 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
|
||||
shapeIndex,
|
||||
visualShapeStoragePtr);
|
||||
if (success) {
|
||||
|
||||
//find the matching texture unique ids.
|
||||
if (visualShapeStoragePtr->m_tinyRendererTextureId>=0)
|
||||
{
|
||||
b3AlignedObjectArray<int> usedHandles;
|
||||
m_data->m_textureHandles.getUsedHandles(usedHandles);
|
||||
|
||||
for (int i=0;i<usedHandles.size();i++)
|
||||
{
|
||||
int texHandle =usedHandles[i];
|
||||
InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
|
||||
if (texH && (texH->m_tinyRendererTextureId == visualShapeStoragePtr->m_tinyRendererTextureId))
|
||||
{
|
||||
visualShapeStoragePtr->m_openglTextureId =texH->m_openglTextureId;
|
||||
visualShapeStoragePtr->m_textureUniqueId = texHandle;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
|
||||
@ -595,6 +595,11 @@ typedef union {
|
||||
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
|
||||
#define VISUAL_SHAPE_MAX_PATH_LEN 1024
|
||||
|
||||
enum b3VisualShapeDataFlags
|
||||
{
|
||||
eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS = 1,
|
||||
};
|
||||
|
||||
struct b3VisualShapeData
|
||||
{
|
||||
int m_objectUniqueId;
|
||||
@ -605,6 +610,10 @@ struct b3VisualShapeData
|
||||
double m_localVisualFrame[7];//pos[3], orn[4]
|
||||
//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
|
||||
double m_rgbaColor[4];
|
||||
int m_tinyRendererTextureId;
|
||||
int m_textureUniqueId;
|
||||
int m_openglTextureId;
|
||||
|
||||
};
|
||||
|
||||
struct b3VisualShapeInformation
|
||||
@ -743,6 +752,8 @@ enum eCONNECT_METHOD {
|
||||
eCONNECT_GUI_SERVER=7,
|
||||
eCONNECT_GUI_MAIN_THREAD=8,
|
||||
eCONNECT_SHARED_MEMORY_SERVER=9,
|
||||
eCONNECT_DART=10,
|
||||
eCONNECT_MUJOCO=11,
|
||||
};
|
||||
|
||||
enum eURDF_Flags
|
||||
|
||||
@ -155,6 +155,69 @@ btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQua
|
||||
return rpy2;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI_NoDirect::loadTexture(const std::string& fileName)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return -1;
|
||||
}
|
||||
btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
{
|
||||
commandHandle = b3InitLoadTexture(m_data->m_physicsClientHandle, fileName.c_str());
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_LOAD_TEXTURE_COMPLETED)
|
||||
{
|
||||
return b3GetStatusTextureUniqueId(statusHandle);
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
bool b3RobotSimulatorClientAPI_NoDirect::changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
|
||||
int objectUniqueId = args.m_objectUniqueId;
|
||||
int jointIndex = args.m_linkIndex;
|
||||
int shapeIndex = args.m_shapeIndex;
|
||||
int textureUniqueId = args.m_textureUniqueId;
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUpdateVisualShape(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex, textureUniqueId);
|
||||
|
||||
if (args.m_hasSpecularColor)
|
||||
{
|
||||
double specularColor[3] = {args.m_specularColor[0],args.m_specularColor[1],args.m_specularColor[2]};
|
||||
b3UpdateVisualShapeSpecularColor(commandHandle,specularColor);
|
||||
}
|
||||
if (args.m_hasRgbaColor)
|
||||
{
|
||||
double rgbaColor[4] = {args.m_rgbaColor[0],args.m_rgbaColor[1],args.m_rgbaColor[2],args.m_rgbaColor[3]};
|
||||
b3UpdateVisualShapeRGBAColor(commandHandle,rgbaColor);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
return (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED);
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
|
||||
{
|
||||
int robotUniqueId = -1;
|
||||
@ -2177,7 +2240,8 @@ void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId)
|
||||
bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
if (sm == 0)
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
@ -2185,17 +2249,17 @@ bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
{
|
||||
commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED);
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) {
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
b3GetVisualShapeInformation(sm, &visualShapeInfo);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::string& path)
|
||||
|
||||
@ -60,6 +60,30 @@ struct b3RobotSimulatorLoadFileResults
|
||||
}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorChangeVisualShapeArgs
|
||||
{
|
||||
int m_objectUniqueId;
|
||||
int m_linkIndex;
|
||||
int m_shapeIndex;
|
||||
int m_textureUniqueId;
|
||||
btVector4 m_rgbaColor;
|
||||
bool m_hasRgbaColor;
|
||||
btVector3 m_specularColor;
|
||||
bool m_hasSpecularColor;
|
||||
|
||||
b3RobotSimulatorChangeVisualShapeArgs()
|
||||
:m_objectUniqueId(-1),
|
||||
m_linkIndex(-1),
|
||||
m_shapeIndex(-1),
|
||||
m_textureUniqueId(-1),
|
||||
m_rgbaColor(0,0,0,1),
|
||||
m_hasRgbaColor(false),
|
||||
m_specularColor(1,1,1),
|
||||
m_hasSpecularColor(false)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct b3RobotSimulatorJointMotorArgs
|
||||
{
|
||||
int m_controlMode;
|
||||
@ -505,6 +529,10 @@ public:
|
||||
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
|
||||
bool saveBullet(const std::string& fileName);
|
||||
|
||||
int loadTexture(const std::string& fileName);
|
||||
|
||||
bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args);
|
||||
|
||||
bool savePythonWorld(const std::string& fileName);
|
||||
|
||||
bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
|
||||
@ -648,7 +676,7 @@ public:
|
||||
|
||||
bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo);
|
||||
|
||||
bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
|
||||
bool getVisualShapeData(int bodyUniqueId, struct b3VisualShapeInformation &visualShapeInfo);
|
||||
|
||||
int saveStateToMemory();
|
||||
void restoreStateFromMemory(int stateId);
|
||||
|
||||
16
examples/SharedMemory/dart/DARTPhysicsC_API.cpp
Normal file
16
examples/SharedMemory/dart/DARTPhysicsC_API.cpp
Normal file
@ -0,0 +1,16 @@
|
||||
#ifdef BT_ENABLE_DART
|
||||
#include "DARTPhysicsC_API.h"
|
||||
#include "DARTPhysicsServerCommandProcessor.h"
|
||||
#include "DARTPhysicsClient.h"
|
||||
|
||||
//think more about naming. The b3ConnectPhysicsLoopback
|
||||
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART()
|
||||
{
|
||||
DARTPhysicsServerCommandProcessor* sdk = new DARTPhysicsServerCommandProcessor;
|
||||
|
||||
DARTPhysicsClient* direct = new DARTPhysicsClient(sdk,true);
|
||||
bool connected;
|
||||
connected = direct->connect();
|
||||
return (b3PhysicsClientHandle )direct;
|
||||
}
|
||||
#endif//BT_ENABLE_DART
|
||||
20
examples/SharedMemory/dart/DARTPhysicsC_API.h
Normal file
20
examples/SharedMemory/dart/DARTPhysicsC_API.h
Normal file
@ -0,0 +1,20 @@
|
||||
#ifndef DART_PHYSICS_C_API_H
|
||||
#define DART_PHYSICS_C_API_H
|
||||
|
||||
#ifdef BT_ENABLE_DART
|
||||
|
||||
#include "../PhysicsClientC_API.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
//think more about naming. The b3ConnectPhysicsLoopback
|
||||
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART();
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //BT_ENABLE_DART
|
||||
#endif //DART_PHYSICS_C_API_H
|
||||
1596
examples/SharedMemory/dart/DARTPhysicsClient.cpp
Normal file
1596
examples/SharedMemory/dart/DARTPhysicsClient.cpp
Normal file
File diff suppressed because it is too large
Load Diff
124
examples/SharedMemory/dart/DARTPhysicsClient.h
Normal file
124
examples/SharedMemory/dart/DARTPhysicsClient.h
Normal file
@ -0,0 +1,124 @@
|
||||
#ifndef DART_PHYSICS_CLIENT_H
|
||||
#define DART_PHYSICS_CLIENT_H
|
||||
|
||||
|
||||
#include "../PhysicsClient.h"
|
||||
|
||||
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
|
||||
class DARTPhysicsClient : public PhysicsClient
|
||||
{
|
||||
protected:
|
||||
|
||||
struct DARTPhysicsDirectInternalData* 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:
|
||||
|
||||
DARTPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
|
||||
|
||||
virtual ~DARTPhysicsClient();
|
||||
|
||||
// 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 bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
|
||||
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
|
||||
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
|
||||
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
|
||||
|
||||
virtual void pushProfileTiming(const char* timingName);
|
||||
virtual void popProfileTiming();
|
||||
};
|
||||
|
||||
#endif //DART_PHYSICS__H
|
||||
@ -0,0 +1,39 @@
|
||||
#include "DARTPhysicsServerCommandProcessor.h"
|
||||
|
||||
|
||||
DARTPhysicsServerCommandProcessor::DARTPhysicsServerCommandProcessor()
|
||||
{
|
||||
}
|
||||
|
||||
DARTPhysicsServerCommandProcessor::~DARTPhysicsServerCommandProcessor()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool DARTPhysicsServerCommandProcessor::connect()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
void DARTPhysicsServerCommandProcessor::disconnect()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool DARTPhysicsServerCommandProcessor::isConnected() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool DARTPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool DARTPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,31 @@
|
||||
#ifndef DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H
|
||||
#define DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H
|
||||
|
||||
#include "../PhysicsCommandProcessorInterface.h"
|
||||
|
||||
class DARTPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
|
||||
{
|
||||
|
||||
public:
|
||||
DARTPhysicsServerCommandProcessor();
|
||||
|
||||
virtual ~DARTPhysicsServerCommandProcessor();
|
||||
|
||||
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){}
|
||||
|
||||
};
|
||||
|
||||
#endif //DART_PHYSICS_COMMAND_PROCESSOR_H
|
||||
15
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp
Normal file
15
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp
Normal file
@ -0,0 +1,15 @@
|
||||
#ifdef BT_ENABLE_MUJOCO
|
||||
#include "MuJoCoPhysicsC_API.h"
|
||||
#include "MuJoCoPhysicsServerCommandProcessor.h"
|
||||
#include "MuJoCoPhysicsClient.h"
|
||||
|
||||
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo()
|
||||
{
|
||||
MuJoCoPhysicsServerCommandProcessor* sdk = new MuJoCoPhysicsServerCommandProcessor;
|
||||
|
||||
MuJoCoPhysicsClient* direct = new MuJoCoPhysicsClient(sdk,true);
|
||||
bool connected;
|
||||
connected = direct->connect();
|
||||
return (b3PhysicsClientHandle )direct;
|
||||
}
|
||||
#endif//BT_ENABLE_MUJOCO
|
||||
20
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h
Normal file
20
examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h
Normal file
@ -0,0 +1,20 @@
|
||||
#ifndef MUJOCO_PHYSICS_C_API_H
|
||||
#define MUJOCO_PHYSICS_C_API_H
|
||||
|
||||
#ifdef BT_ENABLE_MUJOCO
|
||||
|
||||
#include "../PhysicsClientC_API.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
//think more about naming. The b3ConnectPhysicsLoopback
|
||||
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo();
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //BT_ENABLE_MUJOCO
|
||||
#endif //MUJOCO_PHYSICS_C_API_H
|
||||
1598
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp
Normal file
1598
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp
Normal file
File diff suppressed because it is too large
Load Diff
123
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h
Normal file
123
examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h
Normal file
@ -0,0 +1,123 @@
|
||||
#ifndef MUJOCO_PHYSICS_CLIENT_H
|
||||
#define MUJOCO_PHYSICS_CLIENT_H
|
||||
|
||||
#include "../PhysicsClient.h"
|
||||
|
||||
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
|
||||
class MuJoCoPhysicsClient : public PhysicsClient
|
||||
{
|
||||
protected:
|
||||
|
||||
struct MuJoCoPhysicsDirectInternalData* 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:
|
||||
|
||||
MuJoCoPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
|
||||
|
||||
virtual ~MuJoCoPhysicsClient();
|
||||
|
||||
// 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 bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const;
|
||||
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
|
||||
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
|
||||
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
|
||||
|
||||
virtual void pushProfileTiming(const char* timingName);
|
||||
virtual void popProfileTiming();
|
||||
};
|
||||
|
||||
#endif //MUJOCO_PHYSICS_CLIENT_H
|
||||
1032
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp
Normal file
1032
examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp
Normal file
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,45 @@
|
||||
#ifndef MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H
|
||||
#define MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H
|
||||
|
||||
#include "../PhysicsCommandProcessorInterface.h"
|
||||
|
||||
class MuJoCoPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
|
||||
{
|
||||
|
||||
struct MuJoCoPhysicsServerCommandProcessorInternalData* 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 processLoadMJCFCommand(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);
|
||||
|
||||
void resetSimulation();
|
||||
|
||||
public:
|
||||
MuJoCoPhysicsServerCommandProcessor();
|
||||
|
||||
virtual ~MuJoCoPhysicsServerCommandProcessor();
|
||||
|
||||
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){}
|
||||
|
||||
};
|
||||
|
||||
#endif //MUJOCO_PHYSICS_COMMAND_PROCESSOR_H
|
||||
@ -671,12 +671,14 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
|
||||
visualShape.m_rgbaColor[1] = rgbaColor[1];
|
||||
visualShape.m_rgbaColor[2] = rgbaColor[2];
|
||||
visualShape.m_rgbaColor[3] = rgbaColor[3];
|
||||
visualShape.m_openglTextureId = -1;
|
||||
visualShape.m_tinyRendererTextureId = -1;
|
||||
visualShape.m_textureUniqueId = -1;
|
||||
|
||||
{
|
||||
B3_PROFILE("convertURDFToVisualShape");
|
||||
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures, visualShape);
|
||||
}
|
||||
m_data->m_visualShapes.push_back(visualShape);
|
||||
|
||||
if (vertices.size() && indices.size())
|
||||
{
|
||||
@ -701,13 +703,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
|
||||
}
|
||||
visuals->m_renderObjects.push_back(tinyObj);
|
||||
}
|
||||
|
||||
btAssert(textures.size()<=1);
|
||||
for (int i=0;i<textures.size();i++)
|
||||
{
|
||||
if (!textures[i].m_isCached)
|
||||
{
|
||||
free(textures[i].textureData1);
|
||||
}
|
||||
visualShape.m_tinyRendererTextureId = m_data->m_textures.size();
|
||||
m_data->m_textures.push_back(textures[i]);
|
||||
}
|
||||
m_data->m_visualShapes.push_back(visualShape);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1199,13 +1203,13 @@ void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, in
|
||||
for (int v = 0; v < visualArray->m_renderObjects.size(); v++)
|
||||
{
|
||||
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
|
||||
|
||||
if ((shapeIndex < 0) || (shapeIndex == v))
|
||||
{
|
||||
renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -178,17 +178,30 @@ int main(int argc, char *argv[])
|
||||
|
||||
SharedMemoryCommand* cmdPtr = 0;
|
||||
|
||||
int type = *(int*)&bytesReceived[0];
|
||||
|
||||
//performance test
|
||||
if (numBytesRec == sizeof(int))
|
||||
{
|
||||
cmdPtr = &cmd;
|
||||
cmd.m_type = *(int*)&bytesReceived[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
if (numBytesRec == sizeof(SharedMemoryCommand))
|
||||
{
|
||||
cmdPtr = (SharedMemoryCommand*)&bytesReceived[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
if (numBytesRec==36)
|
||||
{
|
||||
cmdPtr = &cmd;
|
||||
memcpy(&cmd, &bytesReceived[0], numBytesRec);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (cmdPtr)
|
||||
{
|
||||
SharedMemoryStatus serverStatus;
|
||||
@ -207,7 +220,7 @@ int main(int argc, char *argv[])
|
||||
}
|
||||
if (gVerboseNetworkMessagesServer)
|
||||
{
|
||||
printf("buffer.size = %d\n", buffer.size());
|
||||
//printf("buffer.size = %d\n", buffer.size());
|
||||
printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
|
||||
}
|
||||
if (hasStatus)
|
||||
@ -233,12 +246,35 @@ int main(int argc, char *argv[])
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
if (cmdPtr->m_type == CMD_REQUEST_VR_EVENTS_DATA)
|
||||
{
|
||||
int headerSize = 16+5 * sizeof(int) + sizeof(smUint64_t) + sizeof(char*) + sizeof(b3VRControllerEvent)*serverStatus.m_sendVREvents.m_numVRControllerEvents;
|
||||
packetData.resize(4 + headerSize);
|
||||
int sz = packetData.size();
|
||||
int curPos = 0;
|
||||
MySerializeInt(sz, &packetData[curPos]);
|
||||
curPos += 4;
|
||||
for (int i = 0; i < headerSize; i++)
|
||||
{
|
||||
packetData[i + curPos] = statBytes[i];
|
||||
}
|
||||
curPos += headerSize;
|
||||
pClient->Send(&packetData[0], packetData.size());
|
||||
}
|
||||
else
|
||||
{
|
||||
//create packetData with [int packetSizeInBytes, status, streamBytes)
|
||||
packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes);
|
||||
int sz = packetData.size();
|
||||
int curPos = 0;
|
||||
|
||||
if (gVerboseNetworkMessagesServer)
|
||||
{
|
||||
//printf("buffer.size = %d\n", buffer.size());
|
||||
printf("serverStatus packed size = %d\n", sz);
|
||||
}
|
||||
|
||||
MySerializeInt(sz, &packetData[curPos]);
|
||||
curPos += 4;
|
||||
for (int i = 0; i < sizeof(SharedMemoryStatus); i++)
|
||||
@ -252,7 +288,8 @@ int main(int argc, char *argv[])
|
||||
packetData[i + curPos] = buffer[i];
|
||||
}
|
||||
|
||||
pClient->Send( &packetData[0], packetData.size() );
|
||||
pClient->Send(&packetData[0], packetData.size());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
40
examples/pybullet/examples/biped2d_pybullet.py
Normal file
40
examples/pybullet/examples/biped2d_pybullet.py
Normal file
@ -0,0 +1,40 @@
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
import os
|
||||
import time
|
||||
GRAVITY = -9.8
|
||||
dt = 1e-3
|
||||
iters=2000
|
||||
|
||||
physicsClient = p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.resetSimulation()
|
||||
#p.setRealTimeSimulation(True)
|
||||
p.setGravity(0,0,GRAVITY)
|
||||
p.setTimeStep(dt)
|
||||
planeId = p.loadURDF("plane.urdf")
|
||||
cubeStartPos = [0,0,1.13]
|
||||
cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0])
|
||||
botId = p.loadURDF("biped/biped2d_pybullet.urdf",
|
||||
cubeStartPos,
|
||||
cubeStartOrientation)
|
||||
|
||||
#disable the default velocity motors
|
||||
#and set some position control with small force to emulate joint friction/return to a rest pose
|
||||
jointFrictionForce=1
|
||||
for joint in range (p.getNumJoints(botId)):
|
||||
p.setJointMotorControl2(botId,joint,p.POSITION_CONTROL,force=jointFrictionForce)
|
||||
|
||||
#for i in range(10000):
|
||||
# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
|
||||
# p.stepSimulation()
|
||||
#import ipdb
|
||||
#ipdb.set_trace()
|
||||
import time
|
||||
p.setRealTimeSimulation(1)
|
||||
while (1):
|
||||
#p.stepSimulation()
|
||||
#p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
|
||||
p.setGravity(0,0,GRAVITY)
|
||||
time.sleep(1/240.)
|
||||
time.sleep(1000)
|
||||
19
examples/pybullet/examples/getTextureUid.py
Normal file
19
examples/pybullet/examples/getTextureUid.py
Normal file
@ -0,0 +1,19 @@
|
||||
import pybullet as p
|
||||
p.connect(p.GUI)
|
||||
plane = p.loadURDF("plane.urdf")
|
||||
visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
|
||||
print(visualData)
|
||||
curTexUid = visualData[0][8]
|
||||
print(curTexUid)
|
||||
texUid = p.loadTexture("tex256.png")
|
||||
print("texUid=",texUid)
|
||||
|
||||
p.changeVisualShape(plane,-1,textureUniqueId=texUid)
|
||||
|
||||
for i in range (100):
|
||||
p.getCameraImage(320,200)
|
||||
p.changeVisualShape(plane,-1,textureUniqueId=curTexUid)
|
||||
|
||||
for i in range (100):
|
||||
p.getCameraImage(320,200)
|
||||
|
||||
32
examples/pybullet/examples/otherPhysicsEngine.py
Normal file
32
examples/pybullet/examples/otherPhysicsEngine.py
Normal file
@ -0,0 +1,32 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
|
||||
#p.connect(p.DIRECT)
|
||||
#p.connect(p.DART)
|
||||
p.connect(p.MuJoCo)
|
||||
|
||||
#p.connect(p.GUI)
|
||||
bodies = p.loadMJCF("mjcf/capsule.xml")
|
||||
print("bodies=",bodies)
|
||||
|
||||
numBodies = p.getNumBodies()
|
||||
print("numBodies=",numBodies)
|
||||
for i in range (numBodies):
|
||||
print("bodyInfo[",i,"]=",p.getBodyInfo(i))
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
timeStep = 1./240.
|
||||
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
|
||||
|
||||
#while (p.isConnected()):
|
||||
for i in range (1000):
|
||||
p.stepSimulation()
|
||||
|
||||
for b in bodies:
|
||||
pos,orn=p.getBasePositionAndOrientation(b)
|
||||
print("pos[",b,"]=",pos)
|
||||
print("orn[",b,"]=",orn)
|
||||
linvel,angvel=p.getBaseVelocity(b)
|
||||
print("linvel[",b,"]=",linvel)
|
||||
print("angvel[",b,"]=",angvel)
|
||||
time.sleep(timeStep)
|
||||
@ -13,8 +13,8 @@ bearStartPos2 = [0,0,0]
|
||||
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
|
||||
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
|
||||
textureId = p.loadTexture("checker_grid.jpg")
|
||||
p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
|
||||
p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
|
||||
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
|
||||
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
|
||||
|
||||
|
||||
useRealTimeSimulation = 1
|
||||
|
||||
273
examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf
Normal file
273
examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf
Normal file
@ -0,0 +1,273 @@
|
||||
|
||||
|
||||
|
||||
<?xml version="1.0"?>
|
||||
<robot name="balance">
|
||||
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
|
||||
<material name="black">
|
||||
<color rgba="0.2 0.2 0.2 1"/>
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 1 1"/>
|
||||
</material>
|
||||
|
||||
<link name="world">
|
||||
<inertial>
|
||||
<mass value="0"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="y_prismatic">
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="y_to_world" type="prismatic">
|
||||
<parent link="world"/>
|
||||
<child link="y_prismatic"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="z_prismatic">
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="z_to_y" type="prismatic">
|
||||
<parent link="y_prismatic"/>
|
||||
<child link="z_prismatic"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="torso_to_z" type="continuous">
|
||||
<parent link="z_prismatic"/>
|
||||
<child link="torso"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 1.4"/>
|
||||
</joint>
|
||||
|
||||
<link name="torso">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.48"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.0 0"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.48"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.0 0"/>
|
||||
<contact_coefficients mu="0.08" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="70"/>
|
||||
<inertia ixx="0.2404" ixy="-0.01" ixz="-0.048" iyy="0.2404" iyz="-0.048" izz="0.02"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.0 0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="r_upperleg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.018 0. -0.21715"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.018 -0.0 -0.21715"/>
|
||||
<contact_coefficients mu="0.08" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="0.018 -0 -0.21715"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="torso_to_rightleg" type="revolute">
|
||||
<parent link="torso"/>
|
||||
<child link="r_upperleg"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="0.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="0.05 0 -0.17"/>
|
||||
</joint>
|
||||
|
||||
<link name="l_upperleg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.018 0. -0.21715"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.018 0.0 -0.21715"/>
|
||||
<contact_coefficients mu="0.08" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="5"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="-0.018 -0 -0.21715"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="torso_to_leftleg" type="revolute">
|
||||
<parent link="torso"/>
|
||||
<child link="l_upperleg"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="10.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.05 0 -0.17"/>
|
||||
</joint>
|
||||
|
||||
<link name="r_lowerleg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.048 0. -0.21715"/>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.048 0.0 -0.21715"/>
|
||||
<contact_coefficients mu="0.08" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="0.048 -0 -0.21715"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="r_knee" type="revolute">
|
||||
<parent link="r_upperleg"/>
|
||||
<child link="r_lowerleg"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="10.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="0.015 0 -.41"/>
|
||||
</joint>
|
||||
|
||||
<link name="l_lowerleg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.048 0. -0.21715"/>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 .45"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.048 0.0 -0.21715"/>
|
||||
<contact_coefficients mu="0.08" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="4"/>
|
||||
<inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
|
||||
<origin rpy="0 0 0" xyz="-0.048 -0 -0.21715"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="l_knee" type="revolute">
|
||||
<parent link="l_upperleg"/>
|
||||
<child link="l_lowerleg"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="10.0" lower="-1.57." upper="1.57" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.015 0 -.41"/>
|
||||
</joint>
|
||||
|
||||
<link name="l_foot">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.2 .04"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.05 0.08 -0.038"/>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.05 0.2 .04"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0.05 0.08 -0.038"/>
|
||||
<contact_coefficients mu="0.5" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.0416" ixy="-0.01" ixz="-0.002" iyy="0.0041" iyz="-0.008" izz="0.0425"/>
|
||||
<origin rpy="0 0 0" xyz="0.05 0.08 -0.038"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="l_ankle" type="revolute">
|
||||
<parent link="l_lowerleg"/>
|
||||
<child link="l_foot"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="10.0" lower="-2" upper="2" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="-0.05 -0.03 -.459"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="r_foot">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.05 0.2 .04"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.05 0.08 -0.038"/>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.05 0.2 .04"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="-0.05 0.08 -0.038"/>
|
||||
<contact_coefficients mu="0.5" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.0416" ixy="-0.01" ixz="-0.002" iyy="0.0041" iyz="-0.008" izz="0.0425"/>
|
||||
<origin rpy="0 0 0" xyz="-0.05 0.08 -0.038"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="r_ankle" type="revolute">
|
||||
<parent link="r_lowerleg"/>
|
||||
<child link="r_foot"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="10.0" lower="-2." upper="2" velocity="1000.0"/>
|
||||
<origin rpy="0 0 0" xyz="0.05 -0.03 -.459"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@ -5,6 +5,14 @@
|
||||
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
|
||||
#endif //BT_ENABLE_ENET
|
||||
|
||||
#ifdef BT_ENABLE_DART
|
||||
#include "../SharedMemory/dart/DARTPhysicsC_API.h"
|
||||
#endif
|
||||
|
||||
#ifdef BT_ENABLE_MUJOCO
|
||||
#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
|
||||
#endif
|
||||
|
||||
#ifdef BT_ENABLE_CLSOCKET
|
||||
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
|
||||
#endif //BT_ENABLE_CLSOCKET
|
||||
@ -407,6 +415,22 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
|
||||
sm = b3ConnectPhysicsDirect();
|
||||
break;
|
||||
}
|
||||
#ifdef BT_ENABLE_DART
|
||||
case eCONNECT_DART:
|
||||
{
|
||||
sm = b3ConnectPhysicsDART();
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BT_ENABLE_MUJOCO
|
||||
case eCONNECT_MUJOCO:
|
||||
{
|
||||
sm = b3ConnectPhysicsMuJoCo();
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case eCONNECT_SHARED_MEMORY:
|
||||
{
|
||||
sm = b3ConnectSharedMemory(key);
|
||||
@ -5428,8 +5452,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
|
||||
PyObject* pyResultList = 0;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
static char* kwlist[] = {"objectUniqueId", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &objectUniqueId, &physicsClientId))
|
||||
int flags=0;
|
||||
|
||||
static char* kwlist[] = {"objectUniqueId", "flags", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &objectUniqueId, &flags, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@ -5450,7 +5476,9 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
|
||||
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
|
||||
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
|
||||
{
|
||||
PyObject* visualShapeObList = PyTuple_New(8);
|
||||
int numFields = flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS ? 9 : 8;
|
||||
|
||||
PyObject* visualShapeObList = PyTuple_New(numFields);
|
||||
PyObject* item;
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
|
||||
PyTuple_SetItem(visualShapeObList, 0, item);
|
||||
@ -5511,6 +5539,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
|
||||
PyTuple_SetItem(rgba, 3, item);
|
||||
PyTuple_SetItem(visualShapeObList, 7, rgba);
|
||||
}
|
||||
if (flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
|
||||
{
|
||||
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_textureUniqueId);
|
||||
PyTuple_SetItem(visualShapeObList, 8, item);
|
||||
}
|
||||
|
||||
PyTuple_SetItem(pyResultList, i, visualShapeObList);
|
||||
}
|
||||
@ -9549,6 +9582,14 @@ initpybullet(void)
|
||||
PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read
|
||||
PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD); // user read
|
||||
PyModule_AddIntConstant(m, "SHARED_MEMORY_SERVER", eCONNECT_SHARED_MEMORY_SERVER); // user read
|
||||
#ifdef BT_ENABLE_DART
|
||||
PyModule_AddIntConstant(m, "DART", eCONNECT_DART); // user read
|
||||
#endif
|
||||
|
||||
#ifdef BT_ENABLE_MUJOCO
|
||||
PyModule_AddIntConstant(m, "MuJoCo", eCONNECT_MUJOCO); // user read
|
||||
#endif
|
||||
|
||||
|
||||
PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY", SHARED_MEMORY_KEY);
|
||||
PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY2", SHARED_MEMORY_KEY+1);
|
||||
@ -9656,6 +9697,8 @@ initpybullet(void)
|
||||
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);
|
||||
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS);
|
||||
|
||||
PyModule_AddIntConstant(m, "VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS", eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS);
|
||||
|
||||
PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
|
||||
|
||||
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user