From 903cc96c1ab80b0c21fdce63eb43ce9c9d4d16d5 Mon Sep 17 00:00:00 2001 From: Sam Hocevar Date: Fri, 1 Sep 2017 18:46:07 +0200 Subject: [PATCH 1/8] Fix one-sided soft bodies. A coding mistake in btSoftBody::addAeroForceToNode() was applying forces in the wrong direction for one-sided soft bodies. --- src/BulletSoftBody/btSoftBody.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index d5de7c1b4..52f8cd897 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -524,7 +524,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) { - if (btSoftBody::eAeroModel::V_TwoSided) + if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); const btScalar dvn = btDot(rel_v,nrm); @@ -619,7 +619,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) { - if (btSoftBody::eAeroModel::F_TwoSided) + if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); const btScalar dvn=btDot(rel_v,nrm); From 6942a9f8ef669867d9434027015c9f775e5d167a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 13:48:37 -0700 Subject: [PATCH 2/8] remove a debug printf --- examples/ExampleBrowser/main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 180f94a77..07c168309 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -29,7 +29,6 @@ static OpenGLExampleBrowser* sExampleBrowser=0; #include static void cleanup(int signo) { -printf("SIG cleanup: %d\n", signo); if (interrupted) { // this is the second time, we're hanging somewhere // if (example) { From 7e0ca070e0d9d20b1db88a248a38ed4b52b0906e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 16:08:30 -0700 Subject: [PATCH 3/8] fix return values in pybullet to be Pythonic. --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 82249434a..4cad53bd7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -448,7 +448,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P sPhysicsClients1[freeIndex] = 0; sPhysicsClientsGUI[freeIndex] = 0; sNumPhysicsClients++; - return -1; + return PyInt_FromLong(-1); } } } From 74475479cfa0fe5d72faaf5c8b2ebc2793695c43 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 17:05:23 -0700 Subject: [PATCH 4/8] fix pybullet, checked wrong type after connection --- examples/pybullet/pybullet.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 4cad53bd7..c1f7e2eb2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -440,7 +440,8 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P command = b3InitSyncBodyInfoCommand(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); - if (statusType != CMD_BODY_INFO_COMPLETED) + + if (statusType != CMD_SYNC_BODY_INFO_COMPLETED) { printf("Connection terminated, couldn't get body info\n"); b3DisconnectSharedMemory(sm); From 2e8a86462f66e2938e9801735bcbf64f488ec783 Mon Sep 17 00:00:00 2001 From: Benelot Date: Thu, 14 Sep 2017 12:52:21 +0200 Subject: [PATCH 5/8] Remove tweaks added by me but not part of roboschool. --- examples/pybullet/gym/pybullet_envs/env_bases.py | 2 +- examples/pybullet/gym/pybullet_envs/robot_pendula.py | 11 ++++------- examples/pybullet/gym/pybullet_envs/scene_abstract.py | 6 ------ 3 files changed, 5 insertions(+), 14 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 8b16cd7be..ec12d8c65 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -44,7 +44,7 @@ class MJCFBaseBulletEnv(gym.Env): else: self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) - + if self.scene is None: self.scene = self.create_single_player_scene() if not self.scene.multiplayer: diff --git a/examples/pybullet/gym/pybullet_envs/robot_pendula.py b/examples/pybullet/gym/pybullet_envs/robot_pendula.py index 1c6b14b05..0c82fea43 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) @@ -16,16 +15,16 @@ class InvertedPendulum(MJCFBasedRobot): self.j1.set_motor_torque(0) def apply_action(self, a): - #assert( np.isfinite(a).all() ) + assert( np.isfinite(a).all() ) 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() x, vx = self.slider.current_position() - #assert( np.isfinite(x) ) + assert( np.isfinite(x) ) if not np.isfinite(x): print("x is inf") @@ -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() diff --git a/examples/pybullet/gym/pybullet_envs/scene_abstract.py b/examples/pybullet/gym/pybullet_envs/scene_abstract.py index 11ae8d489..51fd81f77 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_abstract.py +++ b/examples/pybullet/gym/pybullet_envs/scene_abstract.py @@ -15,10 +15,6 @@ class Scene: self.dt = self.timestep * self.frame_skip self.cpp_world = World(gravity, timestep, frame_skip) - #self.cpp_world.set_glsl_path(os.path.join(os.path.dirname(__file__), "cpp-household/glsl")) - - #self.big_caption = self.cpp_world.test_window_big_caption # that's a function you can call - #self.console_print = self.cpp_world.test_window_print # this too self.test_window_still_open = True # or never opened self.human_render_detected = False # if user wants render("human"), we open test window @@ -52,8 +48,6 @@ class Scene: The idea is: apply motor torques for all robots, then call global_step(), then collect observations from robots using step() with the same action. """ - #if self.human_render_detected: - # self.test_window_still_open = self.cpp_world.test_window() self.cpp_world.step(self.frame_skip) class SingleRobotEmptyScene(Scene): From 3f57fb655aef5ab276e78fc9bd0ab39a3e8b1a84 Mon Sep 17 00:00:00 2001 From: Benelot Date: Thu, 14 Sep 2017 12:53:12 +0200 Subject: [PATCH 6/8] Enable randomized reset for Humanoid. --- .../gym/pybullet_envs/robot_locomotors.py | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index a17055675..98ba67780 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -152,23 +152,25 @@ class Humanoid(WalkerBase): self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"] self.motor_power += [75, 75, 75] self.motors = [self.jdict[n] for n in self.motor_names] - # if self.random_yaw: # TODO: Make leaning work as soon as the rest works - # cpose = cpp_household.Pose() - # yaw = self.np_random.uniform(low=-3.14, high=3.14) - # if self.random_lean and self.np_random.randint(2)==0: - # cpose.set_xyz(0, 0, 1.4) - # if self.np_random.randint(2)==0: - # pitch = np.pi/2 - # cpose.set_xyz(0, 0, 0.45) - # else: - # pitch = np.pi*3/2 - # cpose.set_xyz(0, 0, 0.25) - # roll = 0 - # cpose.set_rpy(roll, pitch, yaw) - # else: - # cpose.set_xyz(0, 0, 1.4) - # cpose.set_rpy(0, 0, yaw) # just face random direction, but stay straight otherwise - # self.cpp_robot.set_pose_and_speed(cpose, 0,0,0) + if self.random_yaw: + position = [0,0,0] + orientation = [0,0,0] + yaw = self.np_random.uniform(low=-3.14, high=3.14) + if self.random_lean and self.np_random.randint(2)==0: + cpose.set_xyz(0, 0, 1.4) + if self.np_random.randint(2)==0: + pitch = np.pi/2 + position = [0, 0, 0.45] + else: + pitch = np.pi*3/2 + position = [0, 0, 0.25] + roll = 0 + orientation = [roll, pitch, yaw] + else: + position = [0, 0, 1.4] + orientation = [0, 0, yaw] # just face random direction, but stay straight otherwise + self.robot_body.reset_position(position) + self.robot_body.reset_orientation(orientation) self.initial_z = 0.8 random_yaw = False From 06b695e0512baeb4db0958cb08f4e47236e9da44 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 14 Sep 2017 08:08:24 -0700 Subject: [PATCH 7/8] remove some warning ,see also Pull Request #302 bump up pybullet version --- setup.py | 2 +- src/Bullet3Common/b3Vector3.h | 1 - src/LinearMath/btVector3.h | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/setup.py b/setup.py index 5cae36c27..b47b95c39 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.4.0', + version='1.4.2', 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', diff --git a/src/Bullet3Common/b3Vector3.h b/src/Bullet3Common/b3Vector3.h index 50504c7f7..16ec02b0e 100644 --- a/src/Bullet3Common/b3Vector3.h +++ b/src/Bullet3Common/b3Vector3.h @@ -1095,7 +1095,6 @@ public: if (m_floats[3] > maxVal) { maxIndex = 3; - maxVal = m_floats[3]; } return maxIndex; diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 8d0b73f88..fdf3fd796 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -1159,7 +1159,6 @@ public: if (m_floats[3] > maxVal) { maxIndex = 3; - maxVal = m_floats[3]; } return maxIndex; From 2e7c0cef38b751a3ca1d82efc75251b1e2806e33 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 14 Sep 2017 15:39:22 -0700 Subject: [PATCH 8/8] pybullet/C-API: fix width/height when getCameraImage has a fallback from OpenGL hardware to TinyRenderer --- .../PhysicsServerCommandProcessor.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index db318f056..1a1b13275 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3477,21 +3477,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm - if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) - { - //m_data->m_guiHelper->copyCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,clientCmd.m_requestPixelDataArguments.m_projectionMatrix,0,0,0,0,0,width,height,0); - } - else - { - if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) - { + { m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); - } - m_data->m_visualConverter.getWidthAndHeight(width,height); - } - + } int numTotalPixels = width*height;