From 51dc3a1710b443175ebe6b20a17edcfd37798b2a Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 8 Mar 2017 19:36:41 -0800 Subject: [PATCH 1/5] VR controller the kuka arm --- examples/pybullet/vr_kuka_control.py | 90 ++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 examples/pybullet/vr_kuka_control.py diff --git a/examples/pybullet/vr_kuka_control.py b/examples/pybullet/vr_kuka_control.py new file mode 100644 index 000000000..d35e8ef45 --- /dev/null +++ b/examples/pybullet/vr_kuka_control.py @@ -0,0 +1,90 @@ +## Assume you have run vr_kuka_setup and have default scene set up +# Require p.setInternalSimFlags(0) in kuka_setup +import pybullet as p +import math +p.connect(p.SHARED_MEMORY) + +kuka = 3 +kuka_gripper = 7 +POSITION = 1 +BUTTONS = 6 + +THRESHOLD = 1.3 +LOWER_LIMITS = [-.967, -2.0, -2.96, 0.19, -2.96, -2.09, -3.05] +UPPER_LIMITS = [.96, 2.0, 2.96, 2.29, 2.96, 2.09, 3.05] +JOINT_RANGE = [5.8, 4, 5.8, 4, 5.8, 4, 6] +REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0] +JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1] +REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001] +MAX_FORCE = 500 + +def euc_dist(posA, posB): + dist = 0. + for i in range(len(posA)): + dist += (posA[i] - posB[i]) ** 2 + return dist + +p.setRealTimeSimulation(1) + +controllers = [e[0] for e in p.getVREvents()] + +while True: + + events = p.getVREvents() + for e in (events): + + # Only use one controller + if e[0] == min(controllers): + break + + sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION]) + + # A simplistic version of gripper control + if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED: + # avg = 0. + for i in range(p.getNumJoints(kuka_gripper)): + p.setJointMotorControl2(kuka_gripper, i, p.VELOCITY_CONTROL, targetVelocity=5, force=50) + # posTarget = 0.1 + (1 - min(0.75, e[3])) * 1.5 * math.pi * 0.29; + # maxPosTarget = 0.55 + # correction = 0. + # jointPosition = p.getJointState(kuka_gripper, i)[0] + # if avg: + # correction = jointPosition - avg + # if jointPosition < 0: + # p.resetJointState(kuka_gripper, i, 0) + # if jointPosition > maxPosTarget: + # p.resetJointState(kuka_gripper, i, maxPosTarget) + # if avg: + + # p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, + # targetPosition=avg, targetVelocity=0., + # positionGain=1, velocityGain=0.5, force=50) + # else: + # p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, + # targetPosition=posTarget, targetVelocity=0., + # positionGain=1, velocityGain=0.5, force=50) + # avg = p.getJointState(kuka_gripper, i)[0] + + + if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED: + for i in range(p.getNumJoints(kuka_gripper)): + p.setJointMotorControl2(kuka_gripper, i, p.VELOCITY_CONTROL, targetVelocity=-5, force=50) + + if sq_len < THRESHOLD * THRESHOLD: + + joint_pos = p.calculateInverseKinematics(kuka, 6, e[POSITION], (0, 1, 0, 0), + lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, + jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP) + for i in range(len(joint_pos)): + p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, + targetPosition=joint_pos[i], targetVelocity=0, + positionGain=0.6, velocityGain=1.0, force=MAX_FORCE) + + else: + # Set back to original rest pose + for jointIndex in range(p.getNumJoints(kuka)): + p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, + REST_JOINT_POS[jointIndex], 0) + + + From 923fbe858851be28e5c523283b5ba2bdeb0333d8 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Fri, 10 Mar 2017 11:22:38 -0800 Subject: [PATCH 2/5] add gym examples --- .../gym/cartpole_bullet_gym_example.py | 31 ++++ examples/pybullet/gym/envs/__init__.py | 17 +++ examples/pybullet/gym/envs/bullet/__init__.py | 2 + .../gym/envs/bullet/cartpole_bullet.py | 94 ++++++++++++ examples/pybullet/gym/envs/bullet/minitaur.py | 142 ++++++++++++++++++ .../gym/envs/bullet/minitaur_bullet.py | 90 +++++++++++ .../gym/minitaurWalk_bullet_gym_example.py | 27 ++++ .../gym/minitaur_bullet_gym_example.py | 27 ++++ 8 files changed, 430 insertions(+) create mode 100644 examples/pybullet/gym/cartpole_bullet_gym_example.py create mode 100644 examples/pybullet/gym/envs/__init__.py create mode 100644 examples/pybullet/gym/envs/bullet/__init__.py create mode 100644 examples/pybullet/gym/envs/bullet/cartpole_bullet.py create mode 100644 examples/pybullet/gym/envs/bullet/minitaur.py create mode 100644 examples/pybullet/gym/envs/bullet/minitaur_bullet.py create mode 100644 examples/pybullet/gym/minitaurWalk_bullet_gym_example.py create mode 100644 examples/pybullet/gym/minitaur_bullet_gym_example.py diff --git a/examples/pybullet/gym/cartpole_bullet_gym_example.py b/examples/pybullet/gym/cartpole_bullet_gym_example.py new file mode 100644 index 000000000..52cdab258 --- /dev/null +++ b/examples/pybullet/gym/cartpole_bullet_gym_example.py @@ -0,0 +1,31 @@ +"""One-line documentation for gym_example module. + +A detailed description of gym_example. +""" + +import gym +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +import setuptools +import time +import numpy as np + + +w = [0.3, 0.02, 0.02, 0.012] + +def main(): + env = gym.make('CartPoleBulletEnv-v0') + for i_episode in range(1): + observation = env.reset() + done = False + t = 0 + while not done: + print(observation) + action = np.array([np.inner(observation, w)]) + print(action) + observation, reward, done, info = env.step(action) + t = t + 1 + if done: + print("Episode finished after {} timesteps".format(t+1)) + break + +main() diff --git a/examples/pybullet/gym/envs/__init__.py b/examples/pybullet/gym/envs/__init__.py new file mode 100644 index 000000000..605d5e531 --- /dev/null +++ b/examples/pybullet/gym/envs/__init__.py @@ -0,0 +1,17 @@ +from gym.envs.registration import registry, register, make, spec + +# ------------bullet------------- + +register( + id='CartPoleBulletEnv-v0', + entry_point='envs.bullet:CartPoleBulletEnv', + timestep_limit=1000, + reward_threshold=950.0, +) + +register( + id='MinitaurBulletEnv-v0', + entry_point='envs.bullet:MinitaurBulletEnv', + timestep_limit=1000, + reward_threshold=5.0, +) diff --git a/examples/pybullet/gym/envs/bullet/__init__.py b/examples/pybullet/gym/envs/bullet/__init__.py new file mode 100644 index 000000000..25da62f87 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/__init__.py @@ -0,0 +1,2 @@ +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +from envs.bullet.minitaur_bullet import MinitaurBulletEnv diff --git a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py new file mode 100644 index 000000000..bf0d85ab9 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py @@ -0,0 +1,94 @@ +""" +Classic cart-pole system implemented by Rich Sutton et al. +Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c +""" +import os +import logging +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time +import subprocess +import pybullet as p + + +logger = logging.getLogger(__name__) + +class CartPoleBulletEnv(gym.Env): + metadata = { + 'render.modes': ['human', 'rgb_array'], + 'video.frames_per_second' : 50 + } + + def __init__(self): + # start the bullet physics server +# cmdStartBulletServer=['/Users/jietan/Projects/bullet3/build_cmake_python3/examples/SharedMemory/App_SharedMemoryPhysics_GUI'] +# subprocess.Popen(cmdStartBulletServer) + # wait to make sure that the physics server is ready +# time.sleep(1) + # connect to the physics server +# p.connect(p.SHARED_MEMORY) + p.connect(p.GUI) + observation_high = np.array([ + np.finfo(np.float32).max, + np.finfo(np.float32).max, + np.finfo(np.float32).max, + np.finfo(np.float32).max]) + action_high = np.array([0.1]) + + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(-observation_high, observation_high) + + self.theta_threshold_radians = 1 + self.x_threshold = 2.4 + self._seed() +# self.reset() + self.viewer = None + self._configure() + + def _configure(self, display=None): + self.display = display + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + p.stepSimulation() +# time.sleep(self.timeStep) + self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] + theta, theta_dot, x, x_dot = self.state + force = action + p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3])) + + done = x < -self.x_threshold \ + or x > self.x_threshold \ + or theta < -self.theta_threshold_radians \ + or theta > self.theta_threshold_radians + reward = 1.0 + + return np.array(self.state), reward, done, {} + + def _reset(self): +# print("-----------reset simulation---------------") + p.resetSimulation() + self.cartpole = p.loadURDF("cartpole.urdf",[0,0,0]) + self.timeStep = 0.01 + p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) + p.setGravity(0,0, -10) + p.setTimeStep(self.timeStep) + p.setRealTimeSimulation(0) + + initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) + initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) + p.resetJointState(self.cartpole, 1, initialAngle) + p.resetJointState(self.cartpole, 0, initialCartPos) + + self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] + + return np.array(self.state) + + def _render(self, mode='human', close=False): + return diff --git a/examples/pybullet/gym/envs/bullet/minitaur.py b/examples/pybullet/gym/envs/bullet/minitaur.py new file mode 100644 index 000000000..74ddedf84 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/minitaur.py @@ -0,0 +1,142 @@ +import pybullet as p +import numpy as np + +class Minitaur: + def __init__(self, urdfRootPath=''): + self.urdfRootPath = urdfRootPath + self.reset() + + def applyAction(self, motorCommands): + motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) + for i in range(self.nMotors): + self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i]) + + def getObservation(self): + observation = [] + observation.extend(self.getMotorAngles().tolist()) + observation.extend(self.getMotorVelocities().tolist()) + observation.extend(self.getMotorTorques().tolist()) + observation.extend(list(self.getBaseOrientation())) + observation.extend(list(self.getBasePosition())) + return observation + + def getActionDimension(self): + return self.nMotors + + def getObservationDimension(self): + return len(self.getObservation()) + + def buildJointNameToIdDict(self): + nJoints = p.getNumJoints(self.quadruped) + self.jointNameToId = {} + for i in range(nJoints): + jointInfo = p.getJointInfo(self.quadruped, i) + self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + self.resetPose() + for i in range(100): + p.stepSimulation() + + def buildMotorIdList(self): + self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint']) + + + def reset(self): + self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3) + self.kp = 1 + self.kd = 0.1 + self.maxForce = 3.5 + self.nMotors = 8 + self.motorIdList = [] + self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] + self.buildJointNameToIdDict() + self.buildMotorIdList() + + + def disableAllMotors(self): + nJoints = p.getNumJoints(self.quadruped) + for i in range(nJoints): + p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0) + + def setMotorAngleById(self, motorId, desiredAngle): + p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) + + def setMotorAngleByName(self, motorName, desiredAngle): + self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) + + + def resetPose(self): + #right front leg + self.disableAllMotors() + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_front_rightR_joint', 1.57) + self.setMotorAngleByName('motor_front_rightL_joint',-1.57) + + #left front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_front_leftR_joint', 1.57) + self.setMotorAngleByName('motor_front_leftL_joint',-1.57) + + #right back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_back_rightR_joint', 1.57) + self.setMotorAngleByName('motor_back_rightL_joint',-1.57) + + #left back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_back_leftR_joint', 1.57) + self.setMotorAngleByName('motor_back_leftL_joint',-1.57) + + def getBasePosition(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return position + + def getBaseOrientation(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return orientation + + def getMotorAngles(self): + motorAngles = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorAngles.append(jointState[0]) + motorAngles = np.multiply(motorAngles, self.motorDir) + return motorAngles + + def getMotorVelocities(self): + motorVelocities = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorVelocities.append(jointState[1]) + motorVelocities = np.multiply(motorVelocities, self.motorDir) + return motorVelocities + + def getMotorTorques(self): + motorTorques = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorTorques.append(jointState[3]) + motorTorques = np.multiply(motorTorques, self.motorDir) + return motorTorques diff --git a/examples/pybullet/gym/envs/bullet/minitaur_bullet.py b/examples/pybullet/gym/envs/bullet/minitaur_bullet.py new file mode 100644 index 000000000..cd4d5f658 --- /dev/null +++ b/examples/pybullet/gym/envs/bullet/minitaur_bullet.py @@ -0,0 +1,90 @@ +import math +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import time + +import pybullet as p +from envs.bullet.minitaur import Minitaur + +class MinitaurBulletEnv(gym.Env): + metadata = { + 'render.modes': ['human', 'rgb_array'], + 'video.frames_per_second' : 50 + } + + def __init__(self): + self._timeStep = 0.01 + self._observation = [] + self._envStepCounter = 0 + self._lastBasePosition = [0, 0, 0] + + p.connect(p.GUI) + + p.resetSimulation() + p.setTimeStep(self._timeStep) + p.loadURDF("plane.urdf") + p.setGravity(0,0,-10) + self._minitaur = Minitaur() + + observationDim = self._minitaur.getObservationDimension() + observation_high = np.array([np.finfo(np.float32).max] * observationDim) + actionDim = 8 + action_high = np.array([math.pi / 2.0] * actionDim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(-observation_high, observation_high) + + self._seed() + self.reset() + self.viewer = None + + def __del__(self): + p.disconnect() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _step(self, action): + if len(action) != self._minitaur.getActionDimension(): + raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(actions.continuous_actions))) + + for i in range(len(action)): + if not -math.pi/2 <= action[i] <= math.pi/2: + raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i])) + action[i] += math.pi / 2 + + self._minitaur.applyAction(action) + p.stepSimulation() + self._observation = self._minitaur.getObservation() + self._envStepCounter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._observation), reward, done, {} + + def _reset(self): + p.resetSimulation() + p.setTimeStep(self._timeStep) + p.loadURDF("plane.urdf") + p.setGravity(0,0,-10) + self._minitaur = Minitaur() + self._observation = self._minitaur.getObservation() + + def _render(self, mode='human', close=False): + return + + def is_fallen(self): + orientation = self._minitaur.getBaseOrientation() + rotMat = p.getMatrixFromQuaternion(orientation) + localUp = rotMat[6:] + return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 + + def _termination(self): + return self.is_fallen() + + def _reward(self): + currentBasePosition = self._minitaur.getBasePosition() + reward = np.dot(np.asarray([-1, 0, 0]), np.asarray(currentBasePosition) - np.asarray(self._lastBasePosition)) + self._lastBasePosition = currentBasePosition + return reward diff --git a/examples/pybullet/gym/minitaurWalk_bullet_gym_example.py b/examples/pybullet/gym/minitaurWalk_bullet_gym_example.py new file mode 100644 index 000000000..4da575341 --- /dev/null +++ b/examples/pybullet/gym/minitaurWalk_bullet_gym_example.py @@ -0,0 +1,27 @@ +"""One-line documentation for gym_example module. + +A detailed description of gym_example. +""" + +import gym +from envs.bullet.minitaur import MinitaurWalkEnv +import setuptools +import time +import numpy as np + + +def main(): + env = gym.make('MinitaurWalkEnv-v0') + for i_episode in range(1): + observation = env.reset() + done = False + while not done: + print(observation) + action = np.array([1.3, 0, 0, 0, 1.3, 0, 0, 0, 1.3, 3.14, 0, 0, 1.3, 3.14, 0, 0]) + print(action) + observation, reward, done, info = env.step(action) + if done: + print("Episode finished after {} timesteps".format(t+1)) + break + +main() diff --git a/examples/pybullet/gym/minitaur_bullet_gym_example.py b/examples/pybullet/gym/minitaur_bullet_gym_example.py new file mode 100644 index 000000000..18c70d43e --- /dev/null +++ b/examples/pybullet/gym/minitaur_bullet_gym_example.py @@ -0,0 +1,27 @@ +import gym +import numpy as np +import math + +from envs.bullet.minitaur_bullet import MinitaurBulletEnv + +def main(): + environment = gym.make('MinitaurBulletEnv-v0') + sum_reward = 0 + steps = 1000 + amplitude = 0.5 + speed = 0.3 + + for stepCounter in range(steps): + a1 = math.sin(stepCounter*speed)*amplitude + a2 = math.sin(stepCounter*speed+3.14)*amplitude + action = [a1, 0, a2, 0, 0, a1, 0, a2] + state, reward, done, info = environment.step(action) + sum_reward += reward + print(state) + if done: + environment.reset() + average_reward = sum_reward / steps + print("avg reward: ", average_reward) + + +main() From 37a809f5d14f56314ea86b394b9d301b7c880d29 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Fri, 10 Mar 2017 12:29:47 -0800 Subject: [PATCH 3/5] added the learning algorithm from RL-lab --- .../gym/envs/bullet/cartpole_bullet.py | 7 +-- .../gym/minitaur_bullet_gym_example.py | 27 ---------- .../pybullet/gym/trpo_cartpole_bullet_gym.py | 51 +++++++++++++++++++ .../gym/trpo_tf_cartpole_bullet_gym.py | 48 +++++++++++++++++ 4 files changed, 100 insertions(+), 33 deletions(-) delete mode 100644 examples/pybullet/gym/minitaur_bullet_gym_example.py create mode 100644 examples/pybullet/gym/trpo_cartpole_bullet_gym.py create mode 100644 examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py diff --git a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py index bf0d85ab9..b1f1a1e35 100644 --- a/examples/pybullet/gym/envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/envs/bullet/cartpole_bullet.py @@ -24,13 +24,8 @@ class CartPoleBulletEnv(gym.Env): def __init__(self): # start the bullet physics server -# cmdStartBulletServer=['/Users/jietan/Projects/bullet3/build_cmake_python3/examples/SharedMemory/App_SharedMemoryPhysics_GUI'] -# subprocess.Popen(cmdStartBulletServer) - # wait to make sure that the physics server is ready -# time.sleep(1) - # connect to the physics server -# p.connect(p.SHARED_MEMORY) p.connect(p.GUI) +# p.connect(p.DIRECT) observation_high = np.array([ np.finfo(np.float32).max, np.finfo(np.float32).max, diff --git a/examples/pybullet/gym/minitaur_bullet_gym_example.py b/examples/pybullet/gym/minitaur_bullet_gym_example.py deleted file mode 100644 index 18c70d43e..000000000 --- a/examples/pybullet/gym/minitaur_bullet_gym_example.py +++ /dev/null @@ -1,27 +0,0 @@ -import gym -import numpy as np -import math - -from envs.bullet.minitaur_bullet import MinitaurBulletEnv - -def main(): - environment = gym.make('MinitaurBulletEnv-v0') - sum_reward = 0 - steps = 1000 - amplitude = 0.5 - speed = 0.3 - - for stepCounter in range(steps): - a1 = math.sin(stepCounter*speed)*amplitude - a2 = math.sin(stepCounter*speed+3.14)*amplitude - action = [a1, 0, a2, 0, 0, a1, 0, a2] - state, reward, done, info = environment.step(action) - sum_reward += reward - print(state) - if done: - environment.reset() - average_reward = sum_reward / steps - print("avg reward: ", average_reward) - - -main() diff --git a/examples/pybullet/gym/trpo_cartpole_bullet_gym.py b/examples/pybullet/gym/trpo_cartpole_bullet_gym.py new file mode 100644 index 000000000..4c257cf6b --- /dev/null +++ b/examples/pybullet/gym/trpo_cartpole_bullet_gym.py @@ -0,0 +1,51 @@ +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +from rllab.algos.trpo import TRPO +from rllab.baselines.linear_feature_baseline import LinearFeatureBaseline +from rllab.envs.gym_env import GymEnv +from rllab.envs.normalized_env import normalize +from rllab.misc.instrument import stub, run_experiment_lite +from rllab.policies.gaussian_mlp_policy import GaussianMLPPolicy +import subprocess +import time + +stub(globals()) + +env = normalize(GymEnv("CartPoleBulletEnv-v0")) + +policy = GaussianMLPPolicy( + env_spec=env.spec, + # The neural network policy should have two hidden layers, each with 32 hidden units. + hidden_sizes=(8,) +) + +baseline = LinearFeatureBaseline(env_spec=env.spec) + +algo = TRPO( + env=env, + policy=policy, + baseline=baseline, + batch_size=5000, + max_path_length=env.horizon, + n_itr=50, + discount=0.999, + step_size=0.01, + # Uncomment both lines (this and the plot parameter below) to enable plotting +# plot=True, +) + +#cmdStartBulletServer=['~/Projects/rllab/bullet_examples/run_physics_server.sh'] +#subprocess.Popen(cmdStartBulletServer, shell=True) +#time.sleep(1) + + +run_experiment_lite( + algo.train(), + # Number of parallel workers for sampling + n_parallel=1, + # Only keep the snapshot parameters for the last iteration + snapshot_mode="last", + # Specifies the seed for the experiment. If this is not provided, a random seed + # will be used + seed=1, + # plot=True, +) diff --git a/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py b/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py new file mode 100644 index 000000000..0e6c1353b --- /dev/null +++ b/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py @@ -0,0 +1,48 @@ +from envs.bullet.cartpole_bullet import CartPoleBulletEnv +from sandbox.rocky.tf.algos.trpo import TRPO +from sandbox.rocky.tf.policies.gaussian_mlp_policy import GaussianMLPPolicy +from sandbox.rocky.tf.envs.base import TfEnv + +from rllab.baselines.linear_feature_baseline import LinearFeatureBaseline +from rllab.envs.gym_env import GymEnv +from rllab.envs.normalized_env import normalize +from rllab.misc.instrument import stub, run_experiment_lite + +stub(globals()) + +env = TfEnv(normalize(GymEnv("CartPoleBulletEnv-v0"))) + +policy = GaussianMLPPolicy( + name = "tf_gaussian_mlp", + env_spec=env.spec, + # The neural network policy should have two hidden layers, each with 32 hidden units. + hidden_sizes=(8,) +) + +baseline = LinearFeatureBaseline(env_spec=env.spec) + +algo = TRPO( + env=env, + policy=policy, + baseline=baseline, + batch_size=5000, + max_path_length=env.horizon, + n_itr=50, + discount=0.999, + step_size=0.01, + force_batch_sampler=True, + # Uncomment both lines (this and the plot parameter below) to enable plotting + #plot=True, +) + +run_experiment_lite( + algo.train(), + # Number of parallel workers for sampling + n_parallel=1, + # Only keep the snapshot parameters for the last iteration + snapshot_mode="last", + # Specifies the seed for the experiment. If this is not provided, a random seed + # will be used + seed=1, + #plot=True, +) From 8c27a62e042cbf444c829f4ae97d9d3bdabaff51 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Fri, 10 Mar 2017 13:41:05 -0800 Subject: [PATCH 4/5] remove the stub calls from rllab --- .../gym/minitaurWalk_bullet_gym_example.py | 27 ------------------- .../pybullet/gym/trpo_cartpole_bullet_gym.py | 22 +-------------- .../gym/trpo_tf_cartpole_bullet_gym.py | 15 +---------- 3 files changed, 2 insertions(+), 62 deletions(-) delete mode 100644 examples/pybullet/gym/minitaurWalk_bullet_gym_example.py diff --git a/examples/pybullet/gym/minitaurWalk_bullet_gym_example.py b/examples/pybullet/gym/minitaurWalk_bullet_gym_example.py deleted file mode 100644 index 4da575341..000000000 --- a/examples/pybullet/gym/minitaurWalk_bullet_gym_example.py +++ /dev/null @@ -1,27 +0,0 @@ -"""One-line documentation for gym_example module. - -A detailed description of gym_example. -""" - -import gym -from envs.bullet.minitaur import MinitaurWalkEnv -import setuptools -import time -import numpy as np - - -def main(): - env = gym.make('MinitaurWalkEnv-v0') - for i_episode in range(1): - observation = env.reset() - done = False - while not done: - print(observation) - action = np.array([1.3, 0, 0, 0, 1.3, 0, 0, 0, 1.3, 3.14, 0, 0, 1.3, 3.14, 0, 0]) - print(action) - observation, reward, done, info = env.step(action) - if done: - print("Episode finished after {} timesteps".format(t+1)) - break - -main() diff --git a/examples/pybullet/gym/trpo_cartpole_bullet_gym.py b/examples/pybullet/gym/trpo_cartpole_bullet_gym.py index 4c257cf6b..bac943217 100644 --- a/examples/pybullet/gym/trpo_cartpole_bullet_gym.py +++ b/examples/pybullet/gym/trpo_cartpole_bullet_gym.py @@ -3,12 +3,7 @@ from rllab.algos.trpo import TRPO from rllab.baselines.linear_feature_baseline import LinearFeatureBaseline from rllab.envs.gym_env import GymEnv from rllab.envs.normalized_env import normalize -from rllab.misc.instrument import stub, run_experiment_lite from rllab.policies.gaussian_mlp_policy import GaussianMLPPolicy -import subprocess -import time - -stub(globals()) env = normalize(GymEnv("CartPoleBulletEnv-v0")) @@ -33,19 +28,4 @@ algo = TRPO( # plot=True, ) -#cmdStartBulletServer=['~/Projects/rllab/bullet_examples/run_physics_server.sh'] -#subprocess.Popen(cmdStartBulletServer, shell=True) -#time.sleep(1) - - -run_experiment_lite( - algo.train(), - # Number of parallel workers for sampling - n_parallel=1, - # Only keep the snapshot parameters for the last iteration - snapshot_mode="last", - # Specifies the seed for the experiment. If this is not provided, a random seed - # will be used - seed=1, - # plot=True, -) +algo.train() diff --git a/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py b/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py index 0e6c1353b..6e055be15 100644 --- a/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py +++ b/examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py @@ -6,9 +6,6 @@ from sandbox.rocky.tf.envs.base import TfEnv from rllab.baselines.linear_feature_baseline import LinearFeatureBaseline from rllab.envs.gym_env import GymEnv from rllab.envs.normalized_env import normalize -from rllab.misc.instrument import stub, run_experiment_lite - -stub(globals()) env = TfEnv(normalize(GymEnv("CartPoleBulletEnv-v0"))) @@ -35,14 +32,4 @@ algo = TRPO( #plot=True, ) -run_experiment_lite( - algo.train(), - # Number of parallel workers for sampling - n_parallel=1, - # Only keep the snapshot parameters for the last iteration - snapshot_mode="last", - # Specifies the seed for the experiment. If this is not provided, a random seed - # will be used - seed=1, - #plot=True, -) +algo.train() From 43b0a8b6fb3017fa14b2e9e8af8f095fc314683c Mon Sep 17 00:00:00 2001 From: Mohi Date: Mon, 13 Mar 2017 10:38:02 -0700 Subject: [PATCH 5/5] Fixing a bug on Visualizer Camera and a few more - resetDebugVisualizerCamera now accepts negative values for pitch and yaw angles - Defining kitchen model to be concave, so as to be able to put floating objects inside - Fixed an indention error in testrender_np.py --- data/kitchens/1.sdf | 346 ++++++++++++++--------------- examples/pybullet/pybullet.c | 6 +- examples/pybullet/testrender_np.py | 4 +- 3 files changed, 178 insertions(+), 178 deletions(-) diff --git a/data/kitchens/1.sdf b/data/kitchens/1.sdf index 5f1046837..0bf6112e9 100644 --- a/data/kitchens/1.sdf +++ b/data/kitchens/1.sdf @@ -16,7 +16,7 @@ 0.166667 - + .1 .1 .1 @@ -55,7 +55,7 @@ 0.166667 - + .1 .1 .1 @@ -94,7 +94,7 @@ 0.166667 - + .1 .1 .1 @@ -133,7 +133,7 @@ 0.166667 - + .1 .1 .1 @@ -172,7 +172,7 @@ 0.166667 - + .1 .1 .1 @@ -211,7 +211,7 @@ 0.166667 - + .1 .1 .1 @@ -250,7 +250,7 @@ 0.166667 - + .1 .1 .1 @@ -289,7 +289,7 @@ 0.166667 - + .1 .1 .1 @@ -328,7 +328,7 @@ 0.166667 - + .1 .1 .1 @@ -367,7 +367,7 @@ 0.166667 - + .1 .1 .1 @@ -406,7 +406,7 @@ 0.166667 - + .1 .1 .1 @@ -445,7 +445,7 @@ 0.166667 - + .1 .1 .1 @@ -484,7 +484,7 @@ 0.166667 - + .1 .1 .1 @@ -523,7 +523,7 @@ 0.166667 - + .1 .1 .1 @@ -562,7 +562,7 @@ 0.166667 - + .1 .1 .1 @@ -601,7 +601,7 @@ 0.166667 - + .1 .1 .1 @@ -640,7 +640,7 @@ 0.166667 - + .1 .1 .1 @@ -679,7 +679,7 @@ 0.166667 - + .1 .1 .1 @@ -718,7 +718,7 @@ 0.166667 - + .1 .1 .1 @@ -757,7 +757,7 @@ 0.166667 - + .1 .1 .1 @@ -796,7 +796,7 @@ 0.166667 - + .1 .1 .1 @@ -835,7 +835,7 @@ 0.166667 - + .1 .1 .1 @@ -874,7 +874,7 @@ 0.166667 - + .1 .1 .1 @@ -913,7 +913,7 @@ 0.166667 - + .1 .1 .1 @@ -952,7 +952,7 @@ 0.166667 - + .1 .1 .1 @@ -991,7 +991,7 @@ 0.166667 - + .1 .1 .1 @@ -1030,7 +1030,7 @@ 0.166667 - + .1 .1 .1 @@ -1069,7 +1069,7 @@ 0.166667 - + .1 .1 .1 @@ -1108,7 +1108,7 @@ 0.166667 - + .1 .1 .1 @@ -1147,7 +1147,7 @@ 0.166667 - + .1 .1 .1 @@ -1186,7 +1186,7 @@ 0.166667 - + .1 .1 .1 @@ -1225,7 +1225,7 @@ 0.166667 - + .1 .1 .1 @@ -1264,7 +1264,7 @@ 0.166667 - + .1 .1 .1 @@ -1303,7 +1303,7 @@ 0.166667 - + .1 .1 .1 @@ -1342,7 +1342,7 @@ 0.166667 - + .1 .1 .1 @@ -1381,7 +1381,7 @@ 0.166667 - + .1 .1 .1 @@ -1420,7 +1420,7 @@ 0.166667 - + .1 .1 .1 @@ -1459,7 +1459,7 @@ 0.166667 - + .1 .1 .1 @@ -1498,7 +1498,7 @@ 0.166667 - + .1 .1 .1 @@ -1537,7 +1537,7 @@ 0.166667 - + .1 .1 .1 @@ -1576,7 +1576,7 @@ 0.166667 - + .1 .1 .1 @@ -1615,7 +1615,7 @@ 0.166667 - + .1 .1 .1 @@ -1654,7 +1654,7 @@ 0.166667 - + .1 .1 .1 @@ -1693,7 +1693,7 @@ 0.166667 - + .1 .1 .1 @@ -1732,7 +1732,7 @@ 0.166667 - + .1 .1 .1 @@ -1771,7 +1771,7 @@ 0.166667 - + .1 .1 .1 @@ -1810,7 +1810,7 @@ 0.166667 - + .1 .1 .1 @@ -1849,7 +1849,7 @@ 0.166667 - + .1 .1 .1 @@ -1888,7 +1888,7 @@ 0.166667 - + .1 .1 .1 @@ -1927,7 +1927,7 @@ 0.166667 - + .1 .1 .1 @@ -1966,7 +1966,7 @@ 0.166667 - + .1 .1 .1 @@ -2005,7 +2005,7 @@ 0.166667 - + .1 .1 .1 @@ -2044,7 +2044,7 @@ 0.166667 - + .1 .1 .1 @@ -2083,7 +2083,7 @@ 0.166667 - + .1 .1 .1 @@ -2122,7 +2122,7 @@ 0.166667 - + .1 .1 .1 @@ -2161,7 +2161,7 @@ 0.166667 - + .1 .1 .1 @@ -2200,7 +2200,7 @@ 0.166667 - + .1 .1 .1 @@ -2239,7 +2239,7 @@ 0.166667 - + .1 .1 .1 @@ -2278,7 +2278,7 @@ 0.166667 - + .1 .1 .1 @@ -2317,7 +2317,7 @@ 0.166667 - + .1 .1 .1 @@ -2356,7 +2356,7 @@ 0.166667 - + .1 .1 .1 @@ -2395,7 +2395,7 @@ 0.166667 - + .1 .1 .1 @@ -2434,7 +2434,7 @@ 0.166667 - + .1 .1 .1 @@ -2473,7 +2473,7 @@ 0.166667 - + .1 .1 .1 @@ -2512,7 +2512,7 @@ 0.166667 - + .1 .1 .1 @@ -2551,7 +2551,7 @@ 0.166667 - + .1 .1 .1 @@ -2590,7 +2590,7 @@ 0.166667 - + .1 .1 .1 @@ -2629,7 +2629,7 @@ 0.166667 - + .1 .1 .1 @@ -2668,7 +2668,7 @@ 0.166667 - + .1 .1 .1 @@ -2707,7 +2707,7 @@ 0.166667 - + .1 .1 .1 @@ -2746,7 +2746,7 @@ 0.166667 - + .1 .1 .1 @@ -2785,7 +2785,7 @@ 0.166667 - + .1 .1 .1 @@ -2824,7 +2824,7 @@ 0.166667 - + .1 .1 .1 @@ -2863,7 +2863,7 @@ 0.166667 - + .1 .1 .1 @@ -2902,7 +2902,7 @@ 0.166667 - + .1 .1 .1 @@ -2941,7 +2941,7 @@ 0.166667 - + .1 .1 .1 @@ -2980,7 +2980,7 @@ 0.166667 - + .1 .1 .1 @@ -3019,7 +3019,7 @@ 0.166667 - + .1 .1 .1 @@ -3058,7 +3058,7 @@ 0.166667 - + .1 .1 .1 @@ -3097,7 +3097,7 @@ 0.166667 - + .1 .1 .1 @@ -3136,7 +3136,7 @@ 0.166667 - + .1 .1 .1 @@ -3175,7 +3175,7 @@ 0.166667 - + .1 .1 .1 @@ -3214,7 +3214,7 @@ 0.166667 - + .1 .1 .1 @@ -3253,7 +3253,7 @@ 0.166667 - + .1 .1 .1 @@ -3292,7 +3292,7 @@ 0.166667 - + .1 .1 .1 @@ -3331,7 +3331,7 @@ 0.166667 - + .1 .1 .1 @@ -3370,7 +3370,7 @@ 0.166667 - + .1 .1 .1 @@ -3409,7 +3409,7 @@ 0.166667 - + .1 .1 .1 @@ -3448,7 +3448,7 @@ 0.166667 - + .1 .1 .1 @@ -3487,7 +3487,7 @@ 0.166667 - + .1 .1 .1 @@ -3526,7 +3526,7 @@ 0.166667 - + .1 .1 .1 @@ -3565,7 +3565,7 @@ 0.166667 - + .1 .1 .1 @@ -3604,7 +3604,7 @@ 0.166667 - + .1 .1 .1 @@ -3643,7 +3643,7 @@ 0.166667 - + .1 .1 .1 @@ -3682,7 +3682,7 @@ 0.166667 - + .1 .1 .1 @@ -3721,7 +3721,7 @@ 0.166667 - + .1 .1 .1 @@ -3760,7 +3760,7 @@ 0.166667 - + .1 .1 .1 @@ -3799,7 +3799,7 @@ 0.166667 - + .1 .1 .1 @@ -3838,7 +3838,7 @@ 0.166667 - + .1 .1 .1 @@ -3877,7 +3877,7 @@ 0.166667 - + .1 .1 .1 @@ -3916,7 +3916,7 @@ 0.166667 - + .1 .1 .1 @@ -3955,7 +3955,7 @@ 0.166667 - + .1 .1 .1 @@ -3994,7 +3994,7 @@ 0.166667 - + .1 .1 .1 @@ -4033,7 +4033,7 @@ 0.166667 - + .1 .1 .1 @@ -4072,7 +4072,7 @@ 0.166667 - + .1 .1 .1 @@ -4111,7 +4111,7 @@ 0.166667 - + .1 .1 .1 @@ -4150,7 +4150,7 @@ 0.166667 - + .1 .1 .1 @@ -4189,7 +4189,7 @@ 0.166667 - + .1 .1 .1 @@ -4228,7 +4228,7 @@ 0.166667 - + .1 .1 .1 @@ -4267,7 +4267,7 @@ 0.166667 - + .1 .1 .1 @@ -4306,7 +4306,7 @@ 0.166667 - + .1 .1 .1 @@ -4345,7 +4345,7 @@ 0.166667 - + .1 .1 .1 @@ -4384,7 +4384,7 @@ 0.166667 - + .1 .1 .1 @@ -4423,7 +4423,7 @@ 0.166667 - + .1 .1 .1 @@ -4462,7 +4462,7 @@ 0.166667 - + .1 .1 .1 @@ -4501,7 +4501,7 @@ 0.166667 - + .1 .1 .1 @@ -4540,7 +4540,7 @@ 0.166667 - + .1 .1 .1 @@ -4579,7 +4579,7 @@ 0.166667 - + .1 .1 .1 @@ -4618,7 +4618,7 @@ 0.166667 - + .1 .1 .1 @@ -4657,7 +4657,7 @@ 0.166667 - + .1 .1 .1 @@ -4696,7 +4696,7 @@ 0.166667 - + .1 .1 .1 @@ -4735,7 +4735,7 @@ 0.166667 - + .1 .1 .1 @@ -4774,7 +4774,7 @@ 0.166667 - + .1 .1 .1 @@ -4813,7 +4813,7 @@ 0.166667 - + .1 .1 .1 @@ -4852,7 +4852,7 @@ 0.166667 - + .1 .1 .1 @@ -4891,7 +4891,7 @@ 0.166667 - + .1 .1 .1 @@ -4930,7 +4930,7 @@ 0.166667 - + .1 .1 .1 @@ -4969,7 +4969,7 @@ 0.166667 - + .1 .1 .1 @@ -5008,7 +5008,7 @@ 0.166667 - + .1 .1 .1 @@ -5047,7 +5047,7 @@ 0.166667 - + .1 .1 .1 @@ -5086,7 +5086,7 @@ 0.166667 - + .1 .1 .1 @@ -5125,7 +5125,7 @@ 0.166667 - + .1 .1 .1 @@ -5164,7 +5164,7 @@ 0.166667 - + .1 .1 .1 @@ -5203,7 +5203,7 @@ 0.166667 - + .1 .1 .1 @@ -5242,7 +5242,7 @@ 0.166667 - + .1 .1 .1 @@ -5281,7 +5281,7 @@ 0.166667 - + .1 .1 .1 @@ -5320,7 +5320,7 @@ 0.166667 - + .1 .1 .1 @@ -5359,7 +5359,7 @@ 0.166667 - + .1 .1 .1 @@ -5398,7 +5398,7 @@ 0.166667 - + .1 .1 .1 @@ -5437,7 +5437,7 @@ 0.166667 - + .1 .1 .1 @@ -5476,7 +5476,7 @@ 0.166667 - + .1 .1 .1 @@ -5515,7 +5515,7 @@ 0.166667 - + .1 .1 .1 @@ -5554,7 +5554,7 @@ 0.166667 - + .1 .1 .1 @@ -5593,7 +5593,7 @@ 0.166667 - + .1 .1 .1 @@ -5632,7 +5632,7 @@ 0.166667 - + .1 .1 .1 @@ -5671,7 +5671,7 @@ 0.166667 - + .1 .1 .1 @@ -5741,7 +5741,7 @@ 0.166667 - + .1 .1 .1 @@ -5780,7 +5780,7 @@ 0.166667 - + .1 .1 .1 @@ -5819,7 +5819,7 @@ 0.166667 - + .1 .1 .1 @@ -5858,7 +5858,7 @@ 0.166667 - + .1 .1 .1 @@ -5897,7 +5897,7 @@ 0.166667 - + .1 .1 .1 @@ -5936,7 +5936,7 @@ 0.166667 - + .1 .1 .1 @@ -5975,7 +5975,7 @@ 0.166667 - + .1 .1 .1 @@ -6014,7 +6014,7 @@ 0.166667 - + .1 .1 .1 @@ -6053,7 +6053,7 @@ 0.166667 - + .1 .1 .1 @@ -6092,7 +6092,7 @@ 0.166667 - + .1 .1 .1 @@ -6131,7 +6131,7 @@ 0.166667 - + .1 .1 .1 @@ -6170,7 +6170,7 @@ 0.166667 - + .1 .1 .1 @@ -6209,7 +6209,7 @@ 0.166667 - + .1 .1 .1 @@ -6248,7 +6248,7 @@ 0.166667 - + .1 .1 .1 @@ -6287,7 +6287,7 @@ 0.166667 - + .1 .1 .1 @@ -6326,7 +6326,7 @@ 0.166667 - + .1 .1 .1 @@ -6365,7 +6365,7 @@ 0.166667 - + .1 .1 .1 @@ -6404,7 +6404,7 @@ 0.166667 - + .1 .1 .1 @@ -6443,7 +6443,7 @@ 0.166667 - + .1 .1 .1 @@ -6482,7 +6482,7 @@ 0.166667 - + .1 .1 .1 @@ -6521,7 +6521,7 @@ 0.166667 - + .1 .1 .1 @@ -6560,7 +6560,7 @@ 0.166667 - + .1 .1 .1 @@ -6599,7 +6599,7 @@ 0.166667 - + .1 .1 .1 @@ -6638,7 +6638,7 @@ 0.166667 - + .1 .1 .1 @@ -6677,7 +6677,7 @@ 0.166667 - + .1 .1 .1 @@ -6716,7 +6716,7 @@ 0.166667 - + .1 .1 .1 @@ -6755,7 +6755,7 @@ 0.166667 - + .1 .1 .1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c0465f7fb..1566dfc9f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3055,8 +3055,8 @@ static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* arg static PyObject* pybullet_resetDebugVisualizerCamera(PyObject* self, PyObject* args, PyObject *keywds) { float cameraDistance = -1; - float cameraYaw = -1; - float cameraPitch = -1; + float cameraYaw = 35; + float cameraPitch = 50; PyObject* cameraTargetPosObj=0; int physicsClientId = 0; @@ -3076,7 +3076,7 @@ static PyObject* pybullet_resetDebugVisualizerCamera(PyObject* self, PyObject* a { b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(sm); - if ((cameraDistance>=0) && (cameraYaw>=0) && (cameraPitch>=0)) + if ((cameraDistance>=0)) { float cameraTargetPosition[3]; if (pybullet_internalSetVector(cameraTargetPosObj,cameraTargetPosition)) diff --git a/examples/pybullet/testrender_np.py b/examples/pybullet/testrender_np.py index c008da09f..9f84fbed9 100644 --- a/examples/pybullet/testrender_np.py +++ b/examples/pybullet/testrender_np.py @@ -43,8 +43,8 @@ for pitch in range (0,360,10) : print ('width = %d height = %d' % (w,h)) #note that sending the data to matplotlib is really slow - - plt.imshow(rgb,interpolation='none') + + plt.imshow(rgb,interpolation='none') plt.pause(0.001) main_stop = time.time()