diff --git a/.travis.yml b/.travis.yml index d2ad176f4..d568af5d1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,7 +13,7 @@ addons: script: - echo "CXX="$CXX - echo "CC="$CC - - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror + - cmake . -DBUILD_PYBULLET=OFF -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - make -j8 - ctest -j8 --output-on-failure # Build again with double precision diff --git a/CMakeLists.txt b/CMakeLists.txt index 01bb95519..9e04fe3e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ IF(COMMAND cmake_policy) endif(POLICY CMP0042) ENDIF(COMMAND cmake_policy) - IF (NOT CMAKE_BUILD_TYPE) # SET(CMAKE_BUILD_TYPE "Debug") SET(CMAKE_BUILD_TYPE "Release") @@ -38,14 +37,18 @@ IF (BULLET2_USE_THREAD_LOCKS) ENDIF (MSVC) ENDIF (BULLET2_USE_THREAD_LOCKS) + IF(NOT WIN32) SET(DL dl) IF(CMAKE_SYSTEM_NAME MATCHES "Linux") - SET(OSDEF -D_LINUX) + MESSAGE("Linux") + SET(OSDEF -D_LINUX) ELSE(CMAKE_SYSTEM_NAME MATCHES "Linux") IF(APPLE) + MESSAGE("Apple") SET(OSDEF -D_DARWIN) ELSE(APPLE) + MESSAGE("BSD?") SET(OSDEF -D_BSD) SET(DL "") ENDIF(APPLE) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8f2f9503d..c3bd7f174 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -2,6 +2,7 @@ SUBDIRS( HelloWorld BasicDemo ) IF(BUILD_BULLET3) SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow ) ENDIF() + IF(BUILD_PYBULLET) SUBDIRS(pybullet) ENDIF(BUILD_PYBULLET) diff --git a/examples/OpenGLWindow/CMakeLists.txt b/examples/OpenGLWindow/CMakeLists.txt index 5d1f37d9b..385b32216 100644 --- a/examples/OpenGLWindow/CMakeLists.txt +++ b/examples/OpenGLWindow/CMakeLists.txt @@ -50,7 +50,7 @@ ENDIF() ADD_LIBRARY(OpenGLWindow ${OpenGLWindow_SRCS} ${OpenGLWindow_HDRS}) if (UNIX AND NOT APPLE) - target_link_libraries(OpenGLWindow ) + target_link_libraries(OpenGLWindow ${DL}) elseif (APPLE) target_link_libraries(OpenGLWindow ${COCOA_LIBRARY}) endif () diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index f804dcb3f..622788999 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -199,29 +199,53 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi btTransform tr; tr.setIdentity(); btScalar rad, len; - if (visual->m_geometry.m_hasFromTo) { + btVector3 center(0,0,0); + btVector3 axis(0,0,1); + btAlignedObjectArray vertices; + int numSteps = 32; + + if (visual->m_geometry.m_hasFromTo) + { btVector3 v = p2 - p1; - btVector3 center = (p2 + p1) * 0.5; - btVector3 up_vector(0,0,1); btVector3 dir = v.normalized(); - btVector3 axis = dir.cross(up_vector); - if (axis.fuzzyZero()) - { - axis = btVector3(0,0,1); - } - else - { - axis.normalize(); - } - btQuaternion q(axis, -acos(dir.dot(up_vector))); - btTransform capsule_orient(q, center); - tr = visual->m_linkLocalFrame * capsule_orient; + tr = visual->m_linkLocalFrame; len = v.length(); rad = visual->m_geometry.m_capsuleRadius; + btVector3 ax1,ax2; + btPlaneSpace1(dir,ax1,ax2); + + for (int i = 0; im_linkLocalFrame; len = visual->m_geometry.m_capsuleHeight; rad = visual->m_geometry.m_capsuleRadius; + for (int i = 0; i vertices; - int numSteps = 32; - for (int i = 0; im_geometry.m_type==URDF_GEOM_CAPSULE) { - // TODO: check if tiny renderer works with that, didn't check -- Oleg - btVector3 pole1(0, 0, + len / 2. + rad); - btVector3 pole2(0, 0, - len / 2. - rad); - vertices.push_back(pole1); - vertices.push_back(pole2); - } - btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + //btCapsuleShape* cylZShape = new btCapsuleShape(rad,len);//btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->setMargin(0.001); convexColShape = cylZShape; break; diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 04340da07..8b16cd7be 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -21,20 +21,23 @@ class MJCFBaseBulletEnv(gym.Env): self.camera = Camera() self.isRender = render self.robot = robot - self._seed() + self._cam_dist = 3 + self._cam_yaw = 0 + self._cam_pitch = -30 + self._render_width =320 + self._render_height = 240 self.action_space = robot.action_space self.observation_space = robot.observation_space - + def configure(self, args): + self.robot.args = args def _seed(self, seed=None): self.np_random, seed = gym.utils.seeding.np_random(seed) self.robot.np_random = self.np_random # use the same np_randomizer for robot as for env return [seed] def _reset(self): - print("self.isRender=") - print(self.isRender) if (self.physicsClientId<0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) @@ -60,6 +63,28 @@ class MJCFBaseBulletEnv(gym.Env): def _render(self, mode, close): if (mode=="human"): self.isRender = True + if mode != "rgb_array": + return np.array([]) + base_pos = self.robot.body_xyz + + view_matrix = p.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = p.computeProjectionMatrixFOV( + fov=60, aspect=float(self._render_width)/self._render_height, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = p.getCameraImage( + width=self._render_width, height=self._render_height, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, + #renderer=p.ER_BULLET_HARDWARE_OPENGL + ) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array def _close(self): if (self.physicsClientId>=0): diff --git a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py index 482b78be3..1ec52c3de 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py +++ b/examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py @@ -51,6 +51,8 @@ def main(): time.sleep(0.01) a = pi.act(obs) obs, r, done, _ = env.step(a) + #print("reward") + #print(r) score += r frame += 1 distance=5 diff --git a/examples/pybullet/gym/pybullet_envs/examples/testEnv.py b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py new file mode 100644 index 000000000..8b8d0ed31 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py @@ -0,0 +1,48 @@ +import os +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) +import pybullet_envs +import gym +import argparse + + +def test(args): + + env = gym.make(args.env) + env.env.configure(args) + if (args.render): + env.render(mode="human") + env.reset() + print("action space:") + sample = env.action_space.sample() + action = sample*0.0 + print("action=") + print(action) + for i in range(args.steps): + obs,rewards,done,_ =env.step(action) + if (args.rgb): + print(env.render(mode="rgb_array")) + print("obs=") + print(obs) + print("rewards") + print (rewards) + print ("done") + print(done) + + +def main(): + import argparse + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--env', help='environment ID', default='AntBulletEnv-v0') + parser.add_argument('--seed', help='RNG seed', type=int, default=0) + parser.add_argument('--render', help='OpenGL Visualizer', type=int, default=0) + parser.add_argument('--rgb',help='rgb_array gym rendering',type=int, default=0) + parser.add_argument('--steps', help='Number of steps', type=int, default=1) + parser.add_argument('--bla',help='bla',type=int, default=42) + args = parser.parse_args() + test(args) + +if __name__ == '__main__': + main() diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index bc75f7bfd..f9f8b1bb1 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -15,7 +15,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): self.walk_target_y = 0 def create_single_player_scene(self): - self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165, frame_skip=4) + self.stadium_scene = SinglePlayerStadiumScene(gravity=9.8, timestep=0.0165/4, frame_skip=4) return self.stadium_scene def _reset(self): @@ -63,15 +63,31 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): feet_collision_cost = 0.0 for i,f in enumerate(self.robot.feet): # TODO: Maybe calculating feet contacts could be done within the robot code contact_ids = set((x[2], x[4]) for x in f.contact_list()) - #print("CONTACT OF '%s' WITH %s" % (f.name, ",".join(contact_names)) ) - self.robot.feet_contact[i] = 1.0 if (self.ground_ids & contact_ids) else 0.0 - if contact_ids - self.ground_ids: - feet_collision_cost += self.foot_collision_cost + #print("CONTACT OF '%d' WITH %d" % (contact_ids, ",".join(contact_names)) ) + if (self.ground_ids & contact_ids): + #see Issue 63: https://github.com/openai/roboschool/issues/63 + #feet_collision_cost += self.foot_collision_cost + self.robot.feet_contact[i] = 1.0 + else: + self.robot.feet_contact[i] = 0.0 + electricity_cost = self.electricity_cost * float(np.abs(a*self.robot.joint_speeds).mean()) # let's assume we have DC motor with controller, and reverse current braking electricity_cost += self.stall_torque_cost * float(np.square(a).mean()) joints_at_limit_cost = float(self.joints_at_limit_cost * self.robot.joints_at_limit) + debugmode=0 + if(debugmode): + print("alive=") + print(alive) + print("progress") + print(progress) + print("electricity_cost") + print(electricity_cost) + print("joints_at_limit_cost") + print(joints_at_limit_cost) + print("feet_collision_cost") + print(feet_collision_cost) self.rewards = [ alive, @@ -80,8 +96,14 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): joints_at_limit_cost, feet_collision_cost ] - + if (debugmode): + print("rewards=") + print(self.rewards) + print("sum rewards") + print(sum(self.rewards)) self.HUD(state, a, done) + self.reward += sum(self.rewards) + return state, sum(self.rewards), bool(done), {} def camera_adjust(self): diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 772dee5c6..a17055675 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -9,7 +9,8 @@ class WalkerBase(MJCFBasedRobot): self.camera_x = 0 self.walk_target_x = 1e3 # kilometer away self.walk_target_y = 0 - + self.body_xyz=[0,0,0] + def robot_specific_reset(self): for j in self.ordered_joints: j.reset_current_position(self.np_random.uniform(low=-0.1, high=0.1), 0) @@ -62,6 +63,16 @@ class WalkerBase(MJCFBasedRobot): def calc_potential(self): # progress in potential field is speed*dt, typical speed is about 2-3 meter per second, this potential will change 2-3 per frame (not per second), # all rewards have rew/frame units and close to 1.0 + debugmode=0 + if (debugmode): + print("calc_potential: self.walk_target_dist") + print(self.walk_target_dist) + print("self.scene.dt") + print(self.scene.dt) + print("self.scene.frame_skip") + print(self.scene.frame_skip) + print("self.scene.timestep") + print(self.scene.timestep) return - self.walk_target_dist / self.scene.dt diff --git a/examples/pybullet/gym/pybullet_envs/scene_abstract.py b/examples/pybullet/gym/pybullet_envs/scene_abstract.py index 152427a25..11ae8d489 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_abstract.py +++ b/examples/pybullet/gym/pybullet_envs/scene_abstract.py @@ -12,8 +12,9 @@ class Scene: self.np_random, seed = gym.utils.seeding.np_random(None) self.timestep = timestep self.frame_skip = frame_skip + self.dt = self.timestep * self.frame_skip - self.cpp_world = World(gravity, timestep) + 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 @@ -60,16 +61,17 @@ class SingleRobotEmptyScene(Scene): class World: - def __init__(self, gravity, timestep): + def __init__(self, gravity, timestep, frame_skip): self.gravity = gravity self.timestep = timestep + self.frame_skip = frame_skip self.clean_everything() def clean_everything(self): p.resetSimulation() p.setGravity(0, 0, -self.gravity) p.setDefaultContactERP(0.9) - p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSolverIterations=5, numSubSteps=4) + p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip) def step(self, frame_skip): p.stepSimulation() diff --git a/setup.py b/setup.py index b67974942..2ef9bd97d 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.3', + version='1.3.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',