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