# 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)