From 3d7ee317db0549c24a90de8c1f75a2003733a939 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 15 Sep 2017 13:09:05 -0700 Subject: [PATCH 1/4] fix pybullet gym environments: InvertedDoublePendulumBulletEnv-v0, InvertedPendulumBulletEnv-v0, InvertedPendulumSwingupBulletEnv-v0 motors were not disabled, extra gains applied etc. --- examples/pybullet/gym/pybullet_envs/robot_bases.py | 3 ++- examples/pybullet/gym/pybullet_envs/robot_pendula.py | 9 +++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 165ba8a09..e3b626adf 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -53,6 +53,7 @@ class MJCFBasedRobot: part_name = part_name.decode("utf8") parts[part_name] = BodyPart(part_name, bodies, i, -1) for j in range(p.getNumJoints(bodies[i])): + p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0) _,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j) joint_name = joint_name.decode("utf8") @@ -209,4 +210,4 @@ class Joint: self.disable_motor() def disable_motor(self): - p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.VELOCITY_CONTROL, force=0) + p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0) diff --git a/examples/pybullet/gym/pybullet_envs/robot_pendula.py b/examples/pybullet/gym/pybullet_envs/robot_pendula.py index 1c6b14b05..47dd5e88b 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_pendula.py +++ b/examples/pybullet/gym/pybullet_envs/robot_pendula.py @@ -3,7 +3,6 @@ import numpy as np class InvertedPendulum(MJCFBasedRobot): swingup = False - force_gain = 12 # TODO: Try to find out why we need to scale the force def __init__(self): MJCFBasedRobot.__init__(self, 'inverted_pendulum.xml', 'cart', action_dim=1, obs_dim=5) @@ -20,7 +19,7 @@ class InvertedPendulum(MJCFBasedRobot): if not np.isfinite(a).all(): print("a is inf") a[0] = 0 - self.slider.set_motor_torque( self.force_gain * 100*float(np.clip(a[0], -1, +1)) ) + self.slider.set_motor_torque( 100*float(np.clip(a[0], -1, +1)) ) def calc_state(self): self.theta, theta_dot = self.j1.current_position() @@ -50,7 +49,6 @@ class InvertedPendulum(MJCFBasedRobot): class InvertedPendulumSwingup(InvertedPendulum): swingup = True - force_gain = 2.2 # TODO: Try to find out why we need to scale the force class InvertedDoublePendulum(MJCFBasedRobot): @@ -70,8 +68,7 @@ class InvertedDoublePendulum(MJCFBasedRobot): def apply_action(self, a): assert( np.isfinite(a).all() ) - force_gain = 0.78 # TODO: Try to find out why we need to scale the force - self.slider.set_motor_torque( force_gain *200*float(np.clip(a[0], -1, +1)) ) + self.slider.set_motor_torque( 200*float(np.clip(a[0], -1, +1)) ) def calc_state(self): theta, theta_dot = self.j1.current_position() @@ -84,4 +81,4 @@ class InvertedDoublePendulum(MJCFBasedRobot): self.pos_x, np.cos(theta), np.sin(theta), theta_dot, np.cos(gamma), np.sin(gamma), gamma_dot, - ]) \ No newline at end of file + ]) From 1881f6679a8df4392bc387ffb0788122f310be9a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 15 Sep 2017 15:38:24 -0700 Subject: [PATCH 2/4] fix issue with rendering of certain capsules from MJCF files. --- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 5ab2e547f..22dad40ea 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -629,12 +629,13 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli textured_detailed_sphere_vertices[i*9+1], textured_detailed_sphere_vertices[i*9+2]); - btVector3 trVer = compound->getChildTransform(0)*(radiusScale*vert); + btVector3 trVer = (radiusScale*vert); if (trVer[up]>0) trVer[up]+=halfHeight; else trVer[up]-=halfHeight; + trVer = compound->getChildTransform(0)*trVer; transformedVertices[i*9+0] = trVer[0]; transformedVertices[i*9+1] = trVer[1]; From f4b28dddce6d8a78081e55ea2460b03f068323ff Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 15 Sep 2017 16:33:22 -0700 Subject: [PATCH 3/4] fix rendering ("rgb_array") for pendulums/pendula --- examples/pybullet/gym/pybullet_envs/env_bases.py | 6 +++++- setup.py | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 2d079e625..ef8ac0c5c 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -67,7 +67,11 @@ class MJCFBaseBulletEnv(gym.Env): self.isRender = True if mode != "rgb_array": return np.array([]) - base_pos = self.robot.body_xyz + + base_pos=[0,0,0] + if (hasattr(self,'robot')): + if (hasattr(self.robot,'body_xyz')): + base_pos = self.robot.body_xyz view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, diff --git a/setup.py b/setup.py index b47b95c39..ecd208318 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.4.2', + version='1.4.5', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement 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', From 2919e51d9d93b17cab575e45f1564d8f1cf41a00 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 17 Sep 2017 15:44:14 -0700 Subject: [PATCH 4/4] fix loading of empty file (after header) remove debug printf --- Extras/Serialize/BulletFileLoader/btBulletFile.cpp | 3 +++ examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/Extras/Serialize/BulletFileLoader/btBulletFile.cpp b/Extras/Serialize/BulletFileLoader/btBulletFile.cpp index 18cb15321..aa1c383fd 100644 --- a/Extras/Serialize/BulletFileLoader/btBulletFile.cpp +++ b/Extras/Serialize/BulletFileLoader/btBulletFile.cpp @@ -128,6 +128,9 @@ void btBulletFile::parseData() mDataStart = 12; remain-=12; + //invalid/empty file? + if (remain < sizeof(bChunkInd)) + return; char *dataPtr = mFileBuffer+mDataStart; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index e8d43c8b1..b82d550a6 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -184,7 +184,6 @@ public: // return non-null if there is a status, nullptr otherwise virtual const struct SharedMemoryStatus* processServerStatus() { - printf("updating graphics!\n"); m_physicsServerExample->updateGraphics(); unsigned long long int curTime = m_clock.getTimeMicroseconds();