def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("%s/plane.urdf" % urdfRoot)
p.setGravity(0, 0, -10)
global minitaur
minitaur = Minitaur(urdfRoot)
start_position = current_position()
last_position = None # for tracing line
total_energy = 0
for i in range(maxNumSteps):
torques = minitaur.getMotorTorques()
velocities = minitaur.getMotorVelocities()
total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
joint_values = evaluate_func_map[evaluateFunc](i, params)
minitaur.applyAction(joint_values)
p.stepSimulation()
if (is_fallen()):
break
if i % 100 == 0:
sys.stdout.write('.')
sys.stdout.flush()
time.sleep(sleepTime)
print(' ')
alpha = objectiveParams[0]
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime
print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
useMaximalCoordinates=maximalCoordinates)
for i in range(p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
for i in range(numObjects):
cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0, 0, -10)
def init(self):
if (self._game_settings['render']):
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = p.connect(p.GUI)
else:
self._physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0,0,self._GRAVITY)
p.setTimeStep(self._dt)
# p.connect(p.GUI)
RLSIMENV_PATH = os.environ['RLSIMENV_PATH']
p.loadURDF("plane.urdf")
self._agent = p.loadURDF(RLSIMENV_PATH + "/rlsimenv/data/cassie/urdf/cassie_collide.urdf",[0,0,0.8], useFixedBase=False)
# gravId = p.addUserDebugParameter("gravity",-10,10,-10)
self._init_root_vel = p.getBaseVelocity(self._agent)
self._init_root_pos = p.getBasePositionAndOrientation(self._agent)
self._init_pose = []
p.setPhysicsEngineParameter(numSolverIterations=100)
p.changeDynamics(self._agent,-1,linearDamping=0, angularDamping=0)
self.computeActionBounds()
p.setRealTimeSimulation(0)
lo = [0.0 for l in self.getObservation()[0]]
hi = [1.0 for l in self.getObservation()[0]]
state_bounds = [lo, hi]
state_bounds = [state_bounds]
self._game_settings['state_bounds'] = [lo, hi]
self._state_length = len(self._game_settings['state_bounds'][0])
# print ("self._state_length: ", self._state_length)
self._observation_space = ActionSpace(self._game_settings['state_bounds'])
self._game_settings['action_bounds'] = self._action_bounds
self._action_space = ActionSpace(self._action_bounds)
self._last_state = self.getState()
self._last_pose = p.getBasePositionAndOrientation(self._agent)[0]
def reset(self):
self.vt = 0
p.resetSimulation()
p.setGravity(0, 0, -9.8)
#self.planeId = p.loadURDF("plane.urdf")
self.plane1 = p.loadURDF(self.plane_urdf, [0, -13, 0],
p.getQuaternionFromEuler([0, 0, 0]))
self.plane2 = p.loadURDF(
self.plane_urdf, [
0, 2 + self.radius * np.cos(self.angle1),
-self.radius * np.sin(self.angle1)
], p.getQuaternionFromEuler([-self.angle1, 0, 0]))
self.plane3 = p.loadURDF(self.plane_urdf, [
0, 3 + self.length1 * np.cos(self.angle1),
-self.length1 * np.sin(self.angle1)
], p.getQuaternionFromEuler([0, 0, 0]))
self.plane4 = p.loadURDF(self.plane_urdf, [
0, 4 + self.length1 * np.cos(self.angle1) +
self.length2 * np.cos(self.angle2) -
self.radius * np.cos(self.angle2),
self.length2 * np.sin(self.angle2) -
self.length1 * np.sin(self.angle1) -
self.radius * np.sin(self.angle2)
], p.getQuaternionFromEuler([self.angle2, 0, 0]))
self.plane5 = p.loadURDF(self.plane_urdf, [
0, 4 + self.length1 * np.cos(self.angle1) +
self.length2 * np.cos(self.angle2) + self.radius,
self.length2 * np.sin(self.angle2) -
self.length1 * np.sin(self.angle1)
], p.getQuaternionFromEuler([0, 0, 0]))
self.botId = p.loadURDF(
"/home/lucas/modules/summer_project/my_models/bot_origin2.urdf",
StartPos, StartOrien)
p.stepSimulation()
observation = self.step_observation()
return observation
def reset(self):
"""Performs common reset functionality for all supported tasks."""
if not self.task:
raise ValueError(
'environment task must be set. Call set_task or pass '
'the task arg in the environment constructor.')
self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []}
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -9.8)
# Temporarily disable rendering to load scene faster.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
pybullet_utils.load_urdf(
p, os.path.join(self.assets_root, PLANE_URDF_PATH), [0, 0, -0.001])
pybullet_utils.load_urdf(
p, os.path.join(self.assets_root, UR5_WORKSPACE_URDF_PATH),
[0.5, 0, 0])
# Load UR5 robot arm equipped with suction end effector.
# TODO(andyzeng): add back parallel-jaw grippers.
self.ur5 = pybullet_utils.load_urdf(
p, os.path.join(self.assets_root, UR5_URDF_PATH))
self.ee = self.task.ee(self.assets_root, self.ur5, 9, self.obj_ids)
self.ee_tip = 10 # Link ID of suction cup.
# Get revolute joint indices of robot (skip fixed joints).
n_joints = p.getNumJoints(self.ur5)
joints = [p.getJointInfo(self.ur5, i) for i in range(n_joints)]
self.joints = [j[0] for j in joints if j[2] == p.JOINT_REVOLUTE]
# Move robot to home joint configuration.
for i in range(len(self.joints)):
p.resetJointState(self.ur5, self.joints[i], self.homej[i])
# Reset end effector.
self.ee.release()
# Reset task.
self.task.reset(self)
# Re-enable rendering.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
obs, _, _, _ = self.step()
return obs
def set_up_env(fixedTimeStep=1, numSubSteps=10, gravity=(0., 0., -10.), plane_id=0):
""" Create basic of the environment """
# Create environment
try:
pb.resetSimulation()
except:
pb.connect(pb.DIRECT)
pb.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
pb.setGravity(gravity[0], gravity[1], gravity[2]) # X-Y-Z
pb.setPhysicsEngineParameter(
fixedTimeStep=fixedTimeStep,
numSolverIterations=10000, # need to be high !
solverResidualThreshold=1e-10,
numSubSteps=numSubSteps # need to be high otherwise cubes go away !
)
pb.loadURDF(f"./data_generation/urdf/plane_{plane_id}/plane.urdf", useMaximalCoordinates=True)
def reset_simulation(self):
print("PandaIPEnv -> reset_simulation()")
# --- reset simulation --- #
p.resetSimulation(physicsClientId=self._physics_client_id)
#p.setPhysicsEngineParameter(numSolverIterations=150, physicsClientId=self._physics_client_id)
#p.setTimeStep(self._timeStep, physicsClientId=self._physics_client_id)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.setGravity(0, 0, -9.8, physicsClientId=self._physics_client_id)
# --- reset robot --- #
self._robot.reset()
# --- reset world --- #
self._world.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
def reset(self):
self.vt = 0
# current velocity pf the wheels
self.maxV = 24.6 # max lelocity, 235RPM = 24,609142453 rad/sec
self.envStepCounter = 0
p.resetSimulation()
p.setGravity(0, 0, -10) # m/s^2
p.setTimeStep(0.01)
# the duration of a step in sec
planeId = p.loadURDF("plane.urdf")
robotStartPos = [0, 0, 0.001]
robotStartOrientation = p.getQuaternionFromEuler(
[self.np_random.uniform(low=-0.3, high=0.3), 0, 0])
path = os.path.abspath(os.path.dirname(__file__))
self.botId = p.loadURDF(os.path.join(path, "balancebot_simple.xml"),
robotStartPos, robotStartOrientation)
self.observation = self.compute_observation()
return np.array(self.observation)
def reset(self, gravity):
self.episode_counter += 1
self.step_counter = 0
p.resetSimulation()
p.setGravity(0, 0, -gravity)
p.resetDebugVisualizerCamera(1, 45, -20, [0, 0, 0])
self.visual_shape = p.createVisualShape(p.GEOM_BOX,
halfExtents=[10, 10, 0.1])
self.coll_shape = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[10, 10, 0.1])
print(self.episode_counter)
p.setAdditionalSearchPath(pd.getDataPath())
self.planeId = p.loadURDF("plane.urdf", [0, 0, 0])
#insert name of robot here or path if not in same folder
self.robotId = p.loadURDF("robot_name.urdf", [0, 0, 0.3])
self.observation = np.array([0] * 12)
return self.observation
def _reset(self):
self.vt = 0
self.vd = 0
self._envStepCounter = 0
p.resetSimulation()
p.setGravity(0, 0, -10)
p.setTimeStep(0.01)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 0.001]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
path = os.path.abspath(os.path.dirname(__file__))
self.botId = p.loadURDF(os.path.join(path, "balancebot_simple.xml"),
cubeStartPos, cubeStartOrientation)
self._observation = self._compute_observation()
return np.array(self._observation)
def reset(self):
'''
when the task is over, reset the state of bipedal character and restart the simulation
return: character's state after reset
'''
p.resetSimulation()
self.configure()
#reset the biped to initial pose and velocity
start_pose=[0, 0, 0., 0.05, 0, 0, 0, 0, 0]
for i in range(p.getNumJoints(self.bipedId, physicsClientId=self.clientId)):
p.resetJointState(self.bipedId, i, start_pose[i], 0, physicsClientId= self.clientId)
self.num_step = 0
self.pdCon.kp = self.pdCon.init_kp.copy()
self.pdCon_kd = self.pdCon.init_kd.copy()
self.vel = 0
self.obv= self.getObv()
return self.obv
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"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 reset(self):
self.agents = {}
self.sensor_groups = {}
p.resetSimulation()
p.configureDebugVisualizer(
p.COV_ENABLE_RENDERING,
0) # we will enable rendering after we loaded everything
p.setGravity(0, 0, -10)
urdfRootPath = pybullet_data.getDataPath()
self.planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"),
basePosition=[0, 0, -0.65])
self.tableUid = p.loadURDF(os.path.join(urdfRootPath,
"table/table.urdf"),
basePosition=[0.5, 0, -0.65])
def reset(self):
p.resetSimulation()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.setGravity(0, 0, -10)
urdfRootPath = pd.getDataPath()
planeUid = p.loadURDF("Plane.urdf", [0, 0, -0.65])
self.curlingUid = p.loadURDF("Curling.urdf", useFixedBase=True)
tableUid = p.loadURDF("table.urdf"), [0.5, 0, -0.65]
tryUid = p.loadURDF("traybox.urdf"), [0.65, 0, 0]
state_object = [
random.uniform(0.5, 0.8),
random.uniform(-0.2, 0.2), 0.05
]
self.objectUid
def _reset(self):
# reset is called once at initialization of simulation
self.vt = 0
self.maxV = 24.6 # 235RPM = 24,609142453 rad/sec
self._envStepCounter = 0
p.resetSimulation()
p.setGravity(0,0,-10) # m/s^2
p.setTimeStep(0.01) # sec
self._load_geometry()
self._load_bot()
self._load_box()
# you *have* to compute and return the observation from reset()
self._observation = self._compute_observation()
return np.array(self._observation)
def reset(self):
#self.freeJointList = []
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self.timeStep)
self._envStepCounter = 0
self.ep_counter = self.ep_counter + 1
#loading Plane
planeId = p.loadURDF("plane.urdf")
#Load bioloid
self.bioId = p.loadURDF(self.urdfRootPath, basePosition = self.basePosition , baseOrientation = self.baseOrientation, useFixedBase= False )
for i in range(len(self.freeJointList)):
p.resetJointState(self.bioId, self.freeJointList[i], 0)
p.setJointMotorControl2(self.bioId, self.freeJointList[i], p.POSITION_CONTROL,targetPosition=0,targetVelocity=0.0,
positionGain=0.25, velocityGain=0.75, force=25)
self._debugGUI()
p.setGravity(0,0,-9.8)
#add debug slider
target_joint_pos = [0,0,0, 0,0,0, 0,0,1.188,-2.315,1.188,0, 0,0,1.188,-2.315,1.188,0]
init_joint_pos =[0,0,0, 0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0]
jointIds=[]
paramIds=[]
joints_num = p.getNumJoints(self.bioId)
i=0
"""
for j in range(26):
info = p.getJointInfo(self.bioId,j)
if info[2] == 0 :
self.freeJointList.append(j)
"""
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
#self._observation = self.getObservation() #[getlinkState[0](3 campi), getLinkState[1](3 campi), jointPoses (18 campi) ]
return self.getExtendedObservation()
def reset(self):
self.completed = False
self.step_counter = 0
p.resetSimulation()
p.configureDebugVisualizer(
p.COV_ENABLE_RENDERING,
0) # we will enable rendering after we loaded everything
urdfRootPath = pybullet_data.getDataPath()
p.setGravity(0, 0, -10)
planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"),
basePosition=[0, 0, -0.65])
self.pandaUid = p.loadURDF(os.path.join(urdfRootPath,
"franka_panda/panda.urdf"),
useFixedBase=True)
rest_poses = [0, -0.215, 0, -2.57, 0, 2.356, 2.356, 0.08, 0.08]
for i in range(7):
p.resetJointState(self.pandaUid, i, rest_poses[i])
p.resetJointState(self.pandaUid, 9, 0.08)
p.resetJointState(self.pandaUid, 10, 0.08)
tableUid = p.loadURDF(os.path.join(urdfRootPath, "table/table.urdf"),
basePosition=[0.5, 0, -0.65])
trayUid = p.loadURDF(os.path.join(urdfRootPath, "tray/traybox.urdf"),
basePosition=[0.65, 0, 0])
posObj = [
np.random.uniform(0.5, 0.7),
np.random.uniform(-0.15, 0.15), 0.05
]
orientationObj = p.getQuaternionFromEuler(
[0, 0, np.random.uniform(-np.pi / 5, np.pi / 5)])
self.objectUid = p.loadURDF(os.path.join(urdfRootPath,
"random_urdfs/000/000.urdf"),
basePosition=posObj,
baseOrientation=orientationObj)
# state_object = np.array(state_object) + np.random.uniform(0.05, 0.1, 3) * np.random.choice([-1, 1])
# secondObject = p.loadURDF(os.path.join(urdfRootPath, "random_urdfs/002/002.urdf"), basePosition=state_object)
self.observation, _ = self.get_obs()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
return self.observation
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=True,
arm='rbx1',
vr=False):
print("init")
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._vr = vr
self.terminated = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
cid = p.connect(p.GUI)
if self._vr:
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
else:
p.connect(p.DIRECT)
self._seed()
self._arm_str = arm
self._reset()
if self._vr:
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setRealTimeSimulation(1)
observationDim = len(self.getSceneObservation())
observation_high = np.array([np.finfo(np.float32).max] *
observationDim)
self.action_space = spaces.Discrete(7)
self.observation_space = spaces.Box(-observation_high,
observation_high)
self.viewer = None
def reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1])
p.loadURDF(
os.path.join(self._urdfRoot, "table/table.urdf"),
0.5000000,
0.00000,
-0.820000,
0.000000,
0.000000,
0.0,
1.0,
)
xpos = 0.55 + self.bias_obj_x + 0.12 * random.random() * self.rnd_obj_x
ypos = 0 + self.bias_obj_y + 0.2 * random.random() * self.rnd_obj_y
ang = (3.14 * 0.5 + self.bias_obj_ang +
3.1415925438 * random.random() * self.rnd_obj_ang)
orn = p.getQuaternionFromEuler([0, 0, ang])
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 = CustomKuka(
jp_override=self.jp_override,
urdfRootPath=self._urdfRoot,
timeStep=self._timeStep,
)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def __init__(self, render=False):
self._observation = []
self._episode_count = 0
self.action_space = spaces.Discrete(
144) # 12 joints, each with 12 angles
# 24 parameters to observe
# pos , 3
# orn , 3
# linear, 3
# angular, 3
# joints , 12
inf = 999
self.observation_space = spaces.Box(np.array([-inf] * 24),
np.array([inf] * 24))
if (render):
self.physicsClient = p.connect(p.GUI)
else:
self.physicsClient = p.connect(p.DIRECT) # non-graphical version
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # used by loadURDF
self._seed()
p.resetSimulation()
self._maxJointForce = 1.96 # 1.96Nm = 20Kg.cm
cameraDistance = 1.2
cameraPitch = -35.4
cameraYaw = 26.8
# cameraTargetPosition=[0.4,0.2,-0.2]
cameraTargetPosition = [0.1, 0.07, -0.16]
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch,
cameraTargetPosition)
p.resetSimulation()
p.setGravity(0, 0, -9.8) # m/s^2
p.setTimeStep(0.01) # sec
planeId = p.loadURDF("plane.urdf")
path = os.path.abspath(os.path.dirname(__file__))
cubeStartPos = [0, 0, 0.28]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
self.botId = p.loadURDF(
"/home/bb8/robot/git/robot_dog/urdf/quadruped.urdf", cubeStartPos,
cubeStartOrientation)
self._stateId = p.saveState()
def __init__(self, config, rendering_config):
p.resetSimulation()
obj_dir = os.path.dirname(__file__)
obj_dir = os.path.join(obj_dir, "../shapes")
self.obj_dir = os.path.realpath(obj_dir)
self.objects = config
self.camera = self.set_camera(rendering_config)
self.object_ids = []
for obj_params in self.objects:
self.object_ids.append(self.add_object(**obj_params))
vis_id = p.createVisualShape(
p.GEOM_MESH,
fileName=os.path.join(self.obj_dir, 'ground.obj'),
meshScale=[10, 10, 10],
rgbaColor=[*(x / 256 for x in [150, 150, 150]), 1])
self.ground_id = p.createMultiBody(baseVisualShapeIndex=vis_id,
basePosition=[0, 0, 0])
def __init__(self, cfg):
"""
Input: cfg contains the custom configuration of the environment
cfg.tacto
cfg.settings
show_gui=False,
dt=0.005,
action_frequency=30,
simulation_frequency=240,
max_episode_steps=500,
"""
# Init logger
self.logger = logging.getLogger(__name__)
# Init logic parameters
self.cfg = cfg
self.difficulty = cfg.settings.difficulty
self.show_gui = cfg.settings.show_gui
self.dt = cfg.settings.dt
self.action_frequency = cfg.settings.action_frequency
self.simulation_frequency = cfg.settings.simulation_frequency
self.max_episode_steps = cfg.settings.max_episode_steps
self.elapsed_steps = 0
# Set interaction parameters
self.action_space = self.get_action_space()
self.observation_space = self.get_observation_space()
# Init environment
self.logger.info("Initializing world")
mode = p.GUI if self.show_gui else p.DIRECT
client_id = px.init(mode=mode)
self.physics_client = px.Client(client_id=client_id)
p.resetDebugVisualizerCamera(**cfg.pybullet_camera)
p.setTimeStep(1/self.simulation_frequency)
p.resetSimulation()
if self.show_gui:
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) # Disable rendering during setup
self.load_objects()
if self.show_gui:
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0,0,-9.8)
def connect(self, gpu_renderer=True, gui=False):
assert not self._connected, 'Already connected'
if gui:
self._client_id = pb.connect(pb.GUI, '--width=640 --height=480')
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI,
1,
physicsClientId=self._client_id)
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING,
1,
physicsClientId=self._client_id)
pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER,
0,
physicsClientId=self._client_id)
else:
self._client_id = pb.connect(pb.DIRECT)
if self._client_id < 0:
raise Exception('Cannot connect to pybullet')
if gpu_renderer and not gui:
os.environ['MESA_GL_VERSION_OVERRIDE'] = '3.3'
os.environ['MESA_GLSL_VERSION_OVERRIDE'] = '330'
# Get EGL device
#assert 'CUDA_VISIBLE_DEVICES' in os.environ
devices = [0] #os.environ.get('CUDA_VISIBLE_DEVICES', ).split(',')
assert len(devices) == 1
out = subprocess.check_output([
'nvidia-smi', '--id=' + str(devices[0]), '-q', '--xml-format'
])
tree = ET.fromstring(out)
gpu = tree.findall('gpu')[0]
dev_id = gpu.find('minor_number').text
#os.environ['EGL_VISIBLE_DEVICES'] = str(dev_id)
egl = pkgutil.get_loader('eglRenderer')
pb.loadPlugin(egl.get_filename(),
"_eglRendererPlugin",
physicsClientId=self._client_id)
pb.resetSimulation(physicsClientId=self._client_id)
self._connected = True
self._client = BulletClient(self._client_id)
self.client.setPhysicsEngineParameter(numSolverIterations=50)
self.client.setPhysicsEngineParameter(
fixedTimeStep=self._simulation_step)
self.client.setGravity(0, 0, -9.8)
def _reset_world(self):
"""
resets the world itself by resetting the simulation too.
:return:
"""
if self._pybullet_client_full_id is not None:
pybullet.resetSimulation(
physicsClientId=self._pybullet_client_full_id)
pybullet.setPhysicsEngineParameter(
deterministicOverlappingPairs=1,
physicsClientId=self._pybullet_client_full_id)
if self._pybullet_client_w_o_goal_id is not None:
pybullet.resetSimulation(
physicsClientId=self._pybullet_client_w_o_goal_id)
pybullet.setPhysicsEngineParameter(
deterministicOverlappingPairs=1,
physicsClientId=self._pybullet_client_w_o_goal_id)
return
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
p.setGravity(0,0,-10)
self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
p.setGravity(0,0,-10)
self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def _reset(self):
bullet.resetSimulation()
bullet.setPhysicsEngineParameter(numSolverIterations=150,
fixedTimeStep=1 / 30)
bullet.setTimeStep(self._timeStep)
bullet.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, 0])
bullet.setGravity(0, 0, -10)
self._kuka.reset()
self._envStepCounter = 0
bullet.stepSimulation()
self.terminated = 0
# Sample a new random goal pose
if self._goalReset or self.goal is None:
self.goal = self.getNewGoal()
self._observation = self.getExtendedObservation()
return self._observation
def __enter__(self):
print("connecting")
optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
cid = pybullet.connect(self.connection_mode,
options=optionstring,
*self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf", [0, 0, -1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0, 0, -10)
def reset(self):
if not self.physics_connected:
# start physics client
if self.render:
self.physics_client = p.connect(p.GUI)
else:
self.physics_client = p.connect(p.DIRECT)
self.physics_connected = True
# add search paths from pybullet for e.g. plane.urdf
p.setAdditionalSearchPath(pybullet_data.getDataPath())
else:
p.resetSimulation()
self.create_rocket()
obs = self.get_obs()
return obs
def reset(self):
self._is_successful_grasp = False
self._terminated = False
self._attempted_grasp = False
self._env_step_counter = 0
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, lightPosition=[0, 0, 10])
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._time_step)
table_id = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), useFixedBase=True,
basePosition=self._table_pos, baseOrientation=self._table_orn)
self._table_workspace_shape = self._compute_workspace(table_id)
robot_base_pos = [self._robot_start_x_offset, 0, self._get_table_height()]
if self._draw_workspace:
self._draw_workspace_limits()
self._panda = PandaEnv(robot_base_pos, self._urdf_root, time_step=self._time_step,
use_ik=self._use_ik, num_controlled_joints=self._num_controlled_joints)
pos = self._get_random_position_on_table()
if self._lock_rotation:
orn = p.getQuaternionFromEuler([0, 0, 0])
else:
orn = self._get_random_z_orientation()
self._target_object_id = p.loadURDF(os.path.join(self._urdf_root, "cube/cube.urdf"),
basePosition=pos, baseOrientation=orn, useFixedBase=False,
globalScaling=.5)
self._goal_position = np.add(pos, [0, 0, self.lift_distance])
p.setGravity(0, 0, -9.8)
p.stepSimulation()
self._observation = self.get_observation()
return self._observation
def reset(self, gravity):
self.episode_counter += 1
self.step_counter = 0
p.resetSimulation()
p.setGravity(0, 0, -gravity)
p.resetDebugVisualizerCamera(5, 45, -20, [0, 0, 0])
self.visual_shape = p.createVisualShape(p.GEOM_BOX,
halfExtents=[10, 10, 0.1])
self.coll_shape = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[10, 10, 0.1])
print(self.episode_counter)
self.planeId = p.loadURDF(
"/Users/caspa/VSCode/RSI_code/experiments/robots/terrain.urdf",
[0, 0, 0])
self.robotId = p.loadURDF(
"/Users/caspa/VSCode/RSI_code/experiments/robots/version1.urdf",
[0, 0, 0.3])
self.observation = np.array([0] * 12)
return self.observation
def reset(self):
self.step_counter = 0
p.resetSimulation()
# p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) # we will enable rendering after we loaded everything
urdfRootPath=pybullet_data.getDataPath()
p.setGravity(0,0,-10)
self.ur5 = self.load_robot()
rest_poses = [0, -math.pi, -math.pi/2, -math.pi/2, -math.pi/2, 0]
for i in range(6):
p.resetJointState(self.ur5,i, rest_poses[i])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
obs = self._get_obs()
return obs
def configure_pybullet(rendering=False,
debug=False,
yaw=50.0,
pitch=-35.0,
dist=1.2,
target=(0.0, 0.0, 0.0)):
if not rendering:
p.connect(p.DIRECT)
else:
p.connect(p.GUI_SERVER)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
if not debug:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
reset_camera(yaw=yaw, pitch=pitch, dist=dist, target=target)
p.setPhysicsEngineParameter(enableFileCaching=0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True)
p.resetSimulation()
p.setRealTimeSimulation(0)
p.setGravity(0, 0, -9.8)
def __enter__(self):
print("connecting")
optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring,*self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0,0,-10)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def _reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
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.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])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def _reset(self):
"""Environment reset called at the beginning of an episode.
"""
# Set the camera settings.
look = [0.23, 0.2, 0.54]
distance = 1.
pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3)
yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3)
roll = 0
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
look, distance, yaw, pitch, roll, 2)
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
aspect = self._width / self._height
near = 0.01
far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
self._attempted_grasp = False
self._env_step = 0
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# Choose the objects in the bin.
urdfList = self._get_random_object(
self._numObjects, self._isTest)
self._objectUids = self._randomly_place_objects(urdfList)
self._observation = self._get_observation()
return np.array(self._observation)
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
minitaur = Minitaur()
amplitude = 0.24795664427
speed = 0.2860877729434
for i in range(1000):
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
minitaur.applyAction(joint_values)
p.stepSimulation()
# print(minitaur.getBasePosition())
time.sleep(timeStep)
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
print(final_distance)
import pybullet as p
import time
p.connect(p.GUI)
logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS,"commandLog.bin")
p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf",[0,0,1])
p.stopStateLogging(logId)
p.resetSimulation();
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS,"commandLog.bin")
while(p.isConnected()):
time.sleep(1./240.)
import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
stop = time.time()
print ("renderImage %f" % (stop - start))
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
#print(rgb)
print ('width = %d height = %d' % (w,h))
#note that sending the data using imshow to matplotlib is really slow, so we use set_data
#plt.imshow(rgb,interpolation='none')
#reshape is needed
np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr*(1./255.)
image.set_data(np_img_arr)
ax.plot([0])
#plt.draw()
#plt.show()
plt.pause(0.01)
main_stop = time.time()
print ("Total time %f" % (main_stop - main_start))
pybullet.resetSimulation()
def clean_everything(self): p.resetSimulation() p.setGravity(0, 0, -self.gravity) p.setDefaultContactERP(0.9) p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip)
import time
from pybullet_utils import urdfEditor
##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb,physicsClientId=org)):
p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
# print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))