Merge pull request #2167 from erwincoumans/master

fix memleak in calculateInverseKinematics + joint limits.

5 files changed, 746 insertions, 193 deletions

diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 0b76fdead..a0dcfe57e 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -149,7 +149,6 @@ SET(BulletExampleBrowser_SRCS
         ../BulletRobotics/BoxStack.cpp
         ../BulletRobotics/JointLimit.cpp
         # ../BulletRobotics/GraspBox.cpp
-        ../BulletRobotics/FixJointBoxes.cpp
 
 	../TinyRenderer/geometry.cpp
 	../TinyRenderer/model.cpp
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 222ad911e..067eb2c64 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -89,7 +89,10 @@ project "App_BulletExampleBrowser"
         "main.cpp",
         "ExampleEntries.cpp",
         "../InverseKinematics/*",
-		"../TinyRenderer/geometry.cpp",
+	"../BulletRobotics/FixJointBoxes.cpp",
+	"../BulletRobotics/BoxStack.cpp",
+	"../BulletRobotics/JointLimit.cpp",
+	"../TinyRenderer/geometry.cpp",
 		"../TinyRenderer/model.cpp",
 		"../TinyRenderer/tgaimage.cpp",
 		"../TinyRenderer/our_gl.cpp",
diff --git a/examples/pybullet/gym/pybullet_data/quadruped/vision60.urdf b/examples/pybullet/gym/pybullet_data/quadruped/vision60.urdf
new file mode 100644
index 000000000..31ebe9e78
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/quadruped/vision60.urdf
@@ -0,0 +1,568 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from vision60.urdf.xacro            | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!-- 
+MIT License (modified)
+
+Copyright (c) 2018 Ghost Robotics
+Authors:
+Avik De <avik@ghostrobotics.io>
+Tom Jacobs <tom.jacobs@ghostrobotics.io>
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this **file** (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE. 
+-->
+<robot name="ngr" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Define parameters -->
+  <!-- Define materials -->
+  <material name="gray">
+    <color rgba="0.5 0.5 0.5 0.7"/>
+  </material>
+  <material name="darkgray">
+    <color rgba="0.3 0.3 0.3 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0 0 0.8 1"/>
+  </material>
+  <material name="black">
+    <color rgba="0 0 0 1"/>
+  </material>
+  <material name="purple">
+    <color rgba="0.5 0.0 0.5 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="0.8 0.0 0.2 1.0"/>
+  </material>
+  <!-- Body -->
+  <link name="body">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.88 0.25 0.19"/>
+      </geometry>
+      <material name="gray"/>
+    </visual>
+    <inertial>
+      <mass value="12"/>
+      <!-- Uniform box -->
+      <inertia ixx="0.0986" ixy="0" ixz="0" iyy="0.8105" iyz="0" izz="0.8369"/>
+    </inertial>
+    <!-- Just copy geometry for collision -->
+    <collision>
+      <geometry>
+        <box size="0.05 0.25 0.19"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Define our leg macro -->
+  <!-- red new legs -->
+  <!-- TODO: clean this, it should be part of main leg macro -->
+  <!-- Define our leg macro -->
+  <!-- Our four legs -->
+  <!-- Hip motor -->
+  <link name="hip0">
+    <visual>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 -0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 -0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.75"/>
+      <inertia ixx="0.00891458" ixy="0" ixz="0" iyy="0.0100573" iyz="0" izz="0.01533698"/>
+    </inertial>
+  </link>
+  <!-- Abduction joint. Joint names are: 8 9 10 11 -->
+  <joint name="8" type="revolute">
+    <parent link="body"/>
+    <child link="hip0"/>
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.325 0.1575 0"/>
+    <limit effort="375" lower="-0.43" upper="0.43" velocity="8.6"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Upper leg -->
+  <link name="upper0">
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.125 0.0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.125 0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.625"/>
+      <inertia ixx="0.0003125" ixy="0" ixz="0" iyy="0.0040625" iyz="0" izz="0.0040625"/>
+    </inertial>
+  </link>
+  <!-- Hip joint. (Motor to upper leg). Joint names are: 0 2 4 6 -->
+  <joint name="0" type="revolute">
+    <parent link="hip0"/>
+    <child link="upper0"/>
+    <axis xyz="0 -1 0"/>
+    <origin xyz="0 0.068 0"/>
+    <!--  rpy="0 -0.3 0" -->
+    <limit effort="87.5" lower="-3.14159265359" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Lower leg -->
+  <link name="lower0">
+    <visual>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <inertia ixx="0.00131075" ixy="0" ixz="0" iyy="0.00131075" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+  <!-- Knee joint. Joint names are: 1 3 5 7 -->
+  <joint name="1" type="revolute">
+    <parent link="upper0"/>
+    <child link="lower0"/>
+    <axis xyz="0 1 0"/>
+    <origin xyz="-0.25 0 0"/>
+    <!--rpy="0 0.5 0"-->
+    <limit effort="87.5" lower="0" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Toe -->
+  <link name="toe0">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="jtoe0" type="fixed">
+    <parent link="lower0"/>
+    <child link="toe0"/>
+    <origin xyz="0.28 0 -0.0461"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Hip motor -->
+  <link name="hip1">
+    <visual>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 -0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+      <material name="purple"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 -0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.75"/>
+      <inertia ixx="0.00891458" ixy="0" ixz="0" iyy="0.0100573" iyz="0" izz="0.01533698"/>
+    </inertial>
+  </link>
+  <!-- Abduction joint. Joint names are: 8 9 10 11 -->
+  <joint name="9" type="revolute">
+    <parent link="body"/>
+    <child link="hip1"/>
+    <axis xyz="1 0 0"/>
+    <origin xyz="-0.325 0.1575 0"/>
+    <limit effort="375" lower="-0.43" upper="0.43" velocity="8.6"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Upper leg -->
+  <link name="upper1">
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.125 0.0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.125 0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.625"/>
+      <inertia ixx="0.0003125" ixy="0" ixz="0" iyy="0.0040625" iyz="0" izz="0.0040625"/>
+    </inertial>
+  </link>
+  <!-- Hip joint. (Motor to upper leg). Joint names are: 0 2 4 6 -->
+  <joint name="2" type="revolute">
+    <parent link="hip1"/>
+    <child link="upper1"/>
+    <axis xyz="0 -1 0"/>
+    <origin xyz="0 0.068 0"/>
+    <!--  rpy="0 -0.3 0" -->
+    <limit effort="87.5" lower="-3.14159265359" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Lower leg -->
+  <link name="lower1">
+    <visual>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <inertia ixx="0.00131075" ixy="0" ixz="0" iyy="0.00131075" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+  <!-- Knee joint. Joint names are: 1 3 5 7 -->
+  <joint name="3" type="revolute">
+    <parent link="upper1"/>
+    <child link="lower1"/>
+    <axis xyz="0 1 0"/>
+    <origin xyz="-0.25 0 0"/>
+    <!--rpy="0 0.5 0"-->
+    <limit effort="87.5" lower="0" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Toe -->
+  <link name="toe1">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="jtoe1" type="fixed">
+    <parent link="lower1"/>
+    <child link="toe1"/>
+    <origin xyz="0.28 0 -0.0461"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Hip motor -->
+  <link name="hip2">
+    <visual>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.75"/>
+      <inertia ixx="0.00891458" ixy="0" ixz="0" iyy="0.0100573" iyz="0" izz="0.01533698"/>
+    </inertial>
+  </link>
+  <!-- Abduction joint. Joint names are: 8 9 10 11 -->
+  <joint name="10" type="revolute">
+    <parent link="body"/>
+    <child link="hip2"/>
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.325 -0.1575 0"/>
+    <limit effort="375" lower="-0.43" upper="0.43" velocity="8.6"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Upper leg -->
+  <link name="upper2">
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.125 -0.0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.125 0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.625"/>
+      <inertia ixx="0.0003125" ixy="0" ixz="0" iyy="0.0040625" iyz="0" izz="0.0040625"/>
+    </inertial>
+  </link>
+  <!-- Hip joint. (Motor to upper leg). Joint names are: 0 2 4 6 -->
+  <joint name="4" type="revolute">
+    <parent link="hip2"/>
+    <child link="upper2"/>
+    <axis xyz="0 -1 0"/>
+    <origin xyz="0 -0.068 0"/>
+    <!--  rpy="0 -0.3 0" -->
+    <limit effort="87.5" lower="-3.14159265359" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Lower leg -->
+  <link name="lower2">
+    <visual>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <inertia ixx="0.00131075" ixy="0" ixz="0" iyy="0.00131075" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+  <!-- Knee joint. Joint names are: 1 3 5 7 -->
+  <joint name="5" type="revolute">
+    <parent link="upper2"/>
+    <child link="lower2"/>
+    <axis xyz="0 1 0"/>
+    <origin xyz="-0.25 0 0"/>
+    <!--rpy="0 0.5 0"-->
+    <limit effort="87.5" lower="0" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Toe -->
+  <link name="toe2">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="jtoe2" type="fixed">
+    <parent link="lower2"/>
+    <child link="toe2"/>
+    <origin xyz="0.28 0 -0.0461"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Hip motor -->
+  <link name="hip3">
+    <visual>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+      <material name="purple"/>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963268 0 0" xyz="0.0 0.039 0"/>
+      <geometry>
+        <box size="0.195 0.1 0.17"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="2.75"/>
+      <inertia ixx="0.00891458" ixy="0" ixz="0" iyy="0.0100573" iyz="0" izz="0.01533698"/>
+    </inertial>
+  </link>
+  <!-- Abduction joint. Joint names are: 8 9 10 11 -->
+  <joint name="11" type="revolute">
+    <parent link="body"/>
+    <child link="hip3"/>
+    <axis xyz="1 0 0"/>
+    <origin xyz="-0.325 -0.1575 0"/>
+    <limit effort="375" lower="-0.43" upper="0.43" velocity="8.6"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Upper leg -->
+  <link name="upper3">
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.125 -0.0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.125 0 0"/>
+      <geometry>
+        <box size="0.25 0.044 0.06"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.625"/>
+      <inertia ixx="0.0003125" ixy="0" ixz="0" iyy="0.0040625" iyz="0" izz="0.0040625"/>
+    </inertial>
+  </link>
+  <!-- Hip joint. (Motor to upper leg). Joint names are: 0 2 4 6 -->
+  <joint name="6" type="revolute">
+    <parent link="hip3"/>
+    <child link="upper3"/>
+    <axis xyz="0 -1 0"/>
+    <origin xyz="0 -0.068 0"/>
+    <!--  rpy="0 -0.3 0" -->
+    <limit effort="87.5" lower="-3.14159265359" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Lower leg -->
+  <link name="lower3">
+    <visual>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963268 0" xyz="0.14 0 -0.0355"/>
+      <geometry>
+        <cylinder length="0.28" radius="0.011"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.2"/>
+      <inertia ixx="0.00131075" ixy="0" ixz="0" iyy="0.00131075" iyz="0" izz="0.0001"/>
+    </inertial>
+  </link>
+  <!-- Knee joint. Joint names are: 1 3 5 7 -->
+  <joint name="7" type="revolute">
+    <parent link="upper3"/>
+    <child link="lower3"/>
+    <axis xyz="0 1 0"/>
+    <origin xyz="-0.25 0 0"/>
+    <!--rpy="0 0.5 0"-->
+    <limit effort="87.5" lower="0" upper="3.14159265359" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <!-- Toe -->
+  <link name="toe3">
+    <contact>
+      <friction_anchor/>
+      <stiffness value="30000.0"/>
+      <damping value="1000.0"/>
+      <spinning_friction value="0.3"/>
+      <lateral_friction value="3.0"/>
+    </contact>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+      <material name="darkgray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.15"/>
+      <inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
+    </inertial>
+  </link>
+  <joint name="jtoe3" type="fixed">
+    <parent link="lower3"/>
+    <child link="toe3"/>
+    <origin xyz="0.28 0 -0.0461"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/random_urdfs.zip b/examples/pybullet/gym/pybullet_data/random_urdfs.zip
new file mode 100644
index 000000000..6f100bde0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/random_urdfs.zip
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index dfd95e524..26f73083e 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -14,7 +14,6 @@
 #include "../SharedMemory/physx/PhysXC_API.h"
 #endif
 
-
 #ifdef BT_ENABLE_MUJOCO
 #include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h"
 #endif
@@ -1233,7 +1232,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	double restitution = -1;
 	double linearDamping = -1;
 	double angularDamping = -1;
-	
+
 	double contactStiffness = -1;
 	double contactDamping = -1;
 	double ccdSweptSphereRadius = -1;
@@ -1244,11 +1243,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	PyObject* localInertiaDiagonalObj = 0;
 	PyObject* anisotropicFrictionObj = 0;
 	double maxJointVelocity = -1;
-	
+
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity",  "physicsClientId", NULL};
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId))
 	{
 		return NULL;
@@ -1341,7 +1340,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity);
 		}
-				
+
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 
@@ -1514,30 +1513,30 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	int minimumSolverIslandSize = -1;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"fixedTimeStep", 
-		"numSolverIterations", 
-		"useSplitImpulse", 
-		"splitImpulsePenetrationThreshold", 
-		"numSubSteps", 
-		"collisionFilterMode", 
-		"contactBreakingThreshold", 
-		"maxNumCmdPer1ms", 
-		"enableFileCaching", 
-		"restitutionVelocityThreshold", 
-		"erp", 
-		"contactERP", 
-		"frictionERP", 
-		"enableConeFriction", 
-		"deterministicOverlappingPairs", 
-		"allowedCcdPenetration", 
-		"jointFeedbackMode", 
-		"solverResidualThreshold", 
-		"contactSlop", 
-		"enableSAT", 
-		"constraintSolverType", 
-		"globalCFM", 
-		"minimumSolverIslandSize", 
-		"physicsClientId", NULL};
+	static char* kwlist[] = {"fixedTimeStep",
+							 "numSolverIterations",
+							 "useSplitImpulse",
+							 "splitImpulsePenetrationThreshold",
+							 "numSubSteps",
+							 "collisionFilterMode",
+							 "contactBreakingThreshold",
+							 "maxNumCmdPer1ms",
+							 "enableFileCaching",
+							 "restitutionVelocityThreshold",
+							 "erp",
+							 "contactERP",
+							 "frictionERP",
+							 "enableConeFriction",
+							 "deterministicOverlappingPairs",
+							 "allowedCcdPenetration",
+							 "jointFeedbackMode",
+							 "solverResidualThreshold",
+							 "contactSlop",
+							 "enableSAT",
+							 "constraintSolverType",
+							 "globalCFM",
+							 "minimumSolverIslandSize",
+							 "physicsClientId", NULL};
 
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
 									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
@@ -2222,7 +2221,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 
 		if ((controlMode != CONTROL_MODE_VELOCITY) &&
 			(controlMode != CONTROL_MODE_TORQUE) &&
-			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)&&
+			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) &&
 			(controlMode != CONTROL_MODE_PD))
 		{
 			PyErr_SetString(SpamError, "Illegal control mode.");
@@ -2469,30 +2468,29 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 	//  return NULL;
 }
 
-
 static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int bodyUniqueId, jointIndex, controlMode;
 
-	double targetPositionArray[4] = { 0, 0, 0, 1 };
-	double targetVelocityArray[3] = { 0, 0, 0 };
-	double targetForceArray[3] = { 100000.0, 100000.0, 100000.0 };
+	double targetPositionArray[4] = {0, 0, 0, 1};
+	double targetVelocityArray[3] = {0, 0, 0};
+	double targetForceArray[3] = {100000.0, 100000.0, 100000.0};
 	int targetPositionSize = 0;
 	int targetVelocitySize = 0;
 	int targetForceSize = 0;
 	PyObject* targetPositionObj = 0;
 	PyObject* targetVelocityObj = 0;
 	PyObject* targetForceObj = 0;
-		
+
 	double kp = 0.1;
 	double kd = 1.0;
 	double maxVelocity = -1;
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = { "bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL };
+	static char* kwlist[] = {"bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOOdddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode,
-		&targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
+									 &targetPositionObj, &targetVelocityObj, &targetForceObj, &kp, &kd, &maxVelocity, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -2514,7 +2512,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		{
 			targetPositionSize = 0;
 		}
-		if (targetPositionSize >4)
+		if (targetPositionSize > 4)
 		{
 			targetPositionSize = 4;
 		}
@@ -2539,7 +2537,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		{
 			targetVelocitySize = 0;
 		}
-		if (targetVelocitySize >3)
+		if (targetVelocitySize > 3)
 		{
 			targetVelocitySize = 3;
 		}
@@ -2564,7 +2562,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		{
 			targetForceSize = 0;
 		}
-		if (targetForceSize >3)
+		if (targetForceSize > 3)
 		{
 			targetForceSize = 3;
 		}
@@ -2578,7 +2576,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		}
 	}
 
-
 	//if (targetPositionSize == 0 && targetVelocitySize == 0)
 	//{
 
@@ -2595,11 +2592,11 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 			return NULL;
 		}
 
-		if (//(controlMode != CONTROL_MODE_VELOCITY)&&
+		if (  //(controlMode != CONTROL_MODE_VELOCITY)&&
 			(controlMode != CONTROL_MODE_TORQUE) &&
-			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)//&&
+			(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)  //&&
 			//(controlMode != CONTROL_MODE_PD)
-			)
+		)
 		{
 			PyErr_SetString(SpamError, "Illegal control mode.");
 			return NULL;
@@ -2608,7 +2605,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 		commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode);
 
 		b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
-				
+
 		switch (controlMode)
 		{
 #if 0
@@ -2621,59 +2618,57 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 			break;
 		}
 
-		
 #endif
-		case CONTROL_MODE_TORQUE:
-		{
-			if (info.m_uSize == targetForceSize)
+			case CONTROL_MODE_TORQUE:
 			{
-				b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
-					targetForceArray, targetForceSize);
+				if (info.m_uSize == targetForceSize)
+				{
+					b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
+																targetForceArray, targetForceSize);
+				}
+				break;
 			}
-			break;
-		}
-		case CONTROL_MODE_POSITION_VELOCITY_PD:
-		case CONTROL_MODE_PD:
-		{
-
-			//make sure size == info.m_qSize
-			
-			if (maxVelocity > 0)
+			case CONTROL_MODE_POSITION_VELOCITY_PD:
+			case CONTROL_MODE_PD:
 			{
-				b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity);
-			}
+				//make sure size == info.m_qSize
 
-			if (info.m_qSize == targetPositionSize)
-			{
-				b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex,
-					targetPositionArray, targetPositionSize);
-			}
-			else
-			{
-				//printf("Warning: targetPosition array size doesn't match joint position size  (got %d, expected %d).",targetPositionSize, info.m_qSize);
-			}
-			
-			b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
-			if (info.m_uSize == targetVelocitySize)
-			{
-				b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex,
-					targetVelocityArray, targetVelocitySize);
-			}
-			else
-			{
-				//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
+				if (maxVelocity > 0)
+				{
+					b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity);
+				}
+
+				if (info.m_qSize == targetPositionSize)
+				{
+					b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex,
+															 targetPositionArray, targetPositionSize);
+				}
+				else
+				{
+					//printf("Warning: targetPosition array size doesn't match joint position size  (got %d, expected %d).",targetPositionSize, info.m_qSize);
+				}
+
+				b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+				if (info.m_uSize == targetVelocitySize)
+				{
+					b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex,
+															 targetVelocityArray, targetVelocitySize);
+				}
+				else
+				{
+					//printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize);
+				}
+				b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+				if (info.m_uSize == targetForceSize || targetForceSize == 1)
+				{
+					b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
+																targetForceArray, targetForceSize);
+				}
+				break;
 			}
-			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
-			if (info.m_uSize == targetForceSize || targetForceSize==1)
+			default:
 			{
-				b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex,
-					targetForceArray, targetForceSize);
 			}
-			break;
-		}
-		default:
-		{
-		}
 		};
 
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
@@ -2685,7 +2680,6 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject*
 	//  return NULL;
 }
 
-
 static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int bodyUniqueId, jointIndex, controlMode;
@@ -3756,15 +3750,15 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
 		b3PhysicsClientHandle sm = 0;
 		int bodyUniqueId;
 		int jointIndex;
-		double targetPositionArray[4] = { 0, 0, 0, 1 };
-		double targetVelocityArray[3] = { 0, 0, 0 };
+		double targetPositionArray[4] = {0, 0, 0, 1};
+		double targetVelocityArray[3] = {0, 0, 0};
 		int targetPositionSize = 0;
 		int targetVelocitySize = 0;
 		PyObject* targetPositionObj = 0;
 		PyObject* targetVelocityObj = 0;
-			
+
 		int physicsClientId = 0;
-		static char* kwlist[] = { "bodyUniqueId", "jointIndex", "targetValue", "targetVelocity", "physicsClientId", NULL };
+		static char* kwlist[] = {"bodyUniqueId", "jointIndex", "targetValue", "targetVelocity", "physicsClientId", NULL};
 		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|Oi", kwlist, &bodyUniqueId, &jointIndex, &targetPositionObj, &targetVelocityObj, &physicsClientId))
 		{
 			return NULL;
@@ -3787,7 +3781,7 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
 			{
 				targetPositionSize = 0;
 			}
-			if (targetPositionSize >4)
+			if (targetPositionSize > 4)
 			{
 				targetPositionSize = 4;
 			}
@@ -3800,7 +3794,7 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
 				Py_DECREF(targetPositionSeq);
 			}
 		}
-		
+
 		if (targetVelocityObj)
 		{
 			int i = 0;
@@ -3812,7 +3806,7 @@ static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args
 			{
 				targetVelocitySize = 0;
 			}
-			if (targetVelocitySize >3)
+			if (targetVelocitySize > 3)
 			{
 				targetVelocitySize = 3;
 			}
@@ -4250,7 +4244,6 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 	PyObject* pyListVelocity;
 	PyObject* pyListJointMotorTorque;
 
-
 	struct b3JointSensorState2 sensorState;
 
 	int bodyUniqueId = -1;
@@ -4261,7 +4254,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 
 	b3PhysicsClientHandle sm = 0;
 	int physicsClientId = 0;
-	static char* kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL };
+	static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId))
 	{
 		return NULL;
@@ -4317,28 +4310,27 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
 				for (i = 0; i < sensorState.m_qDofSize; i++)
 				{
 					PyTuple_SetItem(pyListPosition, i,
-						PyFloat_FromDouble(sensorState.m_jointPosition[i]));
+									PyFloat_FromDouble(sensorState.m_jointPosition[i]));
 				}
-				
+
 				for (i = 0; i < sensorState.m_uDofSize; i++)
 				{
 					PyTuple_SetItem(pyListVelocity, i,
-						PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
+									PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
 
-					PyTuple_SetItem(pyListJointMotorTorque, i, 
-						PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
+					PyTuple_SetItem(pyListJointMotorTorque, i,
+									PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
 				}
-				
+
 				for (j = 0; j < forceTorqueSize; j++)
 				{
 					PyTuple_SetItem(pyListJointForceTorque, j,
-						PyFloat_FromDouble(sensorState.m_jointReactionForceTorque[j]));
+									PyFloat_FromDouble(sensorState.m_jointReactionForceTorque[j]));
 				}
 
 				PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
 
 				PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
-					
 
 				return pyListJointState;
 			}
@@ -5265,7 +5257,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
 	int sizeTo = 0;
 	int parentObjectUniqueId = -1;
 	int parentLinkIndex = -1;
-	
+
 	static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL};
 	int physicsClientId = 0;
 
@@ -5354,9 +5346,9 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
 		}
 	}
 
-	if (parentObjectUniqueId>=0)
+	if (parentObjectUniqueId >= 0)
 	{
-		b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex);
+		b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId, parentLinkIndex);
 	}
 
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
@@ -6129,7 +6121,7 @@ static PyObject* pybullet_changeVisualShape(PyObject* self, PyObject* args, PyOb
 	{
 		commandHandle = b3InitUpdateVisualShape2(sm, objectUniqueId, jointIndex, shapeIndex);
 
-		if (textureUniqueId>=-1)
+		if (textureUniqueId >= -1)
 		{
 			b3UpdateVisualShapeTexture(commandHandle, textureUniqueId);
 		}
@@ -6438,7 +6430,7 @@ static PyObject* pybullet_setCollisionFilterGroupMask(PyObject* self, PyObject*
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	
+
 	commandHandle = b3CollisionFilterCommandInit(sm);
 	b3SetCollisionFilterGroupMask(commandHandle, bodyUniqueIdA, linkIndexA, collisionFilterGroup, collisionFilterMask);
 
@@ -6805,14 +6797,13 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
 	return NULL;
 }
 
-
 static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
 {
-	int numVerticesOut=0;
+	int numVerticesOut = 0;
 
 	if (verticesObj)
 	{
-		PyObject* seqVerticesObj= PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
+		PyObject* seqVerticesObj = PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
 		if (seqVerticesObj)
 		{
 			int numVerticesSrc = PySequence_Size(seqVerticesObj);
@@ -6846,7 +6837,6 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe
 	return numVerticesOut;
 }
 
-
 static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
 {
 	int numUVOut = 0;
@@ -6887,11 +6877,11 @@ static int extractUVs(PyObject* uvsObj, double* uvs, int maxNumVertices)
 }
 static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
 {
-	int numIndicesOut=0;
+	int numIndicesOut = 0;
 
 	if (indicesObj)
 	{
-		PyObject* seqIndicesObj= PySequence_Fast(indicesObj, "expected a sequence of indices");
+		PyObject* seqIndicesObj = PySequence_Fast(indicesObj, "expected a sequence of indices");
 		if (seqIndicesObj)
 		{
 			int numIndicesSrc = PySequence_Size(seqIndicesObj);
@@ -6906,7 +6896,7 @@ static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
 				}
 				for (i = 0; i < numIndicesSrc; i++)
 				{
-					int index = pybullet_internalGetIntFromSequence(seqIndicesObj,i);
+					int index = pybullet_internalGetIntFromSequence(seqIndicesObj, i);
 					if (indices)
 					{
 						indices[numIndicesOut] = index;
@@ -6986,23 +6976,24 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 		}
 		if (shapeType == GEOM_MESH && verticesObj)
 		{
-			int numVertices= extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
-			int numIndices= extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES);
+			int numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
+			int numIndices = extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES);
 			double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0;
 			int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0;
 
 			numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES);
 			pybullet_internalSetVectord(meshScaleObj, meshScale);
-						
+
 			if (indicesObj)
 			{
-				numIndices = extractIndices(indicesObj, indices,B3_MAX_NUM_INDICES);
+				numIndices = extractIndices(indicesObj, indices, B3_MAX_NUM_INDICES);
 			}
 
 			if (numIndices)
 			{
 				shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
-			} else
+			}
+			else
 			{
 				shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices);
 			}
@@ -7777,12 +7768,11 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
 				double* batchPositions = malloc(sizeof(double) * 3 * numBatchPositions);
 				for (i = 0; i < numBatchPositions; i++)
 				{
-					pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3*i]);
+					pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3 * i]);
 				}
 				b3CreateMultiBodySetBatchPositions(sm, commandHandle, batchPositions, numBatchPositions);
 				free(batchPositions);
 			}
-			
 
 			for (i = 0; i < numLinkMasses; i++)
 			{
@@ -9062,7 +9052,7 @@ static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject*
 	int hasQuat = 0;
 	int hasVec = 0;
 
-	static char* kwlist[] = { "quaternion", "vector", "physicsClientId", NULL };
+	static char* kwlist[] = {"quaternion", "vector", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatObj, &vectorObj, &physicsClientId))
 	{
 		return NULL;
@@ -9099,7 +9089,6 @@ static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject*
 	return Py_None;
 }
 
-
 static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
@@ -9111,7 +9100,7 @@ static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject*
 	int hasQuatStart = 0;
 	int hasQuatEnd = 0;
 
-	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "deltaTime", "physicsClientId", NULL };
+	static char* kwlist[] = {"quaternionStart", "quaternionEnd", "deltaTime", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &deltaTime, &physicsClientId))
 	{
 		return NULL;
@@ -9148,8 +9137,7 @@ static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject*
 	return Py_None;
 }
 
-
-static PyObject* pybullet_getQuaternionSlerp(PyObject* self,	PyObject* args, PyObject* keywds)
+static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
 	PyObject* quatEndObj;
@@ -9160,7 +9148,7 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self,	PyObject* args, PyO
 	int hasQuatStart = 0;
 	int hasQuatEnd = 0;
 
-	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "interpolationFraction", "physicsClientId", NULL };
+	static char* kwlist[] = {"quaternionStart", "quaternionEnd", "interpolationFraction", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &interpolationFraction, &physicsClientId))
 	{
 		return NULL;
@@ -9197,7 +9185,6 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self,	PyObject* args, PyO
 	return Py_None;
 }
 
-
 static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int physicsClientId = 0;
@@ -9205,7 +9192,7 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
 	double quat[4];
 	int hasQuat = 0;
 
-	static char* kwlist[] = { "quaternion", "physicsClientId", NULL };
+	static char* kwlist[] = {"quaternion", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|i", kwlist, &quatObj, &physicsClientId))
 	{
 		return NULL;
@@ -9232,7 +9219,7 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
 				PyTuple_SetItem(pylist2, 0, axislist);
 			}
 			PyTuple_SetItem(pylist2, 1, PyFloat_FromDouble(angle));
-			
+
 			return pylist2;
 		}
 	}
@@ -9245,7 +9232,6 @@ static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* a
 	return Py_None;
 }
 
-
 static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* axisObj;
@@ -9254,8 +9240,8 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
 	int physicsClientId = 0;
 	int hasAxis = 0;
 
-	static char* kwlist[] = { "axis", "angle","physicsClientId", NULL };
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "Od|i", kwlist, &axisObj, &angle,&physicsClientId))
+	static char* kwlist[] = {"axis", "angle", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "Od|i", kwlist, &axisObj, &angle, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -9287,7 +9273,6 @@ static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* a
 	return Py_None;
 }
 
-
 static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
@@ -9298,7 +9283,7 @@ static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject*
 	int hasQuatStart = 0;
 	int hasQuatEnd = 0;
 
-	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
+	static char* kwlist[] = {"quaternionStart", "quaternionEnd", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
 	{
 		return NULL;
@@ -9335,7 +9320,6 @@ static PyObject* pybullet_getAxisDifferenceQuaternion(PyObject* self, PyObject*
 	return Py_None;
 }
 
-
 static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject* quatStartObj;
@@ -9345,8 +9329,8 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
 	int physicsClientId = 0;
 	int hasQuatStart = 0;
 	int hasQuatEnd = 0;
-	
-	static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
+
+	static char* kwlist[] = {"quaternionStart", "quaternionEnd", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
 	{
 		return NULL;
@@ -9383,8 +9367,6 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
 	return Py_None;
 }
 
-
-
 /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
 /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
 static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
@@ -9704,6 +9686,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			{
 				PyErr_SetString(SpamError,
 								"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom.");
+				free(lowerLimits); 
+                        	free(upperLimits); 
+                        	free(jointRanges); 
+                        	free(restPoses); 
 				return NULL;
 			}
 			else
@@ -9795,6 +9781,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			free(currentPositions);
 			free(jointDamping);
 
+			free(lowerLimits);
+                        free(upperLimits);
+                        free(jointRanges);
+                        free(restPoses);
+
 			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 
 			result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
@@ -9854,12 +9845,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 		b3PhysicsClientHandle sm = 0;
 		static char* kwlist[] = {"bodyUniqueId", "objPositions",
 								 "objVelocities", "objAccelerations",
-								"flags",
+								 "flags",
 								 "physicsClientId", NULL};
 		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|ii", kwlist,
 										 &bodyUniqueId, &objPositionsQ,
 										 &objVelocitiesQdot, &objAccelerations,
-										&flags,
+										 &flags,
 										 &physicsClientId))
 		{
 			static char* kwlist2[] = {"bodyIndex", "objPositions",
@@ -9885,7 +9876,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 			int szObVel = PySequence_Size(objVelocitiesQdot);
 			int szObAcc = PySequence_Size(objAccelerations);
 
-			
 			if (szObVel == szObAcc)
 			{
 				int szInBytesQ = sizeof(double) * szObPos;
@@ -9895,7 +9885,6 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				double* jointPositionsQ = (double*)malloc(szInBytesQ);
 				double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
 				double* jointAccelerations = (double*)malloc(szInBytesQdot);
-				
 
 				for (i = 0; i < szObPos; i++)
 				{
@@ -9927,13 +9916,13 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 					{
 						int bodyUniqueId;
 						int dofCount;
-						b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,&dofCount, 0);
+						b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0);
 
 						if (dofCount)
 						{
 							double* jointForcesOutput = (double*)malloc(sizeof(double) * dofCount);
-							
-							b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,jointForcesOutput);
+
+							b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput);
 							{
 								{
 									int i;
@@ -9955,7 +9944,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 				free(jointPositionsQ);
 				free(jointVelocitiesQdot);
 				free(jointAccelerations);
-				
+
 				if (pylist) return pylist;
 			}
 			else
@@ -10186,11 +10175,11 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
 		{
 			int szObPos = PySequence_Size(objPositions);
 			///int dofCountQ = b3GetNumJoints(sm, bodyUniqueId);
-			
-			if (szObPos>=0)//(szObPos == dofCountQ))
+
+			if (szObPos >= 0)  //(szObPos == dofCountQ))
 			{
 				int byteSizeJoints = sizeof(double) * szObPos;
-				PyObject* pyResultList=NULL;
+				PyObject* pyResultList = NULL;
 				double* jointPositions = (double*)malloc(byteSizeJoints);
 				double* massMatrix = NULL;
 				int i;
@@ -10215,7 +10204,6 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
 						if (dofCount)
 						{
 							int byteSizeDofCount = sizeof(double) * dofCount;
-							
 
 							massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
 							b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
@@ -10470,8 +10458,8 @@ static PyMethodDef SpamMethods[] = {
 	{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
 	 "Get the state (position, velocity etc) for a joint on a body."},
 
-	 { "getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
-		"Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)" },
+	{"getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
+	 "Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)"},
 
 	{"getJointStates", (PyCFunction)pybullet_getJointStates, METH_VARARGS | METH_KEYWORDS,
 	 "Get the state (position, velocity etc) for multiple joints on a body."},
@@ -10492,10 +10480,10 @@ static PyMethodDef SpamMethods[] = {
 	 "Reset the state (position, velocity etc) for a joint on a body "
 	 "instantaneously, not through physics simulation."},
 
-	 { "resetJointStateMultiDof", (PyCFunction)pybullet_resetJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
-		"resetJointStateMultiDof(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
-		"Reset the state (position, velocity etc) for a joint on a body "
-		"instantaneously, not through physics simulation." },
+	{"resetJointStateMultiDof", (PyCFunction)pybullet_resetJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
+	 "resetJointStateMultiDof(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
+	 "Reset the state (position, velocity etc) for a joint on a body "
+	 "instantaneously, not through physics simulation."},
 
 	{"changeDynamics", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS,
 	 "change dynamics information such as mass, lateral friction coefficient."},
@@ -10512,10 +10500,10 @@ static PyMethodDef SpamMethods[] = {
 	 "Set a single joint motor control mode and desired target value. There is "
 	 "no immediate state change, stepSimulation will process the motors."},
 
-	 { "setJointMotorControlMultiDof", (PyCFunction)pybullet_setJointMotorControlMultiDof, METH_VARARGS | METH_KEYWORDS,
-		"Set a single joint motor control mode and desired target value. There is "
-		"no immediate state change, stepSimulation will process the motors."
-		"This method sets multi-degree-of-freedom motor such as the spherical joint motor." },
+	{"setJointMotorControlMultiDof", (PyCFunction)pybullet_setJointMotorControlMultiDof, METH_VARARGS | METH_KEYWORDS,
+	 "Set a single joint motor control mode and desired target value. There is "
+	 "no immediate state change, stepSimulation will process the motors."
+	 "This method sets multi-degree-of-freedom motor such as the spherical joint motor."},
 
 	{"setJointMotorControlArray", (PyCFunction)pybullet_setJointMotorControlArray, METH_VARARGS | METH_KEYWORDS,
 	 "Set an array of motors control mode and desired target value. There is "
@@ -10650,30 +10638,26 @@ static PyMethodDef SpamMethods[] = {
 	{"getMatrixFromQuaternion", (PyCFunction)pybullet_getMatrixFromQuaternion, METH_VARARGS | METH_KEYWORDS,
 	 "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
 
-	 { "getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS,
-	 "Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]" },
+	{"getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]"},
 
-	 { "getQuaternionFromAxisAngle", (PyCFunction)pybullet_getQuaternionFromAxisAngle, METH_VARARGS | METH_KEYWORDS,
-		 "Compute the quaternion from axis and angle representation." },
+	{"getQuaternionFromAxisAngle", (PyCFunction)pybullet_getQuaternionFromAxisAngle, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the quaternion from axis and angle representation."},
 
-	 { "getAxisAngleFromQuaternion", (PyCFunction)pybullet_getAxisAngleFromQuaternion, METH_VARARGS | METH_KEYWORDS,
-		 "Compute the quaternion from axis and angle representation." },
+	{"getAxisAngleFromQuaternion", (PyCFunction)pybullet_getAxisAngleFromQuaternion, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the quaternion from axis and angle representation."},
 
-	 { "getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
-		 "Compute the quaternion difference from two quaternions." },
+	{"getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the quaternion difference from two quaternions."},
 
-		 { "getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
-		 "Compute the velocity axis difference from two quaternions." },
+	{"getAxisDifferenceQuaternion", (PyCFunction)pybullet_getAxisDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the velocity axis difference from two quaternions."},
 
-		 
+	{"calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
+	 "Compute the angular velocity given start and end quaternion and delta time."},
 
-	 { "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
-		 "Compute the angular velocity given start and end quaternion and delta time." },
-
-	{ "rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS,
-		"Rotate a vector using a quaternion." },
-
-		
+	{"rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS,
+	 "Rotate a vector using a quaternion."},
 
 	{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS,
 	 "Given an object id, joint positions, joint velocities and joint "
@@ -10971,8 +10955,7 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_FROM_MTL);
 	PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL);
 	PyModule_AddIntConstant(m, "URDF_MAINTAIN_LINK_ORDER", URDF_MAINTAIN_LINK_ORDER);
-	
-	
+
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
 	PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);
@@ -11039,9 +11022,9 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "AddFileIOAction", eAddFileIOAction);
 	PyModule_AddIntConstant(m, "RemoveFileIOAction", eRemoveFileIOAction);
 
-	PyModule_AddIntConstant(m, "PosixFileIO", ePosixFileIO );
-	PyModule_AddIntConstant(m, "ZipFileIO", eZipFileIO );
-	PyModule_AddIntConstant(m, "CNSFileIO", eCNSFileIO );
+	PyModule_AddIntConstant(m, "PosixFileIO", ePosixFileIO);
+	PyModule_AddIntConstant(m, "ZipFileIO", eZipFileIO);
+	PyModule_AddIntConstant(m, "CNSFileIO", eCNSFileIO);
 
 	SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
 	Py_INCREF(SpamError);