imitate
This commit is contained in:
aarizona
parent
566285b400
commit
41f99d7636
|
|
@ -4,3 +4,6 @@
|
|||
* Запустить сокет сервер - для теста
|
||||
* Запустить pybullet сервер - визуализация в отдельном ПРОЦЕССЕ
|
||||
* Запустить gui
|
||||
|
||||
### Локальный запуск
|
||||
`poetry run python run_local.py --test-socket --bullet-gui`
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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"]
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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":
|
||||
|
|
|
|||
Loading…
Reference in New Issue