diff --git a/app.log b/app.log index 00d02ea..f1ad37a 100644 --- a/app.log +++ b/app.log @@ -2925,3 +2925,88 @@ 2024-11-22 10:15:56,631 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 2024-11-22 10:15:58,455 - logger - INFO - connected - robot.py - 52 2024-11-22 10:15:59,023 - logger - INFO - Старт одиночного цикла - client_socket.py - 178 +2024-11-22 10:22:30,895 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:22:30,895 - logger - INFO - None - robot.py - 52 +2024-11-22 10:22:30,895 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:22:30,895 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:22:30,895 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:22:30,895 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 10:22:30,895 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 10:22:30,896 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 10:22:31,929 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 10:22:32,639 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 10:22:32,940 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 10:22:34,728 - logger - INFO - connected - robot.py - 52 +2024-11-22 10:22:35,433 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 178 +2024-11-22 10:22:35,998 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 178 +2024-11-22 10:22:38,239 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:23:00,925 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:00,925 - logger - INFO - None - robot.py - 52 +2024-11-22 10:23:00,925 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:00,925 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:00,925 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:00,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 10:23:00,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 10:23:00,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 10:23:01,957 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 10:23:02,850 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 10:23:03,151 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 10:23:04,942 - logger - INFO - connected - robot.py - 52 +2024-11-22 10:23:05,647 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 178 +2024-11-22 10:23:06,214 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 178 +2024-11-22 10:23:08,466 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:21,968 - logger - INFO - None - robot.py - 52 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 10:23:21,968 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 10:23:22,999 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 10:23:24,033 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 10:23:24,333 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 10:23:26,131 - logger - INFO - connected - robot.py - 52 +2024-11-22 10:23:26,846 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 178 +2024-11-22 10:23:27,418 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 178 +2024-11-22 10:23:29,678 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:24:32,772 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:24:32,772 - logger - INFO - None - robot.py - 52 +2024-11-22 10:24:32,772 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:24:32,772 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:24:32,773 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:24:32,773 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 10:24:32,773 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 10:24:32,773 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 10:24:33,805 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 10:24:34,464 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 10:24:34,764 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 10:24:36,584 - logger - INFO - connected - robot.py - 52 +2024-11-22 10:24:37,432 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 178 +2024-11-22 10:24:38,001 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 178 +2024-11-22 10:24:40,265 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:24:48,449 - logger - INFO - Старт одиночного цикла - client_socket.py - 178 +2024-11-22 10:25:04,864 - logger - INFO - Старт одиночного цикла - client_socket.py - 178 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:25:50,094 - logger - INFO - None - robot.py - 52 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 10:25:50,094 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 10:25:51,125 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 10:25:52,382 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 10:25:52,683 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 10:25:54,488 - logger - INFO - connected - robot.py - 52 +2024-11-22 10:25:55,343 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 178 +2024-11-22 10:25:55,917 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 178 +2024-11-22 10:25:58,160 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 178 +2024-11-22 10:25:58,160 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:25:58,747 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 178 +2024-11-22 10:26:00,946 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:26:01,090 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 178 +2024-11-22 10:26:01,672 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 178 +2024-11-22 10:26:03,886 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 178 +2024-11-22 10:26:12,835 - logger - INFO - Старт одиночного цикла - client_socket.py - 178 +2024-11-22 10:26:21,484 - logger - INFO - Старт одиночного цикла - client_socket.py - 178 diff --git a/robot/client_socket.py b/robot/client_socket.py index 85ad645..05b49ac 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -151,7 +151,7 @@ class SocketRobotArm: commands = ( self.steps_from_file() if self.command_type == "base" - else self.convert_file_to_joint() + else self.convert_point_to_free() ) with open(f"log/{self.command_type}.json", "w") as myfile: myfile.write(json.dumps(commands)) @@ -248,7 +248,18 @@ class SocketRobotArm: ) def imitate_func(self): + points = self.convert_point_to_free() + for point in points: + jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)] + num_joints = p.getNumJoints(self.urdf_manager.body_id) + + for i in range(num_joints): + p.resetJointState(self.urdf_manager.body_id, i, jointPoses[i]) + time.sleep(1) + + def convert_point_to_free(self): points = self.steps_from_file() + res = [] for i, point in enumerate(points): if point["action"] == "10": self.imitate_point = i @@ -265,12 +276,8 @@ class SocketRobotArm: jointPoses = self.convert_to_joint_base( [float(p) * 0.001 for p in points], angles ) - - num_joints = p.getNumJoints(self.urdf_manager.body_id) - - for i in range(num_joints): - p.resetJointState(self.urdf_manager.body_id, i, jointPoses[i]) - time.sleep(1) + res.append(self.prepare_data.make_step('free', np.degrees(jointPoses), [0,0,0,0,0,0])) + return res def convert_to_joint_base(self, coordinates, angles): body_id = 0 @@ -303,39 +310,6 @@ class SocketRobotArm: return joint_angles - def convert_file_to_joint(self): - result = [] - with open(f"data/{self.filename}.nc.result", "r") as fp: - for line in fp: - data = line.strip().split(" ") - prep = {} - for item in data: - prep[item[:1]] = float(item[1:]) - - pj = list( - self.convert_to_joint_base( - coordinates=[ - f - for f in ( - prep.get("X", 0) + self.world_coordinates[0], - prep.get("Y", 0) + self.world_coordinates[1], - prep.get("Z", 0) + self.world_coordinates[2], - ) - ], - angles=[ - prep.get("U", 0) + self.world_coordinates[3], - prep.get("V", 0) + self.world_coordinates[4], - prep.get("W", 0) + self.world_coordinates[5], - ], - ) - ) - points = np.degrees(pj) - start_position = [0, 0, 0, 0, 0, 0] - result.append( - self.prepare_data.make_step("free", points, start_position) - ) - return result - def steps_from_file(self): if not self.world_coordinates: return []