From 86840bf50dbb819f633df30e6e6ca1297ea6e0c4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 17:24:17 -0700 Subject: [PATCH 01/11] trigger travis --- CMakeLists.txt | 1 - examples/CMakeLists.txt | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 01bb95519..8b8ab0c75 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") 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) From 52c58a8886a704e6294e12ecfdce1ccfa16eaa29 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Sep 2017 18:39:59 -0700 Subject: [PATCH 02/11] turn off compiling pybullet using cmake, cmake's lack of finding python is broken beyond repair, let's just use pip/setuptools instead. See also: https://travis-ci.org/bulletphysics/bullet3/jobs/273121440 --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 67eb59901ac3170fc9f58c10100bf5f8078306b1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 8 Sep 2017 08:22:40 -0700 Subject: [PATCH 03/11] trigger travis again --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b8ab0c75..22afd0270 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ IF (BULLET2_USE_THREAD_LOCKS) ENDIF (MSVC) ENDIF (BULLET2_USE_THREAD_LOCKS) + IF(NOT WIN32) SET(DL dl) IF(CMAKE_SYSTEM_NAME MATCHES "Linux") From c144d9c0458c7b5dd32fbd8ca84328ed8b3f8f9c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 8 Sep 2017 15:25:16 -0700 Subject: [PATCH 04/11] add simple script to test some of the pybullet gym environments (work-in-progress/experimental) --- .../gym/pybullet_envs/examples/testEnv.py | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_envs/examples/testEnv.py 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..e43131cb8 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py @@ -0,0 +1,45 @@ +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) + 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='Render in OpenGL Window', type=str, 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() From 4f47a223effdfde3dc2e27b28572a5cefb49b45b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 Sep 2017 12:36:53 -0700 Subject: [PATCH 05/11] more fixes in Gym Ant to make reward the same as Roboschool, apparently feet_collision_cost is not properly updated in Roboschool, for now, disable it in pybullet too: see https://github.com/openai/roboschool/issues/63 --- .../pybullet/gym/pybullet_envs/env_bases.py | 6 ++-- .../enjoy_TF_AntBulletEnv_v0_2017may.py | 2 ++ .../gym/pybullet_envs/gym_locomotion_envs.py | 35 +++++++++++++++---- .../gym/pybullet_envs/robot_locomotors.py | 10 ++++++ .../gym/pybullet_envs/scene_abstract.py | 8 +++-- 5 files changed, 48 insertions(+), 13 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 04340da07..35f725822 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -21,20 +21,18 @@ class MJCFBaseBulletEnv(gym.Env): self.camera = Camera() self.isRender = render self.robot = robot - self._seed() 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) 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/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index bc75f7bfd..102a39cf4 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,32 @@ 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): + #disable feet_collision_cost update, for compatibility with Roboschool + #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 +97,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..5a0bf2cf5 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -62,6 +62,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() From c895bd244f9f5a58296bba7a6f26d6ef75032cb0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 Sep 2017 13:05:45 -0700 Subject: [PATCH 06/11] track down DL/dl issue --- CMakeLists.txt | 5 ++++- examples/OpenGLWindow/CMakeLists.txt | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 22afd0270..9e04fe3e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,11 +41,14 @@ 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/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 () From 666c824b81d0f1ceb85c6ddf3bf1dbd3f7310d6c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 Sep 2017 15:27:10 -0700 Subject: [PATCH 07/11] enable pybullet_env Ant Gym rendering, tinyRenderer has some issue with the ant.xml file though --- .../pybullet/gym/pybullet_envs/env_bases.py | 27 +++++++++++++++++++ .../gym/pybullet_envs/robot_locomotors.py | 3 ++- 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 35f725822..8b16cd7be 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -22,6 +22,11 @@ class MJCFBaseBulletEnv(gym.Env): 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 @@ -58,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/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 5a0bf2cf5..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) From ce64aff7af98780eb528d17bcf2445fd026e7fdc Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 Sep 2017 16:00:03 -0700 Subject: [PATCH 08/11] ant env, fix feet_collision reward issue --- examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py | 3 +-- setup.py | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index 102a39cf4..afa592a50 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -65,9 +65,8 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): contact_ids = set((x[2], x[4]) for x in f.contact_list()) #print("CONTACT OF '%d' WITH %d" % (contact_ids, ",".join(contact_names)) ) if (self.ground_ids & contact_ids): - #disable feet_collision_cost update, for compatibility with Roboschool #see Issue 63: https://github.com/openai/roboschool/issues/63 - #feet_collision_cost += self.foot_collision_cost + feet_collision_cost += self.foot_collision_cost self.robot.feet_contact[i] = 1.0 else: self.robot.feet_contact[i] = 0.0 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', From de28334a70ae74ed66097d2e547acb19a122787f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 Sep 2017 16:36:42 -0700 Subject: [PATCH 09/11] tweak to testEnv.py, sample usage: python testEnv.py --rgb=1 --render=1 --step=100000 --env="HumanoidBulletEnv-v0" --- examples/pybullet/gym/pybullet_envs/examples/testEnv.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/gym/pybullet_envs/examples/testEnv.py b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py index e43131cb8..8b8d0ed31 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/testEnv.py +++ b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py @@ -22,6 +22,8 @@ def test(args): 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") @@ -35,7 +37,8 @@ def main(): 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='Render in OpenGL Window', type=str, 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() From 125380ce15101409c0746f58d3eecb1738cf8e6d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 Sep 2017 22:35:48 -0700 Subject: [PATCH 10/11] revert feet related reward in ant --- examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index afa592a50..f9f8b1bb1 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -66,7 +66,7 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): #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 + #feet_collision_cost += self.foot_collision_cost self.robot.feet_contact[i] = 1.0 else: self.robot.feet_contact[i] = 0.0 From ec18adbf0a403d407f1f5723939f1a2d387f7eb3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 10 Sep 2017 07:48:43 -0700 Subject: [PATCH 11/11] fix MJCF visual shape capsule conversion code (fromto case) --- .../TinyRendererVisualShapeConverter.cpp | 73 +++++++++++-------- 1 file changed, 41 insertions(+), 32 deletions(-) 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;