diff --git a/Extras/VHACD/test/src/main.cpp b/Extras/VHACD/test/src/main.cpp index 6b1204726..1c6112339 100644 --- a/Extras/VHACD/test/src/main.cpp +++ b/Extras/VHACD/test/src/main.cpp @@ -38,6 +38,14 @@ using namespace VHACD; using namespace std; +bool replace(std::string& str, const std::string& from, const std::string& to) { + size_t start_pos = str.find(from); + if(start_pos == std::string::npos) + return false; + str.replace(start_pos, from.length(), to); + return true; +} + class MyCallback : public IVHACD::IUserCallback { public: MyCallback(void) {} @@ -293,7 +301,16 @@ void ParseParameters(int argc, char* argv[], Parameters& params) for (int i = 1; i < argc; ++i) { if (!strcmp(argv[i], "--input")) { if (++i < argc) + { params.m_fileNameIn = argv[i]; + //come up with some default output name, if not provided + if (params.m_fileNameOut.length()==0) + { + string tmp = argv[i]; + replace(tmp,".obj",".vhacd.obj"); + params.m_fileNameOut = tmp; + } + } } else if (!strcmp(argv[i], "--output")) { if (++i < argc) diff --git a/examples/pybullet/examples/integrate.py b/examples/pybullet/examples/integrate.py new file mode 100644 index 000000000..a4e4deaa2 --- /dev/null +++ b/examples/pybullet/examples/integrate.py @@ -0,0 +1,13 @@ +import pybullet as p +p.connect(p.GUI) +cube = p.loadURDF("cube.urdf") +frequency = 240 +timeStep = 1./frequency +p.setGravity(0,0,-9.8) +p.changeDynamics(cube,-1,linearDamping=0,angularDamping=0) +p.setPhysicsEngineParameter(fixedTimeStep = timeStep) +for i in range (frequency): + p.stepSimulation() +pos,orn = p.getBasePositionAndOrientation(cube) +print(pos) + diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index 74b9b7c9b..f263bc7cb 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -9,6 +9,14 @@ register( reward_threshold=5.0, ) +register( + id='MinitaurBulletDuckEnv-v0', + entry_point='pybullet_envs.bullet:MinitaurBulletDuckEnv', + timestep_limit=1000, + reward_threshold=5.0, +) + + register( id='RacecarBulletEnv-v0', entry_point='pybullet_envs.bullet:RacecarGymEnv', diff --git a/examples/pybullet/gym/pybullet_envs/agents/configs.py b/examples/pybullet/gym/pybullet_envs/agents/configs.py index 6e787d129..edf4cd23d 100644 --- a/examples/pybullet/gym/pybullet_envs/agents/configs.py +++ b/examples/pybullet/gym/pybullet_envs/agents/configs.py @@ -23,6 +23,7 @@ import functools from agents import ppo from agents.scripts import networks from pybullet_envs.bullet import minitaur_gym_env +from pybullet_envs.bullet import minitaur_duck_gym_env from pybullet_envs.bullet import minitaur_env_randomizer import pybullet_envs.bullet.minitaur_gym_env as minitaur_gym_env import pybullet_envs @@ -33,7 +34,7 @@ def default(): # General algorithm = ppo.PPOAlgorithm num_agents = 10 - eval_episodes = 25 + eval_episodes = 20 use_gpu = False # Network network = networks.feed_forward_gaussian @@ -46,7 +47,7 @@ def default(): init_mean_factor = 0.05 init_logstd = -1 # Optimization - update_every = 25 + update_every = 20 policy_optimizer = 'AdamOptimizer' value_optimizer = 'AdamOptimizer' update_epochs_policy = 50 @@ -99,6 +100,16 @@ def pybullet_ant(): steps = 5e7 # 50M return locals() +def pybullet_kuka_grasping(): + """Configuration for Bullet Kuka grasping task.""" + locals().update(default()) + # Environment + env = 'KukaBulletEnv-v0' + max_length = 1000 + steps = 1e7 # 10M + return locals() + + def pybullet_racecar(): """Configuration for Bullet MIT Racecar task.""" locals().update(default()) @@ -124,3 +135,18 @@ def pybullet_minitaur(): steps = 3e7 # 30M return locals() +def pybullet_duck_minitaur(): + """Configuration specific to minitaur_gym_env.MinitaurBulletDuckEnv class.""" + locals().update(default()) + randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer()) + env = functools.partial( + minitaur_gym_env.MinitaurBulletDuckEnv, + accurate_motor_model_enabled=True, + motor_overheat_protection=True, + pd_control_enabled=True, + env_randomizer=randomizer, + render=False) + max_length = 1000 + steps = 3e7 # 30M + return locals() + diff --git a/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_grasping.py b/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_grasping.py index 5950a593e..1a5b7c2a8 100644 --- a/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_grasping.py +++ b/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_grasping.py @@ -12,7 +12,7 @@ from baselines import deepq def main(): - env = KukaGymEnv(renders=True) + env = KukaGymEnv(renders=True, isDiscrete=True) act = deepq.load("kuka_model.pkl") print(act) while True: diff --git a/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_cam_grasping.py b/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_cam_grasping.py index 053bc2ab2..4fd9f5a48 100644 --- a/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_cam_grasping.py +++ b/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_cam_grasping.py @@ -25,7 +25,7 @@ def callback(lcl, glb): def main(): - env = KukaCamGymEnv(renders=True) + env = KukaCamGymEnv(renders=False, isDiscrete=True) model = deepq.models.cnn_to_mlp( convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)], hiddens=[256], diff --git a/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_grasping.py b/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_grasping.py index 4de8130cb..932a08e77 100644 --- a/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_grasping.py +++ b/examples/pybullet/gym/pybullet_envs/baselines/train_kuka_grasping.py @@ -25,7 +25,7 @@ def callback(lcl, glb): def main(): - env = KukaGymEnv(renders=False) + env = KukaGymEnv(renders=False, isDiscrete=True) model = deepq.models.mlp([64]) act = deepq.learn( env, diff --git a/examples/pybullet/gym/pybullet_envs/bullet/__init__.py b/examples/pybullet/gym/pybullet_envs/bullet/__init__.py index 8b1735fb1..4c08abbba 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/__init__.py @@ -1,5 +1,6 @@ from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv from pybullet_envs.bullet.minitaur_gym_env import MinitaurBulletEnv +from pybullet_envs.bullet.minitaur_duck_gym_env import MinitaurBulletDuckEnv from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kuka.py b/examples/pybullet/gym/pybullet_envs/bullet/kuka.py index 3e7ee6c49..4c745cca1 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kuka.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kuka.py @@ -17,14 +17,15 @@ class Kuka: self.timeStep = timeStep self.maxForce = 200. - self.fingerAForce = 6 - self.fingerBForce = 5.5 - self.fingerTipForce = 6 + self.fingerAForce = 2 + self.fingerBForce = 2.5 + self.fingerTipForce = 2 self.useInverseKinematics = 1 self.useSimulation = 1 - self.useNullSpace = 1 + self.useNullSpace =21 self.useOrientation = 1 self.kukaEndEffectorIndex = 6 + self.kukaGripperIndex = 7 #lower limits for null space self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] #upper limits for null space @@ -76,7 +77,7 @@ class Kuka: def getObservation(self): observation = [] - state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex) + state = p.getLinkState(self.kukaUid,self.kukaGripperIndex) pos = state[0] orn = state[1] euler = p.getEulerFromQuaternion(orn) @@ -106,13 +107,13 @@ class Kuka: self.endEffectorPos[0] = self.endEffectorPos[0]+dx - if (self.endEffectorPos[0]>0.75): - self.endEffectorPos[0]=0.75 - if (self.endEffectorPos[0]<0.45): - self.endEffectorPos[0]=0.45 + if (self.endEffectorPos[0]>0.65): + self.endEffectorPos[0]=0.65 + if (self.endEffectorPos[0]<0.50): + self.endEffectorPos[0]=0.50 self.endEffectorPos[1] = self.endEffectorPos[1]+dy - if (self.endEffectorPos[1]<-0.22): - self.endEffectorPos[1]=-0.22 + if (self.endEffectorPos[1]<-0.17): + self.endEffectorPos[1]=-0.17 if (self.endEffectorPos[1]>0.22): self.endEffectorPos[1]=0.22 @@ -120,10 +121,8 @@ class Kuka: #print (self.endEffectorPos[2]) #print("actualEndEffectorPos[2]") #print(actualEndEffectorPos[2]) - if (dz>0 or actualEndEffectorPos[2]>0.10): - self.endEffectorPos[2] = self.endEffectorPos[2]+dz - if (actualEndEffectorPos[2]<0.10): - self.endEffectorPos[2] = self.endEffectorPos[2]+0.0001 + #if (dz<0 or actualEndEffectorPos[2]<0.5): + self.endEffectorPos[2] = self.endEffectorPos[2]+dz self.endEffectorAngle = self.endEffectorAngle + da @@ -147,7 +146,7 @@ class Kuka: if (self.useSimulation): for i in range (self.kukaEndEffectorIndex+1): #print(i) - p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.03,velocityGain=1) + p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.3,velocityGain=1) else: #reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range (self.numJoints): diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py index ea2b07b82..4e7d7e70e 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py @@ -14,7 +14,10 @@ import pybullet as p from . import kuka import random import pybullet_data +maxSteps = 1000 +RENDER_HEIGHT = 720 +RENDER_WIDTH = 960 class KukaCamGymEnv(gym.Env): metadata = { @@ -26,8 +29,8 @@ class KukaCamGymEnv(gym.Env): urdfRoot=pybullet_data.getDataPath(), actionRepeat=1, isEnableSelfCollision=True, - renders=True): - print("init") + renders=False, + isDiscrete=False): self._timeStep = 1./240. self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat @@ -37,6 +40,7 @@ class KukaCamGymEnv(gym.Env): self._renders = renders self._width = 341 self._height = 256 + self._isDiscrete=isDiscrete self.terminated = 0 self._p = p if self._renders: @@ -54,12 +58,17 @@ class KukaCamGymEnv(gym.Env): #print(observationDim) observation_high = np.array([np.finfo(np.float32).max] * observationDim) - self.action_space = spaces.Discrete(7) + if (self._isDiscrete): + self.action_space = spaces.Discrete(7) + else: + action_dim = 3 + self._action_bound = 1 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4)) self.viewer = None def _reset(self): - print("reset") self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) @@ -68,8 +77,8 @@ class KukaCamGymEnv(gym.Env): p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - xpos = 0.5 +0.05*random.random() - ypos = 0 +0.05*random.random() + xpos = 0.5 +0.2*random.random() + ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) @@ -117,24 +126,36 @@ class KukaCamGymEnv(gym.Env): return self._observation def _step(self, action): - dv = 0.01 - dx = [0,-dv,dv,0,0,0,0][action] - dy = [0,0,0,-dv,dv,0,0][action] - da = [0,0,0,0,0,-0.1,0.1][action] - f = 0.3 - realAction = [dx,dy,-0.002,da,f] + if (self._isDiscrete): + dv = 0.01 + dx = [0,-dv,dv,0,0,0,0][action] + dy = [0,0,0,-dv,dv,0,0][action] + da = [0,0,0,0,0,-0.1,0.1][action] + f = 0.3 + realAction = [dx,dy,-0.002,da,f] + else: + dv = 0.01 + dx = action[0] * dv + dy = action[1] * dv + da = action[2] * 0.1 + f = 0.3 + realAction = [dx,dy,-0.002,da,f] + return self.step2( realAction) def step2(self, action): - self._kuka.applyAction(action) for i in range(self._actionRepeat): + self._kuka.applyAction(action) p.stepSimulation() - if self._renders: - time.sleep(self._timeStep) - self._observation = self.getExtendedObservation() if self._termination(): break + #self._observation = self.getExtendedObservation() self._envStepCounter += 1 + + self._observation = self.getExtendedObservation() + if self._renders: + time.sleep(self._timeStep) + #print("self._envStepCounter") #print(self._envStepCounter) @@ -145,7 +166,25 @@ class KukaCamGymEnv(gym.Env): return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): - return + if mode != "rgb_array": + return np.array([]) + base_pos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + view_matrix = self._p.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._p.computeProjectionMatrixFOV( + fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = self._p.getCameraImage( + width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array def _termination(self): #print (self._kuka.endEffectorPos[2]) @@ -154,25 +193,41 @@ class KukaCamGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - if (self.terminated or self._envStepCounter>1000): + if (self.terminated or self._envStepCounter>maxSteps): self._observation = self.getExtendedObservation() return True - - if (actualEndEffectorPos[2] <= 0.10): + maxDist = 0.005 + closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) + + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 #print("closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 - - for i in range (1000): - graspAction = [0,0,0.001,0,fingerAngle] + for i in range (100): + graspAction = [0,0,0.0001,0,fingerAngle] self._kuka.applyAction(graspAction) p.stepSimulation() fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + + for i in range (1000): + graspAction = [0,0,0.001,0,fingerAngle] + self._kuka.applyAction(graspAction) + p.stepSimulation() + blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) + if (blockPos[2] > 0.23): + #print("BLOCKPOS!") + #print(blockPos[2]) + break + state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) + actualEndEffectorPos = state[0] + if (actualEndEffectorPos[2]>0.5): + break + + self._observation = self.getExtendedObservation() return True return False @@ -181,7 +236,7 @@ class KukaCamGymEnv(gym.Env): #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) reward = -1000 numPt = len(closestPoints) @@ -189,13 +244,13 @@ class KukaCamGymEnv(gym.Env): if (numPt>0): #print("reward:") reward = -closestPoints[0][8]*10 - if (blockPos[2] >0.2): - print("grasped a block!!!") - print("self._envStepCounter") - print(self._envStepCounter) + #print("grasped a block!!!") + #print("self._envStepCounter") + #print(self._envStepCounter) reward = reward+1000 #print("reward") #print(reward) return reward + diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index 5928a7232..d2a47935d 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -14,6 +14,10 @@ from . import kuka import random import pybullet_data +largeValObservation = 100 + +RENDER_HEIGHT = 720 +RENDER_WIDTH = 960 class KukaGymEnv(gym.Env): metadata = { @@ -25,8 +29,11 @@ class KukaGymEnv(gym.Env): urdfRoot=pybullet_data.getDataPath(), actionRepeat=1, isEnableSelfCollision=True, - renders=True): - print("init") + renders=False, + isDiscrete=False, + maxSteps = 1000): + #print("KukaGymEnv __init__") + self._isDiscrete = isDiscrete self._timeStep = 1./240. self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat @@ -34,7 +41,12 @@ class KukaGymEnv(gym.Env): self._observation = [] self._envStepCounter = 0 self._renders = renders + self._maxSteps = maxSteps self.terminated = 0 + self._cam_dist = 1.3 + self._cam_yaw = 180 + self._cam_pitch = -40 + self._p = p if self._renders: cid = p.connect(p.SHARED_MEMORY) @@ -50,13 +62,19 @@ class KukaGymEnv(gym.Env): #print("observationDim") #print(observationDim) - observation_high = np.array([np.finfo(np.float32).max] * observationDim) - self.action_space = spaces.Discrete(7) + observation_high = np.array([largeValObservation] * observationDim) + if (self._isDiscrete): + self.action_space = spaces.Discrete(7) + else: + action_dim = 3 + self._action_bound = 1 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None def _reset(self): - print("reset") + #print("KukaGymEnv _reset") self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) @@ -65,11 +83,11 @@ class KukaGymEnv(gym.Env): p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - xpos = 0.5 +0.05*random.random() - ypos = 0 +0.05*random.random() - ang = 3.1415925438*random.random() + xpos = 0.55 +0.12*random.random() + ypos = 0 +0.2*random.random() + ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) - self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) + self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3]) p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -87,49 +105,104 @@ class KukaGymEnv(gym.Env): def getExtendedObservation(self): self._observation = self._kuka.getObservation() - eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) - endEffectorPos = eeState[0] - endEffectorOrn = eeState[1] + gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex) + gripperPos = gripperState[0] + gripperOrn = gripperState[1] blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid) - invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn) - blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn) - blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE) - self._observation.extend(list(blockPosInEE)) - self._observation.extend(list(blockEulerInEE)) - + invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn) + gripperMat = p.getMatrixFromQuaternion(gripperOrn) + dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]] + dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]] + dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]] + + gripperEul = p.getEulerFromQuaternion(gripperOrn) + #print("gripperEul") + #print(gripperEul) + blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn) + projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]] + blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper) + #print("projectedBlockPos2D") + #print(projectedBlockPos2D) + #print("blockEulerInGripper") + #print(blockEulerInGripper) + + #we return the relative x,y position and euler angle of block in gripper space + blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]] + + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) + + self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation def _step(self, action): - dv = 0.01 - dx = [0,-dv,dv,0,0,0,0][action] - dy = [0,0,0,-dv,dv,0,0][action] - da = [0,0,0,0,0,-0.1,0.1][action] - f = 0.3 - realAction = [dx,dy,-0.002,da,f] + if (self._isDiscrete): + dv = 0.005 + dx = [0,-dv,dv,0,0,0,0][action] + dy = [0,0,0,-dv,dv,0,0][action] + da = [0,0,0,0,0,-0.05,0.05][action] + f = 0.3 + realAction = [dx,dy,-0.002,da,f] + else: + #print("action[0]=", str(action[0])) + dv = 0.005 + dx = action[0] * dv + dy = action[1] * dv + da = action[2] * 0.05 + f = 0.3 + realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) def step2(self, action): - self._kuka.applyAction(action) for i in range(self._actionRepeat): + self._kuka.applyAction(action) p.stepSimulation() - if self._renders: - time.sleep(self._timeStep) - self._observation = self.getExtendedObservation() if self._termination(): break self._envStepCounter += 1 + if self._renders: + time.sleep(self._timeStep) + self._observation = self.getExtendedObservation() + #print("self._envStepCounter") #print(self._envStepCounter) done = self._termination() - reward = self._reward() + npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]]) + actionCost = np.linalg.norm(npaction)*10. + #print("actionCost") + #print(actionCost) + reward = self._reward()-actionCost + #print("reward") + #print(reward) + #print("len=%r" % len(self._observation)) return np.array(self._observation), reward, done, {} - def _render(self, mode='human', close=False): - return + def _render(self, mode="rgb_array", close=False): + if mode != "rgb_array": + return np.array([]) + base_pos,orn = self._p.getBasePositionAndOrientation(self._kuka.kukaUid) + view_matrix = self._p.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._p.computeProjectionMatrixFOV( + fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = self._p.getCameraImage( + width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=self._p.ER_BULLET_HARDWARE_OPENGL) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + def _termination(self): #print (self._kuka.endEffectorPos[2]) @@ -138,25 +211,41 @@ class KukaGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - if (self.terminated or self._envStepCounter>1000): + if (self.terminated or self._envStepCounter>self._maxSteps): self._observation = self.getExtendedObservation() return True - - if (actualEndEffectorPos[2] <= 0.10): + maxDist = 0.005 + closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) + + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - #print("closing gripper, attempting grasp") + #print("terminating, closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 - - for i in range (1000): - graspAction = [0,0,0.001,0,fingerAngle] + for i in range (100): + graspAction = [0,0,0.0001,0,fingerAngle] self._kuka.applyAction(graspAction) p.stepSimulation() fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + + for i in range (1000): + graspAction = [0,0,0.001,0,fingerAngle] + self._kuka.applyAction(graspAction) + p.stepSimulation() + blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) + if (blockPos[2] > 0.23): + #print("BLOCKPOS!") + #print(blockPos[2]) + break + state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) + actualEndEffectorPos = state[0] + if (actualEndEffectorPos[2]>0.5): + break + + self._observation = self.getExtendedObservation() return True return False @@ -165,21 +254,24 @@ class KukaGymEnv(gym.Env): #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) - reward = -1000 + reward = -1000 + numPt = len(closestPoints) #print(numPt) if (numPt>0): #print("reward:") reward = -closestPoints[0][8]*10 - if (blockPos[2] >0.2): - print("grasped a block!!!") - print("self._envStepCounter") - print(self._envStepCounter) - reward = reward+1000 - - #print("reward") + reward = reward+10000 + print("successfully grasped a block!!!") + #print("self._envStepCounter") + #print(self._envStepCounter) + #print("self._envStepCounter") + #print(self._envStepCounter) + #print("reward") + #print(reward) + #print("reward") #print(reward) return reward diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py new file mode 100644 index 000000000..44b41dd7d --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py @@ -0,0 +1,387 @@ +"""This file implements the gym environment of minitaur. + +""" + +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + + +import math +import time +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import pybullet +from . import bullet_client +from . import minitaur +import os +import pybullet_data +from . import minitaur_env_randomizer + +NUM_SUBSTEPS = 5 +NUM_MOTORS = 8 +MOTOR_ANGLE_OBSERVATION_INDEX = 0 +MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS +MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS +BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS +ACTION_EPS = 0.01 +OBSERVATION_EPS = 0.01 +RENDER_HEIGHT = 720 +RENDER_WIDTH = 960 + +duckStartPos = [0,0,0.25] +duckStartOrn = [0.5,0.5,0.5,0.5] + +class MinitaurBulletDuckEnv(gym.Env): + """The gym environment for the minitaur. + + It simulates the locomotion of a minitaur, a quadruped robot. The state space + include the angles, velocities and torques for all the motors and the action + space is the desired motor angle for each motor. The reward function is based + on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 50 + } + + def __init__(self, + urdf_root=pybullet_data.getDataPath(), + action_repeat=1, + distance_weight=1.0, + energy_weight=0.005, + shake_weight=0.0, + drift_weight=0.0, + distance_limit=float("inf"), + observation_noise_stdev=0.0, + self_collision_enabled=True, + motor_velocity_limit=np.inf, + pd_control_enabled=False,#not needed to be true if accurate motor model is enabled (has its own better PD) + leg_model_enabled=True, + accurate_motor_model_enabled=True, + motor_kp=1.0, + motor_kd=0.02, + torque_control_enabled=False, + motor_overheat_protection=True, + hard_reset=True, + on_rack=False, + render=False, + kd_for_pd_controllers=0.3, + env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()): + """Initialize the minitaur gym environment. + + Args: + urdf_root: The path to the urdf data folder. + action_repeat: The number of simulation steps before actions are applied. + distance_weight: The weight of the distance term in the reward. + energy_weight: The weight of the energy term in the reward. + shake_weight: The weight of the vertical shakiness term in the reward. + drift_weight: The weight of the sideways drift term in the reward. + distance_limit: The maximum distance to terminate the episode. + observation_noise_stdev: The standard deviation of observation noise. + self_collision_enabled: Whether to enable self collision in the sim. + motor_velocity_limit: The velocity limit of each motor. + pd_control_enabled: Whether to use PD controller for each motor. + leg_model_enabled: Whether to use a leg motor to reparameterize the action + space. + accurate_motor_model_enabled: Whether to use the accurate DC motor model. + motor_kp: proportional gain for the accurate motor model. + motor_kd: derivative gain for the accurate motor model. + torque_control_enabled: Whether to use the torque control, if set to + False, pose control will be used. + motor_overheat_protection: Whether to shutdown the motor that has exerted + large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time + (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more + details. + hard_reset: Whether to wipe the simulation and load everything when reset + is called. If set to false, reset just place the minitaur back to start + position and set its pose to initial configuration. + on_rack: Whether to place the minitaur on rack. This is only used to debug + the walking gait. In this mode, the minitaur's base is hanged midair so + that its walking gait is clearer to visualize. + render: Whether to render the simulation. + kd_for_pd_controllers: kd value for the pd controllers of the motors + env_randomizer: An EnvRandomizer to randomize the physical properties + during reset(). + """ + self._time_step = 0.01 + self._action_repeat = action_repeat + self._num_bullet_solver_iterations = 300 + self._urdf_root = urdf_root + self._self_collision_enabled = self_collision_enabled + self._motor_velocity_limit = motor_velocity_limit + self._observation = [] + self._env_step_counter = 0 + self._is_render = render + self._last_base_position = [0, 0, 0] + self._distance_weight = distance_weight + self._energy_weight = energy_weight + self._drift_weight = drift_weight + self._shake_weight = shake_weight + self._distance_limit = distance_limit + self._observation_noise_stdev = observation_noise_stdev + self._action_bound = 1 + self._pd_control_enabled = pd_control_enabled + self._leg_model_enabled = leg_model_enabled + self._accurate_motor_model_enabled = accurate_motor_model_enabled + self._motor_kp = motor_kp + self._motor_kd = motor_kd + self._torque_control_enabled = torque_control_enabled + self._motor_overheat_protection = motor_overheat_protection + self._on_rack = on_rack + self._cam_dist = 1.0 + self._cam_yaw = 0 + self._duckId = -1 + self._cam_pitch = -30 + self._hard_reset = True + self._kd_for_pd_controllers = kd_for_pd_controllers + self._last_frame_time = 0.0 + print("urdf_root=" + self._urdf_root) + self._env_randomizer = env_randomizer + # PD control needs smaller time step for stability. + if pd_control_enabled or accurate_motor_model_enabled: + self._time_step /= NUM_SUBSTEPS + self._num_bullet_solver_iterations /= NUM_SUBSTEPS + self._action_repeat *= NUM_SUBSTEPS + + if self._is_render: + self._pybullet_client = bullet_client.BulletClient( + connection_mode=pybullet.GUI) + else: + self._pybullet_client = bullet_client.BulletClient() + + self._seed() + self.reset() + observation_high = ( + self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS) + observation_low = ( + self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS) + action_dim = 8 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(observation_low, observation_high) + self.viewer = None + self._hard_reset = hard_reset # This assignment need to be after reset() + + def set_env_randomizer(self, env_randomizer): + self._env_randomizer = env_randomizer + + def configure(self, args): + self._args = args + + def _reset(self): + if self._hard_reset: + self._pybullet_client.resetSimulation() + self._pybullet_client.setPhysicsEngineParameter( + numSolverIterations=int(self._num_bullet_solver_iterations)) + self._pybullet_client.setTimeStep(self._time_step) + self._groundId = self._pybullet_client.loadURDF("%s/plane.urdf" % self._urdf_root) + self._duckId = self._pybullet_client.loadURDF("%s/duck_vhacd.urdf" % self._urdf_root,duckStartPos,duckStartOrn) + self._pybullet_client.setGravity(0, 0, -10) + acc_motor = self._accurate_motor_model_enabled + motor_protect = self._motor_overheat_protection + self.minitaur = (minitaur.Minitaur( + pybullet_client=self._pybullet_client, + urdf_root=self._urdf_root, + time_step=self._time_step, + self_collision_enabled=self._self_collision_enabled, + motor_velocity_limit=self._motor_velocity_limit, + pd_control_enabled=self._pd_control_enabled, + accurate_motor_model_enabled=acc_motor, + motor_kp=self._motor_kp, + motor_kd=self._motor_kd, + torque_control_enabled=self._torque_control_enabled, + motor_overheat_protection=motor_protect, + on_rack=self._on_rack, + kd_for_pd_controllers=self._kd_for_pd_controllers)) + else: + self.minitaur.Reset(reload_urdf=False) + self._pybullet_client.resetBasePositionAndOrientation(self._duckId,duckStartPos,duckStartOrn) + if self._env_randomizer is not None: + self._env_randomizer.randomize_env(self) + + self._env_step_counter = 0 + self._last_base_position = [0, 0, 0] + self._objectives = [] + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]) + if not self._torque_control_enabled: + for _ in range(100): + if self._pd_control_enabled or self._accurate_motor_model_enabled: + self.minitaur.ApplyAction([math.pi / 2] * 8) + self._pybullet_client.stepSimulation() + return self._noisy_observation() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _transform_action_to_motor_command(self, action): + if self._leg_model_enabled: + for i, action_component in enumerate(action): + if not (-self._action_bound - ACTION_EPS <= action_component <= + self._action_bound + ACTION_EPS): + raise ValueError( + "{}th action {} out of bounds.".format(i, action_component)) + action = self.minitaur.ConvertFromLegModel(action) + return action + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: A list of desired motor angles for eight motors. + + Returns: + observations: The angles, velocities and torques of all motors. + reward: The reward for the current state-action pair. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + if self._is_render: + # Sleep, otherwise the computation takes less time than real time, + # which will make the visualization like a fast-forward video. + time_spent = time.time() - self._last_frame_time + self._last_frame_time = time.time() + time_to_sleep = self._action_repeat * self._time_step - time_spent + if time_to_sleep > 0: + time.sleep(time_to_sleep) + base_pos = self.minitaur.GetBasePosition() + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos) + action = self._transform_action_to_motor_command(action) + for _ in range(self._action_repeat): + self.minitaur.ApplyAction(action) + self._pybullet_client.stepSimulation() + + self._env_step_counter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._noisy_observation()), reward, done, {} + + def _render(self, mode="rgb_array", close=False): + if mode != "rgb_array": + return np.array([]) + base_pos = self.minitaur.GetBasePosition() + view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._pybullet_client.computeProjectionMatrixFOV( + fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = self._pybullet_client.getCameraImage( + width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + + def get_minitaur_motor_angles(self): + """Get the minitaur's motor angles. + + Returns: + A numpy array of motor angles. + """ + return np.array( + self._observation[MOTOR_ANGLE_OBSERVATION_INDEX: + MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_velocities(self): + """Get the minitaur's motor velocities. + + Returns: + A numpy array of motor velocities. + """ + return np.array( + self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX: + MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_torques(self): + """Get the minitaur's motor torques. + + Returns: + A numpy array of motor torques. + """ + return np.array( + self._observation[MOTOR_TORQUE_OBSERVATION_INDEX: + MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_base_orientation(self): + """Get the minitaur's base orientation, represented by a quaternion. + + Returns: + A numpy array of minitaur's orientation. + """ + return np.array(self._observation[BASE_ORIENTATION_OBSERVATION_INDEX:]) + + def lost_duck(self): + points = self._pybullet_client.getContactPoints(self._duckId, self._groundId); + return len(points)>0 + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + If the up directions between the base and the world is larger (the dot + product is smaller than 0.85) or the base is very low on the ground + (the height is smaller than 0.13 meter), the minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_up = rot_mat[6:] + pos = self.minitaur.GetBasePosition() + return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or + pos[2] < 0.13) + + def _termination(self): + position = self.minitaur.GetBasePosition() + distance = math.sqrt(position[0]**2 + position[1]**2) + return self.lost_duck() or self.is_fallen() or distance > self._distance_limit + + def _reward(self): + current_base_position = self.minitaur.GetBasePosition() + forward_reward = current_base_position[0] - self._last_base_position[0] + drift_reward = -abs(current_base_position[1] - self._last_base_position[1]) + shake_reward = -abs(current_base_position[2] - self._last_base_position[2]) + self._last_base_position = current_base_position + energy_reward = np.abs( + np.dot(self.minitaur.GetMotorTorques(), + self.minitaur.GetMotorVelocities())) * self._time_step + reward = ( + self._distance_weight * forward_reward - + self._energy_weight * energy_reward + self._drift_weight * drift_reward + + self._shake_weight * shake_reward) + self._objectives.append( + [forward_reward, energy_reward, drift_reward, shake_reward]) + return reward + + def get_objectives(self): + return self._objectives + + def _get_observation(self): + self._observation = self.minitaur.GetObservation() + return self._observation + + def _noisy_observation(self): + self._get_observation() + observation = np.array(self._observation) + if self._observation_noise_stdev > 0: + observation += (np.random.normal( + scale=self._observation_noise_stdev, size=observation.shape) * + self.minitaur.GetObservationUpperBound()) + return observation diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index bea79bd77..7537d54c1 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -15,6 +15,9 @@ import random from . import bullet_client import pybullet_data +RENDER_HEIGHT = 720 +RENDER_WIDTH = 960 + class RacecarGymEnv(gym.Env): metadata = { 'render.modes': ['human', 'rgb_array'], @@ -136,20 +139,27 @@ class RacecarGymEnv(gym.Env): return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): - width=320 - height=200 - img_arr = self._p.getCameraImage(width,height) - w=img_arr[0] - h=img_arr[1] - rgb=img_arr[2] - dep=img_arr[3] - #print 'width = %d height = %d' % (w,h) - # reshape creates np array - np_img_arr = np.reshape(rgb, (h, w, 4)) - # remove alpha channel - np_img_arr = np_img_arr[:, :, :3] - return np_img_arr + if mode != "rgb_array": + return np.array([]) + base_pos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + view_matrix = self._p.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._p.computeProjectionMatrixFOV( + fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = self._p.getCameraImage( + width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + def _termination(self): return self._envStepCounter>1000 diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index 507f6cb1a..d9546de8c 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -15,6 +15,9 @@ from . import racecar import random import pybullet_data +RENDER_HEIGHT = 720 +RENDER_WIDTH = 960 + class RacecarZEDGymEnv(gym.Env): metadata = { 'render.modes': ['human', 'rgb_array'], @@ -151,7 +154,26 @@ class RacecarZEDGymEnv(gym.Env): return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): - return + if mode != "rgb_array": + return np.array([]) + base_pos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + view_matrix = self._p.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = self._p.computeProjectionMatrixFOV( + fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = self._p.getCameraImage( + width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + def _termination(self): return self._envStepCounter>1000 diff --git a/examples/pybullet/gym/pybullet_envs/examples/kukaCamGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/kukaCamGymEnvTest.py new file mode 100644 index 000000000..00fe0d6b3 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/kukaCamGymEnvTest.py @@ -0,0 +1,40 @@ +#add parent dir to find package. Only needed for source code build, pip install doesn't need it. +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +from pybullet_envs.bullet.kukaCamGymEnv import KukaCamGymEnv +import time + +def main(): + + environment = KukaCamGymEnv(renders=True,isDiscrete=False) + + + motorsIds=[] + #motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537)) + #motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0)) + #motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2)) + #motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0)) + #motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) + + dv = 1 + motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) + + done = False + while (not done): + + action=[] + for motorId in motorsIds: + action.append(environment._p.readUserDebugParameter(motorId)) + + state, reward, done, info = environment.step(action) + obs = environment.getExtendedObservation() + +if __name__=="__main__": + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py index 65120c5f8..3917f08cf 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py @@ -9,7 +9,7 @@ import time def main(): - environment = KukaGymEnv(renders=True) + environment = KukaGymEnv(renders=True,isDiscrete=False, maxSteps = 10000000) motorsIds=[] @@ -19,10 +19,10 @@ def main(): #motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0)) #motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) - dv = 0.001 + dv = 0.01 motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0)) motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0)) - motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv)) + motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,0)) motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0)) motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) @@ -37,4 +37,4 @@ def main(): obs = environment.getExtendedObservation() if __name__=="__main__": - main() \ No newline at end of file + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest2.py b/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest2.py new file mode 100644 index 000000000..5c7f245c3 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest2.py @@ -0,0 +1,38 @@ +#add parent dir to find package. Only needed for source code build, pip install doesn't need it. +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv +import time + +def main(): + + environment = KukaGymEnv(renders=True,isDiscrete=False, maxSteps = 10000000) + + + motorsIds=[] + #motorsIds.append(environment._p.addUserDebugParameter("posX",-1,1,0)) + #motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0)) + #motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2)) + #motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0)) + #motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3)) + + dv = 1 + motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0)) + motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0)) + + done = False + while (not done): + + action=[] + for motorId in motorsIds: + action.append(environment._p.readUserDebugParameter(motorId)) + + state, reward, done, info = environment.step(action) + obs = environment.getExtendedObservation() + +if __name__=="__main__": + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py index d75d2254c..2aebfd79a 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py @@ -44,9 +44,9 @@ def MotorOverheatExample(): motor_kd=0.00, on_rack=False) - action = [2.0] * 8 + action = [.0] * 8 for i in range(8): - action[i] = 2.0 - 0.5 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1) + action[i] = .0 - 0.1 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1) steps = 500 actions_and_observations = [] @@ -112,9 +112,9 @@ def SinePolicyExample(): on_rack=False) sum_reward = 0 steps = 20000 - amplitude_1_bound = 0.5 - amplitude_2_bound = 0.5 - speed = 40 + amplitude_1_bound = 0.1 + amplitude_2_bound = 0.1 + speed = 1 for step_counter in range(steps): time_step = 0.01 @@ -124,9 +124,9 @@ def SinePolicyExample(): amplitude2 = amplitude_2_bound steering_amplitude = 0 if t < 10: - steering_amplitude = 0.5 + steering_amplitude = 0.1 elif t < 20: - steering_amplitude = -0.5 + steering_amplitude = -0.1 else: steering_amplitude = 0 diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py index aa78baa32..d640951b0 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py @@ -41,4 +41,4 @@ def main(): print(obs) if __name__=="__main__": - main() \ No newline at end of file + main() diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py index 64521b121..cee459e70 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py @@ -3,12 +3,13 @@ import os, inspect currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) +isDiscrete = False from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv def main(): - environment = RacecarZEDGymEnv(renders=True, isDiscrete=True) + environment = RacecarZEDGymEnv(renders=True, isDiscrete=isDiscrete) targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) @@ -16,25 +17,28 @@ def main(): while (True): targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider) steeringAngle = environment._p.readUserDebugParameter(steeringSlider) - discreteAction = 0 - if (targetVelocity<-0.33): - discreteAction=0 + if (isDiscrete): + discreteAction = 0 + if (targetVelocity<-0.33): + discreteAction=0 + else: + if (targetVelocity>0.33): + discreteAction=6 + else: + discreteAction=3 + if (steeringAngle>-0.17): + if (steeringAngle>0.17): + discreteAction=discreteAction+2 + else: + discreteAction=discreteAction+1 + action=discreteAction else: - if (targetVelocity>0.33): - discreteAction=6 - else: - discreteAction=3 - if (steeringAngle>-0.17): - if (steeringAngle>0.17): - discreteAction=discreteAction+2 - else: - discreteAction=discreteAction+1 - action=discreteAction - + action=[targetVelocity,steeringAngle] + state, reward, done, info = environment.step(action) obs = environment.getExtendedObservation() - print("obs") - print(obs) + #print("obs") + #print(obs) if __name__=="__main__": main() diff --git a/setup.py b/setup.py index 04e3c4cae..c7109b85f 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.5.8', + version='1.6.3', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index a7b168846..f05a4376c 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -615,7 +615,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) btScalar defaultRollingFrictionImpulse = 0.f; btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse); - rollingFriction = calcRollingFriction(contactPt); + rollingFriction = calcRollingFriction(contactPt)/btScalar(getNumWheels()); } }