def setupWorld(): p.resetSimulation() p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) 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 _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)
import pybullet as p import time import pybullet_data p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(numSolverIterations=5) p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.) p.setPhysicsEngineParameter(numSubSteps=1) p.loadURDF("plane.urdf") objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") ob = objects[0] p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124], [0.710965, 0.218117, 0.519402, -0.420923]) jointPositions = [ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ] for jointIndex in range(p.getNumJoints(ob)): p.resetJointState(ob, jointIndex, jointPositions[jointIndex]) #first let the humanoid fall #p.setRealTimeSimulation(1) #time.sleep(5) p.setRealTimeSimulation(0) #p.saveWorld("lyiing.py")
import pybullet as p import time p.connect(p.GUI) p.loadURDF("plane.urdf",useMaximalCoordinates=True) p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True) gravXid = p.addUserDebugParameter("gravityX",-10,10,0) gravYid = p.addUserDebugParameter("gravityY",-10,10,0) gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10) p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) for i in range (10): for j in range (10): for k in range (5): ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) while True: gravX = p.readUserDebugParameter(gravXid) gravY = p.readUserDebugParameter(gravYid) gravZ = p.readUserDebugParameter(gravZid) p.setGravity(gravX,gravY,gravZ) time.sleep(0.01)
import pybullet as p import time p.connect(p.GUI) p.setGravity(0, 0, -10) p.setPhysicsEngineParameter(enableSAT=1) p.loadURDF("cube_concave.urdf", [0, 0, -25], globalScaling=50, useFixedBase=True, flags=p.URDF_INITIALIZE_SAT_FEATURES) p.loadURDF("cube.urdf", [0, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES) p.loadURDF("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES) while (p.isConnected()): p.stepSimulation() pts = p.getContactPoints() #print("num contacts = ", len(pts)) time.sleep(1. / 240.)
import pybullet as p import time import math cid = p.connect(p.SHARED_MEMORY) if (cid < 0): p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setTimeStep(1. / 120.) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json") #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) p.loadURDF("plane100.urdf", useMaximalCoordinates=True) #disable rendering during creation. p.setPhysicsEngineParameter(contactBreakingThreshold=0.04) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) #disable tinyrenderer, software (CPU) renderer, we don't use it here p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0) shift = [0, -0.02, 0] meshScale = [0.1, 0.1, 0.1] vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000], [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
def birrt_smoothing(): ################################################################ # TODO your code to implement the birrt algorithm with smoothing ################################################################ pass if __name__ == "__main__": args = get_args() # set up simulator physicsClient = p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(enableFileCaching=0) p.setGravity(0, 0, -9.8) p.configureDebugVisualizer(p.COV_ENABLE_GUI, False) p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, True) p.resetDebugVisualizerCamera(cameraDistance=1.400, cameraYaw=58.000, cameraPitch=-42.200, cameraTargetPosition=(0.0, 0.0, 0.0)) # load objects plane = p.loadURDF("plane.urdf") ur5 = p.loadURDF('assets/ur5/ur5.urdf', basePosition=[0, 0, 0.02], useFixedBase=True) obstacle1 = p.loadURDF('assets/block.urdf', basePosition=[1 / 4, 0, 1 / 2],
def setup_simulation(self, gui=False, easy_bookcases=False, clientId=None): ''' Initializes the simulation by setting up the environment and spawning all objects used later. Params: gui (bool): Specifies if a GUI should be spawned. ''' # Setup simulation parameters if clientId is None: mode = pb.GUI if gui else pb.DIRECT self.clientId = pb.connect(mode) else: self.clientId = clientId pb.setGravity(0.0, 0.0, 0.0, self.clientId) pb.setPhysicsEngineParameter(fixedTimeStep=self.p["world"]["tau"], physicsClientId=self.clientId) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) pb.setPhysicsEngineParameter(enableFileCaching=0) # Setup humans self.humans = [ Human(self.clientId, self.tau) for _ in range(self.p['world']['n_humans']) ] # Spawn robot self.robotId = self.spawn_robot() # Spawn setpoint self.spId = self.spawn_setpoint() # Spawn all objects in the environment self.additionalIds = self.spawn_additional_objects() # Enable collision of base and all objects # for id in self.additionalIds: # pb.setCollisionFilterPair(self.robotId, id, -1, -1, True, # self.clientId) # Spawn bookcases self.spawn_kallax() # Figure out joint mapping: self.joint_mapping maps as in # desired_mapping list. self.joint_mapping = np.zeros(7, dtype=int) self.link_mapping = np.zeros(self.n_links, dtype=int) self.joint_limits = np.zeros((7, 2), dtype=float) self.eeLinkId = None self.baseLinkId = None self.lidarLinkId1 = None self.lidarLinkId2 = None joint_names = ["panda_joint{}".format(x) for x in range(1, 8)] link_names = self.p["joints"]["link_names"] for j in range( pb.getNumJoints(self.robotId, physicsClientId=self.clientId)): info = pb.getJointInfo(self.robotId, j, physicsClientId=self.clientId) j_name, l_name = info[1].decode("utf-8"), info[12].decode("utf-8") idx = info[0] if j_name in joint_names: map_idx = joint_names.index(j_name) self.joint_mapping[map_idx] = idx self.joint_limits[map_idx, :] = info[8:10] if l_name in link_names: self.link_mapping[link_names.index(l_name)] = idx if l_name == self.p["joints"]["ee_link_name"]: self.eeLinkId = idx if l_name == self.p["joints"]["base_link_name"]: self.baseLinkId = idx if l_name == self.p["sensors"]["lidar"]["link_id1"]: self.lidarLinkId1 = idx if l_name == self.p["sensors"]["lidar"]["link_id2"]: self.lidarLinkId2 = idx for j in range( pb.getNumJoints(self.spId, physicsClientId=self.clientId)): info = pb.getJointInfo(self.spId, j, physicsClientId=self.clientId) link_name = info[12].decode("utf-8") idx = info[0] if link_name == "grasp_loc": self.spGraspLinkId = idx self.actuator_selection = np.zeros(7, bool) for i, name in enumerate(joint_names): if name in self.p["joints"]["joint_names"]: self.actuator_selection[i] = 1 # Prepare lidar n_scans = self.p["sensors"]["lidar"]["n_scans"] mag_ang = self.p["sensors"]["lidar"]["ang_mag"] scan_range = self.p["sensors"]["lidar"]["range"] angs = ((np.array(range(n_scans)) - (n_scans - 1) / 2.0) * 2.0 / n_scans * mag_ang) r_uv = np.vstack((np.cos(angs), np.sin(angs), np.zeros(angs.shape[0]))) r_from = r_uv * 0.1 r_to = r_uv * scan_range self.rays = (r_from, r_to) for human in self.humans: self.configure_ext_collisions(human.leg_l, self.robotId, self.collision_links) self.configure_ext_collisions(human.leg_r, self.robotId, self.collision_links)
p.connect(p.GUI) useMaximalCoordinates = False p.setGravity(0,0,-10) plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates) p.setRealTimeSimulation(0) velocity = 1 num = 40 p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.setPhysicsEngineParameter(enableConeFriction=1) for i in range (num): print("progress:",i,num) x = velocity*math.sin(2.*3.1415*float(i)/num) y = velocity*math.cos(2.*3.1415*float(i)/num) print("velocity=",x,y) sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates) p.changeDynamics(sphere,-1,lateralFriction=0.02) #p.changeDynamics(sphere,-1,rollingFriction=10) p.changeDynamics(sphere,-1,linearDamping=0) p.changeDynamics(sphere,-1,angularDamping=0) p.resetBaseVelocity(sphere,linearVelocity=[x,y,0]) prevPos = [0,0,0]
import pybullet as p import time p.connect(p.DIRECT) p.setGravity(0,0,-10) p.setPhysicsEngineParameter(numSolverIterations=5) p.setPhysicsEngineParameter(fixedTimeStep=1./240.) p.setPhysicsEngineParameter(numSubSteps=1) objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") ob = objects[0] p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000]) ob = objects[1] p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923]) jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ] for jointIndex in range (p.getNumJoints(ob)): p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) #first let the humanoid fall #p.setRealTimeSimulation(1) #time.sleep(5) p.setRealTimeSimulation(0) #p.saveWorld("lyiing.py") #now do a benchmark print("Starting benchmark") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json") for i in range(1000): p.stepSimulation() p.stopStateLogging(logId)
import pybullet as p import time p.connect(p.GUI) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.loadURDF("plane.urdf") radius = 0.025 distance = 1.86 yaw=135 pitch=-11 targetPos=[0,0,0] p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200) p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos) objectId = -1 for i in range (10): objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius]) p.changeDynamics(objectId,-1,100) timeStep = 1./240. p.setGravity(0,0,-10) while (p.isConnected()): p.stepSimulation() time.sleep(timeStep)
restitutionId = p.addUserDebugParameter("restitution", 0, 1, 1) restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold", 0, 3, 0.2) lateralFrictionId = p.addUserDebugParameter("lateral friction", 0, 1, 0.5) spinningFrictionId = p.addUserDebugParameter("spinning friction", 0, 1, 0.03) rollingFrictionId = p.addUserDebugParameter("rolling friction", 0, 1, 0.03) plane = p.loadURDF("plane_with_restitution.urdf") sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2]) p.setRealTimeSimulation(1) p.setGravity(0, 0, -10) while (1): restitution = p.readUserDebugParameter(restitutionId) restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId) p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold) lateralFriction = p.readUserDebugParameter(lateralFrictionId) spinningFriction = p.readUserDebugParameter(spinningFrictionId) rollingFriction = p.readUserDebugParameter(rollingFrictionId) p.changeDynamics(plane, -1, lateralFriction=1) p.changeDynamics(sphere, -1, lateralFriction=lateralFriction) p.changeDynamics(sphere, -1, spinningFriction=spinningFriction) p.changeDynamics(sphere, -1, rollingFriction=rollingFriction) p.changeDynamics(plane, -1, restitution=restitution) p.changeDynamics(sphere, -1, restitution=restitution) pos, orn = p.getBasePositionAndOrientation(sphere) #print("pos=") #print(pos) time.sleep(0.01)
import pybullet as p import time p.connect(p.GUI) fileIO = p.loadPlugin("fileIOPlugin") if (fileIO>=0): p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO]) objs= p.loadSDF("pickup/model.sdf") dobot =objs[0] p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1]) else: print("fileIOPlugin is disabled.") p.setPhysicsEngineParameter(enableFileCaching=False) while (1): p.stepSimulation() time.sleep(1./240.)
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)
def run_simulation( hand_verts, hand_faces, obj_verts, obj_faces, simulation_step=1 / 240, num_iterations=35, object_friction=3, hand_friction=3, hand_restitution=0, object_restitution=0.5, object_mass=1, verbose=False, vhacd_resolution=1000, vhacd_exe='/home/ecorona/GHANds/v-hacd/bin/linux/testVHACD', wait_time=0, save_video=False, save_video_path=None, save_hand_path=None, save_obj_path=None, save_simul_folder=None, use_gui=False, ): if use_gui: conn_id = p.connect(p.GUI) else: conn_id = p.connect(p.DIRECT) hand_indicies = hand_faces.flatten().tolist() p.resetSimulation(physicsClientId=conn_id) p.setPhysicsEngineParameter(enableFileCaching=0, physicsClientId=conn_id) p.setPhysicsEngineParameter(numSolverIterations=150, physicsClientId=conn_id) p.setPhysicsEngineParameter(fixedTimeStep=simulation_step, physicsClientId=conn_id) p.setGravity(0, 9.8, 0, physicsClientId=conn_id) # add hand base_tmp_dir = "tmp/objs" os.makedirs(base_tmp_dir, exist_ok=True) hand_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir) save_obj(hand_tmp_fname, hand_verts, hand_faces) if save_hand_path is not None: shutil.copy(hand_tmp_fname, save_hand_path) hand_collision_id = p.createCollisionShape( p.GEOM_MESH, fileName=hand_tmp_fname, flags=p.GEOM_FORCE_CONCAVE_TRIMESH, indices=hand_indicies, physicsClientId=conn_id, ) hand_visual_id = p.createVisualShape( p.GEOM_MESH, fileName=hand_tmp_fname, rgbaColor=[0, 0, 1, 1], specularColor=[0, 0, 1], physicsClientId=conn_id, ) hand_body_id = p.createMultiBody( baseMass=0, baseCollisionShapeIndex=hand_collision_id, baseVisualShapeIndex=hand_visual_id, physicsClientId=conn_id, ) p.changeDynamics( hand_body_id, -1, lateralFriction=hand_friction, restitution=hand_restitution, physicsClientId=conn_id, ) obj_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir) os.makedirs(base_tmp_dir, exist_ok=True) # Save object obj if save_obj_path is not None: final_obj_tmp_fname = tempfile.mktemp(suffix=".obj", dir=base_tmp_dir) save_obj(final_obj_tmp_fname, obj_verts, obj_faces) shutil.copy(final_obj_tmp_fname, save_obj_path) # Get obj center of mass obj_center_mass = np.mean(obj_verts, axis=0) obj_verts -= obj_center_mass # add object use_vhacd = True if use_vhacd: if verbose: print("Computing vhacd decomposition") time1 = time.time() # convex hull decomposition save_obj(obj_tmp_fname, obj_verts, obj_faces) if not vhacd(obj_tmp_fname, vhacd_exe, resolution=vhacd_resolution): raise RuntimeError("Cannot compute convex hull " "decomposition for {}".format(obj_tmp_fname)) else: print("Succeeded vhacd decomp") obj_collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=obj_tmp_fname, physicsClientId=conn_id) if verbose: time2 = time.time() print("Computed v-hacd decomposition at res {} {:.6f} s".format( vhacd_resolution, (time2 - time1))) else: obj_collision_id = p.createCollisionShape(p.GEOM_MESH, vertices=obj_verts, physicsClientId=conn_id) obj_visual_id = p.createVisualShape( p.GEOM_MESH, fileName=obj_tmp_fname, rgbaColor=[1, 0, 0, 1], specularColor=[1, 0, 0], physicsClientId=conn_id, ) obj_body_id = p.createMultiBody( baseMass=object_mass, basePosition=obj_center_mass, baseCollisionShapeIndex=obj_collision_id, baseVisualShapeIndex=obj_visual_id, physicsClientId=conn_id, ) p.changeDynamics( obj_body_id, -1, lateralFriction=object_friction, restitution=object_restitution, physicsClientId=conn_id, ) # simulate for several steps if save_video: images = [] if use_gui: renderer = p.ER_BULLET_HARDWARE_OPENGL else: renderer = p.ER_TINY_RENDERER for step_idx in range(num_iterations): p.stepSimulation(physicsClientId=conn_id) if save_video: img = take_picture(renderer, conn_id=conn_id) images.append(img) if save_simul_folder: hand_step_path = os.path.join(save_simul_folder, "{:08d}_hand.obj".format(step_idx)) shutil.copy(hand_tmp_fname, hand_step_path) obj_step_path = os.path.join(save_simul_folder, "{:08d}_obj.obj".format(step_idx)) pos, orn = p.getBasePositionAndOrientation(obj_body_id, physicsClientId=conn_id) mat = np.reshape(p.getMatrixFromQuaternion(orn), (3, 3)) obj_verts_t = pos + np.dot(mat, obj_verts.T).T save_obj(obj_step_path, obj_verts_t, obj_faces) time.sleep(wait_time) if save_video: write_video(images, save_video_path) print("Saved gif to {}".format(save_video_path)) pos_end = p.getBasePositionAndOrientation(obj_body_id, physicsClientId=conn_id)[0] #if use_vhacd: # os.remove(obj_tmp_fname) if save_obj_path is not None: os.remove(final_obj_tmp_fname) os.remove(hand_tmp_fname) distance = np.linalg.norm(pos_end - obj_center_mass) p.disconnect(physicsClientId=conn_id) return distance
p.resetJointState(robot, joint_id["r_leg_kny"], np.pi / 2, 0.) # ankle p.resetJointState(robot, joint_id["l_leg_aky"], -np.pi / 4, 0.) p.resetJointState(robot, joint_id["r_leg_aky"], -np.pi / 4, 0.) if __name__ == "__main__": # Environment Setup p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=120, cameraPitch=-30, cameraTargetPosition=[1, 0.5, 1.5]) p.setGravity(0, 0, -9.8) p.setPhysicsEngineParameter(fixedTimeStep=SimConfig.CONTROLLER_DT, numSubSteps=SimConfig.N_SUBSTEP) if SimConfig.VIDEO_RECORD: if not os.path.exists('video'): os.makedirs('video') for f in os.listdir('video'): os.remove('video/' + f) p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "video/atlas.mp4") # Create Robot, Ground p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF( cwd + "/robot_model/atlas/atlas.urdf", SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT, SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT) p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0])
from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) import pybullet_data p.setTimeOut(10000) useMotionCapture=False useMotionCaptureReset=False#not useMotionCapture useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #explicit PD control requires small timestep timeStep = 1./600. #timeStep = 1./240. p.setPhysicsEngineParameter(fixedTimeStep=timeStep) path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" #path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt" #path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt" #p.loadURDF("plane.urdf",[0,0,-1.03]) print("path = ", path)
def reset(self): look = [1.9, 0.5, 1] roll = -10 pitch = -35 yaw = 110 look_2 = [-0.3, 0.5, 1.3] pitch_2 = -56 yaw_2 = 245 roll_2 = 0 distance = 1. self._view_matrix_2 = p.computeViewMatrixFromYawPitchRoll( look_2, distance, yaw_2, pitch_2, roll_2, 2) 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) # counter and flag self.goal_rotation_angle = 0 self._env_step = 0 self.terminated = 0 self.finger_angle = 0.3 self._success = False self.out_of_range = False self._collision_box = False self.drop_down = False self._rank_before = 0 self.inverse_rank = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) self._planeUid = p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1]) self._tableUid = p.loadURDF( os.path.join(self._urdfRoot, "table/table.urdf"), [0.5000000, 0.00000, -.820000], p.getQuaternionFromEuler(np.radians([0, 0, 90.]))) 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, test=self._isTest) self._objectUids = self._randomly_place_objects(urdfList) self._init_obj_high = self._obj_high() self._rank_before = self._rank_1() self._domain_random() return self._get_observation()
#add parent dir to find package. Only needed for source code build, pip install doesn't need it. import os import 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 pybullet_data import pybullet as p import time p.connect(p.GUI_SERVER) p.setAdditionalSearchPath(pybullet_data.getDataPath()) while(1): #this is a no-op command, to allow GUI updates on Mac OSX (main thread) p.setPhysicsEngineParameter() time.sleep(0.01)
import pybullet as p import time p.connect(p.GUI) p.setPhysicsEngineParameter(allowedCcdPenetration=0.0) terrain_mass=0 terrain_visual_shape_id=-1 terrain_position=[0,0,0] terrain_orientation=[0,0,0,1] terrain_collision_shape_id = p.createCollisionShape( shapeType=p.GEOM_MESH, fileName="terrain.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, meshScale=[0.5, 0.5, 0.5]) p.createMultiBody( terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id, terrain_position, terrain_orientation) useMaximalCoordinates = True sphereRadius = 0.005 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) mass = 1 visualShapeId = -1 for i in range (5):
def reset(self): self.terminated = False p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timestep) p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, 0]) p.setGravity(0, 0, -10) # Init the robot randomly x_start = self._max_x / 2 + self.np_random.uniform( -self._max_x / 3, self._max_x / 3) y_start = self._max_y / 2 + self.np_random.uniform( -self._max_y / 3, self._max_y / 3) self.robot_pos = np.array([x_start, y_start, 0]) # Initialize target position x_pos = 0.9 * self._max_x y_pos = self._max_y * 3 / 4 if self._random_target: margin = 0.1 * self._max_x x_pos = self.np_random.uniform(self._min_x + margin, self._max_x - margin) y_pos = self.np_random.uniform(self._min_y + margin, self._max_y - margin) self.target_uid = p.loadURDF("/urdf/cylinder.urdf", [x_pos, y_pos, 0], useFixedBase=True) self.target_pos = np.array([x_pos, y_pos, 0]) # Add walls # Path to the urdf file wall_urdf = "/urdf/wall.urdf" # Rgba colors red, green, blue = [0.8, 0, 0, 1], [0, 0.8, 0, 1], [0, 0, 0.8, 1] wall_left = p.loadURDF(wall_urdf, [self._max_x / 2, 0, 0], useFixedBase=True) # Change color p.changeVisualShape(wall_left, -1, rgbaColor=red) # getQuaternionFromEuler -> define orientation wall_bottom = p.loadURDF(wall_urdf, [self._max_x, self._max_y / 2, 0], p.getQuaternionFromEuler([0, 0, np.pi / 2]), useFixedBase=True) wall_right = p.loadURDF(wall_urdf, [self._max_x / 2, self._max_y, 0], useFixedBase=True) p.changeVisualShape(wall_right, -1, rgbaColor=green) wall_top = p.loadURDF(wall_urdf, [self._min_x, self._max_y / 2, 0], p.getQuaternionFromEuler([0, 0, np.pi / 2]), useFixedBase=True) p.changeVisualShape(wall_top, -1, rgbaColor=blue) self.walls = [wall_left, wall_bottom, wall_right, wall_top] # Add mobile robot self.robot_uid = p.loadURDF(os.path.join(self._urdf_root, "racecar/racecar.urdf"), self.robot_pos, useFixedBase=True) self._env_step_counter = 0 for _ in range(50): p.stepSimulation() self._observation = self.getObservation() if self.saver is not None: self.saver.reset(self._observation, self.getTargetPos(), self.getRobotPos()) if self.srl_model != "raw_pixels": return self.getSRLState(self._observation) return np.array(self._observation)
maxMotorForce = 5000 maxGearForce = 10000 jointFriction = 0.1 p.connect(p.GUI) amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5) timeScaleId = p.addUserDebugParameter("timeScale",0,10,1) jointTypeNames={} jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE" jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED" p.setPhysicsEngineParameter(numSolverIterations=100) p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True) #disable rendering during creation. #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) jointNamesToIndex={} p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) for j in range(p.getNumJoints(vision)):
def generate_dataset(self): for num_brick in tqdm( range(self.min_num_brick, self.max_num_brick + 1), "Number of Brick Progress", ): # separate dataset for GN-based physics engine (GN Physics) dataset_name = self.dataset_prefix + "_" + str(num_brick) nodes_train, nodes_eval = [], [] edges_train, edges_eval = [], [] node_feats_train, node_feats_eval = [], [] edge_feats_train, edge_feats_eval = [], [] split = int(self.num_episode * self.train_eval_split) for episode in tqdm(range(self.num_episode), "Episode Progress"): if episode < split: nodes_dummy = nodes_train edges_dummy = edges_train node_feats_dummy = node_feats_train edge_feats_dummy = edge_feats_train episode_dummy = copy.deepcopy(episode) else: nodes_dummy = nodes_eval edges_dummy = edges_eval node_feats_dummy = node_feats_eval edge_feats_dummy = edge_feats_eval episode_dummy = copy.deepcopy(episode) - split # use test_simulator.py for debugging and visualization p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1.0 / 60.0) # generate new design self.designer.clear() self.designer.generate_design(num_brick=num_brick) for b in self.designer.built: pos = b.anchor rot = p.getQuaternionFromEuler( np.array(b.rotation) * np.pi / 2) p.loadURDF("urdf/brick.urdf", pos, rot) p.loadURDF("plane.urdf", useMaximalCoordinates=True) # start simulation p.setGravity(0, 0, -10) for frame in range(self.num_frame): p.stepSimulation() time.sleep(1.0 / 240.0) # remember the ground! for objectID in range(num_brick + 1): pos, ort = p.getBasePositionAndOrientation(objectID) rot = p.getEulerFromQuaternion(ort) vel_linear, vel_angular = p.getBaseVelocity(objectID) if objectID < num_brick: mass = 1.0 else: mass = 9999999.0 nodes_dummy.append([episode_dummy, frame, objectID]) node_feats_dummy.append(pos + rot + vel_linear + vel_angular + (mass, )) contact_list = p.getContactPoints() for contact in contact_list: edges_dummy.append( [episode_dummy, frame, contact[1], contact[2]]) edge_feats_dummy.append(contact[5] # positionA + contact[6] # positionB + ( contact[9], # normalForce contact[10], # friction1 contact[12], # friction2 )) p.disconnect() np.savez( self.save_dir / str(dataset_name + "_TRAIN"), nodes=np.array(nodes_train), node_feats=np.array(node_feats_train), edges=np.array(edges_train), edge_feats=np.array(edge_feats_train), ) np.savez( self.save_dir / str(dataset_name + "_EVAL"), nodes=np.array(nodes_eval), node_feats=np.array(node_feats_eval), edges=np.array(edges_eval), edge_feats=np.array(edge_feats_eval), )
import sys import time import numpy as np import pybullet as p from scipy.spatial.transform import Rotation as R from env.robot import Manipulator from env.work import Work if __name__ == '__main__': p.connect(p.GUI) p.setPhysicsEngineParameter(enableFileCaching=0) p.setRealTimeSimulation(False) p.setGravity(0, 0, -9.8) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) # Plane plane_pos = [0, 0, -0.1] p.loadURDF("urdf/plane/plane.urdf", plane_pos) robot_base_pose = [0, 0, 0, 0, 0, 0] #robot_tool_pose = [0, 0, 0, -0.3, 0.4, 0.2] robot_tool_pose = [0, 0, -0.1, 0.0, 0.0, 0.0] robot = Manipulator(tool_pose=robot_tool_pose, base_pose=robot_base_pose) # Reset joint position basic_joint = np.array([ 0.0, -0.5 * np.pi, 1.0 * np.pi, 0.0 * np.pi, 0.5 * np.pi, 0.0 * np.pi ])
def __init__(self, assets_root, task=None, disp=False, shared_memory=False, hz=240): """Creates OpenAI Gym-style environment with PyBullet. Args: assets_root: root directory of assets. task: the task to use. If None, the user must call set_task for the environment to work properly. disp: show environment with PyBullet's built-in display viewer. shared_memory: run with shared memory. hz: PyBullet physics simulation step speed. Set to 480 for deformables. Raises: RuntimeError: if pybullet cannot load fileIOPlugin. """ self.pix_size = 0.003125 self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []} self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi self.agent_cams = cameras.RealSenseD415.CONFIG self.assets_root = assets_root color_tuple = [ gym.spaces.Box(0, 255, config['image_size'] + (3, ), dtype=np.uint8) for config in self.agent_cams ] depth_tuple = [ gym.spaces.Box(0.0, 20.0, config['image_size'], dtype=np.float32) for config in self.agent_cams ] self.observation_space = gym.spaces.Dict({ 'color': gym.spaces.Tuple(color_tuple), 'depth': gym.spaces.Tuple(depth_tuple), }) # TODO(ayzaan): Delete below and uncomment vector box bounds. position_bounds = gym.spaces.Box(low=-1.0, high=1.0, shape=(3, ), dtype=np.float32) # position_bounds = gym.spaces.Box( # low=np.array([0.25, -0.5, 0.], dtype=np.float32), # high=np.array([0.75, 0.5, 0.28], dtype=np.float32), # shape=(3,), # dtype=np.float32) self.action_space = gym.spaces.Dict({ 'pose0': gym.spaces.Tuple((position_bounds, gym.spaces.Box(-1.0, 1.0, shape=(4, ), dtype=np.float32))), 'pose1': gym.spaces.Tuple((position_bounds, gym.spaces.Box(-1.0, 1.0, shape=(4, ), dtype=np.float32))) }) # Start PyBullet. disp_option = p.DIRECT if disp: disp_option = p.GUI if shared_memory: disp_option = p.SHARED_MEMORY client = p.connect(disp_option) file_io = p.loadPlugin('fileIOPlugin', physicsClientId=client) if file_io < 0: raise RuntimeError('pybullet: cannot load FileIO!') if file_io >= 0: p.executePluginCommand(file_io, textArgument=assets_root, intArgs=[p.AddFileIOAction, p.CNSFileIO], physicsClientId=client) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.setPhysicsEngineParameter(enableFileCaching=0) p.setAdditionalSearchPath(assets_root) p.setAdditionalSearchPath(tempfile.gettempdir()) p.setTimeStep(1. / hz) # If using --disp, move default camera closer to the scene. if disp: target = p.getDebugVisualizerCamera()[11] p.resetDebugVisualizerCamera(cameraDistance=1.1, cameraYaw=90, cameraPitch=-25, cameraTargetPosition=target) if task: self.set_task(task)
def init_simulation(self, floor_height=0.3, visualize=True, sim_timestep=0.001, cont_timestep_mult=16, lateral_friction=1.0, joint_damping=0.0, contact_stiffness=10000.0, contact_damping=200.0, contact_damping_multiplier=None): self.visualize = visualize if self.visualize: physicsClient = p.connect(p.GUI) else: physicsClient = p.connect(p.DIRECT) dir_path = os.path.dirname(os.path.realpath(__file__)) cubeStartPos = [0, 0, 0] cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) planeId = p.loadURDF(dir_path + '/urdf/plane_with_restitution.urdf') self.robotId = p.loadURDF(dir_path + '/urdf/teststand.urdf', cubeStartPos, cubeStartOrientation, flags=p.URDF_USE_INERTIA_FROM_FILE) cubePos, cubeOrn = p.getBasePositionAndOrientation(self.robotId) if isinstance(lateral_friction, list): self.random_lateral_friction = True self.lateral_friction_range = lateral_friction self.lateral_friction = np.random.uniform( self.lateral_friction_range[0], self.lateral_friction_range[1]) else: self.random_lateral_friction = False self.lateral_friction = lateral_friction if isinstance(contact_stiffness, list): self.random_contact_stiffness = True self.contact_stiffness_range = contact_stiffness self.contact_stiffness = np.random.uniform( self.contact_stiffness_range[0], self.contact_stiffness_range[1]) else: self.random_contact_stiffness = False self.contact_stiffness = contact_stiffness useRealTimeSimulation = False # Query all the joints. num_joints = p.getNumJoints(self.robotId) print("Number of joints={}".format(num_joints)) for ji in range(num_joints): p.changeDynamics(self.robotId, ji, linearDamping=.04, angularDamping=0.04, restitution=0.0, lateralFriction=self.lateral_friction, maxJointVelocity=1000) p.changeDynamics(self.robotId, ji, jointDamping=joint_damping) p.changeDynamics(planeId, -1, lateralFriction=self.lateral_friction) self.contact_damping = contact_damping self.contact_damping_multiplier = contact_damping_multiplier if self.random_contact_stiffness: self.contact_stiffness = np.random.uniform( self.contact_stiffness_range[0], self.contact_stiffness_range[1]) if self.contact_damping_multiplier is not None: contact_damping = self.contact_damping_multiplier * 2.0 * np.sqrt( self.contact_stiffness) else: if self.contact_damping is None: contact_damping = 2.0 * np.sqrt(self.contact_stiffness) else: contact_damping = self.contact_damping p.changeDynamics(planeId, -1, contactStiffness=self.contact_stiffness, contactDamping=contact_damping) p.setGravity(0.0, 0.0, -9.81) #p.setPhysicsEngineParameter(1e-3, numSubSteps=1) self.sim_timestep = sim_timestep self.cont_timestep_mult = cont_timestep_mult self.dt = self.cont_timestep_mult * self.sim_timestep p.setPhysicsEngineParameter(fixedTimeStep=self.sim_timestep) print(p.getPhysicsEngineParameters()) return p, self.robotId, planeId, [1, 2, 3], [2, 3]
conid = p.connect(p.SHARED_MEMORY) if (conid<0): p.connect(p.GUI) p.setInternalSimFlags(0) p.resetSimulation() plane = p.loadURDF(os.path.join(dataDir, "plane.urdf"),useMaximalCoordinates=useMaximalCoordinates) sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) gravXid = p.addUserDebugParameter("gravityX",-10,10,0) gravYid = p.addUserDebugParameter("gravityY",-10,10,0) gravZid = p.addUserDebugParameter("gravityZ",-10,10,-5) p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) centers = 0 mass = 1 visualShapeId = -1 n_links = 5 link_Masses=[1] * n_links linkCollisionShapeIndices=[colSphereId] * n_links linkVisualShapeIndices=[-1] * n_links linkPositions=[[0,0,0.11 * (j+1)] for j in range(n_links)] linkOrientations=[[0,0,0,1]] * n_links
def __init__(self): # create the physics simulator self.physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -9.81) self.max_communication_distance = 2.0 # We will integrate every 4ms (250Hz update) self.dt = 1. / 250. p.setPhysicsEngineParameter(self.dt, numSubSteps=1) # Create the plane. self.planeId = p.loadURDF("../models/plane.urdf") p.changeDynamics(self.planeId, -1, lateralFriction=5., rollingFriction=0) self.goalId = p.loadURDF("../models/goal.urdf") self.goalId = p.loadURDF("../models/goal2.urdf") # the balls self.ball1 = p.loadURDF("../models/ball1.urdf") p.resetBasePositionAndOrientation(self.ball1, [2., 4., 0.5], (0., 0., 0.5, 0.5)) self.ball2 = p.loadURDF("../models/ball2.urdf") p.resetBasePositionAndOrientation(self.ball2, [4., 2., 0.5], (0., 0., 0.5, 0.5)) p.resetDebugVisualizerCamera(7.0, 90.0, -43.0, (1., 1., 0.0)) # Add objects wallId = p.loadSDF("../models/walls.sdf")[0] p.resetBasePositionAndOrientation(wallId, [0., -1., 0], (0., 0., 0.5, 0.5)) wallId = p.loadSDF("../models/walls.sdf")[0] p.resetBasePositionAndOrientation(wallId, [0., 1., 0], (0., 0., 0.5, 0.5)) wallId = p.loadSDF("../models/walls.sdf")[0] p.resetBasePositionAndOrientation(wallId, [3., -1., 0], (0., 0., 0.5, 0.5)) wallId = p.loadSDF("../models/walls.sdf")[0] p.resetBasePositionAndOrientation(wallId, [3., 1., 0], (0., 0., 0.5, 0.5)) wallId = p.loadSDF("../models/walls.sdf")[0] p.resetBasePositionAndOrientation(wallId, [1., 2., 0], (0., 0., 0., 1.)) wallId = p.loadSDF("../models/walls.sdf")[0] p.resetBasePositionAndOrientation(wallId, [2., -2., 0], (0., 0., 0., 1.)) # tube # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-1., 5., 0], (0., 0., 0., 1.)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-1., 6., 0], (0., 0., 0., 1.)) # #arena # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-2, 4., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-2., 7., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-2., 9., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-2., 11., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-2., 13., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-3., 3., 0], (0., 0., 0., 1.)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-5., 3., 0], (0., 0., 0., 1.)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-7., 3., 0], (0., 0., 0., 1.)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-8, 4., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-8., 6., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-8., 8., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-8., 10., 0], (0., 0., 0.5, 0.5)) # wallId = p.loadSDF("../models/walls.sdf")[0] # p.resetBasePositionAndOrientation(wallId, [-8., 12., 0], (0., 0., 0.5, 0.5)) # create 6 robots self.robots = [] for (i, j) in itertools.product(range(3), range(2)): self.robots.append( Robot([1. * i + 0.5, 1. * j - 0.5, 0.3], 2 * i + j, self.dt)) p.stepSimulation() self.time = 0.0 self.stepSimulation() self.stepSimulation()
16 iiwa_gripper_finger2_inner_knuckle_joint 17 iiwa_gripper_finger2_finger_tip_joint """ p.setGravity(0, 0, 0) t = 0. prevPose = [0, 0, 0] prevPose1 = [0, 0, 0] hasPrevPose = 0 useNullSpace = 1 useOrientation = 1 ikSolver = 0 p.setRealTimeSimulation(0) # one action/simulation_step takes 0.002*20=0.04 seconds in real time p.setPhysicsEngineParameter(fixedTimeStep=0.002 * 20, numSolverIterations=5, numSubSteps=20) # trailDuration is duration (in seconds) after debug lines will be removed automatically # use 0 for no-removal trailDuration = 15 i = 0 mp = 1 g = 0.5 start_time = time.process_time() # 25 simulation_steps = 1 seconds while i < (25 * 50): i += 1 t = t + 0.04 p.stepSimulation()
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)
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): """Environment reset called at the beginning of an episode. """ # Set the camera settings. # look = [0.1, 0., 0.44] # distance = 0.8 # pitch = -90 # yaw = -90 # roll = 180 #[0.4317558029454219, 0.1470448842904527, 0.2876218894185256]#[0.23, 0.2, 0.54] # from where the input is # set 1 # look = [0.1, -0.3, 0.54] # distance = 1.0 # pitch = -45 + self._cameraRandom * np.random.uniform(-3, 3) # yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3) # roll = 145 # pos_range = [0.3, 0.7, -0.2, 0.3] # set 2 # look = [-0.3, -0.8, 0.54] # distance = 0.7 # pitch = -28 + self._cameraRandom * np.random.uniform(-3, 3) # yaw = -45 + self._cameraRandom * np.random.uniform(-3, 3) # roll = 180 # pos_range = [0.1, 0.8, -0.45, 0.15] # set 3 look = [0.4, 0.1, 0.54] distance = 2.0 pitch = -90 yaw = -90 roll = 180 pos_range = [0.3, 0.7, -0.2, 0.3] 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]) self.tableUid = p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.640000, 0.000000, 0.000000, 0.0, 1.0) p.setGravity(0, 0, -10) # self._tm700 = tm700(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() # num of objs to be placed while len(self.model_paths) != 0: urdfList = [] num_obj = random.randint(1, 3) for i in range(num_obj): urdfList.append(self.model_paths.pop()) print('img {img_cnt}, {n_obj} objects'.format(img_cnt=self.img_save_cnt+1, n_obj=len(urdfList))) self._objectUids = self._randomly_place_objects(urdfList, pos_range) self._observation = self._get_observation() self.img_save_cnt += 1 for uid in self._objectUids: p.removeBody(uid) if self.img_save_cnt == 2: raise Exception('stop') return np.array(self._observation)
import pybullet as p import time p.connect(p.GUI) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.setPhysicsEngineParameter( constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM=0.000001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) p.loadURDF("plane.urdf") radius = 0.025 distance = 1.86 yaw = 135 pitch = -11 targetPos = [0, 0, 0] p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200) p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos) objectId = -1 for i in range(10): objectId = p.loadURDF("cube_small.urdf", [1, 1, radius + i * 2 * radius]) p.changeDynamics(objectId, -1, 100) timeStep = 1. / 240. p.setGravity(0, 0, -10) while (p.isConnected()): p.stepSimulation() time.sleep(timeStep)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES) p.changeVisualShape(ground,-1,rgbaColor=[1,1,1,0.8]) #p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) print("hasNumpy = ",p.isNumpyEnabled()) anymal = p.loadURDF("quadruped/ANYmal/robot.urdf",[3,3,3], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False) p.resetSimulation() ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES) #todo, tweak this value to trade solver quality versus performance p.setPhysicsEngineParameter(solverResidualThreshold=1e-2) index = 0 numX = 3 numY = 3 for i in range (numX): for j in range (numY): print("loading animal ", index) index+=1 #anymal = p.loadURDF("atlas/atlas_v4_with_multisense.urdf",[i*3,j*3,1], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES) anymal = p.loadURDF("quadruped/ANYmal/robot.urdf",[(i-numX/2)*2,(j-numY/2)*2,0.6], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False) for j in range(p.getNumJoints(anymal)): p.setJointMotorControl2(anymal,j,p.POSITION_CONTROL,targetPosition=0, force=500)
p.disconnect() except Exception: pass p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() p.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=20, cameraPitch=-40, cameraTargetPosition=[0, 0, 0.1]) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) p.setRealTimeSimulation(0) p.setPhysicsEngineParameter(numSubSteps=4) p.setPhysicsEngineParameter(numSolverIterations=10) p.setGravity(0, 0, GRAVITY) StartPos = [0, 0, 0.3] StartOrientation = p.getQuaternionFromEuler([0, 0, 0]) planeId = p.loadURDF("plane.urdf") botId = p.loadURDF("tiptap.urdf", StartPos, StartOrientation) # Disable the motors for torque control: p.setJointMotorControlArray( botId, [j for j in range(p.getNumJoints(botId))], p.VELOCITY_CONTROL, forces=[0.0 for j in range(p.getNumJoints(botId))] )
def reset(self): self.terminated = False self.n_contacts = 0 self.n_steps_outside = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timestep) p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1]) self.table_uid = p.loadURDF(os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0) # Initialize button position x_pos = 0.5 y_pos = 0 self.button_uid = p.loadURDF("../data/simple_button.urdf", [x_pos, y_pos, Z_TABLE], useFixedBase=True) self.button_pos = np.array([x_pos, y_pos, Z_TABLE]) p.changeDynamics(self.button_uid, 1, contactStiffness=1000000.0, contactDamping=1.0) self.box_uid = p.loadURDF("../data/target_box_new.urdf", [x_pos-0.1, y_pos-0.1, Z_TABLE+0.065], useFixedBase=True) self.box_pos = np.array([x_pos, y_pos, Z_TABLE]) p.changeDynamics(self.box_uid, 0, contactStiffness=1000000.0, contactDamping=1.0) p.setGravity(0, 0, -10) # load my kuka self._kuka = kuka.Kuka(urdf_root_path="../data", timestep=self._timestep, use_inverse_kinematics=(not self.action_joints), small_constraints=(not self._random_target)) self._env_step_counter = 0 # Close the gripper and wait for the arm to be in rest position for _ in range(500): if self.action_joints: self._kuka.applyAction(list(np.array(self._kuka.joint_positions)[:7]) + [0, 0]) else: self._kuka.applyAction([0, 0, 0, 0, 0]) p.stepSimulation() # Randomize init arm pos: take 5 random actions for _ in range(N_RANDOM_ACTIONS_AT_INIT): if self._is_discrete: action = [0, 0, 0, 0, 0] sign = 1 if self.np_random.rand() > 0.5 else -1 action_idx = self.np_random.randint(3) # dx, dy or dz action[action_idx] += sign * DELTA_V else: if self.action_joints: joints = np.array(self._kuka.joint_positions)[:7] joints += DELTA_THETA * self.np_random.normal(joints.shape) action = list(joints) + [0, 0] else: action = np.zeros(5) rand_direction = self.np_random.normal((3,)) # L2 normalize, so that the random direction is not too high or too low rand_direction /= np.linalg.norm(rand_direction, 2) action[:3] += DELTA_V_CONTINUOUS * rand_direction self._kuka.applyAction(list(action)) p.stepSimulation() self._observation = self.getExtendedObservation() self.button_pos = np.array(p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0]) # self.button_pos[2] += BUTTON_DISTANCE_HEIGHT # Set the target position on the top of the button if self.saver is not None: self.saver.reset(self._observation, self.getTargetPos(), self.getGroundTruth()) if self.srl_model != "raw_pixels": return self.getSRLState(self._observation) return self._observation
def __init__(self, robotPath, floor=True, fixed=False, transparent=False, gui=True, realTime=True, panels=False, useUrdfInertia=True, dt=0.002): """Creates an instance of humanoid simulation Keyword Arguments: field {bool} -- enable the display of the field (default: {False}) fixed {bool} -- makes the base of the robot floating/fixed (default: {False}) transparent {bool} -- makes the robot transparent (default: {False}) gui {bool} -- enables the gui visualizer, if False it will runs headless (default {True}) realTime {bool} -- try to have simulation in real time (default {True}) panels {bool} -- show/hide the user interaction pyBullet panels (default {False}) useUrdfInertia {bool} -- use URDF from URDF file (default {True}) dt {float} -- time step (default {0.002}) """ self.dir = os.path.dirname(os.path.abspath(__file__)) self.gui = gui self.realTime = realTime self.t = 0 self.start = time.time() self.dt = dt self.mass = None # Debug lines drawing self.lines = [] self.currentLine = 0 self.lastLinesDraw = 0 self.lineColors = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1]] # Instanciating bullet if gui: physicsClient = p.connect(p.GUI) else: physicsClient = p.connect(p.DIRECT) p.setGravity(0, 0, -9.81) # Light GUI if not panels: p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1) # Loading floor and/or plane ground if floor: self.floor = p.loadURDF(self.dir + '/bullet/plane.urdf') else: self.floor = None # Loading robot startPos = [0, 0, 0] if not fixed: startPos[2] = 1 startOrientation = p.getQuaternionFromEuler([0, 0, 0]) flags = p.URDF_USE_SELF_COLLISION if useUrdfInertia: flags += p.URDF_USE_INERTIA_FROM_FILE self.robot = p.loadURDF(robotPath, startPos, startOrientation, flags=flags, useFixedBase=fixed) # Setting frictions parameters to default ones self.setFloorFrictions() # Engine parameters p.setPhysicsEngineParameter(fixedTimeStep=self.dt, maxNumCmdPer1ms=0) # p.setRealTimeSimulation(0) # p.setPhysicsEngineParameter(numSubSteps=1) # Retrieving joints and frames self.joints = {} self.jointsInfos = {} self.jointsIndexes = {} self.frames = {} self.maxTorques = {} # Collecting the available joints n = 0 for k in range(p.getNumJoints(self.robot)): jointInfo = p.getJointInfo(self.robot, k) name = jointInfo[1].decode('utf-8') if not name.endswith('_fixing') and not name.endswith('_passive'): if '_frame' in name: self.frames[name] = k else: self.jointsIndexes[name] = n n += 1 self.joints[name] = k self.jointsInfos[name] = {'type': jointInfo[2]} if jointInfo[8] < jointInfo[9]: self.jointsInfos[name]['lowerLimit'] = jointInfo[8] self.jointsInfos[name]['upperLimit'] = jointInfo[9] # Changing robot opacity if transparent set to true if transparent: for k in range(p.getNumJoints(self.robot)): p.changeVisualShape(self.robot, k, rgbaColor=[0.3, 0.3, 0.3, 0.3]) print('* Found ' + str(len(self.joints)) + ' DOFs') print('* Found ' + str(len(self.frames)) + ' frames')
def __init__( self, display=True, seed=None, pos_noise=0.05, rpy_noise=0.1, num_rings=5, ring_separation=5., ): # Create random number generator self.rng = np.random.default_rng(seed) # Choose the time step self.dt = 0.01 # Create empty list of drones self.drones = [] self.max_num_drones = 40 # Connect to and configure pybullet self.display = display if self.display: pybullet.connect(pybullet.GUI) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) else: pybullet.connect(pybullet.DIRECT) pybullet.setGravity(0, 0, -9.81) pybullet.setPhysicsEngineParameter(fixedTimeStep=self.dt, numSubSteps=4, restitutionVelocityThreshold=0.05, enableFileCaching=0) # Load plane self.plane_id = pybullet.loadURDF( os.path.join('.', 'urdf', 'plane.urdf'), basePosition=np.array([0., 0., 0.]), baseOrientation=pybullet.getQuaternionFromEuler([0., 0., 0.]), useFixedBase=1) # Load rings self.num_rings = num_rings self.ring_separation = ring_separation self.rings = [] self.add_ring([0., 0., 0.25], [0., -np.pi / 2, 0.], 2.5, 0.5, 'big-ring.urdf') for i in range(num_rings): x = (i + 1) * ring_separation y = self.rng.uniform(low=-5., high=5.) z = self.rng.uniform(low=1., high=3.) self.add_ring([x, y, z], [0., 0., 0.], 1., 0.25, 'ring.urdf') self.add_ring([(num_rings + 1) * ring_separation, 0., 0.25], [0., np.pi / 2, 0.], 2.5, 0.5, 'big-ring.urdf') # # Eliminate linear and angular damping (a poor model of drag) # pybullet.changeDynamics(self.robot_id, -1, linearDamping=0., angularDamping=0.) # Set contact parameters object_ids = [self.plane_id] for ring in self.rings: object_ids.append(ring['id']) for object_id in object_ids: pybullet.changeDynamics(object_id, -1, lateralFriction=1.0, spinningFriction=0.0, rollingFriction=0.0, restitution=0.5, contactDamping=-1, contactStiffness=-1) self.pos_noise = pos_noise self.rpy_noise = rpy_noise self.camera_drone_name = None self.camera_drone_yaw = None self.camera_viewfromstart = True self.camera() self.update_display() self.l = 0.175 self.kF = 7e-6 self.kM = 1e-7 self.min_spin_rate = 100 # <-- rad/s self.max_spin_rate = 900 # <-- rad/s self.s_min = self.min_spin_rate**2 self.s_max = self.max_spin_rate**2 # motor[0]: front (+z spin) # motor[1]: rear (+z spin) # motor[2]: left (-z spin) # motor[3]: right (-z spin) self.M = linalg.inv( np.array([[0., 0., self.kF * self.l, -self.kF * self.l], [-self.kF * self.l, self.kF * self.l, 0., 0.], [-self.kM, -self.kM, self.kM, self.kM], [self.kF, self.kF, self.kF, self.kF]]))
#add parent dir to find package. Only needed for source code build, pip install doesn't need it. import os import 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 pybullet_data import pybullet as p import time p.connect(p.GUI_SERVER) p.setAdditionalSearchPath(pybullet_data.getDataPath()) while (1): #this is a no-op command, to allow GUI updates on Mac OSX (main thread) p.setPhysicsEngineParameter() time.sleep(0.01)
def __init__(self, fixed_slider, init_sliders_pose, with_gui): # store the arguments self.fixed_slider = fixed_slider self.init_sliders_pose = init_sliders_pose self.with_gui = with_gui # connect to the server if self.with_gui: self.physicsClient = p.connect(p.GUI) else: self.physicsClient = p.connect(p.DIRECT) # create sliders if self.with_gui: self.slider_a = p.addUserDebugParameter("a", 0, 1, init_sliders_pose[0]) self.slider_b = p.addUserDebugParameter("b", 0, 1, init_sliders_pose[1]) else: self.slider_a = init_sliders_pose[0] self.slider_b = init_sliders_pose[0] # Load the plain. plain_urdf = os.path.join( rospkg.RosPack().get_path("robot_properties_teststand"), "urdf", "plane_with_restitution.urdf") self.planeId = p.loadURDF(plain_urdf) print("Loaded ground.") self.config = TeststandConfig() # Load the robot robotStartPos = [0., 0., 0.] robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) self.urdf_path = self.config.urdf_path self.robotId = p.loadURDF(self.urdf_path, robotStartPos, robotStartOrientation, flags=p.URDF_USE_INERTIA_FROM_FILE, useFixedBase=True) p.getBasePositionAndOrientation(self.robotId) # Create the robot wrapper in pinocchio. self.pin_robot = self.config.buildRobotWrapper() # Query all the joints. num_joints = p.getNumJoints(self.robotId) for ji in range(num_joints): p.changeDynamics(self.robotId, ji, linearDamping=.04, angularDamping=0.04, restitution=0.0, lateralFriction=0.5) p.setGravity(0, 0, -9.81) p.setPhysicsEngineParameter(1e-3, numSubSteps=1) self.joint_names = ['joint_z', 'HFE', 'KFE'] self.wrapper = PinBulletWrapper(self.robotId, self.pin_robot, self.joint_names, ['END'], useFixedBase=True) # Constrain the slider to a fixed position. if fixed_slider: p.createConstraint(self.robotId, 0, self.robotId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 0.45]) # Initialize the device. self.device = Device('bullet_teststand') self.device.initialize(self.config.yaml_path) # Initialize signals that are not filled in sim2signals. self.device.contact_sensors.value = 1 * [0.] self.device.height_sensors.value = 1 * [0.] self.device.ati_force.value = 3 * [0.] self.device.ati_torque.value = 3 * [0.] # Sync the current robot state to the graph input signals. self._sim2signal() self.steps_ = 0 super(TeststandBulletRobot, self).__init__('bullet_teststand', self.device)
cubeStartOrientation) cubePos, cubeOrn = p.getBasePositionAndOrientation(robot) nJoints = p.getNumJoints(robot) # Drawing inertia boxes if False: drawInertiaBox(robot, -1, [0, 1, 0]) for i in range(nJoints): drawInertiaBox(robot, i, [0, 1, 0]) # Map des joints jointsMap = [] framesMap = [] t = 0 dt = 0.01 p.setPhysicsEngineParameter(fixedTimeStep=dt) # Collecting the available joints for k in range(nJoints): jointInfo = p.getJointInfo(robot, k) name = jointInfo[1].decode('utf-8') if '_fixing' not in name: if '_frame' in name: framesMap.append([name, k]) else: jointsMap.append(k) print('* Found ' + str(len(jointsMap)) + ' DOFs') print('* Found ' + str(len(framesMap)) + ' frames') while True:
import pybullet as p import pybullet_data as pd import time import math usePhysX = True useMaximalCoordinates = True if usePhysX: p.connect(p.PhysX,options="--numCores=8 --solver=pgs") p.loadPlugin("eglRendererPlugin") else: p.connect(p.GUI) p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024) p.setPhysicsEngineParameter(contactBreakingThreshold=0.01) p.setAdditionalSearchPath(pd.getDataPath()) #Always make ground plane maximal coordinates, to avoid performance drop in PhysX #See https://github.com/NVIDIAGameWorks/PhysX/issues/71 p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json") jran = 50 iran = 100 num=64 radius=0.1 numDominoes=0
def setup(self): """Sets up the robot + tray + objects. """ test = self._test if not self._urdf_list: # Load from procedural random objects. if not self._object_filenames: self._object_filenames = self._get_random_objects( num_objects=self._num_objects, test=test, replace=self._allow_duplicate_objects, ) self._urdf_list = self._object_filenames logging.info('urdf_list %s', self._urdf_list) pybullet.resetSimulation(physicsClientId=self.cid) pybullet.setPhysicsEngineParameter(numSolverIterations=150, physicsClientId=self.cid) pybullet.setTimeStep(self._time_step, physicsClientId=self.cid) pybullet.setGravity(0, 0, -10, physicsClientId=self.cid) plane_path = os.path.join(self._urdf_root, 'plane.urdf') pybullet.loadURDF(plane_path, [0, 0, -1], physicsClientId=self.cid) table_path = os.path.join(self._urdf_root, 'table/table.urdf') pybullet.loadURDF(table_path, [0.5, 0.0, -.82], [0., 0., 0., 1.], physicsClientId=self.cid) self._kuka = kuka.Kuka(urdfRootPath=self._urdf_root, timeStep=self._time_step, clientId=self.cid) self._block_uids = [] self._urdf_list = [ ('/home/jonathan/Desktop/Projects/objects/6a0c8c43b13693fa46eb89c2e933753d.obj', 'obj', [4, 4, 4]), ('/home/jonathan/Desktop/Projects/objects/6aec84952a5ffcf33f60d03e1cb068dc.obj', 'obj', [2, 2, 2]), ('/home/jonathan/Desktop/Projects/objects/bed29baf625ce9145b68309557f3a78c.obj', 'obj', [2, 2, 2]), ('/home/jonathan/Desktop/Projects/objects/dac6ea4929f1e47f178d703a993fe24c.obj', 'obj', [2, 2, 2]), ('/home/jonathan/Desktop/Projects/objects/f452c1053f88cd2fc21f7907838a35d1.obj', 'obj', [2, 2, 2]) ] for name in self._urdf_list: if (name[1] == 'urdf'): print(name[0], name[1]) urdf_name = name[0] xpos = 0.4 + self._block_random * random.random() ypos = self._block_random * (random.random() - .5) angle = np.pi / 2 + self._block_random * np.pi * random.random( ) ori = pybullet.getQuaternionFromEuler([0, 0, angle]) uid = pybullet.loadURDF(urdf_name, [xpos, ypos, .15], [ori[0], ori[1], ori[2], ori[3]], physicsClientId=self.cid) self._block_uids.append(uid) for _ in range(500): pybullet.stepSimulation(physicsClientId=self.cid) if (name[1] == 'obj'): obj_name = name[0] scale = name[2] xpos = 0.4 + self._block_random * random.random() ypos = self._block_random * (random.random() - .5) angle = np.pi / 2 + self._block_random * np.pi * random.random( ) ori = pybullet.getQuaternionFromEuler([0, 0, angle]) uid = pybullet.createMultiBody( 0.05, pybullet.createCollisionShape(pybullet.GEOM_MESH, fileName=name[0], meshScale=scale), pybullet.createVisualShape(pybullet.GEOM_MESH, fileName=name[0], meshScale=scale), [xpos, ypos, .15], [ori[0], ori[1], ori[2], ori[3]], physicsClientId=self.cid) self._block_uids.append(uid) for _ in range(500): pybullet.stepSimulation(physicsClientId=self.cid)
import motorController import walkGenerator # motor parameters motor_kp = 0.5 motor_kd = 0.5 motor_torque = 20 motor_max_velocity = 5.0 # physics parameters fixedTimeStep = 1. / 2000 numSolverIterations = 200 physicsClient = p.connect(p.GUI) p.setTimeStep(fixedTimeStep) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # to load plane.urdf p.setGravity(0, 0, 0) p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=10, cameraPitch=-0, cameraTargetPosition=[0.4, 0, 0.1]) planeID = p.loadURDF("plane.urdf") robotID = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) + '/humanoid_leg_12dof.8.urdf', [0, 0, 0.31], p.getQuaternionFromEuler([0, 0, 0]), useFixedBase=False)
def generate_dataset(self): # combined dataset for graph classification (GC) combined_dataset_name = dataset_prefix combined_dataset_dir = self.save_dir / combined_dataset_name pathlib.Path(combined_dataset_dir).mkdir(parents=True, exist_ok=True) combined_node_id = 0 combined_graph_id = 0 combined_graphnode2id = {} combined_a = open( combined_dataset_dir / str(combined_dataset_name + "_A.txt"), "w") combined_graph_indicator = open( combined_dataset_dir / str(combined_dataset_name + "_graph_indicator.txt"), "w", ) combined_graph_labels = open( combined_dataset_dir / str(combined_dataset_name + "_graph_labels.txt"), "w") combined_node_attributes = open( combined_dataset_dir / str(combined_dataset_name + "_node_attributes.txt"), "w", ) for num_brick in tqdm( range(self.min_num_brick, self.max_num_brick + 1), "Number of Brick Progress", ): # separate dataset for graph classification (GC) dataset_name = self.dataset_prefix + "_" + str(num_brick) dataset_dir = self.save_dir / dataset_name pathlib.Path(dataset_dir).mkdir(parents=True, exist_ok=True) node_id = 0 graphnode2id = {} a = open(dataset_dir / str(dataset_name + "_A.txt"), "w") graph_indicator = open( dataset_dir / str(dataset_name + "_graph_indicator.txt"), "w") graph_labels = open( dataset_dir / str(dataset_name + "_graph_labels.txt"), "w") node_attributes = open( dataset_dir / str(dataset_name + "_node_attributes.txt"), "w") for episode in tqdm(range(self.num_episode), "Episode Progress"): # use test_simulator.py for debugging and visualization p.connect(p.DIRECT) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1.0 / 60.0) # generate new design self.designer.clear() self.designer.generate_design(num_brick=num_brick) # update combined graph_id graph_id = episode + 1 combined_graph_id += 1 init_pos_list = [] final_pos_list = [] for i, b in enumerate(self.designer.built): # write to separate GC dataset node_id += 1 graphnode2id[str(graph_id) + "_" + str(i)] = node_id graph_indicator.write(str(graph_id) + "\n") node_attributes.write(",".join( str(i) for i in b.bounding.flatten().tolist()) + "\n") # write to combined GC dataset combined_node_id += 1 combined_graphnode2id[str(combined_graph_id) + "_" + str(i)] = combined_node_id combined_graph_indicator.write( str(combined_graph_id) + "\n") combined_node_attributes.write(",".join( str(i) for i in b.bounding.flatten().tolist()) + "\n") pos = b.anchor rot = p.getQuaternionFromEuler( np.array(b.rotation) * np.pi / 2) brickID = p.loadURDF("urdf/brick.urdf", pos, rot) init_pos_list.append(pos) p.loadURDF("plane.urdf", useMaximalCoordinates=True) p.setGravity(0, 0, -10) for frame in range(self.num_frame): p.stepSimulation() time.sleep(1.0 / 240.0) if frame == self.num_frame - 1: for brickID in len(num_brick): pos, _ = p.getBasePositionAndOrientation(brickID) final_pos_list.append(pos) for edge in self.designer.G.edges: # write to separate GC dataset row = graphnode2id[str(graph_id) + "_" + str(edge[0])] col = graphnode2id[str(graph_id) + "_" + str(edge[1])] a.write(str(row) + ", " + str(col) + "\n") a.write(str(col) + ", " + str(row) + "\n") # write to combined GC dataset combined_row = combined_graphnode2id[str(combined_graph_id) + "_" + str(edge[0])] combined_col = combined_graphnode2id[str(combined_graph_id) + "_" + str(edge[1])] combined_a.write( str(combined_row) + ", " + str(combined_col) + "\n") combined_a.write( str(combined_col) + ", " + str(combined_row) + "\n") label = int( self.determine_stable(init_pos_list, final_pos_list)) graph_labels.write(str(label) + "\n") combined_graph_labels.write(str(label) + "\n") p.disconnect() a.close() graph_indicator.close() graph_labels.close() node_attributes.close() shutil.make_archive(self.save_dir / dataset_name, "zip", dataset_dir) shutil.rmtree(dataset_dir) combined_a.close() combined_graph_indicator.close() combined_graph_labels.close() combined_node_attributes.close() shutil.make_archive(self.save_dir / combined_dataset_name, "zip", combined_dataset_dir) shutil.rmtree(combined_dataset_dir)
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)
import pybullet as p import time useMaximalCoordinates = 0 p.connect(p.GUI) p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(fixedTimeStep=1./120.) sphereRadius = 0.05 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) mass = 1 visualShapeId = -1 for i in range (5): for j in range (5): for k in range (5): if (k&2): sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates) else: sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates) p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) p.setGravity(0,0,-10) p.setRealTimeSimulation(1)