hmmm
This commit is contained in:
parent
d7f1edce9c
commit
775395e9ec
|
|
@ -85,7 +85,6 @@ class MainContentComponent(QWidget):
|
|||
def handle_value_change(self, new_value):
|
||||
panels = [
|
||||
self.imitatorPanel,
|
||||
self.visPanel,
|
||||
]
|
||||
for p in panels:
|
||||
if new_value == "not_connected":
|
||||
|
|
|
|||
|
|
@ -32,11 +32,14 @@ class Visualize(QWidget):
|
|||
# Таймер для обновления изображения
|
||||
self.timer = QTimer()
|
||||
self.timer.timeout.connect(self.update_image)
|
||||
self.timer.start(1000) # Обновление каждые 100 мс
|
||||
self.timer.start(int(1000/24)) # Обновление каждые 100 мс
|
||||
|
||||
def update_image(self):
|
||||
(rgb, width, height) = self.get_pybullet_image()
|
||||
res = self.get_pybullet_image()
|
||||
|
||||
if not res: return
|
||||
|
||||
(rgb, width, height) = res
|
||||
# logger.info(f"Image size: {width}x{height}, RGB data length: {len(rgb)}")
|
||||
|
||||
# rgbim = Image.fromarray(rgb)
|
||||
|
|
|
|||
9
main.py
9
main.py
|
|
@ -3,7 +3,6 @@ import sys
|
|||
import time
|
||||
import argparse
|
||||
import threading
|
||||
import queue
|
||||
from PyQt5.QtCore import QThread
|
||||
from PyQt5.QtWidgets import QApplication
|
||||
|
||||
|
|
@ -16,9 +15,6 @@ class MyApp:
|
|||
|
||||
def __init__(self, mode):
|
||||
self.mode = mode
|
||||
# self.robot_thread = RobotThread()
|
||||
# self.robot_thread.start()
|
||||
# self.robot_thread.finished.connect(self.startGui)
|
||||
self.startRobot()
|
||||
self.startGui()
|
||||
|
||||
|
|
@ -33,7 +29,7 @@ class MyApp:
|
|||
|
||||
def startGui(self):
|
||||
app = QApplication(sys.argv)
|
||||
|
||||
self.robot_app.q_app = app
|
||||
mainWindow = MainContentComponent(
|
||||
get_status=self.get_status,
|
||||
robotPanel={
|
||||
|
|
@ -65,7 +61,8 @@ class MyApp:
|
|||
threading.Thread(target=self.run_robot_arm, daemon=True).start()
|
||||
|
||||
def run_robot_arm(self):
|
||||
self.robot_app.start()
|
||||
time.sleep(1)
|
||||
self.robot_app.start('GUI')
|
||||
|
||||
def updateRobot(self, robot):
|
||||
if robot in self.robots:
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ class SocketRobotArm:
|
|||
laser_id = 14
|
||||
|
||||
filename = "test"
|
||||
urdf_filename = "sample"
|
||||
urdf_filename = "fanucM16ib"
|
||||
|
||||
pass_size = 4
|
||||
Status = Literal["connected", "not_connected", "error"]
|
||||
|
|
@ -51,30 +51,40 @@ class SocketRobotArm:
|
|||
print("exiting")
|
||||
self.socket.close()
|
||||
|
||||
def start(self):
|
||||
self.run_pybullet()
|
||||
def start(self, type="DIRECT"):
|
||||
logger.info(type)
|
||||
self.run_pybullet(type)
|
||||
|
||||
def run_pybullet(self):
|
||||
self.physics_client = p.connect(p.GUI)
|
||||
p.loadPlugin("eglRendererPlugin")
|
||||
p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
|
||||
def run_pybullet(self, type="DIRECT"):
|
||||
self.physics_client = p.connect(getattr(p, type))
|
||||
# p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
|
||||
|
||||
urdf_path = os.path.join("urdf", f"{self.urdf_filename}.urdf")
|
||||
if not os.path.exists(urdf_path):
|
||||
raise FileNotFoundError(f"URDF file not found: {urdf_path}")
|
||||
threading.Thread(target=self.load_models, daemon=True).start()
|
||||
threading.Thread(target=self.simulation_loop, daemon=True).start()
|
||||
|
||||
def load_models(self):
|
||||
p.loadURDF("plane.urdf")
|
||||
self.body_id = p.loadURDF(urdf_path, [0, 0, 0], useFixedBase=True)
|
||||
logger.info(f"Загружена модель {urdf_path}, id: {self.body_id}")
|
||||
|
||||
# self.cube_id = p.loadURDF("r2d2.urdf", [0, 0, 1], useFixedBase=0)
|
||||
urdf_path = os.path.join("urdf", f"{self.urdf_filename}.urdf")
|
||||
self.body_id = p.loadURDF(urdf_path, [0, 0, 0], useFixedBase=True, globalScaling=2)
|
||||
time.sleep(1)
|
||||
|
||||
self.simulation_loop()
|
||||
# Получение AABB
|
||||
aabb = p.getAABB(self.body_id)
|
||||
|
||||
# Вычисление размеров модели
|
||||
size_x = aabb[1][0] - aabb[0][0] # Длина по оси X
|
||||
size_y = aabb[1][1] - aabb[0][1] # Длина по оси Y
|
||||
size_z = aabb[1][2] - aabb[0][2] # Длина по оси Z
|
||||
|
||||
print(f"Размеры модели (длина x, длина y, длина z): ({size_x}, {size_y}, {size_z})")
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
|
||||
|
||||
def simulation_loop(self):
|
||||
logger.info(f"Нчат цикл симуляции")
|
||||
logger.info(f"Начат цикл симуляции")
|
||||
while True:
|
||||
bodyUniqueId = self.body_id
|
||||
position, orientation = p.getBasePositionAndOrientation(bodyUniqueId)
|
||||
|
|
@ -91,29 +101,32 @@ class SocketRobotArm:
|
|||
joint_log.append(
|
||||
f"Joint {i} - Position: {joint_position}, Velocity: {joint_velocity}"
|
||||
)
|
||||
logger.info("\n".join(joint_log))
|
||||
# logger.info("\n".join(joint_log))
|
||||
|
||||
p.stepSimulation()
|
||||
if self.q_app:
|
||||
logger.info(self.q_app)
|
||||
self.q_app.processEvents()
|
||||
|
||||
time.sleep(1 / 24) # 24 кадра в секунду
|
||||
|
||||
def get_pybullet_image(self):
|
||||
if self.physics_client is None:
|
||||
return
|
||||
|
||||
width, height, rgb, _, _ = p.getCameraImage(
|
||||
width=640,
|
||||
height=640,
|
||||
viewMatrix=p.computeViewMatrix(
|
||||
cameraEyePosition=[2, 2, 2],
|
||||
cameraTargetPosition=[0, 0, 0],
|
||||
cameraUpVector=[0, 0, 1],
|
||||
),
|
||||
projectionMatrix=p.computeProjectionMatrixFOV(
|
||||
fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0
|
||||
),
|
||||
renderer=p.ER_BULLET_HARDWARE_OPENGL,
|
||||
physicsClientId=self.physics_client,
|
||||
# viewMatrix=p.computeViewMatrix(
|
||||
# cameraEyePosition=[2, 2, 2],
|
||||
# cameraTargetPosition=[0, 0, 0],
|
||||
# cameraUpVector=[0, 0, 1],
|
||||
# ),
|
||||
# projectionMatrix=p.computeProjectionMatrixFOV(
|
||||
# fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0
|
||||
# ),
|
||||
# renderer=p.ER_TINY_RENDERER,
|
||||
# physicsClientId=self.physics_client,
|
||||
flags=p.ER_NO_SEGMENTATION_MASK,
|
||||
)
|
||||
|
||||
return (rgb, width, height)
|
||||
|
|
@ -212,7 +225,7 @@ class SocketRobotArm:
|
|||
if coordinates is None:
|
||||
return
|
||||
for joint_index in range(0, num_joints):
|
||||
if coordinates[joint_index] is None:
|
||||
if len(coordinates) <= joint_index or coordinates[joint_index] is None:
|
||||
return
|
||||
p.setJointMotorControl2(
|
||||
bodyUniqueId=self.body_id,
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ from robot.client_socket import SocketRobotArm
|
|||
|
||||
if __name__ == "__main__":
|
||||
robot_arm = SocketRobotArm()
|
||||
robot_arm.start()
|
||||
robot_arm.start('GUI')
|
||||
try:
|
||||
while True:
|
||||
time.sleep(1) # Просто удерживаем основной поток активным
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/base_link.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/base_link.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/base_link.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -26,7 +26,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_1.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_1.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -35,7 +35,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_1.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -43,7 +43,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_2.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_2.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -52,7 +52,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_2.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -60,7 +60,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_3.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_3.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -69,7 +69,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_3.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -77,7 +77,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_4.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_4.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -86,7 +86,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_4.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_4.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -94,7 +94,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_5.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_5.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -103,7 +103,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_5.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_5.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -111,7 +111,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_6.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_6.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.15 0.15 0.15 1.0"/>
|
||||
|
|
@ -120,7 +120,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_6.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_6.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -25,7 +25,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -34,7 +34,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -42,7 +42,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -51,7 +51,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -59,7 +59,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -68,7 +68,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -76,7 +76,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -85,7 +85,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -93,7 +93,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
|
|
@ -102,7 +102,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
@ -110,7 +110,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.15 0.15 0.15 1.0"/>
|
||||
|
|
@ -119,7 +119,7 @@
|
|||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl"/>
|
||||
<mesh filename="../urdf_support/fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
|
|
|||
Loading…
Reference in New Issue