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

more fixes in pybullet_gym envs/data.

Browse Source 
implement pybullet.setAdditionalSearchPath
This commit is contained in:
Erwin Coumans
2017-08-27 19:34:00 -07:00
parent d9faea8c1c
commit 1569f3845c
18 changed files with 151 additions and 197 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3768,6 +3768,7 @@ void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds)
}
double b3GetTimeOut(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -3779,6 +3780,25 @@ double b3GetTimeOut(b3PhysicsClientHandle physClient)
return -1;
}
b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SET_ADDITIONAL_SEARCH_PATH;
command->m_updateFlags = 0;
int len = strlen(path);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_searchPathArgs.m_path,path);
}
return (b3SharedMemoryCommandHandle)command;
}
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4])
{
b3Transform trA;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -482,6 +482,8 @@ void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle comma
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path);
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]);
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]);

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4001,6 +4001,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
case CMD_SET_ADDITIONAL_SEARCH_PATH:
{
BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH");
b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path);
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
case CMD_LOAD_URDF:
{
BT_PROFILE("CMD_LOAD_URDF");

8
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -1,4 +1,3 @@
#ifndef SHARED_MEMORY_COMMANDS_H
#define SHARED_MEMORY_COMMANDS_H
@@ -105,6 +104,12 @@ struct MjcfArgs
int m_flags;
};
struct b3SearchPathfArgs
{
char m_path[MAX_FILENAME_LENGTH];
};
struct BulletDataStreamArgs
{
char m_bulletFileName[MAX_FILENAME_LENGTH];
@@ -960,6 +965,7 @@ struct SharedMemoryCommand
struct b3CreateMultiBodyArgs m_createMultiBodyArgs;
struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs;
struct b3ChangeTextureArgs m_changeTextureArgs;
struct b3SearchPathfArgs m_searchPathArgs;
};
};

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -4,7 +4,9 @@
#define SHARED_MEMORY_KEY 12347
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201707140
#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
//#define SHARED_MEMORY_MAGIC_NUMBER 201706001
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024
@@ -69,6 +71,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_COLLISION_INFO,
CMD_REQUEST_MOUSE_EVENTS_DATA,
CMD_CHANGE_TEXTURE,
CMD_SET_ADDITIONAL_SEARCH_PATH,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,

30
examples/Utils/b3ResourcePath.cpp
View File

@@ -67,6 +67,24 @@ struct TempResourcePath
}
};
static char sAdditionalSearchPath[B3_MAX_EXE_PATH_LEN] = {0};
void b3ResourcePath::setAdditionalSearchPath(const char* path)
{
if (path)
{
int len = strlen(path);
if (len<(B3_MAX_EXE_PATH_LEN-1))
{
strcpy(sAdditionalSearchPath,path);
sAdditionalSearchPath[len] = 0;
}
} else
{
sAdditionalSearchPath[0] = 0;
}
}
int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes)
{
//first find in a resource/<exeName> location, then in various folders within 'data' using b3FileUtils
@@ -78,6 +96,18 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
return strlen(resourcePathOut);
}
if (sAdditionalSearchPath[0])
{
TempResourcePath tmpPath(resourcePathMaxNumBytes+1024);
char* resourcePathIn = tmpPath.m_path;
sprintf(resourcePathIn,"%s/%s",sAdditionalSearchPath,resourceName);
//printf("try resource at %s\n", resourcePath);
if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
{
return strlen(resourcePathOut);
}
}
int l = b3ResourcePath::getExePath(exePath, B3_MAX_EXE_PATH_LEN);
if (l)
{

1
examples/Utils/b3ResourcePath.h
View File

@@ -8,6 +8,7 @@ class b3ResourcePath
public:
static int getExePath(char* path, int maxPathLenInBytes);
static int findResourcePath(const char* sourceName, char* resourcePath, int maxResourcePathLenInBytes);
static void setAdditionalSearchPath(const char* path);
};
#endif

13
examples/pybullet/examples/racecar.py
View File

@@ -1,4 +1,13 @@
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
parentdir = os.path.join(currentdir,"../gym")
os.sys.path.insert(0,parentdir)
import pybullet as p
import pybullet_data
import time
cid = p.connect(p.SHARED_MEMORY)
@@ -14,9 +23,9 @@ useRealTimeSim = 1
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
#p.loadURDF("plane.urdf")
p.loadSDF("stadium.sdf")
p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf"))
car = p.loadURDF("racecar/racecar.urdf")
car = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"racecar/racecar.urdf"))
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))

17
examples/pybullet/examples/test.py
View File

@@ -1,16 +1,27 @@
#os.sys.path.insert is only needed when pybullet is not installed
#but running from github repo instead
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
parentdir = os.path.join(currentdir,"../gym")
os.sys.path.insert(0,parentdir)
import pybullet
import pybullet_data
import time
#choose connection method: GUI, DIRECT, SHARED_MEMORY
pybullet.connect(pybullet.GUI)
pybullet.loadURDF("plane.urdf",0,0,-1)
pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"),0,0,-1)
#load URDF, given a relative or absolute file+path
obj = pybullet.loadURDF("r2d2.urdf")
obj = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"))
posX=0
posY=3
posZ=2
obj2 = pybullet.loadURDF("kuka_lwr/kuka.urdf",posX,posY,posZ)
obj2 = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(),"kuka_iiwa/model.urdf"),posX,posY,posZ)
#query the number of joints of the object
numJoints = pybullet.getNumJoints(obj)

6
examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py
View File

@@ -2,10 +2,10 @@
Classic cart-pole system implemented by Rich Sutton et al.
Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c
"""
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
os.sys.path.insert(0,currentdir)
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import logging
import math

6
examples/pybullet/gym/pybullet_envs/bullet/kuka.py
View File

@@ -1,7 +1,7 @@
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
os.sys.path.insert(0,currentdir)
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import pybullet as p
import numpy as np

7
examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py
View File

@@ -1,7 +1,8 @@
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
os.sys.path.insert(0,currentdir)
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import math
import gym

1
examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
View File

@@ -1,4 +1,3 @@
import os
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))

7
examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py
View File

@@ -1,8 +1,7 @@
import os
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
os.sys.path.insert(0,currentdir)
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import math
import gym

2
examples/pybullet/gym/pybullet_envs/robot_bases.py
View File

@@ -3,7 +3,7 @@ import gym, gym.spaces, gym.utils
import numpy as np
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
parentdir = os.path.dirname(currentdir)
os.sys.path.insert(0,parentdir)
import pybullet_data

4
examples/pybullet/gym/pybullet_envs/scene_stadium.py
View File

@@ -1,11 +1,11 @@
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
parentdir = os.path.dirname(currentdir)
os.sys.path.insert(0,parentdir)
import pybullet_data
from .scene_abstract import Scene
from pybullet_envs.scene_abstract import Scene
import pybullet as p

34
examples/pybullet/pybullet.c
View File

@@ -3701,6 +3701,36 @@ static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObj
return Py_None;
}
static PyObject* pybullet_setAdditionalSearchPath(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"path", "physicsClientId", NULL};
char* path = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist,
&path, &physicsClientId))
return NULL;
if (path)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3SetAdditionalSearchPath(sm, path);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"timeOutInSeconds", "physicsClientId", NULL};
@@ -7215,6 +7245,10 @@ static PyMethodDef SpamMethods[] = {
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
"Set the timeOut in seconds, used for most of the API calls."},
{"setAdditionalSearchPath", (PyCFunction)pybullet_setAdditionalSearchPath,
METH_VARARGS | METH_KEYWORDS,
"Set an additional search path, used to load URDF/SDF files."},
{"getAPIVersion", (PyCFunction)pybullet_getAPIVersion,
METH_VARARGS | METH_KEYWORDS,
"Get version of the API. Compatibility exists for connections using the same API version. Make sure both client and server use the same number of bits (32-bit or 64bit)."},