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

reduce stack usage (cause some crashes in low-stack tests)

Browse Source 
fix crashing bug in changeVisualShape
add differential gear version of racecar (only 2 back wheels are powered)
This commit is contained in:
Erwin Coumans
2017-06-20 20:22:14 -07:00
parent 61f27a5c72
commit 71170d6384
7 changed files with 857 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3181,24 +3181,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
} else
{
SharedMemoryStatus tmpCmd = serverStatusOut;
b3OpenGLVisualizerCameraInfo tmpCamResult;
bool result = this->m_data->m_guiHelper->getCameraInfo(
&tmpCmd.m_visualizerCameraResultArgs.m_width,
&tmpCmd.m_visualizerCameraResultArgs.m_height,
tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix,
tmpCmd.m_visualizerCameraResultArgs.m_camUp,
tmpCmd.m_visualizerCameraResultArgs.m_camForward,
tmpCmd.m_visualizerCameraResultArgs.m_horizontal,
tmpCmd.m_visualizerCameraResultArgs.m_vertical,
&tmpCmd.m_visualizerCameraResultArgs.m_yaw,
&tmpCmd.m_visualizerCameraResultArgs.m_pitch,
&tmpCmd.m_visualizerCameraResultArgs.m_dist,
tmpCmd.m_visualizerCameraResultArgs.m_target);
&tmpCamResult.m_width,
&tmpCamResult.m_height,
tmpCamResult.m_viewMatrix,
tmpCamResult.m_projectionMatrix,
tmpCamResult.m_camUp,
tmpCamResult.m_camForward,
tmpCamResult.m_horizontal,
tmpCamResult.m_vertical,
&tmpCamResult.m_yaw,
&tmpCamResult.m_pitch,
&tmpCamResult.m_dist,
tmpCamResult.m_target);
if (result)
{
m_data->m_visualConverter.render(tmpCmd.m_visualizerCameraResultArgs.m_viewMatrix,
tmpCmd.m_visualizerCameraResultArgs.m_projectionMatrix);
m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix,
tmpCamResult.m_projectionMatrix);
} else
{
m_data->m_visualConverter.render();

22
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -686,17 +686,19 @@ void TinyRendererVisualShapeConverter::changeRGBAColor(int bodyUniqueId, int lin
break;
}
}
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(start);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
for (int v=0;v<visualArray->m_renderObjects.size();v++)
if (start>=0)
{
visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(start);
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(start);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
float rgba[4] = {rgbaColor[0], rgbaColor[1], rgbaColor[2], rgbaColor[3]};
for (int v=0;v<visualArray->m_renderObjects.size();v++)
{
visualArray->m_renderObjects[v]->m_model->setColorRGBA(rgba);
}
}
}

71
examples/pybullet/examples/racecar_differential.py Normal file
View File

@@ -0,0 +1,71 @@
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=1000)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
#p.loadURDF("plane.urdf")
p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
steering = [0,2]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce)
for steer in steering:
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle)
steering
if (useRealTimeSim==0):
p.stepSimulation()
time.sleep(0.01)

3
examples/pybullet/gym/envs/bullet/kuka.py
View File

@@ -72,9 +72,10 @@ class Kuka:
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
pos = state[0]
orn = state[1]
euler = p.getEulerFromQuaternion(orn)
observation.extend(list(pos))
observation.extend(list(orn))
observation.extend(list(euler))
return observation

21
examples/pybullet/gym/envs/bullet/kukaGymEnv.py
View File

@@ -31,6 +31,7 @@ class KukaGymEnv(gym.Env):
self._p = p
if self._renders:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
else:
p.connect(p.DIRECT)
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
@@ -52,8 +53,7 @@ class KukaGymEnv(gym.Env):
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
if self._renders:
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.05*random.random()
@@ -78,14 +78,21 @@ class KukaGymEnv(gym.Env):
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
pos,orn = p.getBasePositionAndOrientation(self.blockUid)
self._observation.extend(list(pos))
self._observation.extend(list(orn))
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
endEffectorPos = eeState[0]
endEffectorOrn = eeState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
self._observation.extend(list(blockPosInEE))
self._observation.extend(list(blockEulerInEE))
return self._observation
def _step(self, action):
dv = 0.002
dv = 0.01
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]