Python pybullet.createMultiBody方法代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def generate_targets(self):
        self.target_indices_to_ignore = []
        if self.gender == 'male':
            self.upperarm, self.upperarm_length, self.upperarm_radius = 5, 0.279, 0.043
            self.forearm, self.forearm_length, self.forearm_radius = 7, 0.257, 0.033
        else:
            self.upperarm, self.upperarm_length, self.upperarm_radius = 5, 0.264, 0.0355
            self.forearm, self.forearm_length, self.forearm_radius = 7, 0.234, 0.027

        self.targets_pos_on_upperarm = self.util.capsule_points(p1=np.array([0, 0, 0]), p2=np.array([0, 0, -self.upperarm_length]), radius=self.upperarm_radius, distance_between_points=0.03)
        self.targets_pos_on_forearm = self.util.capsule_points(p1=np.array([0, 0, 0]), p2=np.array([0, 0, -self.forearm_length]), radius=self.forearm_radius, distance_between_points=0.03)

        sphere_collision = -1
        sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.01, rgbaColor=[0, 1, 1, 1], physicsClientId=self.id)
        self.targets_upperarm = []
        self.targets_forearm = []
        for _ in range(len(self.targets_pos_on_upperarm)):
            self.targets_upperarm.append(p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=[0, 0, 0], useMaximalCoordinates=False, physicsClientId=self.id))
        for _ in range(len(self.targets_pos_on_forearm)):
            self.targets_forearm.append(p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=[0, 0, 0], useMaximalCoordinates=False, physicsClientId=self.id))
        self.total_target_count = len(self.targets_upperarm) + len(self.targets_forearm)
        self.update_targets() 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def _flag_reposition(self):
        self.walk_target_x = self.np_random.uniform(low=-self.scene.stadium_halflen,
                                                    high=+self.scene.stadium_halflen)
        self.walk_target_y = self.np_random.uniform(low=-self.scene.stadium_halfwidth,
                                                    high=+self.scene.stadium_halfwidth)

        more_compact = 0.5  # set to 1.0 whole football field
        self.walk_target_x *= more_compact / self.robot.mjcf_scaling
        self.walk_target_y *= more_compact / self.robot.mjcf_scaling

        self.flag = None
        #self.flag = self.scene.cpp_world.debug_sphere(self.walk_target_x, self.walk_target_y, 0.2, 0.2, 0xFF8080)
        self.flag_timeout = 3000 / self.scene.frame_skip
        #print('targetxy', self.flagid, self.walk_target_x, self.walk_target_y, p.getBasePositionAndOrientation(self.flagid))
        #p.resetBasePositionAndOrientation(self.flagid, posObj = [self.walk_target_x, self.walk_target_y, 0.5], ornObj = [0,0,0,0])
        if self.gui:
            if self.lastid:
                p.removeBody(self.lastid)

            self.lastid = p.createMultiBody(baseVisualShapeIndex=self.visualid, baseCollisionShapeIndex=-1, basePosition=[self.walk_target_x, self.walk_target_y, 0.5])

        self.robot.walk_target_x = self.walk_target_x
        self.robot.walk_target_y = self.walk_target_y 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def load_mesh(self, mesh, scale=None, position=None, orientation=None, mass=1):
        if scale is None:
            scale = [1, 1, 1]
        if position is None:
            position = [0, 0, 0.1]
        if orientation is None:
            # Random orientation
            r = np.random.rand()
            orientation = [r ** 2, 0, 0, (1 - r) ** 2]
        c_id = pb.createCollisionShape(shapeType=pb.GEOM_MESH, fileName=mesh, meshScale=scale)
        o_id = pb.createMultiBody(baseMass=mass, baseCollisionShapeIndex=c_id, basePosition=position,
                                baseOrientation=orientation)
        self.objects.append(o_id)
        return o_id 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def display_cup_points(self):
        sphere_collision = -1
        sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.01, rgbaColor=[0, 1, 1, 1], physicsClientId=self.id)
        cup_pos, cup_orient = p.getBasePositionAndOrientation(self.cup, physicsClientId=self.id)
        cup_pos, cup_orient = p.multiplyTransforms(cup_pos, cup_orient, [0, 0.06, 0], p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
        top_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_top_center_offset, [0, 0, 0, 1], physicsClientId=self.id)
        bottom_center_pos, _ = p.multiplyTransforms(cup_pos, cup_orient, self.cup_bottom_center_offset, [0, 0, 0, 1], physicsClientId=self.id)
        self.cup_top_center = p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=top_center_pos, useMaximalCoordinates=False, physicsClientId=self.id)
        self.cup_bottom_center = p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=bottom_center_pos, useMaximalCoordinates=False, physicsClientId=self.id)

        cylinder_collision = -1
        cylinder_visual = p.createVisualShape(shapeType=p.GEOM_CYLINDER, radius=0.05, length=0.14, rgbaColor=[0, 1, 1, 0.25], physicsClientId=self.id)
        self.cup_cylinder = p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=cylinder_collision, baseVisualShapeIndex=cylinder_visual, basePosition=cup_pos, baseOrientation=cup_orient, useMaximalCoordinates=False, physicsClientId=self.id) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0):
        if left:
            gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
        else:
            gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
        transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id)
        if self.task == 'scratch_itch':
            tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
        elif self.task == 'bed_bathing':
            tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
        elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']:
            if self.task == 'drinking':
                visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj')
                collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj')
            elif self.task in ['scooping', 'feeding']:
                visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj')
                collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj')
            elif self.task == 'arm_manipulation':
                visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj')
                collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj')
            tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id)
            tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id)
            tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id)
        if left:
            # Disable collisions between the tool and robot
            for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
                for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
                    p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
            # Create constraint that keeps the tool in the gripper
            constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
        else:
            # Disable collisions between the tool and robot
            for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
                for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
                    p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
            # Create constraint that keeps the tool in the gripper
            constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
        p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
        return tool 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def generate_target(self):
        # Randomly select either upper arm or forearm for the target limb to scratch
        if self.gender == 'male':
            self.limb, length, radius = [[5, 0.279, 0.043], [7, 0.257, 0.033]][self.np_random.randint(2)]
        else:
            self.limb, length, radius = [[5, 0.264, 0.0355], [7, 0.234, 0.027]][self.np_random.randint(2)]
        self.target_on_arm = self.util.point_on_capsule(p1=np.array([0, 0, 0]), p2=np.array([0, 0, -length]), radius=radius, theta_range=(0, np.pi*2))
        arm_pos, arm_orient = p.getLinkState(self.human, self.limb, computeForwardKinematics=True, physicsClientId=self.id)[:2]
        target_pos, target_orient = p.multiplyTransforms(arm_pos, arm_orient, self.target_on_arm, [0, 0, 0, 1], physicsClientId=self.id)

        sphere_collision = -1
        sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.01, rgbaColor=[0, 1, 1, 1], physicsClientId=self.id)
        self.target = p.createMultiBody(baseMass=0.0, baseCollisionShapeIndex=sphere_collision, baseVisualShapeIndex=sphere_visual, basePosition=target_pos, useMaximalCoordinates=False, physicsClientId=self.id)

        self.update_targets() 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def _flag_reposition(self):
        walk_target_x, walk_target_y, _ = self.robot.get_target_position()

        self.flag = None
        if self.gui and not self.config["display_ui"]:
            self.visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.5, 0.5, 0.5], rgbaColor=[1, 0, 0, 0])
            self.last_flagId = p.createMultiBody(baseVisualShapeIndex=self.visual_flagId, baseCollisionShapeIndex=-1, basePosition=[walk_target_x, walk_target_y, 0.5]) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def _flag_reposition(self):
        target_pos = self.robot.target_pos

        self.flag = None
        if self.gui and not self.config["display_ui"]:
            self.visual_flagId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.5, 0.5, 0.5], rgbaColor=[1, 0, 0, 0.7])
            self.last_flagId = p.createMultiBody(baseVisualShapeIndex=self.visual_flagId, baseCollisionShapeIndex=-1, basePosition=[target_pos[0], target_pos[1], 0.5]) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createMultiBody [as 別名]
def __init__(self, config, gpu_count=0):
        #assert(self.config["envname"] == self.__class__.__name__ or self.config["envname"] == "TestEnv")
        self.config = self.parse_config(config)
        SemanticRobotEnv.__init__(self, self.config, gpu_count, 
                                  scene_type="building",
                                  tracking_camera=tracking_camera)
        self.robot_introduce(Husky(self.config, env=self))
        self.scene_introduce()

        self.total_reward = 0
        self.total_frame = 0
        self.flag_timeout = 1
        self.visualid = -1
        self.lastid = None
        self.gui = self.config["mode"] == "gui"
        
        if self.gui:
            self.visualid = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.2, 0.2, 0.2], rgbaColor=[1, 0, 0, 0.7])
        self.colisionid = p.createCollisionShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.2, 0.2, 0.2])

        self.lastid = None
        self.obstacle_dist = 100

        self.semantic_flagIds = []

        debug_semantic = 1
        if debug_semantic:
            for i in range(self.semantic_pos.shape[0]):
                pos = self.semantic_pos[i]
                pos[2] += 0.2   # make flag slight above object 
                visualId = p.createVisualShape(p.GEOM_MESH, fileName=os.path.join(pybullet_data.getDataPath(), 'cube.obj'), meshScale=[0.1, 0.1, 0.1], rgbaColor=[1, 0, 0, 0.7])
                flagId = p.createMultiBody(baseVisualShapeIndex=visualId, baseCollisionShapeIndex=-1, basePosition=pos)
                self.semantic_flagIds.append(flagId) 

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

        # 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(get_model_path(model_id), "mesh_z_up.obj")
        #filename = os.path.join(get_model_path(model_id), "3d", "blender.obj")
        #textureID = p.loadTexture(os.path.join(get_model_path(model_id), "3d", "rgb.mtl"))

        if robot.model_type == "MJCF":
            MJCF_SCALING = robot.mjcf_scaling
            scaling = [1.0/MJCF_SCALING, 1.0/MJCF_SCALING, 0.6/MJCF_SCALING]
        else:
            scaling  = [1, 1, 1]
        magnified = [2, 2, 2]

        collisionId = p.createCollisionShape(p.GEOM_MESH, fileName=filename, meshScale=scaling, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)


        view_only_mesh = os.path.join(get_model_path(model_id), "mesh_view_only_z_up.obj")
        if os.path.exists(view_only_mesh):
            visualId = p.createVisualShape(p.GEOM_MESH,
                                       fileName=view_only_mesh,
                                       meshScale=scaling, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
        else:
            visualId = -1

        boundaryUid = p.createMultiBody(baseCollisionShapeIndex = collisionId, baseVisualShapeIndex = visualId)
        p.changeDynamics(boundaryUid, -1, lateralFriction=1)
        #print(p.getDynamicsInfo(boundaryUid, -1))
        self.scene_obj_list = [(boundaryUid, -1)]       # baselink index -1
        

        planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
        self.ground_plane_mjcf = p.loadMJCF(planeName)
        
        if "z_offset" in self.env.config:
            z_offset = self.env.config["z_offset"]
        else:
            z_offset = -10 #with hole filling, we don't need ground plane to be the same height as actual floors

        p.resetBasePositionAndOrientation(self.ground_plane_mjcf[0], posObj = [0,0,z_offset], ornObj = [0,0,0,1])
        p.changeVisualShape(boundaryUid, -1, rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
                            specularColor=[0.5, 0.5, 0.5])