157 lines
4.8 KiB
Python
157 lines
4.8 KiB
Python
"""This file implements the functionalities of an example simulated system using pybullet.
|
|
|
|
"""
|
|
import copy
|
|
import math
|
|
import numpy as np
|
|
import os
|
|
import pybullet_data
|
|
import random
|
|
|
|
|
|
class BoxStackPyBulletSim(object):
|
|
"""The ExamplePyBulletSim class that adds some objects to the scene, steps the sim and return a reward.
|
|
|
|
"""
|
|
|
|
def __init__(self,
|
|
pybullet_client,
|
|
urdf_root= pybullet_data.getDataPath(),
|
|
time_step=0.01):
|
|
"""Constructs an example simulation and reset it to the initial states.
|
|
|
|
Args:
|
|
pybullet_client: The instance of BulletClient to manage different
|
|
simulations.
|
|
urdf_root: The path to the urdf folder.
|
|
time_step: The time step of the simulation.
|
|
"""
|
|
self._pybullet_client = pybullet_client
|
|
self._urdf_root = urdf_root
|
|
self.m_actions_taken_since_reset=0
|
|
self.time_step = time_step
|
|
self.stateId = -1
|
|
self.Reset(reload_urdf=True)
|
|
|
|
|
|
def Reset(self, reload_urdf=False):
|
|
"""Reset the minitaur to its initial states.
|
|
|
|
Args:
|
|
reload_urdf: Whether to reload the urdf file. If not, Reset() just place
|
|
the minitaur back to its starting position.
|
|
"""
|
|
self.m_actions_taken_since_reset=0
|
|
xPosRange=0.025
|
|
yPosRange=0.025
|
|
boxHalfExtents = 0.025
|
|
|
|
if reload_urdf:
|
|
camInfo = self._pybullet_client.getDebugVisualizerCamera()
|
|
cameraDistance=camInfo[10]
|
|
print("cameraYaw=",camInfo[8])
|
|
print("cameraPitch=",camInfo[9])
|
|
print("camtarget=",camInfo[11])
|
|
print("projectionMatrix=",camInfo[3])
|
|
self._pybullet_client.resetDebugVisualizerCamera(cameraDistance=0.3, cameraYaw=camInfo[8], cameraPitch=camInfo[9],cameraTargetPosition=camInfo[11])
|
|
|
|
plane = self._pybullet_client.loadURDF("plane.urdf")
|
|
texUid = self._pybullet_client.loadTexture("checker_blue.png")
|
|
self._pybullet_client.changeVisualShape(plane,-1, textureUniqueId = texUid)
|
|
|
|
|
|
self._numObjects=4 #random number?
|
|
|
|
|
|
self._cubes=[]
|
|
|
|
red=[0.97,0.25,0.25,1]
|
|
green=[0.41,0.68,0.31,1]
|
|
yellow=[0.92,0.73,0,1]
|
|
blue=[0,0.55,0.81,1]
|
|
colors=[red,green,yellow,blue]
|
|
|
|
for i in range (self._numObjects):
|
|
pos=[0,0,boxHalfExtents + i*2*boxHalfExtents]
|
|
orn = self._pybullet_client.getQuaternionFromEuler([0,0,0])
|
|
orn=[0,0,0,1]
|
|
cube = self._pybullet_client.loadURDF("cube_small.urdf",pos,orn)
|
|
self._pybullet_client.changeVisualShape(cube,-1,rgbaColor=colors[i])
|
|
self._cubes.append(cube)
|
|
|
|
self._pybullet_client.setGravity(0, 0, -10)
|
|
self.stateId = self._pybullet_client.saveState()
|
|
else:
|
|
if (self.stateId>=0):
|
|
self._pybullet_client.restoreState(self.stateId)
|
|
index=0
|
|
for i in self._cubes:
|
|
posX = random.uniform(-xPosRange,xPosRange)
|
|
posY = random.uniform(-yPosRange,yPosRange)
|
|
yaw = random.uniform(-math.pi,math.pi)
|
|
pos=[posX,posY,boxHalfExtents + index*2*boxHalfExtents]
|
|
index+=1
|
|
orn = self._pybullet_client.getQuaternionFromEuler([0,0,yaw])
|
|
self._pybullet_client.resetBasePositionAndOrientation(i,pos,orn)
|
|
|
|
def GetActionDimension(self):
|
|
"""Get the length of the action list.
|
|
|
|
Returns:
|
|
The length of the action list.
|
|
"""
|
|
return 4#self.num_motors
|
|
|
|
def GetObservationUpperBound(self):
|
|
"""Get the upper bound of the observation.
|
|
|
|
Returns:
|
|
The upper bound of an observation. See GetObservation() for the details
|
|
of each element of an observation.
|
|
"""
|
|
upper_bound = np.array([0.0] * self.GetObservationDimension())
|
|
upper_bound[0:] = 1.0 # Quaternion of base orientation.
|
|
return upper_bound
|
|
|
|
def GetObservationLowerBound(self):
|
|
"""Get the lower bound of the observation."""
|
|
return -self.GetObservationUpperBound()
|
|
|
|
def GetObservationDimension(self):
|
|
"""Get the length of the observation list.
|
|
|
|
Returns:
|
|
The length of the observation list.
|
|
"""
|
|
sz = len(self.GetObservation())
|
|
print("sz=",sz)
|
|
return sz
|
|
|
|
def GetObservation(self):
|
|
"""Get the observations of minitaur.
|
|
|
|
Returns:
|
|
The observation list. observation[0:4] is the base orientation in quaternion form.
|
|
"""
|
|
observation = []
|
|
for i in self._cubes:
|
|
pos,orn=self._pybullet_client.getBasePositionAndOrientation(i)
|
|
observation.extend(list(pos))
|
|
observation.extend(list(orn))
|
|
return observation
|
|
|
|
def ApplyAction(self, action):
|
|
"""Set the desired action.
|
|
"""
|
|
self.m_actions_taken_since_reset+=1
|
|
|
|
|
|
def Termination(self):
|
|
return self.m_actions_taken_since_reset>=200
|
|
|
|
def CreateSim(pybullet_client,urdf_root,time_step):
|
|
sim = BoxStackPyBulletSim(pybullet_client=pybullet_client,
|
|
urdf_root=urdf_root,
|
|
time_step=time_step)
|
|
return sim
|