diff --git a/data/stadium.sdf b/data/stadium.sdf
index 04ee4ea52..585b4eb16 100644
--- a/data/stadium.sdf
+++ b/data/stadium.sdf
@@ -48,12 +48,11 @@
- 0 0 -0.5 0 0 0
-
-
- 100 100 1
-
+
+ 0 0 1
+ 100 100
+
diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf
index 98ae9f995..7fda241bd 100644
--- a/data/two_cubes.sdf
+++ b/data/two_cubes.sdf
@@ -89,6 +89,7 @@
1
0.512455 -1.58317 0.5 0 -0 0
+
1
@@ -102,8 +103,6 @@
- 0 0 -1 0 0 0
-
1 1 1
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 6ae007607..72aaecfce 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -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;
}
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index ed7d238f6..431abcb95 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -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
diff --git a/examples/pybullet/examples/humanoid_knee_position_control.py b/examples/pybullet/examples/humanoid_knee_position_control.py
index a75c66263..b62bd76a1 100644
--- a/examples/pybullet/examples/humanoid_knee_position_control.py
+++ b/examples/pybullet/examples/humanoid_knee_position_control.py
@@ -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]
diff --git a/examples/pybullet/tensorflow/humanoid_running.py b/examples/pybullet/tensorflow/humanoid_running.py
index 8dea38298..c4c741f10 100644
--- a/examples/pybullet/tensorflow/humanoid_running.py
+++ b/examples/pybullet/tensorflow/humanoid_running.py
@@ -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)
diff --git a/setup.py b/setup.py
index f509fe88f..8c81da317 100644
--- a/setup.py
+++ b/setup.py
@@ -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',
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 8d89cac7d..6dad0afeb 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -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;
}
//