From f9ff09a6b58cd7b44efeb2c36f009ce32a0be33e Mon Sep 17 00:00:00 2001 From: Jelle Date: Wed, 24 Oct 2018 17:20:25 +0200 Subject: [PATCH 01/17] Added extern declaration The extern declaration was missing from this file. --- .../CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 14e8a7ada..a940d840e 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -34,6 +34,8 @@ class btCollisionObject; class btCollisionShape; +extern btShapePairCallback gCompoundCompoundChildShapePairCallback; + /// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm { From fc4d531e3cd9249a4731e391921e8000ecd1c162 Mon Sep 17 00:00:00 2001 From: Max Argus Date: Sat, 27 Oct 2018 21:54:33 +0200 Subject: [PATCH 02/17] pip egl install: added btThreads.cpp --- setup.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/setup.py b/setup.py index 86b07206a..826e90b39 100644 --- a/setup.py +++ b/setup.py @@ -463,15 +463,16 @@ egl_renderer_sources = \ +["src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp"]\ +["src/Bullet3Common/b3Logging.cpp"]\ +["src/LinearMath/btAlignedAllocator.cpp"]\ -+["src/LinearMath/btGeometryUtil.cpp"]\ +["src/LinearMath/btConvexHull.cpp"]\ -+["src/LinearMath/btConvexHullComputer.cpp"]\ ++["src/LinearMath/btConvexHullComputer.cpp"] \ ++["src/LinearMath/btGeometryUtil.cpp"]\ ++["src/LinearMath/btQuickprof.cpp"] \ ++["src/LinearMath/btThreads.cpp"] \ +["src/Bullet3Common/b3AlignedAllocator.cpp"] \ +["examples/ThirdPartyLibs/glad/gl.c"]\ +["examples/OpenGLWindow/GLInstancingRenderer.cpp"]\ +["examples/OpenGLWindow/GLRenderToTexture.cpp"] \ -+["examples/OpenGLWindow/LoadShader.cpp"] \ -+["src/LinearMath/btQuickprof.cpp"] ++["examples/OpenGLWindow/LoadShader.cpp"] if 'BT_USE_EGL' in CXX_FLAGS: sources += ['examples/ThirdPartyLibs/glad/egl.c'] From 5b90e7e0b7d790a943240a46e902da3125196280 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Janiszewski?= Date: Sun, 28 Oct 2018 21:50:28 +0100 Subject: [PATCH 03/17] Update include guard in btGImpactBvh.h --- src/BulletCollision/Gimpact/btGImpactBvh.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/BulletCollision/Gimpact/btGImpactBvh.h b/src/BulletCollision/Gimpact/btGImpactBvh.h index 79dcda957..3cd8fa24e 100644 --- a/src/BulletCollision/Gimpact/btGImpactBvh.h +++ b/src/BulletCollision/Gimpact/btGImpactBvh.h @@ -1,5 +1,5 @@ -#ifndef GIM_BOX_SET_H_INCLUDED -#define GIM_BOX_SET_H_INCLUDED +#ifndef BT_GIMPACT_BVH_H_INCLUDED +#define BT_GIMPACT_BVH_H_INCLUDED /*! \file gim_box_set.h \author Francisco Leon Najera @@ -306,4 +306,4 @@ public: btPairSet& collision_pairs); }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // BT_GIMPACT_BVH_H_INCLUDED From 44976780fadd32b667a7f5e47f709bedc5b602c9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 29 Oct 2018 19:23:54 -0700 Subject: [PATCH 04/17] remove ARS files --- .../pybullet/gym/pybullet_envs/ARS/ars.py | 397 ------------------ .../gym/pybullet_envs/ARS/ars_server.py | 62 --- .../gym/pybullet_envs/ARS/config_ars.py | 83 ---- .../gym/pybullet_envs/ARS/eval_ars.py | 99 ----- .../pybullet/gym/pybullet_envs/ARS/filter.py | 280 ------------ .../gym/pybullet_envs/ARS/log/config.yaml | 29 -- .../ARS/log/lin_policy_plus_990_bla.npz | Bin 1407 -> 0 bytes .../pybullet/gym/pybullet_envs/ARS/logz.py | 104 ----- .../gym/pybullet_envs/ARS/optimizers.py | 35 -- .../gym/pybullet_envs/ARS/policies.py | 72 ---- .../gym/pybullet_envs/ARS/shared_noise.py | 40 -- .../pybullet_envs/ARS/start_ars_servers.py | 27 -- .../gym/pybullet_envs/ARS/train_ars.borg | 93 ---- .../gym/pybullet_envs/ARS/train_ars.py | 64 --- .../gym/pybullet_envs/ARS/train_ars_test.py | 29 -- .../pybullet/gym/pybullet_envs/ARS/utility.py | 52 --- .../pybullet/gym/pybullet_envs/ARS/utils.py | 28 -- 17 files changed, 1494 deletions(-) delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/ars.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/ars_server.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/config_ars.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/filter.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/logz.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/optimizers.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/policies.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/train_ars.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/utility.py delete mode 100644 examples/pybullet/gym/pybullet_envs/ARS/utils.py diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py deleted file mode 100644 index cf59ede85..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/ars.py +++ /dev/null @@ -1,397 +0,0 @@ -"""Internal implementation of the Augmented Random Search method.""" - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -import os, inspect -currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) -os.sys.path.insert(0,currentdir) - -from concurrent import futures -import copy -import os -import time -import gym -import numpy as np -import logz -import utils -import optimizers -#from google3.pyglib import gfile -import policies -import shared_noise -import utility - -class Worker(object): - """Object class for parallel rollout generation.""" - - def __init__(self, - env_seed, - env_callback, - policy_params=None, - deltas=None, - rollout_length=1000, - delta_std=0.02): - - # initialize OpenAI environment for each worker - self.env = env_callback() - self.env.seed(env_seed) - - # each worker gets access to the shared noise table - # with independent random streams for sampling - # from the shared noise table. - self.deltas = shared_noise.SharedNoiseTable(deltas, env_seed + 7) - self.policy_params = policy_params - if policy_params['type'] == 'linear': - self.policy = policies.LinearPolicy(policy_params) - else: - raise NotImplementedError - - self.delta_std = delta_std - self.rollout_length = rollout_length - - def get_weights_plus_stats(self): - """ - Get current policy weights and current statistics of past states. - """ - assert self.policy_params['type'] == 'linear' - return self.policy.get_weights_plus_stats() - - def rollout(self, shift=0., rollout_length=None): - """Performs one rollout of maximum length rollout_length. - - At each time-step it substracts shift from the reward. - """ - - if rollout_length is None: - rollout_length = self.rollout_length - - total_reward = 0. - steps = 0 - - ob = self.env.reset() - for i in range(rollout_length): - action = self.policy.act(ob) - ob, reward, done, _ = self.env.step(action) - steps += 1 - total_reward += (reward - shift) - if done: - break - - return total_reward, steps - - def do_rollouts(self, w_policy, num_rollouts=1, shift=1, evaluate=False): - """ - Generate multiple rollouts with a policy parametrized by w_policy. - """ - print('Doing {} rollouts'.format(num_rollouts)) - rollout_rewards, deltas_idx = [], [] - steps = 0 - - for i in range(num_rollouts): - - if evaluate: - self.policy.update_weights(w_policy) - deltas_idx.append(-1) - - # set to false so that evaluation rollouts are not used for updating state statistics - self.policy.update_filter = False - - # for evaluation we do not shift the rewards (shift = 0) and we use the - # default rollout length (1000 for the MuJoCo locomotion tasks) - reward, r_steps = self.rollout( - shift=0., rollout_length=self.rollout_length) - rollout_rewards.append(reward) - - else: - idx, delta = self.deltas.get_delta(w_policy.size) - - delta = (self.delta_std * delta).reshape(w_policy.shape) - deltas_idx.append(idx) - - # set to true so that state statistics are updated - self.policy.update_filter = True - - # compute reward and number of timesteps used for positive perturbation rollout - self.policy.update_weights(w_policy + delta) - pos_reward, pos_steps = self.rollout(shift=shift) - - # compute reward and number of timesteps used for negative pertubation rollout - self.policy.update_weights(w_policy - delta) - neg_reward, neg_steps = self.rollout(shift=shift) - steps += pos_steps + neg_steps - - rollout_rewards.append([pos_reward, neg_reward]) - - return { - 'deltas_idx': deltas_idx, - 'rollout_rewards': rollout_rewards, - 'steps': steps - } - - def stats_increment(self): - self.policy.observation_filter.stats_increment() - return - - def get_weights(self): - return self.policy.get_weights() - - def get_filter(self): - return self.policy.observation_filter - - def sync_filter(self, other): - self.policy.observation_filter.sync(other) - return - - -class ARSLearner(object): - """ - Object class implementing the ARS algorithm. - """ - - def __init__(self, - env_callback, - policy_params=None, - num_workers=32, - num_deltas=320, - deltas_used=320, - delta_std=0.02, - logdir=None, - rollout_length=1000, - step_size=0.01, - shift='constant zero', - params=None, - seed=123): - - logz.configure_output_dir(logdir) - # params_to_save = copy.deepcopy(params) - # params_to_save['env'] = None - # logz.save_params(params_to_save) - utility.save_config(params, logdir) - env = env_callback() - - self.timesteps = 0 - self.action_size = env.action_space.shape[0] - self.ob_size = env.observation_space.shape[0] - self.num_deltas = num_deltas - self.deltas_used = deltas_used - self.rollout_length = rollout_length - self.step_size = step_size - self.delta_std = delta_std - self.logdir = logdir - self.shift = shift - self.params = params - self.max_past_avg_reward = float('-inf') - self.num_episodes_used = float('inf') - - # create shared table for storing noise - print('Creating deltas table.') - deltas = shared_noise.create_shared_noise() - self.deltas = shared_noise.SharedNoiseTable(deltas, seed=seed + 3) - print('Created deltas table.') - - # initialize workers with different random seeds - print('Initializing workers.') - self.num_workers = num_workers - self.workers = [ - Worker( - seed + 7 * i, - env_callback=env_callback, - policy_params=policy_params, - deltas=deltas, - rollout_length=rollout_length, - delta_std=delta_std) for i in range(num_workers) - ] - - # initialize policy - if policy_params['type'] == 'linear': - self.policy = policies.LinearPolicy(policy_params) - self.w_policy = self.policy.get_weights() - else: - raise NotImplementedError - - # initialize optimization algorithm - self.optimizer = optimizers.SGD(self.w_policy, self.step_size) - print('Initialization of ARS complete.') - - def aggregate_rollouts(self, num_rollouts=None, evaluate=False): - """ - Aggregate update step from rollouts generated in parallel. - """ - - if num_rollouts is None: - num_deltas = self.num_deltas - else: - num_deltas = num_rollouts - - results_one = [] #rollout_ids_one - results_two = [] #rollout_ids_two - - t1 = time.time() - num_rollouts = int(num_deltas / self.num_workers) -# if num_rollouts > 0: -# with futures.ThreadPoolExecutor( -# max_workers=self.num_workers) as executor: -# workers = [ -# executor.submit( -# worker.do_rollouts, -# self.w_policy, -# num_rollouts=num_rollouts, -# shift=self.shift, -# evaluate=evaluate) for worker in self.workers -# ] -# for worker in futures.as_completed(workers): -# results_one.append(worker.result()) -# -# workers = [ -# executor.submit( -# worker.do_rollouts, -# self.w_policy, -# num_rollouts=1, -# shift=self.shift, -# evaluate=evaluate) -# for worker in self.workers[:(num_deltas % self.num_workers)] -# ] -# for worker in futures.as_completed(workers): -# results_two.append(worker.result()) - - # parallel generation of rollouts - rollout_ids_one = [ - worker.do_rollouts( - self.w_policy, - num_rollouts=num_rollouts, - shift=self.shift, - evaluate=evaluate) for worker in self.workers - ] - - rollout_ids_two = [ - worker.do_rollouts( - self.w_policy, num_rollouts=1, shift=self.shift, evaluate=evaluate) - for worker in self.workers[:(num_deltas % self.num_workers)] - ] - results_one = rollout_ids_one - results_two = rollout_ids_two -# gather results - - rollout_rewards, deltas_idx = [], [] - - for result in results_one: - if not evaluate: - self.timesteps += result['steps'] - deltas_idx += result['deltas_idx'] - rollout_rewards += result['rollout_rewards'] - - for result in results_two: - if not evaluate: - self.timesteps += result['steps'] - deltas_idx += result['deltas_idx'] - rollout_rewards += result['rollout_rewards'] - - deltas_idx = np.array(deltas_idx) - rollout_rewards = np.array(rollout_rewards, dtype=np.float64) - - print('Maximum reward of collected rollouts:', rollout_rewards.max()) - info_dict = { - "max_reward": rollout_rewards.max() - } - t2 = time.time() - - print('Time to generate rollouts:', t2 - t1) - - if evaluate: - return rollout_rewards - - # select top performing directions if deltas_used < num_deltas - max_rewards = np.max(rollout_rewards, axis=1) - if self.deltas_used > self.num_deltas: - self.deltas_used = self.num_deltas - - idx = np.arange(max_rewards.size)[max_rewards >= np.percentile( - max_rewards, 100 * (1 - (self.deltas_used / self.num_deltas)))] - deltas_idx = deltas_idx[idx] - rollout_rewards = rollout_rewards[idx, :] - - # normalize rewards by their standard deviation - rollout_rewards /= np.std(rollout_rewards) - - t1 = time.time() - # aggregate rollouts to form g_hat, the gradient used to compute SGD step - g_hat, count = utils.batched_weighted_sum( - rollout_rewards[:, 0] - rollout_rewards[:, 1], - (self.deltas.get(idx, self.w_policy.size) for idx in deltas_idx), - batch_size=500) - g_hat /= deltas_idx.size - t2 = time.time() - print('time to aggregate rollouts', t2 - t1) - return g_hat, info_dict - - def train_step(self): - """ - Perform one update step of the policy weights. - """ - - g_hat, info_dict = self.aggregate_rollouts() - print('Euclidean norm of update step:', np.linalg.norm(g_hat)) - self.w_policy -= self.optimizer._compute_step(g_hat).reshape( - self.w_policy.shape) - return info_dict - - def train(self, num_iter): - - start = time.time() - for i in range(num_iter): - - t1 = time.time() - info_dict = self.train_step() - t2 = time.time() - print('total time of one step', t2 - t1) - print('iter ', i, ' done') - - # record statistics every 10 iterations - if ((i) % 10 == 0): - - rewards = self.aggregate_rollouts(num_rollouts=8, evaluate=True) - w = self.workers[0].get_weights_plus_stats() - - checkpoint_filename = os.path.join( - self.logdir, 'lin_policy_plus_{:03d}.npz'.format(i)) - print('Save checkpoints to {}...', checkpoint_filename) - checkpoint_file = open(checkpoint_filename, 'w') - np.savez(checkpoint_file, w) - print('End save checkpoints.') - print(sorted(self.params.items())) - logz.log_tabular('Time', time.time() - start) - logz.log_tabular('Iteration', i + 1) - logz.log_tabular('AverageReward', np.mean(rewards)) - logz.log_tabular('StdRewards', np.std(rewards)) - logz.log_tabular('MaxRewardRollout', np.max(rewards)) - logz.log_tabular('MinRewardRollout', np.min(rewards)) - logz.log_tabular('timesteps', self.timesteps) - logz.dump_tabular() - - t1 = time.time() - # get statistics from all workers - for j in range(self.num_workers): - self.policy.observation_filter.update(self.workers[j].get_filter()) - self.policy.observation_filter.stats_increment() - - # make sure master filter buffer is clear - self.policy.observation_filter.clear_buffer() - # sync all workers - #filter_id = ray.put(self.policy.observation_filter) - setting_filters_ids = [ - worker.sync_filter(self.policy.observation_filter) - for worker in self.workers - ] - # waiting for sync of all workers - #ray.get(setting_filters_ids) - - increment_filters_ids = [ - worker.stats_increment() for worker in self.workers - ] - # waiting for increment of all workers - #ray.get(increment_filters_ids) - t2 = time.time() - print('Time to sync statistics:', t2 - t1) - - return info_dict diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars_server.py b/examples/pybullet/gym/pybullet_envs/ARS/ars_server.py deleted file mode 100644 index f680dd632..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/ars_server.py +++ /dev/null @@ -1,62 +0,0 @@ -""" -blaze build -c opt //experimental/users/jietan/ARS:ars_server - -blaze-bin/experimental/users/jietan/ARS/ars_server \ ---config_name=MINITAUR_GYM_CONFIG -""" - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -import time -from absl import app -from absl import flags -from concurrent import futures -import grpc -from grpc import loas2 -from google3.robotics.reinforcement_learning.minitaur.envs import minitaur_gym_env -from google3.robotics.reinforcement_learning.minitaur.envs import minitaur_reactive_env -from google3.robotics.reinforcement_learning.minitaur.envs.env_randomizers import minitaur_env_randomizer -from google3.robotics.reinforcement_learning.minitaur.envs.env_randomizers import minitaur_env_randomizer_from_config as randomizer_config_lib -from google3.experimental.users.jietan.ARS import ars_evaluation_service_pb2_grpc -from google3.experimental.users.jietan.ARS import ars_evaluation_service - -FLAGS = flags.FLAGS -flags.DEFINE_integer("server_id", 0, "number of servers") -flags.DEFINE_integer("port", 20000, "port number.") -flags.DEFINE_string("config_name", None, "The name of the config dictionary.") -flags.DEFINE_bool('run_on_borg', False, - 'Whether the servers are running on borg.') - -_ONE_DAY_IN_SECONDS = 60 * 60 * 24 - - -def main(unused_argv): - servers = [] - server_creds = loas2.loas2_server_credentials() - port = FLAGS.port - if not FLAGS.run_on_borg: - port = 20000 + FLAGS.server_id - server = grpc.server( - futures.ThreadPoolExecutor(max_workers=10), ports=(port,)) - servicer = ars_evaluation_service.ParameterEvaluationServicer( - FLAGS.config_name, worker_id=FLAGS.server_id) - ars_evaluation_service_pb2_grpc.add_EvaluationServicer_to_server( - servicer, server) - server.add_secure_port("[::]:{}".format(port), server_creds) - servers.append(server) - server.start() - print("Start server {}".format(FLAGS.server_id)) - - # prevent the main thread from exiting - try: - while True: - time.sleep(_ONE_DAY_IN_SECONDS) - except KeyboardInterrupt: - for server in servers: - server.stop(0) - - -if __name__ == "__main__": - app.run(main) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/config_ars.py b/examples/pybullet/gym/pybullet_envs/ARS/config_ars.py deleted file mode 100644 index e333f88ab..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/config_ars.py +++ /dev/null @@ -1,83 +0,0 @@ - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -import functools -from pybullet_envs.minitaur.envs import minitaur_gym_env -from pybullet_envs.minitaur.envs import minitaur_reactive_env -from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer -from pybullet_envs.minitaur.envs.env_randomizers import minitaur_env_randomizer_from_config as randomizer_config_lib - -MAX_LENGTH = 1000 - - -def merge_two_dicts(x, y): - """Given two dicts, merge them into a new dict as a shallow copy.""" - z = dict(x) - z.update(y) - return z - - -# The default configurations. -DEFAULT_CONFIG = dict( - num_workers=8, - num_directions=8, - num_iterations=1000, - deltas_used=8, - step_size=0.02, - delta_std=0.03, - rollout_length=MAX_LENGTH, - shift=0, - seed=237, - policy_type="linear", - filter="MeanStdFilter", -) - -# Configuration specific to minitaur_gym_env.MinitaurGymEnv class. -MINITAUR_GYM_CONFIG_ADDITIONS = dict( - env=functools.partial( - minitaur_gym_env.MinitaurGymEnv, - urdf_version=minitaur_gym_env.DERPY_V0_URDF_VERSION, - accurate_motor_model_enabled=True, - motor_overheat_protection=True, - pd_control_enabled=True, - env_randomizer=None,#minitaur_env_randomizer.MinitaurEnvRandomizer(), - render=False, - num_steps_to_log=MAX_LENGTH)) -MINITAUR_GYM_CONFIG = merge_two_dicts(DEFAULT_CONFIG, - MINITAUR_GYM_CONFIG_ADDITIONS) - -# Configuration specific to MinitaurReactiveEnv class. -MINITAUR_REACTIVE_CONFIG_ADDITIONS = dict( - env=functools.partial( - minitaur_reactive_env.MinitaurReactiveEnv, - urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION, - energy_weight=0.005, - accurate_motor_model_enabled=True, - pd_latency=0.003, - control_latency=0.02, - motor_kd=0.015, - remove_default_joint_damping=True, - env_randomizer=None, - render=False, - num_steps_to_log=MAX_LENGTH)) -MINITAUR_REACTIVE_CONFIG = merge_two_dicts(DEFAULT_CONFIG, - MINITAUR_REACTIVE_CONFIG_ADDITIONS) - -# Configuration specific to MinitaurReactiveEnv class with randomizer. -MINITAUR_REACTIVE_RANDOMIZER_CONFIG_ADDITIONS = dict( - env=functools.partial( - minitaur_reactive_env.MinitaurReactiveEnv, - urdf_version=minitaur_gym_env.RAINBOW_DASH_V0_URDF_VERSION, - energy_weight=0.005, - accurate_motor_model_enabled=True, - pd_latency=0.003, - control_latency=0.02, - motor_kd=0.015, - remove_default_joint_damping=True, - env_randomizer=randomizer_config_lib.MinitaurEnvRandomizerFromConfig(), - render=False, - num_steps_to_log=MAX_LENGTH)) -MINITAUR_REACTIVE_RANDOMIZER_CONFIG = merge_two_dicts( - DEFAULT_CONFIG, MINITAUR_REACTIVE_RANDOMIZER_CONFIG_ADDITIONS) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py b/examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py deleted file mode 100644 index d0c144bfa..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/eval_ars.py +++ /dev/null @@ -1,99 +0,0 @@ -""" -blaze run -c opt //experimental/users/jietan/ARS:eval_ars -- \ ---logdir=/cns/ij-d/home/jietan/experiment/ARS/ars_react_nr01.191950338.191950550/ \ ---checkpoint=lin_policy_plus_990.npz \ ---num_rollouts=10 -""" - - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -import os, inspect -import time - -currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) -os.sys.path.insert(0,currentdir) - -from absl import app -from absl import flags - -import pdb -import os -import numpy as np -import gym -import config_ars -import utility -import policies - -FLAGS = flags.FLAGS - -flags.DEFINE_string('logdir', None, 'The path of the checkpoint.') -flags.DEFINE_string('checkpoint', None, 'The file name of the checkpoint.') -flags.DEFINE_integer('num_rollouts', 1, 'The number of rollouts.') - - -def main(argv): - del argv # Unused. - - print('loading and building expert policy') - checkpoint_file = os.path.join(FLAGS.logdir, FLAGS.checkpoint) - lin_policy = np.load(checkpoint_file, encoding='bytes') - lin_policy = lin_policy.items()[0][1] - - M = lin_policy[0] - # mean and std of state vectors estimated online by ARS. - mean = lin_policy[1] - std = lin_policy[2] - - config = utility.load_config(FLAGS.logdir) - print("config=",config) - env = config['env'](hard_reset=True, render=True) - ob_dim = env.observation_space.shape[0] - ac_dim = env.action_space.shape[0] - - # set policy parameters. Possible filters: 'MeanStdFilter' for v2, 'NoFilter' for v1. - policy_params = { - 'type': 'linear', - 'ob_filter': config['filter'], - 'ob_dim': ob_dim, - 'ac_dim': ac_dim, - "weights": M, - "mean": mean, - "std": std, - } - policy = policies.LinearPolicy(policy_params, update_filter=False) - returns = [] - observations = [] - actions = [] - for i in range(FLAGS.num_rollouts): - print('iter', i) - obs = env.reset() - done = False - totalr = 0. - steps = 0 - while not done: - action = policy.act(obs) - observations.append(obs) - actions.append(action) - - obs, r, done, _ = env.step(action) - time.sleep(1./100.) - totalr += r - steps += 1 - if steps % 100 == 0: - print('%i/%i' % (steps, config['rollout_length'])) - if steps >= config['rollout_length']: - break - returns.append(totalr) - - print('returns', returns) - print('mean return', np.mean(returns)) - print('std of return', np.std(returns)) - - -if __name__ == '__main__': - flags.mark_flag_as_required('logdir') - flags.mark_flag_as_required('checkpoint') - app.run(main) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/filter.py b/examples/pybullet/gym/pybullet_envs/ARS/filter.py deleted file mode 100644 index ac68a2341..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/filter.py +++ /dev/null @@ -1,280 +0,0 @@ -# Code in this file is copied and adapted from -# https://github.com/ray-project/ray/blob/master/python/ray/rllib/utils/filter.py - - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -import numpy as np - - -class Filter(object): - """Processes input, possibly statefully.""" - - def update(self, other, *args, **kwargs): - """Updates self with "new state" from other filter.""" - raise NotImplementedError - - def copy(self): - """Creates a new object with same state as self. - - Returns: - copy (Filter): Copy of self""" - raise NotImplementedError - - def sync(self, other): - """Copies all state from other filter to self.""" - raise NotImplementedError - - -class NoFilter(Filter): - def __init__(self, *args): - pass - - def __call__(self, x, update=True): - return np.asarray(x, dtype = np.float64) - - def update(self, other, *args, **kwargs): - pass - - def copy(self): - return self - - def sync(self, other): - pass - - def stats_increment(self): - pass - - def clear_buffer(self): - pass - - def get_stats(self): - return 0, 1 - - @property - def mean(self): - return 0 - - @property - def var(self): - return 1 - - @property - def std(self): - return 1 - - - -# http://www.johndcook.com/blog/standard_deviation/ -class RunningStat(object): - - def __init__(self, shape=None): - self._n = 0 - self._M = np.zeros(shape, dtype = np.float64) - self._S = np.zeros(shape, dtype = np.float64) - self._M2 = np.zeros(shape, dtype = np.float64) - - def copy(self): - other = RunningStat() - other._n = self._n - other._M = np.copy(self._M) - other._S = np.copy(self._S) - return other - - def push(self, x): - x = np.asarray(x) - # Unvectorized update of the running statistics. - assert x.shape == self._M.shape, ("x.shape = {}, self.shape = {}" - .format(x.shape, self._M.shape)) - n1 = self._n - self._n += 1 - if self._n == 1: - self._M[...] = x - else: - delta = x - self._M - deltaM2 = np.square(x) - self._M2 - self._M[...] += delta / self._n - self._S[...] += delta * delta * n1 / self._n - - - def update(self, other): - n1 = self._n - n2 = other._n - n = n1 + n2 - delta = self._M - other._M - delta2 = delta * delta - M = (n1 * self._M + n2 * other._M) / n - S = self._S + other._S + delta2 * n1 * n2 / n - self._n = n - self._M = M - self._S = S - - def __repr__(self): - return '(n={}, mean_mean={}, mean_std={})'.format( - self.n, np.mean(self.mean), np.mean(self.std)) - - @property - def n(self): - return self._n - - @property - def mean(self): - return self._M - - @property - def var(self): - return self._S / (self._n - 1) if self._n > 1 else np.square(self._M) - - @property - def std(self): - return np.sqrt(self.var) - - @property - def shape(self): - return self._M.shape - - -class MeanStdFilter(Filter): - """Keeps track of a running mean for seen states""" - - def __init__(self, shape, demean=True, destd=True): - self.shape = shape - self.demean = demean - self.destd = destd - self.rs = RunningStat(shape) - # In distributed rollouts, each worker sees different states. - # The buffer is used to keep track of deltas amongst all the - # observation filters. - - self.buffer = RunningStat(shape) - - self.mean = np.zeros(shape, dtype = np.float64) - self.std = np.ones(shape, dtype = np.float64) - - def clear_buffer(self): - self.buffer = RunningStat(self.shape) - return - - def update(self, other, copy_buffer=False): - """Takes another filter and only applies the information from the - buffer. - - Using notation `F(state, buffer)` - Given `Filter1(x1, y1)` and `Filter2(x2, yt)`, - `update` modifies `Filter1` to `Filter1(x1 + yt, y1)` - If `copy_buffer`, then `Filter1` is modified to - `Filter1(x1 + yt, yt)`. - """ - self.rs.update(other.buffer) - if copy_buffer: - self.buffer = other.buffer.copy() - return - - def copy(self): - """Returns a copy of Filter.""" - other = MeanStdFilter(self.shape) - other.demean = self.demean - other.destd = self.destd - other.rs = self.rs.copy() - other.buffer = self.buffer.copy() - return other - - def sync(self, other): - """Syncs all fields together from other filter. - - Using notation `F(state, buffer)` - Given `Filter1(x1, y1)` and `Filter2(x2, yt)`, - `sync` modifies `Filter1` to `Filter1(x2, yt)` - """ - assert other.shape == self.shape, "Shapes don't match!" - self.demean = other.demean - self.destd = other.destd - self.rs = other.rs.copy() - self.buffer = other.buffer.copy() - return - - def __call__(self, x, update=True): - x = np.asarray(x, dtype = np.float64) - if update: - if len(x.shape) == len(self.rs.shape) + 1: - # The vectorized case. - for i in range(x.shape[0]): - self.rs.push(x[i]) - self.buffer.push(x[i]) - else: - # The unvectorized case. - self.rs.push(x) - self.buffer.push(x) - if self.demean: - x = x - self.mean - if self.destd: - x = x / (self.std + 1e-8) - return x - - def stats_increment(self): - self.mean = self.rs.mean - self.std = self.rs.std - - # Set values for std less than 1e-7 to +inf to avoid - # dividing by zero. State elements with zero variance - # are set to zero as a result. - self.std[self.std < 1e-7] = float("inf") - return - - def get_stats(self): - return self.rs.mean, (self.rs.std + 1e-8) - - def __repr__(self): - return 'MeanStdFilter({}, {}, {}, {}, {}, {})'.format( - self.shape, self.demean, - self.rs, self.buffer) - - -def get_filter(filter_config, shape = None): - if filter_config == "MeanStdFilter": - return MeanStdFilter(shape) - elif filter_config == "NoFilter": - return NoFilter() - else: - raise Exception("Unknown observation_filter: " + - str(filter_config)) - - -def test_running_stat(): - for shp in ((), (3,), (3, 4)): - li = [] - rs = RunningStat(shp) - for _ in range(5): - val = np.random.randn(*shp) - rs.push(val) - li.append(val) - m = np.mean(li, axis=0) - assert np.allclose(rs.mean, m) - v = np.square(m) if (len(li) == 1) else np.var(li, ddof=1, axis=0) - assert np.allclose(rs.var, v) - - -def test_combining_stat(): - for shape in [(), (3,), (3, 4)]: - li = [] - rs1 = RunningStat(shape) - rs2 = RunningStat(shape) - rs = RunningStat(shape) - for _ in range(5): - val = np.random.randn(*shape) - rs1.push(val) - rs.push(val) - li.append(val) - for _ in range(9): - rs2.push(val) - rs.push(val) - li.append(val) - rs1.update(rs2) - assert np.allclose(rs.mean, rs1.mean) - assert np.allclose(rs.std, rs1.std) - - -test_running_stat() -test_combining_stat() diff --git a/examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml b/examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml deleted file mode 100644 index 36690d51d..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/log/config.yaml +++ /dev/null @@ -1,29 +0,0 @@ -delta_std: 0.03 -deltas_used: 8 -env: !!python/object/apply:functools.partial - args: - - &id001 !!python/name:pybullet_envs.minitaur.envs.minitaur_reactive_env.MinitaurReactiveEnv '' - state: !!python/tuple - - *id001 - - !!python/tuple [] - - accurate_motor_model_enabled: true - control_latency: 0.02 - energy_weight: 0.005 - env_randomizer: null - motor_kd: 0.015 - num_steps_to_log: 1000 - pd_latency: 0.003 - remove_default_joint_damping: true - render: false - urdf_version: rainbow_dash_v0 - - null -filter: MeanStdFilter -num_directions: 8 -num_iterations: 1000 -num_workers: 8 -policy_type: linear -rollout_length: 1000 -seed: 237 -shift: 0 -step_size: 0.02 - diff --git a/examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz b/examples/pybullet/gym/pybullet_envs/ARS/log/lin_policy_plus_990_bla.npz deleted file mode 100644 index 54fd39ba7d8a7d2e84f4c63d47c08415bc2e5357..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1407 zcmZ`(doxrS_bCzw^D{_dVbHyw6|n!(x&(jWKo1 z+bP|8Z#a>nr-fnatxI45LZSZ}+t6_FIS-$0EMF4lhDBNO_#&>*a)Y^Lw5KJ_+;V@I z5Ell7`iBX5d^PMA5G>*oxJVEX&L`ehbec8IJjVRPpu}l#LnA}N#kSlqA>TG6G8hjc zv;xEwe<7b67AnGpkzAY-L3&?B3FW>2jnH6X5+4%hly?N#ib-OUC4Uw0aB(=F5~1m% z;kk~9F-b&$mKBriLyGqB@Nl0c;$30_&XIlI?Fz4USfJxwRhL}}O}l(DrpL3;g=%r1@zJ}GV>mZy=S&CO_&HFz zcD@{ydnkiNX6-07<6_CEwG63Tiej3jUFgT7O6r1~N@S$x5)!7@4$~Cw=B?!02e;4EoIMfCp3^YdEKMyCU~T~!mhnPc#V>Qas3c|p04?0Vsu&B%J12q8yW zo=qO!jvV&wfB2jshm-o1p8NXmg3Hj?L&1RxIP%4=cq2g>yr>SYE>KQP_r7k^Q z4(WQ8HmeL8KxVpnHn6z~{GzkGtt`aopfokdQPv9bTLLZjb%}`Kb-6V#sS4Z+hIH_h zW>mW789m!nf!<8FG;-$lK`WIb9i33Z`Oco=jT_`B`Bh2*ws0@H{|#DkwyP214)2Ru zR@;sEcLH^H;zh(1eKbJ2>H*T12~5QdFAX@WSG^pEL-olz6<#yo>~oq;8F>p)56X-dFPQ*)UhP7G2ewIH z+|<`%GzH$tK9KhHd;N0kMLKvHi#8uwmDAEJxzBBP. - """ - - def __init__(self, policy_params, update_filter=True): - Policy.__init__(self, policy_params) - self.weights = np.zeros(self.ac_dim * self.ob_dim, dtype=np.float64) - if "weights" in policy_params: - self.weights = policy_params["weights"] - if "mean" in policy_params: - self.observation_filter.mean = policy_params["mean"] - if "std" in policy_params: - self.observation_filter.std = policy_params["std"] - self.update_filter = update_filter - - def act(self, ob): - ob = self.observation_filter(ob, update=self.update_filter) - matrix_weights = np.reshape(self.weights, (self.ac_dim, self.ob_dim)) - return np.clip(np.dot(matrix_weights, ob), -1.0, 1.0) - - def get_weights_plus_stats(self): - - mu, std = self.observation_filter.get_stats() - aux = np.asarray([self.weights, mu, std]) - return aux diff --git a/examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py b/examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py deleted file mode 100644 index 036f6b26c..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/shared_noise.py +++ /dev/null @@ -1,40 +0,0 @@ -""" -Code in this file is copied and adapted from -https://github.com/ray-project/ray/tree/master/python/ray/rllib/es -""" - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function -import numpy as np - - -def create_shared_noise(): - """ - Create a large array of noise to be shared by all workers. Used - for avoiding the communication of the random perturbations delta. - """ - - seed = 12345 - count = 250000000 - noise = np.random.RandomState(seed).randn(count).astype(np.float64) - return noise - - -class SharedNoiseTable(object): - def __init__(self, noise, seed = 11): - - self.rg = np.random.RandomState(seed) - self.noise = noise - assert self.noise.dtype == np.float64 - - def get(self, i, dim): - return self.noise[i:i + dim] - - def sample_index(self, dim): - return self.rg.randint(0, len(self.noise) - dim + 1) - - def get_delta(self, dim): - idx = self.sample_index(dim) - return idx, self.get(idx, dim) - diff --git a/examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py b/examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py deleted file mode 100644 index 7dc27547e..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/start_ars_servers.py +++ /dev/null @@ -1,27 +0,0 @@ -""" - -blaze build -c opt //experimental/users/jietan/ARS:start_ars_servers -blaze-bin/experimental/users/jietan/ARS/start_ars_servers - -""" - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function -import time -import subprocess -from absl import app -from absl import flags - -FLAGS = flags.FLAGS -flags.DEFINE_integer("num_servers", 8, "number of servers") - -def main(argv): - del argv # Unused. - for server_id in xrange(FLAGS.num_servers): - args = ["blaze-bin/experimental/users/jietan/ARS/ars_server", "--config_name=MINITAUR_GYM_CONFIG", "--server_id={}".format(server_id), "--run_on_borg=False"] - subprocess.Popen(args) - - -if __name__ == '__main__': - app.run(main) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg b/examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg deleted file mode 100644 index c09775b50..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/train_ars.borg +++ /dev/null @@ -1,93 +0,0 @@ -// Example borg file to do a parameter sweep. -// -// To run: -// echo `srcfs get_readonly`-`g4 p | head -1 | awk '{print $2}'` -// blaze build -c opt experimental/users/jietan/ARS:ars_server.par -// blaze build -c opt experimental/users/jietan/ARS:ars_client.par -// borgcfg --skip_confirmation --vars 'base_cl=191950338,my_cl=191950550,label=ars_react_nr01,config=MINITAUR_REACTIVE_CONFIG' experimental/users/jietan/ARS/train_ars.borg reload -// borgcfg --skip_confirmation --vars 'base_cl=191950338,my_cl=191950550,label=ars_react_rd01,config=MINITAUR_REACTIVE_RANDOMIZER_CONFIG' experimental/users/jietan/ARS/train_ars.borg reload - - -import '//production/borg/templates/lambda/buildtool_support.borg' as build -import '//production/borg/templates/lambda/dnsname.borg' as dns - -vars = { - cell = 'atlanta' - charged_user = 'robotics' - base_cl = 0 - my_cl = 0 - label = external - user = real_username() - workers = 8 - config = external - cns_home = "/cns/ij-d/home/%user%" - logdir = "%cns_home%/experiment/ARS/%label%.%base_cl%.%my_cl%/" -} - -service augmented_random_search { - runtime { - cell = vars.cell - } - - scheduling = { - priority = 100 - batch_quota = { - strategy = 'RUN_SOON' - } - deadline = 3600 * 24 - } - accounting = { - charged_user = vars.charged_user - } - requirements { - autopilot = true - } - params = { - mygoogle3 = build.google3dir(myfilename()) - experiment_dir = 'experimental/users/jietan/ARS/' - } - - job ars_server = { - runtime { - cell = vars.cell - } - name = real_username() + '_server_' + vars.label - replicas = vars.workers - binary_path = build.binfile_v2(params.mygoogle3, - params.experiment_dir + 'ars_server') - runfiles = binary_path + '.runfiles/google3/' - packages = { - package third_party = { - directory = runfiles + 'third_party/' - } - } - binary = build.binfile(params.mygoogle3, - params.experiment_dir + 'ars_server.par') - args = { - server_id = '%task%' - config_name = vars.config - port = '%port%' - run_on_borg = true - } - } - job ars_client = { - name = real_username() + '_client_' + vars.label - binary_path = build.binfile_v2(params.mygoogle3, - params.experiment_dir + 'ars_client') - runfiles = binary_path + '.runfiles/google3/' - packages = { - package third_party = { - directory = runfiles + 'third_party/' - } - } - binary = build.binfile(params.mygoogle3, - params.experiment_dir + 'ars_client.par') - args = { - server_address = dns.borg_dns_name(ars_server) - num_servers = vars.workers - config_name = vars.config - logdir = vars.logdir - run_on_borg = true - } - } -} diff --git a/examples/pybullet/gym/pybullet_envs/ARS/train_ars.py b/examples/pybullet/gym/pybullet_envs/ARS/train_ars.py deleted file mode 100644 index 2b07f4aa0..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/train_ars.py +++ /dev/null @@ -1,64 +0,0 @@ -""" - -blaze build -c opt //experimental/users/jietan/ARS:train_ars -blaze-bin/experimental/users/jietan/ARS/train_ars \ ---logdir=/cns/ij-d/home/jietan/experiment/ARS/test1 \ ---config_name=MINITAUR_GYM_CONFIG -""" - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - - -import os -from absl import app -from absl import flags -import ars -import config_ars - -FLAGS = flags.FLAGS -flags.DEFINE_string('logdir', None, 'The directory to write the log file.') -flags.DEFINE_string('config_name', None, 'The name of the config dictionary') - - -def run_ars(config, logdir): - - env = config["env"]() - ob_dim = env.observation_space.shape[0] - ac_dim = env.action_space.shape[0] - - # set policy parameters. Possible filters: 'MeanStdFilter' for v2, 'NoFilter' for v1. - policy_params = { - 'type': 'linear', - 'ob_filter': config['filter'], - 'ob_dim': ob_dim, - 'ac_dim': ac_dim - } - - ARS = ars.ARSLearner( - env_callback=config['env'], - policy_params=policy_params, - num_deltas=config['num_directions'], - deltas_used=config['deltas_used'], - step_size=config['step_size'], - delta_std=config['delta_std'], - logdir=logdir, - rollout_length=config['rollout_length'], - shift=config['shift'], - params=config, - seed=config['seed']) - - return ARS.train(config['num_iterations']) - - -def main(argv): - del argv # Unused. - config = getattr(config_ars, FLAGS.config_name) - run_ars(config=config, logdir=FLAGS.logdir) - - -if __name__ == '__main__': - flags.mark_flag_as_required('logdir') - flags.mark_flag_as_required('config_name') - app.run(main) diff --git a/examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py b/examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py deleted file mode 100644 index 13f149fc5..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/train_ars_test.py +++ /dev/null @@ -1,29 +0,0 @@ -"""Tests for google3.experimental.users.jietan.ARS.train_ars. -blaze build -c opt //experimental/users/jietan/ARS:train_ars_test -blaze-bin/experimental/users/jietan/ARS/train_ars_test -""" - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - -from absl import flags -from google3.testing.pybase import googletest -from google3.experimental.users.jietan.ARS import train_ars -from google3.experimental.users.jietan.ARS import config_ars - -FLAGS = flags.FLAGS -MAX_RETURN_AFTER_TWO_ITEATIONS = 0.0890905394617 - -class TrainArsTest(googletest.TestCase): - - def testArsTwoStepResult(self): - config = getattr(config_ars, "MINITAUR_REACTIVE_CONFIG") - config['num_iterations'] = 2 - info = train_ars.run_ars(config=config, logdir=FLAGS.test_tmpdir) - print (info) - self.assertAlmostEqual(info["max_reward"], MAX_RETURN_AFTER_TWO_ITEATIONS) - - -if __name__ == '__main__': - googletest.main() diff --git a/examples/pybullet/gym/pybullet_envs/ARS/utility.py b/examples/pybullet/gym/pybullet_envs/ARS/utility.py deleted file mode 100644 index 591418be5..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/utility.py +++ /dev/null @@ -1,52 +0,0 @@ - -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function -import os -import ruamel.yaml as yaml - -def save_config(config, logdir): - """Save a new configuration by name. - - If a logging directory is specified, is will be created and the configuration - will be stored there. Otherwise, a log message will be printed. - - Args: - config: Configuration object. - logdir: Location for writing summaries and checkpoints if specified. - - Returns: - Configuration object. - """ - message = 'Start a new run and write summaries and checkpoints to {}.' - print(message.format(logdir)) - config_path = os.path.join(logdir, 'config.yaml') - yaml.dump(config, config_path, default_flow_style=False) - return config - - -def load_config(logdir): - """Load a configuration from the log directory. - - Args: - logdir: The logging directory containing the configuration file. - - Raises: - IOError: The logging directory does not contain a configuration file. - - Returns: - Configuration object. - """ - config_path = logdir and os.path.join(logdir, 'config.yaml') - if not config_path: - message = ( - 'Cannot resume an existing run since the logging directory does not ' - 'contain a configuration file.') - raise IOError(message) - print("config_path=",config_path) - - stream = open(config_path, 'r') - config = yaml.load(stream) - message = 'Resume run and write summaries and checkpoints to {}.' - print(message.format(logdir)) - return config diff --git a/examples/pybullet/gym/pybullet_envs/ARS/utils.py b/examples/pybullet/gym/pybullet_envs/ARS/utils.py deleted file mode 100644 index 3b7e03afa..000000000 --- a/examples/pybullet/gym/pybullet_envs/ARS/utils.py +++ /dev/null @@ -1,28 +0,0 @@ -# Code in this file is copied and adapted from -# https://github.com/openai/evolution-strategies-starter. - -import numpy as np - -def itergroups(items, group_size): - assert group_size >= 1 - group = [] - for x in items: - group.append(x) - if len(group) == group_size: - yield tuple(group) - del group[:] - if group: - yield tuple(group) - - - -def batched_weighted_sum(weights, vecs, batch_size): - total = 0 - num_items_summed = 0 - for batch_weights, batch_vecs in zip(itergroups(weights, batch_size), - itergroups(vecs, batch_size)): - assert len(batch_weights) == len(batch_vecs) <= batch_size - total += np.dot(np.asarray(batch_weights, dtype=np.float64), - np.asarray(batch_vecs, dtype=np.float64)) - num_items_summed += len(batch_weights) - return total, num_items_summed From 47b8b98a64ca1159d19555ff985ca7952b24be60 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 30 Oct 2018 22:16:13 -0700 Subject: [PATCH 05/17] PyBullet: call the right method (although equivalent at the moment) --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5e61b5c89..c025ee9cd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6464,7 +6464,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P { pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation); } - b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation); + b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); From 624212c64170392d362076f58059e474c44b0790 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 31 Oct 2018 09:17:58 -0700 Subject: [PATCH 06/17] add humanoid.urdf, converted from mjcf using pybullet_utils.urdfEditor.UrdfEditor() --- data/humanoid.urdf | 586 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 586 insertions(+) create mode 100644 data/humanoid.urdf diff --git a/data/humanoid.urdf b/data/humanoid.urdf new file mode 100644 index 000000000..b4f991241 --- /dev/null +++ b/data/humanoid.urdf @@ -0,0 +1,586 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8e82de1b008cd6275b8800243da5a06bd1a1425b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 31 Oct 2018 11:02:19 -0700 Subject: [PATCH 07/17] add rudimentary MuJoCo mjcf xml to ROS URDF file, based on pybullet_utils.urdfEditor --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 33 +++++++++++------- .../PhysicsServerCommandProcessor.cpp | 34 ++++++++++++------- .../gym/pybullet_utils/examples/mjcf2urdf.py | 21 ++++++++++++ .../pybullet/gym/pybullet_utils/urdfEditor.py | 12 +++++-- 4 files changed, 74 insertions(+), 26 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 943b26ddd..9add4bddc 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -2,8 +2,8 @@ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -85,7 +85,7 @@ BulletURDFInternalData m_urdfParser.setGlobalScaling(scaling); } - + }; void BulletURDFImporter::printTree() @@ -160,11 +160,11 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) //read file int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); - + char destBuffer[8192]; char* line = 0; - do + do { line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); if (line) @@ -190,7 +190,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) BulletErrorLogger loggie; m_data->m_urdfParser.setParseSDF(false); bool result = false; - + if (xml_string.length()) { result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS)); @@ -235,10 +235,10 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase) //read file int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r"); - + char destBuffer[8192]; char* line = 0; - do + do { line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192); if (line) @@ -883,6 +883,15 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu switch (visual->m_geometry.m_type) { + case URDF_GEOM_CAPSULE: + { + btScalar radius = visual->m_geometry.m_capsuleRadius; + btScalar height = visual->m_geometry.m_capsuleHeight; + btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height); + convexColShape = capsuleShape; + convexColShape->setMargin(gUrdfDefaultCollisionMargin); + break; + } case URDF_GEOM_CYLINDER: { btAlignedObjectArray vertices; @@ -932,7 +941,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu { case UrdfGeometry::FILE_OBJ: { - + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO)) { if (meshData.m_textureImage1) @@ -1172,7 +1181,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR)) { UrdfMaterialColor matCol; - + if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) { matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0], @@ -1186,7 +1195,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP meshData.m_rgbaColor[2], 1); } - + matCol.m_specularColor.setValue(meshData.m_specularColor[0], meshData.m_specularColor[1], meshData.m_specularColor[2]); @@ -1204,7 +1213,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP m_data->m_linkColors.insert(linkIndex, matCol); } } - + } } if (vertices.size() && indices.size()) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d251ca53e..537f3e555 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1652,7 +1652,7 @@ struct PhysicsServerCommandProcessorInternalData std::string m_profileTimingFileName; struct GUIHelperInterface* m_guiHelper; - + int m_sharedMemoryKey; bool m_enableTinyRenderer; @@ -1777,7 +1777,7 @@ struct PhysicsServerCommandProcessorInternalData } #endif - + m_vrControllerEvents.init(); @@ -2271,7 +2271,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface double globalScaling = 1.f; //todo! int flags = 0; CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); - + BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(),fileIO, globalScaling, flags); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); @@ -2755,7 +2755,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, SaveWorldObjectData sd; sd.m_fileName = fileName; - + for (int m = 0; m < u2b.getNumModels(); m++) { @@ -2971,7 +2971,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, } { - + if (m_data->m_pluginManager.getRenderInterface()) { @@ -4267,7 +4267,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES; break; } - + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); if (foundFile) { @@ -5210,7 +5210,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co } } } - + BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); batchRayCaster.castRays(numThreads); @@ -6770,7 +6770,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S useMultiBody = false; } - + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); @@ -6912,7 +6912,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar const std::string& error_message_prefix = ""; std::string out_found_filename; int out_type; - + bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); std::vector shapes; std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); @@ -9642,6 +9642,16 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape switch (colShape->getShapeType()) { + case STATIC_PLANE_PROXYTYPE: + { + btStaticPlaneShape* plane = (btStaticPlaneShape*) colShape; + collisionShapeBuffer[0].m_collisionGeometryType = GEOM_PLANE; + collisionShapeBuffer[0].m_dimensions[0] = plane->getPlaneNormal()[0]; + collisionShapeBuffer[0].m_dimensions[1] = plane->getPlaneNormal()[1]; + collisionShapeBuffer[0].m_dimensions[2] = plane->getPlaneNormal()[2]; + numConverted += 1; + break; + } case CONVEX_HULL_SHAPE_PROXYTYPE: { UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape); @@ -10054,7 +10064,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share { int width, height, n; unsigned char* imageData = 0; - + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); if (fileIO) { @@ -10083,7 +10093,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share { imageData = stbi_load(relativeFileName, &width, &height, &n, 3); } - + if (imageData) { texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height); @@ -10161,7 +10171,7 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar buffer.reserve(1024); if (fileIO) { - + int fileId = -1; found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024); if (found) diff --git a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py new file mode 100644 index 000000000..a4fc7ca0e --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py @@ -0,0 +1,21 @@ +#rudimentary MuJoCo mjcf to ROS URDF converter using the UrdfEditor + +import pybullet_utils.bullet_client as bc +import pybullet_data as pd + +import pybullet_utils.urdfEditor as ed +import argparse +parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) +parser.add_argument('--mjcf', help='MuJoCo xml file to be converted to URDF', default='mjcf/humanoid.xml') +args = parser.parse_args() + +p = bc.BulletClient() +p.setAdditionalSearchPath(pd.getDataPath()) +objs = p.loadMJCF(args.mjcf, flags=p.URDF_USE_IMPLICIT_CYLINDER) + +for o in objs: + print("o=",o, p.getBodyInfo(o), p.getNumJoints(o)) + humanoid = objs[o] + ed0 = ed.UrdfEditor() + ed0.initializeFromBulletBody(humanoid, p._client) + ed0.saveUrdf(p.getBodyInfo(0)[1]+"_"+p.getBodyInfo(o)[0]+".urdf") diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index b29ce6c60..6b05ce72c 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -1,7 +1,6 @@ import pybullet as p import time - class UrdfInertial(object): def __init__(self): self.mass = 1 @@ -201,6 +200,10 @@ class UrdfEditor(object): file.write("\t\t\n") def writeVisualShape(self,file,urdfVisual, precision=5): + #we don't support loading capsule types from visuals, so auto-convert from collision shape + if urdfVisual.geom_type == p.GEOM_CAPSULE: + return + file.write("\t\t\n") str = '\t\t\t\n'.format(\ urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2], @@ -276,8 +279,13 @@ class UrdfEditor(object): file.write("\">\n") self.writeInertial(file,urdfLink.urdf_inertial) + hasCapsules = False for v in urdfLink.urdf_visual_shapes: - self.writeVisualShape(file,v) + if (v.geom_type == p.GEOM_CAPSULE): + hasCapsules = True + if (not hasCapsules): + for v in urdfLink.urdf_visual_shapes: + self.writeVisualShape(file,v) for c in urdfLink.urdf_collision_shapes: self.writeCollisionShape(file,c) file.write("\t\n") From a48b6b9ca0dbbcaab4774258015ce8e1638c9310 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 31 Oct 2018 17:00:34 -0700 Subject: [PATCH 08/17] fix some thread sanitizer (read/write integer, should be a harmless warning) --- .../TaskScheduler/btThreadSupportPosix.cpp | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp index 47f57943b..25bfda0bf 100644 --- a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp +++ b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp @@ -1,3 +1,4 @@ + /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2018 Erwin Coumans http://bulletphysics.com @@ -72,7 +73,7 @@ public: pthread_t thread; //each tread will wait until this signal to start its work sem_t* startSemaphore; - + btCriticalSection* m_cs; // this is a copy of m_mainSemaphore, //each tread will signal once it is finished with its work sem_t* m_mainSemaphore; @@ -90,7 +91,7 @@ private: void startThreads(const ConstructionInfo& threadInfo); void stopThreads(); int waitForResponse(); - + btCriticalSection* m_cs; public: btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo); virtual ~btThreadSupportPosix(); @@ -119,6 +120,7 @@ public: btThreadSupportPosix::btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo) { + m_cs = createCriticalSection(); startThreads(threadConstructionInfo); } @@ -126,6 +128,8 @@ btThreadSupportPosix::btThreadSupportPosix(const ConstructionInfo& threadConstru btThreadSupportPosix::~btThreadSupportPosix() { stopThreads(); + deleteCriticalSection(m_cs); + m_cs=0; } #if (defined(__APPLE__)) @@ -181,14 +185,18 @@ static void* threadFunction(void* argument) { btAssert(status->m_status); status->m_userThreadFunc(userPtr); + status->m_cs->lock(); status->m_status = 2; + status->m_cs->unlock(); checkPThreadFunction(sem_post(status->m_mainSemaphore)); status->threadUsed++; } else { //exit Thread + status->m_cs->lock(); status->m_status = 3; + status->m_cs->unlock(); checkPThreadFunction(sem_post(status->m_mainSemaphore)); printf("Thread with taskId %i exiting\n", status->m_taskId); break; @@ -206,7 +214,7 @@ void btThreadSupportPosix::runTask(int threadIndex, void* userData) btThreadStatus& threadStatus = m_activeThreadStatus[threadIndex]; btAssert(threadIndex >= 0); btAssert(threadIndex < m_activeThreadStatus.size()); - + threadStatus.m_cs = m_cs; threadStatus.m_commandId = 1; threadStatus.m_status = 1; threadStatus.m_userPtr = userData; @@ -231,7 +239,10 @@ int btThreadSupportPosix::waitForResponse() for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t) { - if (2 == m_activeThreadStatus[t].m_status) + m_cs->lock(); + bool hasFinished = (2 == m_activeThreadStatus[t].m_status); + m_cs->unlock(); + if (hasFinished) { last = t; break; @@ -273,15 +284,15 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi printf("starting thread %d\n", i); btThreadStatus& threadStatus = m_activeThreadStatus[i]; threadStatus.startSemaphore = createSem("threadLocal"); - checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); - threadStatus.m_userPtr = 0; + threadStatus.m_cs = m_cs; threadStatus.m_taskId = i; threadStatus.m_commandId = 0; threadStatus.m_status = 0; threadStatus.m_mainSemaphore = m_mainSemaphore; threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; threadStatus.threadUsed = 0; + checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); printf("started thread %d \n", i); } From 131535a99ffb362b89c0f52064f6e05fcf136cc4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 31 Oct 2018 21:24:44 -0700 Subject: [PATCH 09/17] remove debug stats --- examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py | 2 +- .../NarrowPhaseCollision/btGjkPairDetector.cpp | 5 ----- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py index a4fc7ca0e..c1e39745d 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py @@ -14,7 +14,7 @@ p.setAdditionalSearchPath(pd.getDataPath()) objs = p.loadMJCF(args.mjcf, flags=p.URDF_USE_IMPLICIT_CYLINDER) for o in objs: - print("o=",o, p.getBodyInfo(o), p.getNumJoints(o)) + #print("o=",o, p.getBodyInfo(o), p.getNumJoints(o)) humanoid = objs[o] ed0 = ed.UrdfEditor() ed0.initializeFromBulletBody(humanoid, p._client) diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 936130753..803f6e067 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -36,9 +36,6 @@ btScalar gGjkEpaPenetrationTolerance = 1.0e-12; btScalar gGjkEpaPenetrationTolerance = 0.001; #endif -//temp globals, to improve GJK/EPA/penetration calculations -int gNumDeepPenetrationChecks = 0; -int gNumGjkChecks = 0; btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver) : m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)), @@ -708,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu btScalar marginA = m_marginA; btScalar marginB = m_marginB; - gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) @@ -1021,7 +1017,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu // Penetration depth case. btVector3 tmpPointOnA, tmpPointOnB; - gNumDeepPenetrationChecks++; m_cachedSeparatingAxis.setZero(); bool isValid2 = m_penetrationDepthSolver->calcPenDepth( From 9e58a217324d555bb11eb5194825906f977c82e6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 31 Oct 2018 21:50:49 -0700 Subject: [PATCH 10/17] more aesthetic wireframe, pure visual improvement (no impact on simulation/performance) fixes https://github.com/bulletphysics/bullet3/issues/1970 Issue 1970 --- data/multibody.bullet | Bin 16836 -> 16836 bytes examples/Benchmarks/BenchmarkDemo.cpp | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/data/multibody.bullet b/data/multibody.bullet index 0283009946cd6302966b345ef4522b929709667e..32b9e7f37411e082f0138d19b5176450d62d6f50 100644 GIT binary patch delta 466 zcmX@o%y^`kaRVs+=(e(}t3=IE)07!uWh|L0KHP}Oi7#M*JK^O-}N3$Y0 zj6gO>j~7tUsd|L?l7hYaK)eI<-mLh;&YVftxfEovOdk@eGko&#^>0e0JfsB8^%)r6GAi@B1A4ng_oezQZV<6rP z#Fv?X1Oz0&XnUCRzycBsmjz<&6DIlZ3FKo`00$fs-=4kN3#;t;oUH6XY8oH>-N$Da zurF3IV)A?D)X7uW#U}4!Z7Zx-cVZw3H~A#~XQ delta 419 zcmX@o%y^`kaRV6m=!q9ZCJV5bOg_ORz_O%Z@4m@~Od^(oj0|9~Aj{y3 zNEib{MD(_Ln|cO@2|!UsFmwRRD}YG^F=66e$;o<59g`iHgn%ZROg3Q>n{3C*F{; zpP>OL2Q`bqh5@7*Ss{?UnStSe9Y_oWQYSlu6x+WDI=WYJ_jEg;;r6p;&IHOgOn%Rt z>N!DQ791J{o}WL1LInmIp$vg=Td;h>YaK8>VT%fwW-x~611bZX9|#O%{mExoI5><= X^$ZMj6e>60VXinitializePolyhedralFeatures(); + convexHullShape->initializePolyhedralFeatures(); btTransform trans; trans.setIdentity(); From 438e082b330f7fa84c6d3f9de7236ddaaa3a2234 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 1 Nov 2018 07:27:37 -0700 Subject: [PATCH 11/17] PyBullet: remove a potential race condition --- examples/SharedMemory/PhysicsServerExample.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 93292ee89..2dc7145be 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1737,6 +1737,11 @@ void PhysicsServerExample::initPhysics() m_args[w].m_cs2 = m_threadSupport->createCriticalSection(); m_args[w].m_cs3 = m_threadSupport->createCriticalSection(); m_args[w].m_csGUI = m_threadSupport->createCriticalSection(); + m_multiThreadedHelper->setCriticalSection(m_args[w].m_cs); + m_multiThreadedHelper->setCriticalSection2(m_args[w].m_cs2); + m_multiThreadedHelper->setCriticalSection3(m_args[w].m_cs3); + m_multiThreadedHelper->setCriticalSectionGUI(m_args[w].m_csGUI); + m_args[w].m_cs->lock(); m_args[w].m_cs->setSharedParam(0, eMotionIsUnInitialized); m_args[w].m_cs->unlock(); @@ -1760,10 +1765,6 @@ void PhysicsServerExample::initPhysics() } m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs); - m_multiThreadedHelper->setCriticalSection2(m_args[0].m_cs2); - m_multiThreadedHelper->setCriticalSection3(m_args[0].m_cs3); - m_multiThreadedHelper->setCriticalSectionGUI(m_args[0].m_csGUI); m_args[0].m_cs2->lock(); From 336a4b65feb8a8003dcabf602230a8f11848244b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 1 Nov 2018 07:41:35 -0700 Subject: [PATCH 12/17] disable some internal statistics, reporting a race condition. --- src/Bullet3Common/b3AlignedAllocator.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/Bullet3Common/b3AlignedAllocator.cpp b/src/Bullet3Common/b3AlignedAllocator.cpp index 6e54c5fb7..d546d5e06 100644 --- a/src/Bullet3Common/b3AlignedAllocator.cpp +++ b/src/Bullet3Common/b3AlignedAllocator.cpp @@ -15,9 +15,11 @@ subject to the following restrictions: #include "b3AlignedAllocator.h" +#ifdef B3_ALLOCATOR_STATISTICS int b3g_numAlignedAllocs = 0; int b3g_numAlignedFree = 0; int b3g_totalBytesAlignedAllocs = 0; //detect memory leaks +#endif static void *b3AllocDefault(size_t size) { @@ -109,10 +111,10 @@ void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filenam { void *ret; char *real; - +#ifdef B3_ALLOCATOR_STATISTICS b3g_totalBytesAlignedAllocs += size; b3g_numAlignedAllocs++; - +#endif real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1)); if (real) { @@ -135,14 +137,16 @@ void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filenam void b3AlignedFreeInternal(void *ptr, int line, char *filename) { void *real; +#ifdef B3_ALLOCATOR_STATISTICS b3g_numAlignedFree++; - +#endif if (ptr) { real = *((void **)(ptr)-1); int size = *((int *)(ptr)-2); +#ifdef B3_ALLOCATOR_STATISTICS b3g_totalBytesAlignedAllocs -= size; - +#endif b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size); b3s_freeFunc(real); @@ -157,7 +161,9 @@ void b3AlignedFreeInternal(void *ptr, int line, char *filename) void *b3AlignedAllocInternal(size_t size, int alignment) { +#ifdef B3_ALLOCATOR_STATISTICS b3g_numAlignedAllocs++; +#endif void *ptr; ptr = b3s_alignedAllocFunc(size, alignment); // b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr); @@ -170,8 +176,9 @@ void b3AlignedFreeInternal(void *ptr) { return; } - +#ifdef B3_ALLOCATOR_STATISTICS b3g_numAlignedFree++; +#endif // b3Printf("b3AlignedFreeInternal %x\n",ptr); b3s_alignedFreeFunc(ptr); } From 750133694c83ac33bd33b7baee5b2e6d3a7cc8fc Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 1 Nov 2018 08:19:50 -0700 Subject: [PATCH 13/17] Disable btQuickprof.h profiling by default. We use custom profiling functions, see b3ChromeUtilsStartTimings. --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 3 +- examples/SharedMemory/SharedMemoryPublic.h | 4 +-- examples/Utils/ChromeTraceUtil.cpp | 8 ++--- src/LinearMath/btQuickprof.cpp | 34 ++++++++++--------- src/LinearMath/btQuickprof.h | 9 ++--- 5 files changed, 30 insertions(+), 28 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 1ed8c7a37..d5c9ccac5 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -251,7 +251,6 @@ void MyKeyboardCallback(int key, int state) if (key == 'p') { -#ifndef BT_NO_PROFILE if (state) { b3ChromeUtilsStartTimings(); @@ -260,7 +259,6 @@ void MyKeyboardCallback(int key, int state) { b3ChromeUtilsStopTimingsAndWriteJsonFile("timings"); } -#endif //BT_NO_PROFILE } #ifndef NO_OPENGL3 @@ -1129,6 +1127,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) gui2->registerFileOpenCallback(fileOpenCallback); gui2->registerQuitCallback(quitCallback); + } return true; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 166de101c..eb74d3f3c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -928,7 +928,7 @@ enum eFileIOTypes }; //limits for vertices/indices in PyBullet::createCollisionShape -#define B3_MAX_NUM_VERTICES 1024 -#define B3_MAX_NUM_INDICES 1024 +#define B3_MAX_NUM_VERTICES 16 +#define B3_MAX_NUM_INDICES 16 #endif //SHARED_MEMORY_PUBLIC_H diff --git a/examples/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp index 2a25678ed..78d478162 100644 --- a/examples/Utils/ChromeTraceUtil.cpp +++ b/examples/Utils/ChromeTraceUtil.cpp @@ -174,7 +174,7 @@ void MyEnterProfileZoneFunc(const char* msg) { if (gProfileDisabled) return; -#ifndef BT_NO_PROFILE + int threadId = btQuickprofGetCurrentThreadIndex2(); if (threadId < 0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT) return; @@ -191,13 +191,13 @@ void MyEnterProfileZoneFunc(const char* msg) gStartTimes[threadId][gStackDepths[threadId]] = 1 + gStartTimes[threadId][gStackDepths[threadId] - 1]; } gStackDepths[threadId]++; -#endif + } void MyLeaveProfileZoneFunc() { if (gProfileDisabled) return; -#ifndef BT_NO_PROFILE + int threadId = btQuickprofGetCurrentThreadIndex2(); if (threadId < 0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT) return; @@ -214,7 +214,7 @@ void MyLeaveProfileZoneFunc() unsigned long long int endTime = clk.getTimeNanoseconds(); gTimings[threadId].addTiming(name, threadId, startTime, endTime); -#endif //BT_NO_PROFILE + } void b3ChromeUtilsStartTimings() diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index 8d3310981..86fd1d781 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -694,6 +694,24 @@ void CProfileManager::dumpAll() CProfileManager::Release_Iterator(profileIterator); } + +void btEnterProfileZoneDefault(const char* name) +{ +} +void btLeaveProfileZoneDefault() +{ +} + +#else +void btEnterProfileZoneDefault(const char* name) +{ +} +void btLeaveProfileZoneDefault() +{ +} +#endif //BT_NO_PROFILE + + // clang-format off #if defined(_WIN32) && (defined(__MINGW32__) || defined(__MINGW64__)) #define BT_HAVE_TLS 1 @@ -743,22 +761,6 @@ unsigned int btQuickprofGetCurrentThreadIndex2() #endif //BT_THREADSAFE } -void btEnterProfileZoneDefault(const char* name) -{ -} -void btLeaveProfileZoneDefault() -{ -} - -#else -void btEnterProfileZoneDefault(const char* name) -{ -} -void btLeaveProfileZoneDefault() -{ -} -#endif //BT_NO_PROFILE - static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault; static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault; diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index 18ba4d5fb..990d401d5 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -61,18 +61,19 @@ btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc(); void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc); void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc); -#ifndef BT_NO_PROFILE // FIX redefinition -//To disable built-in profiling, please comment out next line -//#define BT_NO_PROFILE 1 +#ifndef BT_ENABLE_PROFILE +#define BT_NO_PROFILE 1 #endif //BT_NO_PROFILE const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64; -#ifndef BT_NO_PROFILE //btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined, //otherwise returns thread index in range [0..maxThreads] unsigned int btQuickprofGetCurrentThreadIndex2(); +#ifndef BT_NO_PROFILE + + #include //@todo remove this, backwards compatibility #include "btAlignedAllocator.h" From f6ea2a73796438734be1a0cb5f8124da8e976be8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 1 Nov 2018 10:20:54 -0700 Subject: [PATCH 14/17] fix a potential data race condition. remove createObstacleCourse programmatic creation part until we can steam vertices/indices. --- .../SharedMemory/PhysicsServerExample.cpp | 3 ++- .../pybullet/examples/createObstacleCourse.py | 22 ------------------- 2 files changed, 2 insertions(+), 23 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 2dc7145be..36ec1f18b 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1764,8 +1764,9 @@ void PhysicsServerExample::initPhysics() } } + m_args[0].m_cs->lock(); m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); - + m_args[0].m_cs->unlock(); m_args[0].m_cs2->lock(); { diff --git a/examples/pybullet/examples/createObstacleCourse.py b/examples/pybullet/examples/createObstacleCourse.py index 1194a213a..ef145c72c 100644 --- a/examples/pybullet/examples/createObstacleCourse.py +++ b/examples/pybullet/examples/createObstacleCourse.py @@ -21,28 +21,6 @@ colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) #convex mesh from obj stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj") -#concave mesh from obj -stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH) - - -verts=[[-0.246350, -0.246483, -0.000624], - [ -0.151407, -0.176325, 0.172867], - [ -0.246350, 0.249205, -0.000624], - [ -0.151407, 0.129477, 0.172867], - [ 0.249338, -0.246483, -0.000624], - [ 0.154395, -0.176325, 0.172867], - [ 0.249338, 0.249205, -0.000624], - [ 0.154395, 0.129477, 0.172867]] -#convex mesh from vertices -stoneConvexId = p.createCollisionShape(p.GEOM_MESH,vertices=verts) - -indices=[0,3,2,3,6,2,7,4,6,5,0,4,6,0,2,3,5,7,0,1,3,3,7,6,7,5,4,5,1,0,6,4,0,3,1,5] - -#concave mesh from vertices+indices -stoneConcaveId = p.createCollisionShape(p.GEOM_MESH,vertices=verts, indices=indices) - -stoneId = stoneConvexId -#stoneId = stoneConcaveId boxHalfLength = 0.5 From 8b53e47fe8531710eb1ae0533035ef4e51f855d4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 2 Nov 2018 11:19:46 -0700 Subject: [PATCH 15/17] add simpler ARS implementation, thanks to Alexis Jacq and Hadelin de Ponteves (will add save/restore of policy and rendering movies through command-line arguments soon) --- .../pybullet/gym/pybullet_envs/ARS/ars.py | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_envs/ARS/ars.py diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py new file mode 100644 index 000000000..b4c0c3127 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/ARS/ars.py @@ -0,0 +1,142 @@ +# AI 2018 + +# Importing the libraries +import os +import numpy as np +import gym +from gym import wrappers +import pybullet_envs + +# Setting the Hyper Parameters + +class Hp(): + + def __init__(self): + self.nb_steps = 1000 + self.episode_length = 1000 + self.learning_rate = 0.02 + self.nb_directions = 16 + self.nb_best_directions = 16 + assert self.nb_best_directions <= self.nb_directions + self.noise = 0.03 + self.seed = 1 + self.env_name = 'HalfCheetahBulletEnv-v0' + +# Normalizing the states + +class Normalizer(): + + def __init__(self, nb_inputs): + self.n = np.zeros(nb_inputs) + self.mean = np.zeros(nb_inputs) + self.mean_diff = np.zeros(nb_inputs) + self.var = np.zeros(nb_inputs) + + def observe(self, x): + self.n += 1. + last_mean = self.mean.copy() + self.mean += (x - self.mean) / self.n + self.mean_diff += (x - last_mean) * (x - self.mean) + self.var = (self.mean_diff / self.n).clip(min = 1e-2) + + def normalize(self, inputs): + obs_mean = self.mean + obs_std = np.sqrt(self.var) + return (inputs - obs_mean) / obs_std + +# Building the AI + +class Policy(): + + def __init__(self, input_size, output_size): + self.theta = np.zeros((output_size, input_size)) + print("self.theta=",self.theta) + def evaluate(self, input, delta = None, direction = None): + if direction is None: + return np.clip(self.theta.dot(input), -1.0, 1.0) + elif direction == "positive": + return np.clip((self.theta + hp.noise*delta).dot(input), -1.0, 1.0) + else: + return np.clip((self.theta - hp.noise*delta).dot(input), -1.0, 1.0) + + def sample_deltas(self): + return [np.random.randn(*self.theta.shape) for _ in range(hp.nb_directions)] + + def update(self, rollouts, sigma_r): + step = np.zeros(self.theta.shape) + for r_pos, r_neg, d in rollouts: + step += (r_pos - r_neg) * d + self.theta += hp.learning_rate / (hp.nb_best_directions * sigma_r) * step + +# Exploring the policy on one specific direction and over one episode + +def explore(env, normalizer, policy, direction = None, delta = None): + state = env.reset() + done = False + num_plays = 0. + sum_rewards = 0 + while not done and num_plays < hp.episode_length: + normalizer.observe(state) + state = normalizer.normalize(state) + action = policy.evaluate(state, delta, direction) + state, reward, done, _ = env.step(action) + reward = max(min(reward, 1), -1) + sum_rewards += reward + num_plays += 1 + return sum_rewards + +# Training the AI + +def train(env, policy, normalizer, hp): + + for step in range(hp.nb_steps): + + # Initializing the perturbations deltas and the positive/negative rewards + deltas = policy.sample_deltas() + positive_rewards = [0] * hp.nb_directions + negative_rewards = [0] * hp.nb_directions + + # Getting the positive rewards in the positive directions + for k in range(hp.nb_directions): + positive_rewards[k] = explore(env, normalizer, policy, direction = "positive", delta = deltas[k]) + + # Getting the negative rewards in the negative/opposite directions + for k in range(hp.nb_directions): + negative_rewards[k] = explore(env, normalizer, policy, direction = "negative", delta = deltas[k]) + + # Gathering all the positive/negative rewards to compute the standard deviation of these rewards + all_rewards = np.array(positive_rewards + negative_rewards) + sigma_r = all_rewards.std() + + # Sorting the rollouts by the max(r_pos, r_neg) and selecting the best directions + scores = {k:max(r_pos, r_neg) for k,(r_pos,r_neg) in enumerate(zip(positive_rewards, negative_rewards))} + order = sorted(scores.keys(), key = lambda x:scores[x])[:hp.nb_best_directions] + rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k]) for k in order] + + # Updating our policy + policy.update(rollouts, sigma_r) + + # Printing the final reward of the policy after the update + reward_evaluation = explore(env, normalizer, policy) + print('Step:', step, 'Reward:', reward_evaluation) + +# Running the main code + +def mkdir(base, name): + path = os.path.join(base, name) + if not os.path.exists(path): + os.makedirs(path) + return path +work_dir = mkdir('exp', 'brs') +monitor_dir = mkdir(work_dir, 'monitor') + +hp = Hp() +np.random.seed(hp.seed) +env = gym.make(hp.env_name) +# env.render(mode = "human") +#env = wrappers.Monitor(env, monitor_dir, force = True) +nb_inputs = env.observation_space.shape[0] +nb_outputs = env.action_space.shape[0] +policy = Policy(nb_inputs, nb_outputs) +normalizer = Normalizer(nb_inputs) +train(env, policy, normalizer, hp) From ac18c95ea1df312c9af7538bf290b712c5e17ebf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 5 Nov 2018 10:04:19 -0800 Subject: [PATCH 16/17] fix some race conditions --- examples/SharedMemory/PhysicsServerExample.cpp | 15 +++++++-------- .../TaskScheduler/btThreadSupportPosix.cpp | 10 ---------- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 36ec1f18b..e864508b4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -27,7 +27,6 @@ bool gEnableRendering = true; bool gActivedVRRealTimeSimulation = false; bool gEnableSyncPhysicsRendering = true; -bool gEnableUpdateDebugDrawLines = true; static int gCamVisualizerWidth = 228; static int gCamVisualizerHeight = 192; @@ -177,6 +176,7 @@ struct MotionArgs { MotionArgs() : m_debugDrawFlags(0), + m_enableUpdateDebugDrawLines(true), m_physicsServerPtr(0) { for (int i = 0; i < MAX_VR_CONTROLLERS; i++) @@ -201,7 +201,7 @@ struct MotionArgs b3CriticalSection* m_csGUI; int m_debugDrawFlags; - + bool m_enableUpdateDebugDrawLines; btAlignedObjectArray m_mouseCommands; b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS]; @@ -424,13 +424,13 @@ void MotionThreadFunc(void* userPtr, void* lsMemory) args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents, numSendVrControllers, keyEvents, args->m_sendKeyEvents.size(), mouseEvents, args->m_sendMouseEvents.size()); } { - if (gEnableUpdateDebugDrawLines) + args->m_csGUI->lock(); + if (args->m_enableUpdateDebugDrawLines) { - args->m_csGUI->lock(); args->m_physicsServerPtr->physicsDebugDraw(args->m_debugDrawFlags); - gEnableUpdateDebugDrawLines = false; - args->m_csGUI->unlock(); + args->m_enableUpdateDebugDrawLines = false; } + args->m_csGUI->unlock(); } deltaTimeInSeconds = 0; } @@ -1763,7 +1763,6 @@ void PhysicsServerExample::initPhysics() #endif } } - m_args[0].m_cs->lock(); m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle); m_args[0].m_cs->unlock(); @@ -2845,7 +2844,7 @@ void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) //draw stuff and flush? this->m_multiThreadedHelper->m_debugDraw->drawDebugDrawerLines(); m_args[0].m_debugDrawFlags = debugDrawFlags; - gEnableUpdateDebugDrawLines = true; + m_args[0].m_enableUpdateDebugDrawLines = true; m_args[0].m_csGUI->unlock(); } } diff --git a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp index 25bfda0bf..02f4ed163 100644 --- a/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp +++ b/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp @@ -198,12 +198,10 @@ static void* threadFunction(void* argument) status->m_status = 3; status->m_cs->unlock(); checkPThreadFunction(sem_post(status->m_mainSemaphore)); - printf("Thread with taskId %i exiting\n", status->m_taskId); break; } } - printf("Thread TERMINATED\n"); return 0; } @@ -272,7 +270,6 @@ void btThreadSupportPosix::waitForAllTasks() void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo) { m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already - printf("%s creating %i threads.\n", __FUNCTION__, m_numThreads); m_activeThreadStatus.resize(m_numThreads); m_startedThreadsMask = 0; @@ -281,7 +278,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi for (int i = 0; i < m_numThreads; i++) { - printf("starting thread %d\n", i); btThreadStatus& threadStatus = m_activeThreadStatus[i]; threadStatus.startSemaphore = createSem("threadLocal"); threadStatus.m_userPtr = 0; @@ -294,7 +290,6 @@ void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructi threadStatus.threadUsed = 0; checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus)); - printf("started thread %d \n", i); } } @@ -304,20 +299,15 @@ void btThreadSupportPosix::stopThreads() for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t) { btThreadStatus& threadStatus = m_activeThreadStatus[t]; - printf("%s: Thread %i used: %ld\n", __FUNCTION__, int(t), threadStatus.threadUsed); threadStatus.m_userPtr = 0; checkPThreadFunction(sem_post(threadStatus.startSemaphore)); checkPThreadFunction(sem_wait(m_mainSemaphore)); - printf("destroy semaphore\n"); destroySem(threadStatus.startSemaphore); - printf("semaphore destroyed\n"); checkPThreadFunction(pthread_join(threadStatus.thread, 0)); } - printf("destroy main semaphore\n"); destroySem(m_mainSemaphore); - printf("main semaphore destroyed\n"); m_activeThreadStatus.clear(); } From 882252f8c0ddbdb8beb55c25971497c7942b33b3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 5 Nov 2018 10:50:03 -0800 Subject: [PATCH 17/17] move global from btMultiBody into dynamicsWorld.getSolverInfo --- .../invdyn_bullet_comparison.cpp | 2 +- .../MultiBody/InvertedPendulumPDControl.cpp | 9 +++--- .../MultiBody/MultiBodyConstraintFeedback.cpp | 9 +++--- examples/MultiBody/TestJointTorqueSetup.cpp | 9 +++--- .../PhysicsServerCommandProcessor.cpp | 10 +++--- .../ConstraintSolver/btContactSolverInfo.h | 4 +++ .../Featherstone/btMultiBody.cpp | 17 +++++----- src/BulletDynamics/Featherstone/btMultiBody.h | 21 +++++++------ .../Featherstone/btMultiBodyDynamicsWorld.cpp | 31 ++++++++++++++----- 9 files changed, 64 insertions(+), 48 deletions(-) diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp index ffa2a8e07..02adce603 100644 --- a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp @@ -176,7 +176,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g btAlignedObjectArray world_to_local; btAlignedObjectArray local_origin; btmb->forwardKinematics(world_to_local, local_origin); - btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); + btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); // read generalized accelerations back from btMultiBody // the mapping from scratch variables to accelerations is taken from the implementation diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index ff0c8d2b3..d842bb43b 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -48,9 +48,6 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase) { @@ -315,8 +312,10 @@ void InvertedPendulumPDControl::initPhysics() } int upAxis = 1; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; + + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + m_guiHelper->setUpAxis(upAxis); diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index 4e2bf60fd..b8b421fc1 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -44,14 +44,9 @@ MultiBodyConstraintFeedbackSetup::~MultiBodyConstraintFeedbackSetup() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; void MultiBodyConstraintFeedbackSetup::initPhysics() { int upAxis = 2; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = @@ -69,6 +64,10 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); + + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + //create a static ground object if (1) { diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 27122ba3c..7963713d2 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -44,15 +44,10 @@ TestJointTorqueSetup::~TestJointTorqueSetup() { } -///this is a temporary global, until we determine if we need the option or not -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; void TestJointTorqueSetup::initPhysics() { int upAxis = 1; - gJointFeedbackInWorldSpace = true; - gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); @@ -71,6 +66,10 @@ void TestJointTorqueSetup::initPhysics() //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits); + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true; + m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true; + + //create a static ground object if (1) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 537f3e555..2a48843ed 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -96,8 +96,6 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif -extern bool gJointFeedbackInWorldSpace; -extern bool gJointFeedbackInJointFrame; int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -7697,11 +7695,11 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0; - if (gJointFeedbackInWorldSpace) + if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace) { serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE; } - if (gJointFeedbackInJointFrame) + if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame) { serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME; } @@ -7747,8 +7745,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) { - gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; - gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0; + m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0; + m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0; } if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME) diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 439cc4428..89f8db8b1 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -62,6 +62,8 @@ struct btContactSolverInfoData btScalar m_singleAxisRollingFrictionThreshold; btScalar m_leastSquaresResidualThreshold; btScalar m_restitutionVelocityThreshold; + bool m_jointFeedbackInWorldSpace; + bool m_jointFeedbackInJointFrame; }; struct btContactSolverInfo : public btContactSolverInfoData @@ -94,6 +96,8 @@ struct btContactSolverInfo : public btContactSolverInfoData m_singleAxisRollingFrictionThreshold = 1e30f; ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. m_leastSquaresResidualThreshold = 0.f; m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution + m_jointFeedbackInWorldSpace = false; + m_jointFeedbackInJointFrame = false; } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a890da46f..172472988 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -30,9 +30,6 @@ //#include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM -///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals -bool gJointFeedbackInWorldSpace = false; -bool gJointFeedbackInJointFrame = false; namespace { @@ -718,10 +715,12 @@ inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //ren // void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m, - bool isConstraintPass) + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame) { // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) // and the base linear & angular accelerations. @@ -1124,7 +1123,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec; btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec; - if (gJointFeedbackInJointFrame) + if (jointFeedbackInJointFrame) { //shift the reaction forces to the joint frame //linear (force) component is the same @@ -1132,7 +1131,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); } - if (gJointFeedbackInWorldSpace) + if (jointFeedbackInWorldSpace) { if (isConstraintPass) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 145ed4677..8a693f539 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -338,17 +338,20 @@ public: btAlignedObjectArray & scratch_r, btAlignedObjectArray & scratch_v, btAlignedObjectArray & scratch_m, - bool isConstraintPass = false); + bool isConstraintPass, + bool jointFeedbackInWorldSpace, + bool jointFeedbackInJointFrame + ); ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray & scratch_r, - btAlignedObjectArray & scratch_v, - btAlignedObjectArray & scratch_m, - bool isConstraintPass = false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); - } + //void stepVelocitiesMultiDof(btScalar dt, + // btAlignedObjectArray & scratch_r, + // btAlignedObjectArray & scratch_v, + // btAlignedObjectArray & scratch_m, + // bool isConstraintPass = false) + //{ + // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false); + //} // calcAccelerationDeltasMultiDof // input: force vector (in same format as jacobian, i.e.: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a452d61b7..1557987bc 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -491,11 +491,14 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_scratch_v.resize(bod->getNumLinks() + 1); m_scratch_m.resize(bod->getNumLinks() + 1); bool doNotUpdatePos = false; - + bool isConstraintPass = false; { if (!bod->isUsingRK4Integration()) { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } else { @@ -593,7 +596,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btScalar h = solverInfo.m_timeStep; #define output &m_scratch_r[bod->getNumDofs()] //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); @@ -603,7 +608,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd1 from: q1 & qd1 pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); @@ -613,7 +620,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd2 from: q2 & qd2 pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); @@ -623,7 +632,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); pCopy(output, scratch_qdd3, 0, numDofs); // @@ -660,7 +671,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { for (int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } } } @@ -708,7 +721,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if (!bod->isUsingRK4Integration()) { bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); } } }