Merge pull request #1164 from erwincoumans/master

allow plane collision in SDF, 	fix some warnings
This commit is contained in:
erwincoumans 2017-06-02 08:05:50 -07:00 committed by GitHub
commit e47a4d61c1
12 changed files with 95 additions and 73 deletions

View File

@ -48,12 +48,11 @@
</inertia>
</inertial>
<collision name='collision_1'>
<pose frame=''>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>100 100 1</size>
</box>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name='visual'>

View File

@ -89,6 +89,7 @@
<static>1</static>
<pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
<link name='unit_box_0::link'>
<inertial>
<mass>1</mass>
<inertia>
@ -102,8 +103,6 @@
</inertial>
<collision name='collision'>
<geometry>
<pose frame=''>0 0 -1 0 0 0</pose>
<box>
<size>1 1 1</size>
</box>

View File

@ -568,6 +568,15 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
switch (collision->m_geometry.m_type)
{
case URDF_GEOM_PLANE:
{
btVector3 planeNormal = collision->m_geometry.m_planeNormal;
btScalar planeConstant = 0;//not available?
btStaticPlaneShape* plane = new btStaticPlaneShape(planeNormal,planeConstant);
shape = plane;
shape ->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_CAPSULE:
{
btScalar radius = collision->m_geometry.m_capsuleRadius;
@ -789,7 +798,7 @@ upAxisMat.setIdentity();
default:
b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
// for example, URDF_GEOM_PLANE
}
return shape;
}

View File

@ -413,10 +413,10 @@ bool GLInstancingRenderer::readSingleInstanceTransformToCPU(float* position, flo
return false;
}
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
if (pg==0)
@ -439,9 +439,9 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
}
void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId, float* position, float* orientation)
void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex2, float* position, float* orientation)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -457,9 +457,9 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId,
orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -469,9 +469,9 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, in
m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]);
}
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -481,9 +481,9 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int
m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -492,11 +492,11 @@ void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int
m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
}
void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const double* specular, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
int graphicsIndex = pg->m_internalInstanceIndex;
int totalNumInstances = 0;
@ -505,7 +505,7 @@ void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const double* s
for (int i=0;i<m_graphicsInstances.size();i++)
{
totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
if (srcIndex<totalNumInstances)
if (srcIndex2<totalNumInstances)
{
gfxObjIndex = i;
break;
@ -518,9 +518,9 @@ void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const double* s
m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[2] = specular[2];
}
}
void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const float* specular, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -531,7 +531,7 @@ void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const float* sp
for (int i=0;i<m_graphicsInstances.size();i++)
{
totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
if (srcIndex<totalNumInstances)
if (srcIndex2<totalNumInstances)
{
gfxObjIndex = i;
break;
@ -546,9 +546,9 @@ void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const float* sp
}
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int bodyUniqueId)
void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int srcIndex2)
{
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -765,8 +765,8 @@ void GLInstancingRenderer::rebuildGraphicsInstances()
for (int i=0;i<usedObjects.size();i++)
{
int bodyUniqueId = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
int srcIndex2 = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
b3Assert(pg);
int srcIndex = pg->m_internalInstanceIndex;
@ -793,9 +793,9 @@ void GLInstancingRenderer::rebuildGraphicsInstances()
}
for (int i=0;i<usedObjects.size();i++)
{
int bodyUniqueId = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(bodyUniqueId);
int srcIndex2 = usedObjects[i];
b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(srcIndex2);
}
int curOffset = 0;

View File

@ -103,8 +103,8 @@ public:
virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex);
virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex);
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex);
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex);
virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2);
virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2);
virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex);
virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex);

View File

@ -3940,38 +3940,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
else
{
if (mb->getLinkCollider(linkIndex))
if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
if (mb->getLinkCollider(linkIndex))
{
mb->getLinkCollider(linkIndex)->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{
mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
mb->getLinkCollider(linkIndex)->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
{
mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
mb->getLink(linkIndex).m_mass = mass;
if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
{
btVector3 localInertia;
mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia);
mb->getLink(linkIndex).m_inertiaLocal = localInertia;
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
mb->getLink(linkIndex).m_mass = mass;
if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
{
btVector3 localInertia;
mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia);
mb->getLink(linkIndex).m_inertiaLocal = localInertia;
}
}
}
}
} else

View File

@ -94,7 +94,7 @@ public:
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
virtual void stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
@ -103,10 +103,10 @@ public:
virtual void setTimeOut(double timeOutInSeconds);
virtual const btVector3& getVRTeleportPosition() const;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos);
virtual void setVRTeleportPosition(const btVector3& vrTeleportPos);
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn);
virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

View File

@ -41,10 +41,10 @@ public:
virtual void removePickingConstraint();
virtual const btVector3& getVRTeleportPosition() const;
virtual void setVRTeleportPosition(const btVector3& vrReleportPos);
virtual void setVRTeleportPosition(const btVector3& vrTeleportPos);
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn);
virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);

View File

@ -13,7 +13,7 @@ p.setRealTimeSimulation(useRealTime)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf")
p.loadSDF("stadium.sdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0]

View File

@ -1,21 +1,30 @@
import tensorflow as tf
import sys
import numpy as np
import argparse
import pybullet as p
import time
p.connect(p.GUI) #DIRECT is much faster, but GUI shows the running gait
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI) #DIRECT is much faster, but GUI shows the running gait
p.setGravity(0,0,-9.8)
p.setPhysicsEngineParameter(fixedTimeStep=1.0/60., numSolverIterations=5, numSubSteps=2)
#this mp4 recording requires ffmpeg installed
#mp4log = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,"humanoid.mp4")
p.loadSDF("stadium.sdf")
#p.loadURDF("plane.urdf")
plane, human = p.loadMJCF("mjcf/humanoid_symmetric.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
objs = p.loadMJCF("mjcf/humanoid_symmetric_no_ground.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
human = objs[0]
ordered_joints = []
ordered_joint_indices = []
parser = argparse.ArgumentParser()
parser.add_argument('--profile')
jdict = {}
for j in range( p.getNumJoints(human) ):
@ -175,7 +184,7 @@ def demo_run():
p.setJointMotorControlArray(human, motors,controlMode=p.TORQUE_CONTROL, forces=forces)
p.stepSimulation()
#time.sleep(0.01)
time.sleep(0.01)
distance=5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(human)

View File

@ -418,7 +418,7 @@ else:
setup(
name = 'pybullet',
version='1.1.0',
version='1.1.1',
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',

View File

@ -142,7 +142,11 @@ public:
btMultiBodyLinkCollider* getLinkCollider(int index)
{
return m_colliders[index];
if (index >= 0 && index < m_colliders.size())
{
return m_colliders[index];
}
return 0;
}
//