def create(self): p.connect(p.GUI) p.createMultiBody(0, 0) p.setGravity(0, 0, -9.8) p.setAdditionalSearchPath(data.getDataPath()) p.loadSDF("stadium.sdf") #p.setRealTimeSimulation(True) #bulid a plate self.plateId = p.createCollisionShape( p.GEOM_BOX, halfExtents=[self.plateLong, self.plateWidth, self.plateHigh]) #bulid a motor self.motorId = p.createCollisionShape(p.GEOM_CYLINDER, radius=self.motorRadius, height=self.motorHeight) #bulid a ball self.ballId = p.createCollisionShape(p.GEOM_SPHERE, radius=self.ballRadius, height=self.motorHeight) self.ballUid = p.createMultiBody(self.ballMass, self.ballId, self.visualShapeId, self.ballStartPos, self.ballStartOrn) #bulit a multi-body self.linkCollisionShapeIndices = [self.motorId] self.controlerUid = p.createMultiBody( self.mass, self.plateId, self.visualShapeId, self.basePosition, self.baseOrientation, linkMasses=self.link_Masses, linkCollisionShapeIndices=self.linkCollisionShapeIndices, linkVisualShapeIndices=self.linkVisualShapeIndices, linkPositions=self.linkPositions, linkOrientations=self.linkOrientations, linkInertialFramePositions=self.linkInertialFramePositions, linkInertialFrameOrientations=self.linkInertialFrameOrientations, linkParentIndices=self.indices, linkJointTypes=self.jointTypes, linkJointAxis=self.axis) #change some physical parameters p.changeDynamics(self.controlerUid, -1, spinningFriction=0.01, rollingFriction=0.01, linearDamping=0.0) #constraint objects #p.createConstraint(self.motorId,-1,-1,-1,p.JOINT_POINT2POINT,[0,0,0],[0,0,0],[0,0,1.5]) p.createConstraint(self.controlerUid, -1, -1, -1, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0, 0], [0, 0, 1.5]) p.setRealTimeSimulation(self.useRealTimeSim)
def main(): p = argparse.ArgumentParser() p.add_argument('sdf') args = p.parse_args() sdffile = str(path.Path(args.sdf).resolve()) print(sdffile) pb.connect(pb.GUI) # type: ignore pb.loadSDF(sdffile) # type: ignore input()
def __init__(self, init_pos, robot_id, dt): self.id = robot_id self.dt = dt self.pybullet_id = p.loadSDF("../models/robot.sdf")[0] self.joint_ids = list(range(p.getNumJoints(self.pybullet_id))) self.initial_position = init_pos self.reset() self.passedtime = 0 self.converttime = 10 self.is_formation = 0 self.formation = [] self.x = None self.y = None # No friction between body and surface. p.changeDynamics(self.pybullet_id, -1, lateralFriction=5., rollingFriction=0.) # Friction between joint links and surface. for i in range(p.getNumJoints(self.pybullet_id)): p.changeDynamics(self.pybullet_id, i, lateralFriction=5., rollingFriction=0.) self.messages_received = [] self.messages_to_send = [] self.neighbors = []
def reset(self): self._kuka_hand = p.loadSDF("model.sdf") self.kuka_handId = self._kuka_hand[0] print("self._kuka_hand", self._kuka_hand) print("self.kuka_handId", self.kuka_handId) #self.cameraSetup() # reset joints to the middle valu p.resetBasePositionAndOrientation( self.kuka_handId, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000]) self.jointInfo.get_infoForAll_joints(self._kuka_hand) self.numJoints = p.getNumJoints(self.kuka_handId) self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints() self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints() self.motorNames = [] self.motorIndices = [] for i in range(self.numJoints): jointInfo = p.getJointInfo(self.kuka_handId, i) qIndex = jointInfo[3] if qIndex > -1: self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i)
def main(): # Open GUI and set pybullet_data in the path p.connect(p.GUI) #p.setAdditionalSearchPath(pybullet_data.getDataPath()) # Load plane contained in pybullet_data p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf")) # Set gravity for simulation p.setGravity(0,0,-9.8) # load robot model robotIds = p.loadSDF(os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf")) icubId = robotIds[0] # set constraint between base_link and world p.createConstraint(icubId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0], [p.getBasePositionAndOrientation(icubId)[0][0], p.getBasePositionAndOrientation(icubId)[0][1], p.getBasePositionAndOrientation(icubId)[0][2]*1.2], p.getBasePositionAndOrientation(icubId)[1]) ##init_pos for standing # without FT_sensors init_pos = [0]*15 + [-29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0, 0, 0] # with FT_sensors #init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0] # all set to zero #init_pos = [0]*p.getNumJoints(icubId) # Load other objects p.loadURDF(os.path.join(pybullet_data.getDataPath(),"table/table.urdf"), [1.1, 0.0, 0.0]) p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"), [0.5,0.0,0.8]) # add debug slider jointIds=[] paramIds=[] joints_num = p.getNumJoints(icubId) print("len init_pos ",len(init_pos)) print("len num joints", joints_num) for j in range (joints_num): info = p.getJointInfo(icubId,j) jointName = info[1] #jointType = info[2] jointIds.append(j) paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), info[8], info[9], init_pos[j]/180*m.pi)) while True: for i in range(joints_num): p.setJointMotorControl2(icubId, i, p.POSITION_CONTROL, targetPosition=p.readUserDebugParameter(i), targetVelocity=0.0, positionGain=0.25, velocityGain=0.75, force=50) p.stepSimulation() time.sleep(0.01)
def parseWorld(p, filepath): # load element tree from file tree = ET.parse(filepath) root = tree.getroot() if 'version' in root.attrib: print("version=",root.attrib['version']) models = {} log("Parsing models...", 'INFO') for model_xml in root.iter('model'): log(" Adding link {}.".format(model_xml.attrib['name']), 'DEBUG') newmodel = parseModel(model_xml, "") models[model_xml.attrib['name']] = newmodel if 'uri' in newmodel.keys() and newmodel['uri'] is not None: bodies = p.loadSDF(newmodel['uri']) #transform all bodies using the 'pose' for b in bodies: old_pos,old_orn = p.getBasePositionAndOrientation(b) pose_xyz = newmodel['pose_xyz'] pose_rpy = newmodel['pose_rpy'] pose_orn = p.getQuaternionFromEuler(pose_rpy) new_pos, new_orn = p.multiplyTransforms(pose_xyz, pose_orn, old_pos,old_orn) p.resetBasePositionAndOrientation(b, new_pos, new_orn) p.changeDynamics(b, -1, mass=0) print("bodies=",bodies) return models
def reset(self): objects = p.loadSDF( os.path.join(self.urdfRootPath, "mz07_with_gripper.sdf")) self.robotUid = objects[0] p.resetBasePositionAndOrientation( self.robotUid, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000]) self.jointPositions = [0, 0, 0, 0, 0, 0] self.numJoints = p.getNumJoints(self.robotUid) for jointIndex in range(self.numJoints): p.resetJointState(self.robotUid, jointIndex, self.jointPositions[jointIndex]) p.setJointMotorControl2( self.robotUid, jointIndex, p.POSITION_CONTROL, targetPosition=self.jointPositions[jointIndex], force=self.maxForce) self.motorNames = [] self.motorIndices = [] for i in range(self.numJoints): jointInfo = p.getJointInfo(self.robotUid, i) qIndex = jointInfo[3] if qIndex > -1: self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i)
def placeRandBlock(self): # randomly generate a 3d pose of the block to grasp # limit is at xpos = 0.215, center is xpos = 0.195 #xpos = 0.198 + 0.035 * (random.random()-0.5) #ypos = 0 + 0.056 * (random.random()-0.5) #ang = 3.14 * 0.5 + 3.1415925438 * random.random() """ xpos = 0.195 ypos = -0.015 ang = 3.14 * 0.5 orn = p.getQuaternionFromEuler([0, 1.57079632679, ang]) self.blockUid = p.loadURDF("data/block.urdf", xpos, ypos, -0.0149, orn[0], orn[1], orn[2], orn[3]) """ xpos = 0.195 + 0.04 #xpos = 0.195 + 0.065 + 0.01 #xpos = 0.195 + 0.065 + 0.025 * (random.random()-0.5) ypos = 0.000 #ypos = 0+ 0.055 * (random.random()-0.5) q = R.from_euler('xyz', [90, 0, 0], degrees=True).as_quat() #self.mugUid = p.loadURDF("data/mug.urdf", xpos, ypos, -0.18, # q[0], q[1], q[2], q[3]) self.mugUid = p.loadSDF("data/mug.sdf", useMaximalCoordinates = False)[0] # set the position of the base to be on the table p.resetBasePositionAndOrientation(self.mugUid, [xpos, ypos, -0.15], q)
def reset(self): robot_path = resource_filename(__name__, "/kuka_handlit_model/model.sdf") self._kuka_hand = p.loadSDF(robot_path) self.kuka_handId = self._kuka_hand[0] if self.kuka_loc != None: p.resetBasePositionAndOrientation(self.kuka_handId, self.kuka_loc[0:3], self.kuka_loc[3:]) self.jointInfo.get_infoForAll_joints(self._kuka_hand) self.numJoints = p.getNumJoints(self.kuka_handId) self.num_Active_joint = self.jointInfo.getNumberOfActiveJoints() self.indexOf_activeJoints = self.jointInfo.getIndexOfActiveJoints() resetJoints = p.calculateInverseKinematics(self.kuka_handId, self.kukaEndEffectorIndex, self.ee_xyz) self.motorNames = [] self.motorIndices = [] for i in range(self.numJoints): jointInfo = p.getJointInfo(self.kuka_handId, i) qIndex = jointInfo[3] if qIndex > -1: self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i) for i in range(self.num_Active_joint): p.resetJointState(self.kuka_handId, self.indexOf_activeJoints[i], resetJoints[i])
def _reset(self): #print("KukaGymEnv _reset") self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) texUid = p.loadTexture("./cube_new/aaa.png") cube_objects = p.loadSDF("./cube_new/model.sdf") self.cubeId = cube_objects[0] p.changeVisualShape(cube_objects[0], -1,rgbaColor =[1,1,1,1]) p.changeVisualShape(cube_objects[0], -1, textureUniqueId = texUid) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) self.tray = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) self.table = p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-0.820000,0.000000,0.000000,0.0,1.0) #print("table:::",self.table) #print("tray:::",self.tray) #This is where the object spawn at random i should adjust this so it spawn at random position with specific area bound if reward increased xpos = 0.58 ypos = 0.04 ang = 3.14*0.5 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]) #This where robot is reset i should modefy this to adjust height of end-effector p.setGravity(0,0,-10) self._kuka_hand = Kuka_Handlit(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 p.stepSimulation() self._observation = self.getExtendedObservation() return np.array([self._observation])
def reset(self): objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) #p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,0,0) self.endEffectorPos = [0.537,0.0,0.5] self.endEffectorAngle = 0 self.motorNames = [] self.motorIndices = [] import random xpos = 0.55 +0.12*random.random() ypos = 0 +0.2*random.random() ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) orn = np.array((-0.0, 1, 0.0, 0.0)) pos = (0.5321085918022986, -0.0011177175302174629, 0.2522914797947443), self.constraint = p.createConstraint(self.kukaUid,7,-1,-1,p.JOINT_FIXED,pos,orn,[0.500000,0.300006,0.700000])
def _reset(self): p.resetSimulation() #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) #p.loadURDF("%splane.urdf" % self._urdfRoot) stadiumobjects = p.loadSDF("%sstadium.sdf" % self._urdfRoot) #move the stadium objects slightly above 0 for i in stadiumobjects: pos, orn = p.getBasePositionAndOrientation(i) newpos = [pos[0], pos[1], pos[2] + 0.1] p.resetBasePositionAndOrientation(i, newpos, orn) dist = 5 + 2. * random.random() ang = 2. * 3.1415925438 * random.random() ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 self._ballUniqueId = p.loadURDF("sphere2red.urdf", [ballx, bally, ballz]) p.setGravity(0, 0, -10) self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 for i in range(100): p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation)
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants self.stadium = p.loadSDF("stadium.sdf") self.ground_plane_mjcf = p.loadMJCF("mjcf/ground_plane.xml")
def load_pybullet_from_urdf_or_sdf(inp_path, position = [0, 0, 0], quaternion = [0, 0, 0, 1], fixed = True, packageMap = None): full_path = os.path.expandvars(inp_path) ext = full_path.split(".")[-1] if ext == "urdf": print "Loading ", full_path, " as URDF in pos ", position if packageMap is not None: # load text of URDF, resolve package URLS to absolute paths, # and save it out to a temp directory with open(full_path, 'r') as urdf_file: full_text = urdf_file.read() with open("/tmp/resolved_urdf.urdf", 'w') as out_file: # Find package references matches = re.finditer(r'package:\/\/[^\/]*\/', full_text) curr_ind = 0 # Replace them in output file with the global path for match in matches: out_file.write(full_text[curr_ind:match.start()]) out_file.write(packageMap.resolveFilename(match.group(0))) curr_ind = match.end() out_file.write(full_text[curr_ind:-1]) full_path = "/tmp/resolved_urdf.urdf" return pybullet.loadURDF(full_path, basePosition=position, baseOrientation=quaternion, useFixedBase=fixed) elif ext == "sdf": print "Loading ", full_path, " as SDF in pos ", position return pybullet.loadSDF(full_path) else: print "Unknown extension in path ", full_path, ": ", ext exit(-1)
def reset(self): objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] #for i in range (p.getNumJoints(self.kukaUid)): # print(p.getJointInfo(self.kukaUid,i)) p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce) self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) self.endEffectorPos = [0.537,0.0,0.5] self.endEffectorAngle = 0 self.motorNames = [] self.motorIndices = [] for i in range (self.numJoints): jointInfo = p.getJointInfo(self.kukaUid,i) qIndex = jointInfo[3] if qIndex > -1: #print("motorname") #print(jointInfo[1]) self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i)
def load_model(self, path, pose=None, fixed_base=True, scaling=1.0): """ Load the objects in the scene :param path: str Path to the object :param pose: Pose Position and Quaternion :param fixed_base: str Fixed in the scene :param scaling: float Scale object :return: int ID of the loaded object """ if path.endswith('.urdf'): body = p.loadURDF(path, useFixedBase=fixed_base, flags=0, globalScaling=scaling, physicsClientId=self.client_id) elif path.endswith('.sdf'): body = p.loadSDF(path, physicsClientId=self.client_id) elif path.endswith('.xml'): body = p.loadMJCF(path, physicsClientId=self.client_id) elif path.endswith('.bullet'): body = p.loadBullet(path, physicsClientId=self.client_id) else: raise ValueError(path) if pose is not None: self.set_pose(body, pose) return body
def reset(self): """Resets the robot back to a deterministic starting position.""" objects = p.loadSDF(os.path.join(self.urdf_rootpath, "kuka_iiwa/kuka_with_gripper2.sdf")) self.kuka_uid = objects[0] p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000]) self.joint_positions = [ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.num_joints = p.getNumJoints(self.kuka_uid) for joint_index in range(self.num_joints): p.resetJointState(self.kuka_uid, joint_index, self.joint_positions[joint_index]) p.setJointMotorControl2(self.kuka_uid, joint_index, p.POSITION_CONTROL, targetPosition=self.joint_positions[joint_index], force=self.max_force) self.tray_uid = p.loadURDF(os.path.join(self.urdf_rootpath, "tray/tray.urdf"), 0.640000, 0.075000, -0.190000, 0.000000, 0.000000, 1.000000, 0.000000) self.end_effector_pos = [0.537, 0.0, 0.5] self.end_effector_angle = 0 self.motor_names = [] self.motor_indices = [] for i in range(self.num_joints): joint_info = p.getJointInfo(self.kuka_uid, i) q_idx = joint_info[3] if q_idx > -1: self.motor_names.append(str(joint_info[1])) self.motor_indices.append(i)
def __init__(self, init_pos, robot_id, dt): self.id = robot_id self.dt = dt self.pybullet_id = p.loadSDF(os.path.join(os.path.dirname(__file__),"../models/robot.sdf"))[0] self.joint_ids = list(range(p.getNumJoints(self.pybullet_id))) self.initial_position = init_pos self.reset() self.time_elapsed = 0 self.switch_time = 10 self.is_goal = 0 self.goal = [] self.gx = None self.gy = None # No friction between bbody and surface. p.changeDynamics(self.pybullet_id, -1, lateralFriction=5., rollingFriction=0.) # Friction between joint links and surface. for i in range(p.getNumJoints(self.pybullet_id)): p.changeDynamics(self.pybullet_id, i, lateralFriction=5., rollingFriction=0.) self.messages_received = [] self.messages_to_send = [] self.neighbors = []
def __init__(self, init_pos, robot_id, dt): self.id = robot_id self.dt = dt self.pybullet_id = p.loadSDF("../models/robot.sdf")[0] self.joint_ids = list(range(p.getNumJoints(self.pybullet_id))) self.initial_position = init_pos self.reset() # No friction between body and surface. p.changeDynamics(self.pybullet_id, -1, lateralFriction=5., rollingFriction=0.) # Friction between joint links and surface. for i in range(p.getNumJoints(self.pybullet_id)): p.changeDynamics(self.pybullet_id, i, lateralFriction=5., rollingFriction=0.) self.messages_received = [] self.messages_to_send = [] self.neighbors = [] self.state = np.zeros((6, 3)) self.p = [[0.5, -1], [0.5, 1], [1.5, -1], [1.5, 1], [2.5, -1], [2.5, 1]] # square
def __init__(self, render=False, torque_limits=[100] * 6, init_noise=.005, ): self.render = render self.torque_limits = np.array(torque_limits) self.init_noise = init_noise low = -np.ones(6,dtype=np.float32) self.action_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32) low = -np.ones(17, dtype=np.float32)*np.inf self.observation_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32) if render: p.connect(p.GUI) else: p.connect(p.DIRECT) self.plane_id = p.loadSDF(pybullet_data.getDataPath() + "/plane_stadium.sdf")[0] self.walker_id = p.loadMJCF(pybullet_data.getDataPath() + "/mjcf/walker2d.xml")[0] p.setGravity(0, 0, -9.8) self.dt = p.getPhysicsEngineParameters()['fixedTimeStep'] self.reset()
def __init__(self, mech, k=[2000.0, 20.0], d=[0.45, 0.45]): """ This class defines the actions a gripper can take such as grasping a handle and executing PD control :param mech: the gen.generator_busybox.Mechanism being actuated :param k: a vector of length 2 where the first entry is the linear position (stiffness) gain and the second entry is the angular position gain :param d: a vector of length 2 where the first entry is the linear derivative (damping) gain and the second entry is the angular derivative gain """ self.use_gripper = False if self.use_gripper: self.id = p.loadSDF("models/gripper/gripper_high_fric.sdf")[0] self._left_finger_tip_id = 2 self._right_finger_tip_id = 5 self._left_finger_base_joint_id = 0 self._right_finger_base_joint_id = 3 self._finger_force = 20 self.pose_tip_world_reset = util.Pose([0.0, 0.0, 0.2], \ [0.50019904, 0.50019904, -0.49980088, 0.49980088]) # get mass of gripper mass = 0 for link in range(p.getNumJoints(self.id)): mass += p.getDynamicsInfo(self.id, link)[0] self._mass = mass self.errors = [] self.forces = [] # control parameters self.k = k self.d = d
def scene_2D_object(p): plane = [p.loadSDF(os.path.join(urdfRoot, "plane_stadium.sdf"))] colcubeId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.05, 0.05, 0.05]) cube = [p.createMultiBody(2, colcubeId, 2, [0.3, 0.3, 0.0])] colwallId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.05, 1.5, 0.5]) wall = [ p.createMultiBody(0, colwallId, 2, [1.4, 0, 0.0], p.getQuaternionFromEuler([0, 0, 0])) ] wall = [ p.createMultiBody(0, colwallId, 2, [-1.4, 0, 0.0], p.getQuaternionFromEuler([0, 0, 0])) ] wall = [ p.createMultiBody(0, colwallId, 2, [0, 1.4, 0], p.getQuaternionFromEuler([0, 0, math.pi / 2])) ] wall = [ p.createMultiBody(0, colwallId, 2, [0, -1.4, 0], p.getQuaternionFromEuler([0, 0, math.pi / 2])) ] return [cube]
def _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() # see https://github.com/bulletphysics/bullet3/issues/1934 to load multiple colors if self.render: visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.1, rgbaColor=[1, 0, 0, 1], specularColor=[0.4, .4, 0]) self.desired_pos_sphere = p.createMultiBody( baseMass=0, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=visualShapeId) rand_ori = self.np_random.uniform( low=-1, high=1, size=(4, )) + np.asarray([0, 0, 0, 2]) rand_ori = rand_ori / np.linalg.norm(rand_ori) rand_pos = self.np_random.uniform(low=-0.5, high=0.5, size=(3, )) #TODO Start with random vel self.quad = p.loadURDF(os.path.join(currentdir, "quad.urdf"), [0, 0, 0.5] + rand_pos, rand_ori, flags=p.URDF_USE_INERTIA_FROM_FILE) filename = os.path.join(pybullet_data.getDataPath(), "plane_stadium.sdf") self.ground_plane_mjcf = p.loadSDF(filename) # filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") # self.ground_plane_mjcf = self._p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i, -1, lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.8]) self.timeStep = 0.01 self.gravity = -9.8 p.setGravity(0, 0, self.gravity) # Default 0.04 for linear and angular damping. # p.changeDynamics(self.quad, -1, linearDamping=1) # p.changeDynamics(self.quad, -1, angularDamping=1) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) info = p.getDynamicsInfo(self.quad, -1) self.mass = info[0] self.zero_thrust = -self.mass * self.gravity initialCartPos = self.np_random.uniform(low=-2, high=2, size=(1, )) initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1, )) # p.resetJointState(self.quad, 1, initialAngle) # p.resetJointState(self.quad, 0, initialCartPos) world_pos, world_ori = p.getBasePositionAndOrientation(self.quad) world_vel, world_rot_vel = p.getBaseVelocity(self.quad) self.state = world_pos + world_ori + world_vel + world_rot_vel return np.array(self.state)
def reset(self): objects = p.loadSDF( os.path.join(self.urdfRootPath, "kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] p.resetBasePositionAndOrientation( self.kukaUid, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000], ) self.jointPositions = [ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200, ] if self.jp_override: for j, v in self.jp_override.items(): j_ix = int(j) - 1 if j_ix >= 0 and j_ix <= 13: self.jointPositions[j_ix] = v self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range(self.numJoints): p.resetJointState(self.kukaUid, jointIndex, self.jointPositions[jointIndex]) p.setJointMotorControl2( self.kukaUid, jointIndex, p.POSITION_CONTROL, targetPosition=self.jointPositions[jointIndex], force=self.maxForce, ) self.trayUid = p.loadURDF( os.path.join(self.urdfRootPath, "tray/tray.urdf"), 0.640000, 0.075000, -0.190000, 0.000000, 0.000000, 1.000000, 0.000000, ) self.endEffectorPos = [0.537, 0.0, 0.5] self.endEffectorAngle = 0 self.motorNames = [] self.motorIndices = []
def __init__(self): sdf = 'salamandra_robotica.sdf' self.identity = pybullet.loadSDF(sdf)[0] pybullet.resetBasePositionAndOrientation( bodyUniqueId=self.identity, posObj=[0, 0, 0.1], ornObj=[0, 0, 0, 1], )
def _load_model_file_into_sim(self, filepath): sim_id = None if filepath[-5:] == '.urdf': sim_id = p.loadURDF(filepath, flags=p.URDF_MERGE_FIXED_LINKS) elif filepath[-4:] == '.sdf': sim_id = p.loadSDF(filepath) if isinstance(sim_id, tuple): sim_id = sim_id[0] return sim_id
def _load_scene(self): if self.hoop is None: self.hoop = pb.loadSDF(os.path.join( os.path.dirname(__file__), 'assets/bbbot_gazebo/models/hoop/model.sdf'), physicsClientId=self.client)[0] pb.resetBasePositionAndOrientation(self.hoop, self.hoopStartPos, self.hoopStartOrientation, physicsClientId=self.client) if self.cyl is None: self.cyl = pb.loadSDF(os.path.join( os.path.dirname(__file__), 'assets/bbbot_gazebo/models/cylinder/model.sdf'), physicsClientId=self.client)[0] pb.resetBasePositionAndOrientation(self.cyl, self.cylStartPos, self.cylStartOrientation, physicsClientId=self.client) if self.backboard is None: self.backboard = pb.loadSDF(os.path.join( os.path.dirname(__file__), 'assets/bbbot_gazebo/models/backboard/model.sdf'), physicsClientId=self.client)[0] pb.changeDynamics(self.backboard, -1, restitution=1.0) pb.resetBasePositionAndOrientation(self.backboard, self.backboardStartPos, self.backboardStartOrientation, physicsClientId=self.client) if self.ball is None: # TODO: why is this not respecting SDL restitution? self.ball = pb.loadSDF(os.path.join( os.path.dirname(__file__), 'assets/bbbot_gazebo/models/basketball/model.sdf'), physicsClientId=self.client)[0] # print (pb.getDynamicsInfo(self.ball, -1)) pb.changeDynamics(self.ball, -1, restitution=0.853) # print (pb.getDynamicsInfo(self.ball, -1)) pb.resetBasePositionAndOrientation(self.ball, self.ballStartPos, self.ballStartOrientation, physicsClientId=self.client)
def __init__(self): self.gravity = -9.8 self.state = None self.stateWOCrop = None self.believedState = None self.pixelWidth = 1000 self.pixelHeight = 1000 self.camDistance = -3 self.camTargetPos = [0, 0, 0] self.near = 1 self.far = 1000 self.fov = 50 self.boxcount = 0 self.Y = 0 self.X = 0 self.ldcWidth = 0.68 self.ldcHeight = 0.68 self.ldcLength = 1.2 self.slabWidth = 0.02 self.resp = 0 self.resw = 0 # This will be given value in the render function self.action_space = None physicsClient = p.connect(p.DIRECT) p.setGravity(0, 0, self.gravity) # this sets the gravity p.setPhysicsEngineParameter(fixedTimeStep=1 / 60, numSolverIterations=10) p.loadSDF('ldc-model-big_center.sdf') # p.setRealTimeSimulation(1) ###### Adding floor ########k planeId = p.createCollisionShape(p.GEOM_PLANE) planeVisualId = p.createVisualShape( p.GEOM_PLANE, rgbaColor=[0.7686, 0.847, 0.933, 1]) p.createMultiBody(0, planeId, planeVisualId, [0, 0, 0], [0, 0, 0, 1]) self.resPix() self.resWorld()
def reset(self): objects = p.loadSDF( os.path.join(self.urdfRootPath, "kuka_iiwa/kuka_with_gripper2.sdf")) self.kukaUid = objects[0] p.resetBasePositionAndOrientation( self.kukaUid, [-0.100000, 0.000000, 0.070000], [0.000000, 0.000000, 0.000000, 1.000000]) # self.jointPositions = [0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, # -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200] self.init_joints = [ 0.0052822753, 0.63178448, -0.009966143, -1.7201607, 0.008282072, 0.7897110, -0.00860167, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] self.numJoints = p.getNumJoints(self.kukaUid) for jointIndex in range(self.numJoints): p.resetJointState(self.kukaUid, jointIndex, self.init_joints[jointIndex]) p.setJointMotorControl2( self.kukaUid, jointIndex, p.POSITION_CONTROL, targetPosition=self.init_joints[jointIndex], force=self.maxForce) goal_pose = spaces.Box( low=np.array([0.54, -0.155, 0.31630]) - np.array([0.2, 0.2, 0.1]), high=np.array([0.54, -0.155, 0.31630]) + np.array([0.2, 0.2, 0.1])) # 爪子初始位置 随机化 for _ in range(200): self.applyAction(goal_pose.sample(), 0, fingerAngle=0.25) p.stepSimulation() # 框子 self.trayUid = p.loadURDF( os.path.join(self.urdfRootPath, "tray/tray.urdf"), 0.540000, -0.255000, -0.190000, 0.000000, 0.000000, 1.000000, 0.000000) self.endEffectorAngle = 0 self.current_endEffectorAngle = 0 self.motorNames = [] self.motorIndices = [] for i in range(self.numJoints): jointInfo = p.getJointInfo(self.kukaUid, i) qIndex = jointInfo[3] if qIndex > -1: self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i)
def __init__( self, render=False, torque_limits=[100] * 6, init_noise=.005, physics_params=None, dynamics_params=None, ): self.args = locals() self.torque_limits = np.array(torque_limits) self.init_noise = init_noise self.cur_step = 0 low = -np.ones(6) self.action_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32) low = -np.ones(19) * np.inf self.observation_space = gym.spaces.Box(low=low, high=-low, dtype=np.float32) if render: p.connect(p.GUI) else: p.connect(p.DIRECT) self.plane_id = p.loadSDF(pybullet_data.getDataPath() + "/plane_stadium.sdf")[0] self.walker_id = p.loadMJCF(pybullet_data.getDataPath() + "/mjcf/walker2d.xml")[0] #flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)[0] # TODO not sure the self collision needs to be here.. p.setGravity(0, 0, -9.8) if physics_params is None: physics_params = {} if dynamics_params is None: dynamics_params = {} p.changeDynamics(self.plane_id, -1, **dynamics_params) for i in range(p.getNumJoints(self.walker_id)): p.changeDynamics(self.walker_id, i, **dynamics_params) p.changeDynamics(self.walker_id, -1, **dynamics_params) p.setPhysicsEngineParameter(**physics_params) self.dt = p.getPhysicsEngineParameters()['fixedTimeStep'] self.reset()
def reset(self): self.ordered_joints = [] self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( # TODO: Not sure if this works, try it with kuka p.loadSDF(os.path.join("models_robot", self.model_sdf))) self.robot_specific_reset() s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use self.potential = self.calc_potential() return s
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.stadium = p.loadSDF(filename) planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") self.ground_plane_mjcf = p.loadMJCF(planeName) for i in self.ground_plane_mjcf: p.changeVisualShape(i,-1,rgbaColor=[0,0,0,0])
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.ground_plane_mjcf = p.loadSDF(filename) for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) for j in range(p.getNumJoints(i)): p.changeDynamics(i,j,lateralFriction=0)
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"plane_stadium.sdf") self.ground_plane_mjcf=p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") #self.ground_plane_mjcf = p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
import pybullet as p import pybullet_data import time p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadSDF("stadium.sdf") p.setGravity(0, 0, -10) objects = p.loadMJCF("mjcf/sphere.xml") sphere = objects[0] p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1]) p.changeDynamics(sphere, -1, linearDamping=0.9) p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1]) forward = 0 turn = 0 forwardVec = [2, 0, 0] cameraDistance = 1 cameraYaw = 35 cameraPitch = -35 while (1): spherePos, orn = p.getBasePositionAndOrientation(sphere) cameraTargetPosition = spherePos p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition) camInfo = p.getDebugVisualizerCamera() camForward = camInfo[5] keys = p.getKeyboardEvents()
# split by alignment word chunks = wholeFile.split(b'\xaa\xbb') log = list() for chunk in chunks: if len(chunk) == sz: values = struct.unpack(fmt, chunk) record = list() for i in range(ncols): record.append(values[i]) log.append(record) return log #clid = p.connect(p.SHARED_MEMORY) p.connect(p.GUI) p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") p.loadURDF("tray/tray.urdf",[0,0,0]) p.loadURDF("block.urdf",[0,0,2]) log = readLogFile("data/block_grasp_log.bin") recordNum = len(log) itemNum = len(log[0]) objectNum = p.getNumBodies() print('record num:'), print(recordNum) print('item num:'), print(itemNum) def Step(stepIndex):
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)
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.)
import pybullet as p import time p.connect(p.GUI) p.resetSimulation() timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json") p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) print("load plane.urdf") p.loadURDF("plane.urdf") print("load r2d2.urdf") p.loadURDF("r2d2.urdf",0,0,1) print("load kitchen/1.sdf") p.loadSDF("kitchens/1.sdf") print("load 100 times plate.urdf") for i in range (100): p.loadURDF("dinnerware/plate.urdf",0,i,1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.stopStateLogging(timinglog) print("stopped state logging") p.getCameraImage(320,200) while (1): p.stepSimulation()
import time cid = p.connect(p.SHARED_MEMORY) if (cid<0): p.connect(p.GUI) p.resetSimulation() p.setGravity(0,0,-10) useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this #p.loadURDF("plane.urdf") p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf")) car = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"racecar/racecar.urdf")) for i in range (p.getNumJoints(car)): print (p.getJointInfo(car,i)) inactive_wheels = [3,5,7] wheels = [2] for wheel in inactive_wheels: p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) steering = [4,6] targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0) maxForceSlider = p.addUserDebugParameter("maxForce",0,10,10)
import pybullet as p p.connect(p.DIRECT) p.loadPlugin("eglRendererPlugin") p.loadSDF("newsdf.sdf") while (1): p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK) p.stepSimulation()
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]) p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)] objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") kuka_gripper = objects[0] print ("kuka gripper=") print(kuka_gripper) p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970]) jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] for jointIndex in range (p.getNumJoints(kuka_gripper)): p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex]) p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
uiCube = p.createMultiBody(baseMass=0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0, 1, 0], useMaximalCoordinates=True) textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963]) numLines = 1 lines = [-1] * numLines p.stepSimulation() #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)] kitchenObj = p.loadSDF("kitchens/1.sdf") #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]) p.setJointMotorControl2(pr2_gripper, jointIndex, p.POSITION_CONTROL, targetPosition=0, force=0) pr2_cid = p.createConstraint(pr2_gripper, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.2, 0, 0],