nbv_reconstruction/runners/simulator.py

import pybullet as p
import pybullet_data
import numpy as np
import os
import time
from PytorchBoot.runners.runner import Runner
import PytorchBoot.stereotype as stereotype
from PytorchBoot.config import ConfigManager
from utils.control import ControlUtil


@stereotype.runner("simulator")
class Simulator(Runner):
    CREATE: str = "create"
    SIMULATE: str = "simulate"
    INIT_GRIPPER_POSE:np.ndarray = np.asarray(
        [[0.41869126  ,0.87596275 , 0.23951774 , 0.36005292],
        [ 0.70787907 ,-0.4800251  , 0.51813998 ,-0.40499909],
        [ 0.56884584, -0.04739109 ,-0.82107382  ,0.76881103],
        [ 0.         , 0.    ,      0.      ,    1.        ]])
    TURNTABLE_WORLD_TO_PYBULLET_WORLD:np.ndarray = np.asarray(
        [[1, 0, 0, 0.8],
        [0, 1, 0, 0],
        [0, 0, 1, 0.5],
        [0, 0, 0, 1]])
    
    debug_pose = np.asarray([
        [
            0.992167055606842,
            -0.10552699863910675,
            0.06684812903404236,
            -0.07388903945684433
        ],
        [
            0.10134342312812805,
            0.3670985698699951,
            -0.9246448874473572,
            -0.41582486033439636
        ],
        [
            0.07303514331579208,
            0.9241767525672913,
            0.37491756677627563,
            1.0754833221435547
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ]])
    
    def __init__(self, config_path):
        super().__init__(config_path)
        self.config_path = config_path
        self.robot_id = None
        self.turntable_id = None
        self.target_id = None
        camera_config = ConfigManager.get("simulation", "camera")
        self.camera_params = {
            'width': camera_config["width"],
            'height': camera_config["height"],
            'fov': camera_config["fov"],
            'near': camera_config["near"],
            'far': camera_config["far"]
        }
        self.sim_config = ConfigManager.get("simulation")
        
    def run(self, cmd):
        print(f"Simulator run {cmd}")
        if cmd == self.CREATE:
            self.prepare_env()
            self.create_env()
        elif cmd == self.SIMULATE:
            self.simulate()

    def simulate(self):
        self.reset()
        self.init()
        debug_pose = Simulator.debug_pose
        offset = np.asarray([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
        debug_pose = debug_pose @ offset
        for _ in range(10000):
            debug_pose_2 = np.eye(4)
            debug_pose_2[0,0] = -1
            debug_pose_2[2,3] = 0.5
            self.move_to(debug_pose_2)
            # Wait for the system to stabilize
            for _ in range(20):  # Simulate 20 steps to ensure stability
                p.stepSimulation()
                time.sleep(0.001)  # Add small delay to ensure physics simulation
            
            depth_img, segm_img = self.take_picture()
            p.stepSimulation()

    def prepare_env(self):
        p.connect(p.GUI)
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        p.setGravity(0, 0, 0)
        p.loadURDF("plane.urdf")
        
    def create_env(self):
        print(self.config)
        robot_config = self.sim_config["robot"]
        turntable_config = self.sim_config["turntable"]
        target_config = self.sim_config["target"]
        
        self.robot_id = p.loadURDF(
            robot_config["urdf_path"],
            robot_config["initial_position"],
            p.getQuaternionFromEuler(robot_config["initial_orientation"]),
            useFixedBase=True
        )
        
        p.changeDynamics(
            self.robot_id,
            linkIndex=-1,
            mass=0,
            linearDamping=0,
            angularDamping=0,
            lateralFriction=0
        )
        
        visual_shape_id = p.createVisualShape(
            shapeType=p.GEOM_CYLINDER,
            radius=turntable_config["radius"],
            length=turntable_config["height"],
            rgbaColor=[0.7, 0.7, 0.7, 1]
        )
        collision_shape_id = p.createCollisionShape(
            shapeType=p.GEOM_CYLINDER,
            radius=turntable_config["radius"],
            height=turntable_config["height"]
        )
        self.turntable_id = p.createMultiBody(
            baseMass=0,  # 设置质量为0使其成为静态物体
            baseCollisionShapeIndex=collision_shape_id,
            baseVisualShapeIndex=visual_shape_id,
            basePosition=turntable_config["center_position"]
        )
        
        # 禁用转盘的动力学
        p.changeDynamics(
            self.turntable_id,
            -1,  # -1 表示基座
            mass=0,
            linearDamping=0,
            angularDamping=0,
            lateralFriction=0
        )
        
        
        obj_path = os.path.join(target_config["obj_dir"], target_config["obj_name"], "mesh.obj")

        assert os.path.exists(obj_path), f"Error: File not found at {obj_path}"

        # 加载OBJ文件作为目标物体
        target_visual = p.createVisualShape(
            shapeType=p.GEOM_MESH,
            fileName=obj_path,
            rgbaColor=target_config["rgba_color"],
            specularColor=[0.4, 0.4, 0.4],
            meshScale=[target_config["scale"]] * 3
        )
        
        # 使用简化的碰撞形状
        target_collision = p.createCollisionShape(
            shapeType=p.GEOM_MESH,
            fileName=obj_path,
            meshScale=[target_config["scale"]] * 3,
            flags=p.GEOM_FORCE_CONCAVE_TRIMESH  # 尝试使用凹面网格
        )
        
        
        # 创建目标物体
        self.target_id = p.createMultiBody(
            baseMass=0,  # 设置质量为0使其成为静态物体
            baseCollisionShapeIndex=target_collision,
            baseVisualShapeIndex=target_visual,
            basePosition=[
                turntable_config["center_position"][0],
                turntable_config["center_position"][1],
                turntable_config["height"] + turntable_config["center_position"][2]
            ],
            baseOrientation=p.getQuaternionFromEuler([np.pi/2, 0, 0])
        )

        # 禁用目标物体的动力学
        p.changeDynamics(
            self.target_id,
            -1,  # -1 表示基座
            mass=0,
            linearDamping=0,
            angularDamping=0,
            lateralFriction=0
        )

        # 创建固定约束，将目标物体固定在转盘上
        cid = p.createConstraint(
            parentBodyUniqueId=self.turntable_id,
            parentLinkIndex=-1,  # -1 表示基座
            childBodyUniqueId=self.target_id,
            childLinkIndex=-1,  # -1 表示基座
            jointType=p.JOINT_FIXED,
            jointAxis=[0, 0, 0],
            parentFramePosition=[0, 0, 0],  # 相对于转盘中心的偏移
            childFramePosition=[0, 0, 0]  # 相对于物体中心的偏移
        )

        # 设置约束参数
        p.changeConstraint(cid, maxForce=100)  # 设置最大力，确保约束稳定

    def move_robot_to_pose(self, target_matrix):
        # 从4x4齐次矩阵中提取位置（前3个元素）
        position = target_matrix[:3, 3]
        
        # 从3x3旋转矩阵中提取方向四元数
        R = target_matrix[:3, :3]
        
        # 计算四元数的w分量
        w = np.sqrt(max(0, 1 + R[0,0] + R[1,1] + R[2,2])) / 2
        
        # 避免除零错误，同时处理不同情况
        if abs(w) < 1e-8:
            # 当w接近0时的特殊情况
            x = np.sqrt(max(0, 1 + R[0,0] - R[1,1] - R[2,2])) / 2
            y = np.sqrt(max(0, 1 - R[0,0] + R[1,1] - R[2,2])) / 2
            z = np.sqrt(max(0, 1 - R[0,0] - R[1,1] + R[2,2])) / 2
            
            # 确定符号
            if R[2,1] - R[1,2] < 0: x = -x
            if R[0,2] - R[2,0] < 0: y = -y
            if R[1,0] - R[0,1] < 0: z = -z
        else:
            # 正常情况
            x = (R[2,1] - R[1,2]) / (4 * w)
            y = (R[0,2] - R[2,0]) / (4 * w)
            z = (R[1,0] - R[0,1]) / (4 * w)
        
        orientation = (x, y, z, w)
        
        # 设置IK求解参数
        num_joints = p.getNumJoints(self.robot_id)
        lower_limits = []
        upper_limits = []
        joint_ranges = []
        rest_poses = []
        
        # 获取关节限制和默认姿态
        for i in range(num_joints):
            joint_info = p.getJointInfo(self.robot_id, i)
            lower_limits.append(joint_info[8])
            upper_limits.append(joint_info[9])
            joint_ranges.append(joint_info[9] - joint_info[8])
            rest_poses.append(0)  # 可以设置一个较好的默认姿态
        
        # 使用增强版IK求解器，考虑碰撞避障
        joint_poses = p.calculateInverseKinematics(
            self.robot_id,
            7,  # end effector link index
            position,
            orientation,
            lowerLimits=lower_limits,
            upperLimits=upper_limits,
            jointRanges=joint_ranges,
            restPoses=rest_poses,
            maxNumIterations=100,
            residualThreshold=1e-4
        )
        
        # 分步移动到目标位置，同时检查碰撞
        current_poses = [p.getJointState(self.robot_id, i)[0] for i in range(7)]
        steps = 50  # 分50步移动
        
        for step in range(steps):
            # 线性插值计算中间位置
            intermediate_poses = []
            for current, target in zip(current_poses, joint_poses):
                t = (step + 1) / steps
                intermediate = current + (target - current) * t
                intermediate_poses.append(intermediate)
            
            # 设置关节位置
            for i in range(7):
                p.setJointMotorControl2(
                    self.robot_id,
                    i,
                    p.POSITION_CONTROL,
                    intermediate_poses[i]
                )
            
            # 执行一步模拟
            p.stepSimulation()
            
            # 检查碰撞
            if p.getContactPoints(self.robot_id, self.turntable_id):
                print("检测到潜在碰撞，停止移动")
                return False
        
        return True
    
    
    def rotate_turntable(self, angle_degrees):
        # 旋转转盘
        current_pos, current_orn = p.getBasePositionAndOrientation(self.turntable_id)
        current_orn = p.getEulerFromQuaternion(current_orn)
        
        new_orn = list(current_orn)
        new_orn[2] += np.radians(angle_degrees)
        new_orn_quat = p.getQuaternionFromEuler(new_orn)
        
        p.resetBasePositionAndOrientation(
            self.turntable_id,
            current_pos,
            new_orn_quat
        )
        
        # 同时旋转目标物体
        target_pos, target_orn = p.getBasePositionAndOrientation(self.target_id)
        target_orn = p.getEulerFromQuaternion(target_orn)
        
        # 更新目标物体的方向
        target_orn = list(target_orn)
        target_orn[2] += np.radians(angle_degrees)
        target_orn_quat = p.getQuaternionFromEuler(target_orn)
        
        # 计算物体新的位置（绕转盘中心旋转）
        turntable_center = current_pos
        relative_pos = np.array(target_pos) - np.array(turntable_center)
        
        # 创建旋转矩阵
        theta = np.radians(angle_degrees)
        rotation_matrix = np.array([
            [np.cos(theta), -np.sin(theta), 0],
            [np.sin(theta), np.cos(theta), 0],
            [0, 0, 1]
        ])
        
        # 计算新的相对位置
        new_relative_pos = rotation_matrix.dot(relative_pos)
        new_pos = np.array(turntable_center) + new_relative_pos
        
        # 更新目标物体的位置和方向
        p.resetBasePositionAndOrientation(
            self.target_id,
            new_pos,
            target_orn_quat
        )

    def get_camera_pose(self):
        end_effector_link = 7  # Franka末端执行器的链接索引
        state = p.getLinkState(self.robot_id, end_effector_link)
        ee_pos = state[0]  # 世界坐标系中的位置
        camera_orn = state[1]  # 世界坐标系中的朝向（四元数）
        
        # 计算相机的视角矩阵
        rot_matrix = p.getMatrixFromQuaternion(camera_orn)
        rot_matrix = np.array(rot_matrix).reshape(3, 3)
        
        # 相机的前向向量（与末端执行器的x轴对齐）
        camera_forward = rot_matrix.dot(np.array([0, 0, 1]))  # x轴方向
        
        # 将相机位置向前偏移0.1米
        offset = 0.12
        camera_pos = np.array(ee_pos) + camera_forward * offset
        camera_target = camera_pos + camera_forward
        
        # 相机的上向量（与末端执行器的z轴对齐）
        camera_up = rot_matrix.dot(np.array([1, 0, 0]))  # z轴方向
        
        return camera_pos, camera_target, camera_up
        
    def take_picture(self):
        camera_pos, camera_target, camera_up = self.get_camera_pose()
        
        view_matrix = p.computeViewMatrix(
            cameraEyePosition=camera_pos,
            cameraTargetPosition=camera_target,
            cameraUpVector=camera_up
        )
        
        projection_matrix = p.computeProjectionMatrixFOV(
            fov=self.camera_params['fov'],
            aspect=self.camera_params['width'] / self.camera_params['height'],
            nearVal=self.camera_params['near'],
            farVal=self.camera_params['far']
        )
        
        _,_,rgb_img,depth_img,segm_img = p.getCameraImage(
            width=self.camera_params['width'],
            height=self.camera_params['height'],
            viewMatrix=view_matrix,
            projectionMatrix=projection_matrix,
            renderer=p.ER_BULLET_HARDWARE_OPENGL
        )

        depth_img = self.camera_params['far'] * self.camera_params['near'] / (
            self.camera_params['far'] - (self.camera_params['far'] - self.camera_params['near']) * depth_img)

        depth_img = np.array(depth_img)
        segm_img = np.array(segm_img)
        
        return depth_img, segm_img

    def reset(self):
        target_pos = [0.5, 0, 1]
        target_orn = p.getQuaternionFromEuler([np.pi, 0, 0])
        target_matrix = np.eye(4)
        target_matrix[:3, 3] = target_pos
        target_matrix[:3, :3] = np.asarray(p.getMatrixFromQuaternion(target_orn)).reshape(3,3)
        self.move_robot_to_pose(target_matrix)

    def init(self):
        self.move_to(Simulator.INIT_GRIPPER_POSE)

    def move_to(self, pose: np.ndarray):
        #delta_degree, min_new_cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
        #print(delta_degree)
        min_new_cam_to_pybullet_world = Simulator.TURNTABLE_WORLD_TO_PYBULLET_WORLD@pose
        self.move_to_cam_pose(min_new_cam_to_pybullet_world)
        #self.rotate_turntable(delta_degree)
        
    
        
    def __del__(self):
        p.disconnect()

    def create_experiment(self, backup_name=None):
        return super().create_experiment(backup_name)
    
    def load_experiment(self, backup_name=None):
        super().load_experiment(backup_name)

    def move_to_cam_pose(self, camera_pose: np.ndarray):
        # 从相机位姿矩阵中提取位置和旋转矩阵
        camera_pos = camera_pose[:3, 3]
        R_camera = camera_pose[:3, :3]
        
        # 相机的朝向向量（z轴）
        forward = R_camera[:, 2]
        
        # 由于相机与末端执行器之间有固定偏移，需要计算末端执行器位置
        # 相机在末端执行器前方0.12米
        gripper_pos = camera_pos - forward * 0.12
        
        # 末端执行器的旋转矩阵需要考虑与相机坐标系的固定变换
        # 假设相机的forward对应gripper的z轴，相机的x轴对应gripper的x轴
        R_gripper = R_camera
        
        # 构建4x4齐次变换矩阵
        gripper_pose = np.eye(4)
        gripper_pose[:3, :3] = R_gripper
        gripper_pose[:3, 3] = gripper_pos
        print(gripper_pose)
        # 移动机器人到计算出的位姿
        return self.move_robot_to_pose(gripper_pose)