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

Merge pull request #1186 from erwincoumans/master

Browse Source 
add debug view for getCameraImage (RGB, depth, segmentation mask)
This commit is contained in:
erwincoumans
2017-06-13 18:55:11 -07:00
committed by GitHub
parent 5692137554 a0ded43a69
commit 7929ba2929
15 changed files with 4265 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
data/sphere2red.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="urdf_robot">
<link name="baseLink">
<contact>
<rolling_friction value="0.03"/>
<spinning_friction value="0.03"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="red">
<color rgba="1 0.2 0.2 1"/>
<specular rgb="1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.5"/>
</geometry>
</collision>
</link>
</robot>

10
data/sphere_smooth.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 1. 0.2 0.2
Ks 0.8 0.8 0.8
d 1
illum 2

3725
data/sphere_smooth.obj Normal file
View File

File diff suppressed because it is too large Load Diff

6
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

@@ -83,6 +83,12 @@ struct GUIHelperInterface
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)=0;
virtual void debugDisplayCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied){}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) =0;

6
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -22,8 +22,8 @@ struct MyMotorInfo2
int m_qIndex;
};
int camVisualizerWidth = 320;//1024/3;
int camVisualizerHeight = 240;//768/3;
static int camVisualizerWidth = 320;//1024/3;
static int camVisualizerHeight = 240;//768/3;
enum CustomCommands
{
@@ -863,8 +863,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
btClamp(yIndex,0,imageData.m_pixelHeight);
if (m_canvasDepthIndex >=0)
{

15
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3009,6 +3009,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,width,height,&numPixelsCopied);
m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,
depthBuffer,numRequestedPixels,
0, numRequestedPixels,
startPixelIndex,width,height,&numPixelsCopied);
} else
{
@@ -3067,6 +3075,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,&width,&height,&numPixelsCopied);
m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels,
depthBuffer,numRequestedPixels,
segmentationMaskBuffer, numRequestedPixels,
startPixelIndex,width,height,&numPixelsCopied);
}
//each pixel takes 4 RGBA values and 1 float = 8 bytes

220
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -5,7 +5,7 @@
#include "PhysicsServerExample.h"
#include "../CommonInterfaces/Common2dCanvasInterface.h"
#include "PhysicsServerSharedMemory.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "SharedMemoryCommon.h"
@@ -27,6 +27,8 @@ bool gEnableTeleporting=true;
bool gEnableRendering= true;
bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true;
static int gCamVisualizerWidth = 320;
static int gCamVisualizerHeight = 240;
//extern btVector3 gLastPickPos;
@@ -114,6 +116,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
eGUIHelperCreateRigidBodyGraphicsObject,
eGUIHelperRemoveAllGraphicsInstances,
eGUIHelperCopyCameraImageData,
eGUIHelperDisplayCameraImageData,
eGUIHelperAutogenerateGraphicsObjects,
eGUIUserDebugAddText,
eGUIUserDebugAddLine,
@@ -939,7 +942,7 @@ public:
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
return m_childGuiHelper->get2dCanvasInterface();
}
virtual CommonParameterInterface* getParameterInterface()
@@ -1014,6 +1017,32 @@ public:
workerThreadWait();
}
virtual void debugDisplayCameraImageData(const float viewMatrix[16], const float projectionMatrix[16],
unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
float* depthBuffer, int depthBufferSizeInPixels,
int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
{
m_cs->lock();
for (int i=0;i<16;i++)
{
m_viewMatrix[i] = viewMatrix[i];
m_projectionMatrix[i] = projectionMatrix[i];
}
m_pixelsRGBA = pixelsRGBA;
m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
m_depthBuffer = depthBuffer;
m_depthBufferSizeInPixels = depthBufferSizeInPixels;
m_segmentationMaskBuffer = segmentationMaskBuffer;
m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
m_startPixelIndex = startPixelIndex;
m_destinationWidth = destinationWidth;
m_destinationHeight = destinationHeight;
m_numPixelsCopied = numPixelsCopied;
m_cs->setSharedParam(1,eGUIHelperDisplayCameraImageData);
workerThreadWait();
}
btDiscreteDynamicsWorld* m_dynamicsWorld;
@@ -1179,6 +1208,12 @@ class PhysicsServerExample : public SharedMemoryCommon
bool m_isConnected;
btClock m_clock;
bool m_replay;
struct Common2dCanvasInterface* m_canvas;
int m_canvasRGBIndex;
int m_canvasDepthIndex;
int m_canvasSegMaskIndex;
// int m_options;
#ifdef BT_ENABLE_VR
@@ -1492,7 +1527,11 @@ PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper,
m_physicsServer(commandProcessorCreator,sharedMem,0),
m_wantsShutdown(false),
m_isConnected(false),
m_replay(false)
m_replay(false),
m_canvas(0),
m_canvasRGBIndex(-1),
m_canvasDepthIndex(-1),
m_canvasSegMaskIndex(-1)
//m_options(options)
#ifdef BT_ENABLE_VR
,m_tinyVrGui(0)
@@ -1507,6 +1546,15 @@ m_replay(false)
PhysicsServerExample::~PhysicsServerExample()
{
if (m_canvas)
{
if (m_canvasRGBIndex>=0)
m_canvas->destroyCanvas(m_canvasRGBIndex);
if (m_canvasDepthIndex>=0)
m_canvas->destroyCanvas(m_canvasDepthIndex);
if (m_canvasSegMaskIndex>=0)
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
#ifdef BT_ENABLE_VR
delete m_tinyVrGui;
@@ -1580,6 +1628,52 @@ void PhysicsServerExample::initPhysics()
m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
{
m_canvas = m_guiHelper->get2dCanvasInterface();
if (m_canvas)
{
m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight);
//m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight);
//m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight);
for (int i=0;i<gCamVisualizerWidth;i++)
{
for (int j=0;j<gCamVisualizerHeight;j++)
{
unsigned char red=255;
unsigned char green=255;
unsigned char blue=255;
unsigned char alpha=255;
if (i==j)
{
red = 0;
green=0;
blue=0;
}
m_canvas->setPixel(m_canvasRGBIndex,i,j,red,green,blue,alpha);
if (m_canvasSegMaskIndex>=0)
m_canvas->setPixel(m_canvasDepthIndex,i,j,red,green,blue,alpha);
if (m_canvasSegMaskIndex>=0)
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,red,green,blue,alpha);
}
}
m_canvas->refreshImageData(m_canvasRGBIndex);
if (m_canvasDepthIndex>=0)
m_canvas->refreshImageData(m_canvasDepthIndex);
if (m_canvasSegMaskIndex>=0)
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
}
}
@@ -1760,7 +1854,124 @@ void PhysicsServerExample::updateGraphics()
break;
}
case eGUIHelperDisplayCameraImageData:
{
if (m_canvas)
{
int numBytesPerPixel= 4;
int startRGBIndex = m_multiThreadedHelper->m_startPixelIndex*numBytesPerPixel;
int endRGBIndex = startRGBIndex + (*m_multiThreadedHelper->m_numPixelsCopied*numBytesPerPixel);
int startDepthIndex = m_multiThreadedHelper->m_startPixelIndex;
int endDepthIndex = startDepthIndex + (*m_multiThreadedHelper->m_numPixelsCopied);
int startSegIndex = m_multiThreadedHelper->m_startPixelIndex;
int endSegIndex = startSegIndex + (*m_multiThreadedHelper->m_numPixelsCopied);
btScalar frustumZNear = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]-1);
btScalar frustumZFar = 20;//m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]+1);
for (int i=0;i<gCamVisualizerWidth;i++)
{
for (int j=0;j<gCamVisualizerHeight;j++)
{
int xIndex = int(float(i)*(float(m_multiThreadedHelper->m_destinationWidth)/float(gCamVisualizerWidth)));
int yIndex = int(float(j)*(float(m_multiThreadedHelper->m_destinationHeight)/float(gCamVisualizerHeight)));
btClamp(xIndex,0,m_multiThreadedHelper->m_destinationWidth);
btClamp(yIndex,0,m_multiThreadedHelper->m_destinationHeight);
int bytesPerPixel = 4; //RGBA
if (m_canvasRGBIndex >=0)
{
int rgbPixelIndex = (xIndex+yIndex*m_multiThreadedHelper->m_destinationWidth)*bytesPerPixel;
if (rgbPixelIndex >= startRGBIndex && rgbPixelIndex < endRGBIndex)
{
m_canvas->setPixel(m_canvasRGBIndex,i,j,
m_multiThreadedHelper->m_pixelsRGBA[rgbPixelIndex-startRGBIndex],
m_multiThreadedHelper->m_pixelsRGBA[rgbPixelIndex+1-startRGBIndex],
m_multiThreadedHelper->m_pixelsRGBA[rgbPixelIndex+2-startRGBIndex],
255); //alpha set to 255
}
}
if (m_canvasDepthIndex >=0 && 0!= m_multiThreadedHelper->m_depthBuffer)
{
int depthPixelIndex = (xIndex+yIndex*m_multiThreadedHelper->m_destinationWidth);
if (depthPixelIndex >= startDepthIndex && depthPixelIndex < endDepthIndex)
{
float depthValue = m_multiThreadedHelper->m_depthBuffer[depthPixelIndex-startDepthIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue>-1e20)
{
int rgb = 0;
btScalar minDepthValue = 0.98;//todo: compute more reasonably min/max depth range
btScalar maxDepthValue = 1;
if (maxDepthValue!=minDepthValue)
{
rgb = (depthValue-minDepthValue)*(255. / (btFabs(maxDepthValue-minDepthValue)));
if (rgb<0 || rgb>255)
{
//printf("rgb=%d\n",rgb);
}
}
m_canvas->setPixel(m_canvasDepthIndex,i,j,
rgb,
rgb,
255, 255); //alpha set to 255
} else
{
m_canvas->setPixel(m_canvasDepthIndex,i,j,
0,
0,
0, 255); //alpha set to 255
}
}
}
if (m_canvasSegMaskIndex>=0 && (0!=m_multiThreadedHelper->m_segmentationMaskBuffer))
{
int segmentationMaskPixelIndex = (xIndex+yIndex*m_multiThreadedHelper->m_destinationWidth);
if (segmentationMaskPixelIndex >= startSegIndex && segmentationMaskPixelIndex < endSegIndex)
{
int segmentationMask = m_multiThreadedHelper->m_segmentationMaskBuffer[segmentationMaskPixelIndex-startSegIndex];
btVector4 palette[4] = {btVector4(32,255,32,255),
btVector4(32,32,255,255),
btVector4(255,255,32,255),
btVector4(32,255,255,255)};
if (segmentationMask>=0)
{
btVector4 rgb = palette[segmentationMask&3];
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
rgb.x(),
rgb.y(),
rgb.z(), 255); //alpha set to 255
} else
{
m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
0,
0,
0, 255); //alpha set to 255
}
}
}
}
}
if (m_canvasRGBIndex >=0)
m_canvas->refreshImageData(m_canvasRGBIndex);
if (m_canvasDepthIndex >=0)
m_canvas->refreshImageData(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >=0)
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperCopyCameraImageData:
{
m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix,
@@ -1775,7 +1986,8 @@ void PhysicsServerExample::updateGraphics()
m_multiThreadedHelper->m_destinationWidth,
m_multiThreadedHelper->m_destinationHeight,
m_multiThreadedHelper->m_numPixelsCopied);
m_multiThreadedHelper->mainThreadRelease();
m_multiThreadedHelper->mainThreadRelease();
break;
}
case eGUIHelperAutogenerateGraphicsObjects:

26
examples/pybullet/gym/enjoy_pybullet_zed_racecar.py Normal file
View File

@@ -0,0 +1,26 @@
import gym
from envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
from baselines import deepq
def main():
env = RacecarZEDGymEnv(renders=True)
act = deepq.load("racecar_zed_model.pkl")
print(act)
while True:
obs, done = env.reset(), False
print("===================================")
print("obs")
print(obs)
episode_rew = 0
while not done:
env.render()
obs, rew, done, _ = env.step(act(obs[None])[0])
episode_rew += rew
print("Episode reward", episode_rew)
if __name__ == '__main__':
main()

7
examples/pybullet/gym/envs/__init__.py
View File

@@ -22,3 +22,10 @@ register(
timestep_limit=1000,
reward_threshold=5.0,
)
register(
id='RacecarZedBulletEnv-v0',
entry_point='envs.bullet:RacecarZEDGymEnv',
timestep_limit=1000,
reward_threshold=5.0,
)

148
examples/pybullet/gym/envs/bullet/racecarZEDGymEnv.py Normal file
View File

@@ -0,0 +1,148 @@
import math
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import time
import pybullet as p
from . import racecar
import random
class RacecarZEDGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
def __init__(self,
urdfRoot="",
actionRepeat=100,
isEnableSelfCollision=True,
renders=True):
print("init")
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._ballUniqueId = -1
self._envStepCounter = 0
self._renders = renders
self._width = 100
self._height = 10
self._p = p
if self._renders:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(6)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
self.viewer = None
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
#p.loadURDF("%splane.urdf" % self._urdfRoot)
stadiumobjects = p.loadSDF("%sstadium.sdf" % self._urdfRoot)
#move the stadium objects slightly above 0
for i in stadiumobjects:
pos,orn = p.getBasePositionAndOrientation(i)
newpos = [pos[0],pos[1],pos[2]+0.1]
p.resetBasePositionAndOrientation(i,newpos,orn)
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
self._ballUniqueId = p.loadURDF("sphere2red.urdf",[ballx,bally,ballz])
p.setGravity(0,0,-10)
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
for i in range(100):
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def __del__(self):
p.disconnect()
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def getExtendedObservation(self):
carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
carmat = p.getMatrixFromQuaternion(carorn)
ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId)
invCarPos,invCarOrn = p.invertTransform(carpos,carorn)
ballPosInCar,ballOrnInCar = p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn)
dist0 = 0.3
dist1 = 1.
eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3]
targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3]
up = [carmat[2],carmat[5],carmat[8]]
viewMat = p.computeViewMatrix(eyePos,targetPos,up)
#viewMat = p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
#print("projectionMatrix:")
#print(p.getDebugVisualizerCamera()[3])
projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
rgb=img_arr[2]
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
self._observation = np_img_arr
return self._observation
def _step(self, action):
if (self._renders):
basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
#p.resetDebugVisualizerCamera(1, 30, -40, basePos)
fwd = [5,0,5,10,10,10]
steerings = [-0.5,0,0.5,-0.3,0,0.3]
forward = fwd[action]
steer = steerings[action]
realaction = [forward,steer]
self._racecar.applyAction(realaction)
for i in range(self._actionRepeat):
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
if self._termination():
break
self._envStepCounter += 1
reward = self._reward()
done = self._termination()
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
return
def _termination(self):
return self._envStepCounter>1000
def _reward(self):
closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
numPt = len(closestPoints)
reward=-1000
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]
#print(reward)
return reward

2
examples/pybullet/gym/racecarGymEnvTest.py
View File

@@ -1,7 +1,7 @@
from envs.bullet.racecarGymEnv import RacecarGymEnv
print ("hello")
environment = RacecarGymEnv(render=True)
environment = RacecarGymEnv(renders=True)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)

41
examples/pybullet/gym/train_pybullet_zed_racecar.py Normal file
View File

@@ -0,0 +1,41 @@
import gym
from envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
from baselines import deepq
import datetime
def callback(lcl, glb):
# stop training if reward exceeds 199
total = sum(lcl['episode_rewards'][-101:-1]) / 100
totalt = lcl['t']
is_solved = totalt > 2000 and total >= -50
return is_solved
def main():
env = RacecarZEDGymEnv(renders=False)
model = deepq.models.cnn_to_mlp(
convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)],
hiddens=[256],
dueling=False
)
act = deepq.learn(
env,
q_func=model,
lr=1e-3,
max_timesteps=10000,
buffer_size=50000,
exploration_fraction=0.1,
exploration_final_eps=0.02,
print_freq=10,
callback=callback
)
print("Saving model to racecar_zed_model.pkl")
act.save("racecar_zed_model.pkl")
if __name__ == '__main__':
main()

2
setup.py
View File

@@ -419,7 +419,7 @@ else:
setup(
name = 'pybullet',
version='1.1.4',
version='1.1.6',
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

26
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -154,6 +154,8 @@ void btMultiBody::setupFixed(int i,
m_links[i].m_mass = mass;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].setAxisTop(0, 0., 0., 0.);
m_links[i].setAxisBottom(0, btVector3(0,0,0));
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector = thisPivotToThisComOffset;
m_links[i].m_eVector = parentComToThisPivotOffset;
@@ -522,7 +524,8 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
vel[0] = quatRotate(m_baseQuat ,getBaseVel());
for (int i = 0; i < num_links; ++i) {
for (int i = 0; i < num_links; ++i)
{
const int parent = m_links[i].m_parent;
// transform parent vel into this frame, store in omega[i+1], vel[i+1]
@@ -531,9 +534,24 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
omega[i+1], vel[i+1]);
// now add qidot * shat_i
omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
}
//only supported for revolute/prismatic joints, todo: spherical and planar joints
switch(m_links[i].m_jointType)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
btVector3 axisTop = m_links[i].getAxisTop(0);
btVector3 axisBottom = m_links[i].getAxisBottom(0);
btScalar jointVel = getJointVel(i);
omega[i+1] += jointVel * axisTop;
vel[i+1] += jointVel * axisBottom;
break;
}
default:
{
}
}
}
}
btScalar btMultiBody::getKineticEnergy() const

15
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -96,9 +96,18 @@ struct btMultibodyLink
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
void setAxisBottom(int dof, const btVector3 &axis)
{
m_axes[dof].m_bottomVec = axis;
}
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_topVec.setValue(x, y, z);
}
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_bottomVec.setValue(x, y, z);
}
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }