def reset(self):
self.terminated = False
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
useFixedBase=True)
# Load robot
self._panda = pandaEnv(self._urdfRoot,
timeStep=self._timeStep,
basePosition=[0, 0, 0.625],
useInverseKinematics=self._useIK,
action_space=self.action_dim,
includeVelObs=self.includeVelObs)
# Load table and object for simulation
tableId = p.loadURDF(os.path.join(self._urdfRoot,
"franka_description/table.urdf"),
useFixedBase=True)
table_info = p.getVisualShapeData(tableId, -1)[0]
self._h_table = table_info[5][2] + table_info[3][2]
#limit panda workspace to table plane
self._panda.workspace_lim[2][0] = self._h_table
# Randomize start position of object and target.
#we take the target point
self.obj_pose, self.target_pose = self._sample_pose()
if (self.fixedPositionObj):
#we use a fixed starting position for the cube
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=[0.7, 0.0, 0.64])
else:
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
#useful to see where is the taget point
self._targetID = p.loadURDF(
os.path.join(self._urdfRoot,
"franka_description/domino/domino.urdf"),
self.target_pose)
self._debugGUI()
p.setGravity(0, 0, -9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def reset(self):
if (self.test_phase):
global test_steps, test_done
if (test_steps != 0):
self.save_data_test()
test_steps = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"),
useFixedBase=True)
# Load robot
self._panda = pandaEnv(self._urdfRoot,
timeStep=self._timeStep,
basePosition=[0, 0, 0.625],
useInverseKinematics=self._useIK,
action_space=self.action_dim,
includeVelObs=self.includeVelObs)
# Load table and object for simulation
tableId = p.loadURDF(os.path.join(self._urdfRoot,
"franka_description/table.urdf"),
useFixedBase=True)
table_info = p.getVisualShapeData(tableId, -1)[0]
self._h_table = table_info[5][2] + table_info[3][2]
#limit panda workspace to table plane
self._panda.workspace_lim[2][0] = self._h_table
# Randomize start position of object and target.
#we take the target point
if (self.fixedPositionObj):
if (self.object_position == 0):
#we have completely fixed position
self.obj_pose = [0.6, 0.1, 0.64]
self.target_pose = [0.4, 0.45, 0.64]
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
elif (self.object_position == 1):
#we have completely fixed position
self.obj_pose = [
np.random.uniform(0.5, 0.6),
np.random.uniform(0, 0.1), 0.64
]
self.target_pose = [0.4, 0.45, 0.64]
# self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
elif (self.object_position == 2):
#we have completely fixed position
self.obj_pose = [
np.random.uniform(0.4, 0.6),
np.random.uniform(0, 0.2), 0.64
]
self.target_pose = [
np.random.uniform(0.3, 0.5),
np.random.uniform(0.35, 0.55), 0.64
]
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
elif (self.object_position == 3):
print("")
else:
self.obj_pose, self.target_pose = self._sample_pose()
self._objID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/cube_small.urdf"),
basePosition=self.obj_pose)
#useful to see where is the taget point
self._targetID = p.loadURDF(os.path.join(
self._urdfRoot, "franka_description/domino/domino.urdf"),
basePosition=self.target_pose)
if self.type_physics == 1:
# Randomizing the physics of the object...
self.currentMass = np.random.uniform(0.1, 0.8)
self.currentFriction = np.random.uniform(0.1, 0.7)
self.currentDamping = np.random.uniform(0.01, 0.2)
p.changeDynamics(self._objID,
linkIndex=-1,
mass=self.currentMass,
lateralFriction=self.currentFriction,
linearDamping=self.currentDamping)
# Randomizing the physics of the robot... (only joints damping and controller gains)
for i in range(7):
p.changeDynamics(self._panda.pandaId,
linkIndex=i,
linearDamping=np.random.uniform(0.25, 20))
elif self.type_physics == 2:
# Randomizing the physics of the object...
self.currentMass = 0.8
self.currentFriction = 0.2
self.currentDamping = 0.2
p.changeDynamics(self._objID,
linkIndex=-1,
mass=self.currentMass,
lateralFriction=self.currentFriction,
linearDamping=self.currentDamping)
# Randomizing the physics of the robot... (only joints damping and controller gains)
for i in range(7):
p.changeDynamics(self._panda.pandaId,
linkIndex=i,
linearDamping=0.25)
self._debugGUI()
p.setGravity(0, 0, -9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
#we take the dimension of the observation
return self.getExtendedObservation()
def reset(self):
if (self.test_phase):
global test_steps,test_done
if (test_steps != 0 ):
self.save_data_test()
test_steps = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
self.ep_counter = self.ep_counter + 1
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"), useFixedBase= True)
# Load robot
self._panda = pandaEnv(self._urdfRoot, timeStep=self._timeStep, basePosition =[0,0,0.625],
useInverseKinematics= self._useIK, action_space = self.action_dim, includeVelObs = self.includeVelObs)
# Load table and object for simulation
tableId = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/table.urdf"), useFixedBase=True)
table_info = p.getVisualShapeData(tableId,-1)[0]
self._h_table =table_info[5][2] + table_info[3][2]
#limit panda workspace to table plane
self._panda.workspace_lim[2][0] = self._h_table
# Randomize start position of object and target.
#we take the target point
if (self.fixedPositionObj):
if(self.object_position==0):
#we have completely fixed position
self.obj_pose = [0.6,0.1,0.64]
self.target_pose = [0.4,0.45,0.64]
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose)
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
elif(self.object_position==1):
if (self.keep_fixed_position):
if (self.ep_counter == 100):
self.obj_pose = [np.random.uniform(0.5,0.6),np.random.uniform(0,0.1),0.64]
self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
self.ep_counter = -1
else:
self.obj_pose = [np.random.uniform(0.5,0.6),np.random.uniform(0,0.1),0.64]
self.target_pose = self.target_pose = [0.4,0.45,0.64]
#self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose)
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
elif(self.object_position==2):
self.obj_pose = [np.random.uniform(0.4,0.6),np.random.uniform(0,0.2),0.64]
self.target_pose = [np.random.uniform(0.3,0.5),np.random.uniform(0.35,0.55),0.64]
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose)
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
elif(self.object_position==3):
print("")
else:
self.obj_pose, self.target_pose = self._sample_pose()
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition= self.obj_pose)
#useful to see where is the taget point
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
if(self.load_physics):
self.load_physics_params(0.71,0.41,0.35,20)
self._debugGUI()
p.setGravity(0,0,-9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
#we take the dimension of the observation
return self.getExtendedObservation()