Merge branch 'master' of https://github.com/erwincoumans/bullet3

11 files changed, 31 insertions, 46 deletions

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 <unistd.h>
 static void cleanup(int signo)
 {
-printf("SIG cleanup: %d\n", signo);
 
     if (interrupted) {  // this is the second time, we're hanging somewhere
       //  if (example) {
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;
diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py
index f291d556b..2d079e625 100644
--- a/examples/pybullet/gym/pybullet_envs/env_bases.py
+++ b/examples/pybullet/gym/pybullet_envs/env_bases.py
@@ -46,7 +46,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_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
diff --git a/examples/pybullet/gym/pybullet_envs/robot_pendula.py b/examples/pybullet/gym/pybullet_envs/robot_pendula.py
index 47dd5e88b..39c2c3878 100644
--- a/examples/pybullet/gym/pybullet_envs/robot_pendula.py
+++ b/examples/pybullet/gym/pybullet_envs/robot_pendula.py
@@ -15,7 +15,7 @@ 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
@@ -24,7 +24,7 @@ class InvertedPendulum(MJCFBasedRobot):
 	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")
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):
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 82249434a..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);
@@ -448,7 +449,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 				sPhysicsClients1[freeIndex] = 0;
                        		sPhysicsClientsGUI[freeIndex] = 0;
                         	sNumPhysicsClients++;
-				return -1;
+				return  PyInt_FromLong(-1);
 			}
 		}
 	}
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/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);
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;