Python pybullet.loadSDF方法代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
def _setup(self):
        '''
        Create the mug at a random position on the ground, handle facing
        roughly towards the robot. Robot's job is to grab and lift.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        sdf_dir = os.path.join(path, self.sdf_dir)
        obj_to_add = os.path.join(sdf_dir, self.model, self.model_file_name)

        identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
        try:
            obj_id_list = pb.loadSDF(obj_to_add)
            for obj_id in obj_id_list:
                random_position = np.random.rand(
                    3) * self.spawn_pos_delta + self.spawn_pos_min
                pb.resetBasePositionAndOrientation(
                    obj_id, random_position, identity_orientation)
        except Exception, e:
            print e 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
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) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
def load(self, path, pos=[0, 0, 0], euler=[0, 0, 0], fixed=False):
        """Load an body into the simulation."""
        # Set data path
        assert osp.exists(path), \
                'Model path {} does not exist.'.format(path)
        # Load body model
        model_name, ext = osp.splitext(path)
        if ext == '.urdf':
            quat = self.quat_from_euler(euler)
            uid = p.loadURDF(path, pos, quat, useFixedBase=fixed)
        elif ext == '.sdf':
            uid = p.loadSDF(path)
        else:
            raise ValueError('Unrecognized extension {}.'.format(ext))
        return uid 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
def reset(self):
        """
        Reset the environment
        """
        objects = p.loadSDF(os.path.join(self.urdf_root_path, "kuka_iiwa/kuka_with_gripper2.sdf"))
        self.kuka_uid = objects[0]

        p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, -0.15],
                                          [0.000000, 0.000000, 0.000000, 1.000000])
        self.joint_positions = [0.006418, 0.113184, -0.011401, -1.289317, 0.005379, 1.737684, -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 jointIndex in range(self.num_joints):
            p.resetJointState(self.kuka_uid, jointIndex, self.joint_positions[jointIndex])
            p.setJointMotorControl2(self.kuka_uid, jointIndex, p.POSITION_CONTROL,
                                    targetPosition=self.joint_positions[jointIndex], force=self.max_force)

        self.end_effector_pos = np.array([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_index = joint_info[3]
            if q_index > -1:
                self.motor_names.append(str(joint_info[1]))
                self.motor_indices.append(i) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
def _setup(self):
        '''
        Create task by adding objects to the scene
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        urdf_dir = os.path.join(path, self.urdf_dir)
        sdf_dir = os.path.join(path, self.sdf_dir)
        objs = [obj for obj in os.listdir(
            sdf_dir) if os.path.isdir(os.path.join(sdf_dir, obj))]

        randn = np.random.randint(1, len(objs))

        objs_name_to_add = np.random.choice(objs, randn)
        objs_to_add = [os.path.join(sdf_dir, obj, self.model_file_name)
                       for obj in objs_name_to_add]

        identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
        # load sdfs for all objects and initialize positions
        for obj_index, obj in enumerate(objs_to_add):
            if objs_name_to_add[obj_index] in self.models:
                try:
                    print 'Loading object: ', obj
                    obj_id_list = pb.loadSDF(obj)
                    for obj_id in obj_id_list:
                        self.objs.append(obj_id)
                        random_position = np.random.rand(
                            3) * self.spawn_pos_delta + self.spawn_pos_min
                        pb.resetBasePositionAndOrientation(
                            obj_id, random_position, identity_orientation)
                except Exception, e:
                    print e 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
def _setup(self):
        '''
        Create random objects at random positions. Load random objects from the
        scene and create them in different places. In the future we may want to
        switch from using the list of "all" objects to a subset that we can
        actually pick up and manipulate.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        sdf_dir = os.path.join(path, self.sdf_dir)
        objs = [obj for obj in os.listdir(
            sdf_dir) if os.path.isdir(os.path.join(sdf_dir, obj))]

        randn = np.random.randint(1, len(objs))

        objs_name_to_add = np.random.choice(objs, randn)
        objs_to_add = [os.path.join(sdf_dir, obj, self.model_file_name)
                       for obj in objs_name_to_add]

        identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
        # load sdfs for all objects and initialize positions
        for obj_index, obj in enumerate(objs_to_add):
            if objs_name_to_add[obj_index] in self.models:
                try:
                    print 'Loading object: ', obj
                    obj_id_list = pb.loadSDF(obj)
                    for obj_id in obj_id_list:
                        self.objs.append(obj_id)
                        random_position = np.random.rand(
                            3) * self.spawn_pos_delta + self.spawn_pos_min
                        pb.resetBasePositionAndOrientation(
                            obj_id, random_position, identity_orientation)
                except Exception, e:
                    print e 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
def __init__(self, robot, gravity, timestep, frame_skip, env=None):
        Scene.__init__(self, gravity, timestep, frame_skip, env)

        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:
            pos, orn = p.getBasePositionAndOrientation(i)
            p.resetBasePositionAndOrientation(i, [pos[0], pos[1], pos[2] - 0.005], orn)

        for i in self.ground_plane_mjcf:
            p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.5])

        self.scene_obj_list = self.stadium 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import loadSDF [as 別名]
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(os.path.join(os.path.dirname(__file__), "other_assets", "stadium.sdf"))
		self.ground_plane_mjcf = p.loadMJCF(os.path.join(os.path.dirname(__file__), "mujoco_assets", "ground_plane.xml"))