Merge remote-tracking branch 'bp/master'

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()

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,
+)

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

examples/pybullet/gym/envs/bullet/cartpole_bullet.py

@@ -0,0 +1,89 @@
+"""
+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
+    p.connect(p.GUI)
+#    p.connect(p.DIRECT)
+    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

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

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

examples/pybullet/gym/trpo_cartpole_bullet_gym.py

@@ -0,0 +1,31 @@
+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.policies.gaussian_mlp_policy import GaussianMLPPolicy
+
+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,
+)
+
+algo.train()

examples/pybullet/gym/trpo_tf_cartpole_bullet_gym.py

@@ -0,0 +1,35 @@
+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
+
+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,
+)
+
+algo.train()

examples/pybullet/pybullet.c

@@ -3069,8 +3069,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;
@@ -3090,7 +3090,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))

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()

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)
+
+
+