ksenia_mikhailova
/
modbus_test
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

imitate

This commit is contained in:
aarizona 2024-11-01 11:56:43 +03:00
parent 566285b400
commit 41f99d7636
7 changed files with 2049 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@ -4,3 +4,6 @@
* Запустить сокет сервер - для теста
* Запустить pybullet сервер - визуализация в отдельном ПРОЦЕССЕ
* Запустить gui
### Локальный запуск
`poetry run python run_local.py --test-socket --bullet-gui`

1956
app.log
View File

File diff suppressed because it is too large Load Diff

28
data/test.nc.result
View File

@ -1,9 +1,19 @@
X0 Y0
X0 Y10
X0 Y20
X10 Y20
X20 Y20
X20 Y10
X20 Y0
X10 Y0
X0 Z0
X0 Y0 Z0 U0
X0 Y0 Z0 U-33.333
X100 Y0 Z0 U-33.333
X200 Y0 Z0 U-33.333
X200 Y100 Z0 U-33.333
X200 Y200 Z0 U-33.333
X100 Y200 Z0 U-33.333
X0 Y200 Z0 U-33.333
X0 Y100 Z0 U-33.333
X0 Y0 Z0 U-33.333
X0 Y0 Z-100 W-33.333
X100 Y0 Z-100 W-33.333
X200 Y0 Z-100 W-33.333
X200 Y100 Z-100 W-33.333
X200 Y200 Z-100 W-33.333
X100 Y200 Z-100 W-33.333
X0 Y200 Z-100 W-33.333
X0 Y100 Z-100 W-33.333
X0 Y0 Z-100 W-33.333

59
robot/client_socket.py
View File

@ -2,6 +2,7 @@ import socket
import json
import os
import math
import numpy as np
from typing import Literal
from pprint import pprint
@ -75,7 +76,14 @@ class SocketRobotArm:
urdf_path = os.path.join("urdf", f"{self.urdf_filename}.urdf")
logger.info(urdf_path)
self.body_id = p.loadURDF(
urdf_path, [0, 0, 0], useFixedBase=1, physicsClientId=self.physics_client
urdf_path,
[
-0.075458, # новое значение x в метрах
0.068477, # новое значение y в метрах
0.082201, # новое значение z в метрах
],
useFixedBase=1,
physicsClientId=self.physics_client,
)
time.sleep(1)
@ -106,7 +114,7 @@ class SocketRobotArm:
projectionMatrix=p.computeProjectionMatrixFOV(
fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0
),
renderer=p.ER_TINY_RENDERER,
# renderer=p.ER_TINY_RENDERER,
physicsClientId=self.physics_client,
)
@ -142,12 +150,34 @@ class SocketRobotArm:
self.get_world()
self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
time.sleep(0.5)
# self.set_joint(self.convert_to_joint(self.start_world_coordinates[0:3]))
self.set_joint(
self.convert_to_joint(
[s * 0.001 for s in self.start_world_coordinates[0:3]]
)
)
self._get_command_count()
self.set_text(text=f"Команд в очереди {self.remote_command_count}")
time.sleep(0.5)
num_joints = p.getNumJoints(self.body_id)
end_effector_state = p.getLinkState(
self.body_id, num_joints - 1, computeForwardKinematics=True
)
# Позиция end-effector
end_effector_position = end_effector_state[4]
# Ориентация end-effector (кватернион)
end_effector_orientation = end_effector_state[5]
# Преобразуем кватернион в углы Эйлера
euler_orientation = p.getEulerFromQuaternion(end_effector_orientation)
euler_orientation = np.degrees(euler_orientation)
logger.info(end_effector_position)
logger.info(euler_orientation)
def cycle_base(self):
self.upd_model()
@ -192,14 +222,25 @@ class SocketRobotArm:
for i, point in enumerate(points):
if point["action"] == "10":
self.imitate_point = i
points = [
point["m0"],
point["m1"],
point["m2"],
]
coor = [(float(point) * 0.001) for point in points]
angles = self.convert_to_joint(
[float(point) for point in [point["m0"], point["m1"], point["m2"]]]
coor,
[
point["m3"],
point["m4"],
point["m5"],
],
)
# logger.info(f"point {point} {angles}")
self.set_joint((*angles, point["m2"], point["m3"], point["m4"]))
self.set_joint(angles)
time.sleep(1)
def convert_to_joint(self, coordinates):
def convert_to_joint(self, coordinates, angles=[]):
num_joints = p.getNumJoints(self.body_id)
joint_info = [p.getJointInfo(self.body_id, i) for i in range(num_joints)]
@ -207,6 +248,9 @@ class SocketRobotArm:
self.body_id,
endEffectorLinkIndex=num_joints - 1,
targetPosition=coordinates,
targetOrientation=p.getQuaternionFromEuler(
[math.radians(float(a)) for a in angles]
) if len(angles) == 3 else None,
)
logger.info(f"convrt to joint {joint_angles}")
# logger.info(joint_angles)
@ -302,6 +346,9 @@ class SocketRobotArm:
def round(v):
return round(v, 3)
def relative_axis_coordinates(self):
start = self.start_axis_coordinates
def make_world_step(self, type, point):
step = {
"oneshot": "1",

7
run_local.py
View File

@ -33,7 +33,8 @@ signal.signal(signal.SIGTERM, lambda *_: cleanup())
# Парсер аргументов командной строки
parser = argparse.ArgumentParser(description="Python launcher script.")
parser.add_argument("--test", action="store_true", help="Запуск в режиме тестирования")
parser.add_argument("--test-socket", action="store_true", help="Запуск в режиме тестирования")
parser.add_argument("--bullet-gui", action="store_true", help="Вывод визуального pybullet")
args = parser.parse_args()
# Установка команды для pybullet_server
@ -46,7 +47,7 @@ pybullet_command = [
] # Путь к исполняемому файлу
# Если включен режим тестирования, добавляем флаг --test
if args.test:
if args.bullet_gui:
pybullet_command.append("--test")
# Запуск pybullet_server.exe
@ -59,7 +60,7 @@ if server_process.poll() is not None:
sys.exit(1)
# Если включен режим тестирования, запускаем test_socket_server.exe
if args.test:
if args.test_socket:
print("Запуск test_socket_server...")
test_server_process = subprocess.Popen(
["poetry", "run", "python", "utils/test_socket_server.py"]

2
urdf/sample.urdf
View File

@ -160,7 +160,7 @@
<limit lower="-2.007" upper="2.007" effort="0" velocity="0"/>
<parent link="link_4" />
<child link="link_5" />
<axis xyz="0 1 0" />
<axis xyz="0 -1 0" />
<origin rpy="0 0 0" xyz="0.867755 0.037912 0.000177" />
</joint>

30
utils/test_socket_server.py
View File

@ -17,28 +17,22 @@ def handle_client(conn, addr):
if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
res["queryData"] = [
-60,
40,
-10,
-60,
-75,
0,
-8.487,
-8.681,
33.058,
88.070,
-75.010,
-10.566,
]
if "queryAddr" in req and "world-0" in req["queryAddr"]:
res["queryData"] = [
643.622,
-1289.604,
254.682,
124.70,
24.209,
-58.492,
# 0,
# 0,
# 0,
# 0,
# 0,
# 0,
1282.244,
-75.427,
1772.476,
84.629,
34.519,
153.999,
]
if req["reqType"] == "command":