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

test tool

This commit is contained in:
aarizona 2024-11-22 16:48:08 +03:00
parent c59d7f1e54
commit 3119951879
7 changed files with 1479 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1381
app.log
View File

File diff suppressed because it is too large Load Diff

24
robot/client_socket.py
View File

@ -105,9 +105,9 @@ class SocketRobotArm:
num_joints = p.getNumJoints(self.urdf_manager.body_id)
axisJointPoses = self.axis_coordinates
for i in range(num_joints):
p.resetJointState(
self.urdf_manager.body_id, i, math.radians(axisJointPoses[i])
for i in range(num_joints-1):
p.setJointMotorControl2(
self.urdf_manager.body_id, i, p.POSITION_CONTROL,math.radians(axisJointPoses[i])
)
time.sleep(1)
@ -123,7 +123,8 @@ class SocketRobotArm:
[float(p) * 0.001 for p in coordinates], [math.radians(p) for p in angles]
)
for i in range(num_joints):
p.resetJointState(self.urdf_manager.body_id, i, worldJointPoses[i])
p.setJointMotorControl2(self.urdf_manager.body_id, i,p.POSITION_CONTROL, worldJointPoses[i])
time.sleep(2)
calcWorldPosition = p.getLinkState(
self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
@ -132,6 +133,10 @@ class SocketRobotArm:
[p * 1000 for p in calcWorldPosition[0]]
+ [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
)
time.sleep(2)
position_diff = np.linalg.norm(self.world_coordinates[:3] - np.array(calcWorldPosition[:3]))
print("Position Difference:", position_diff)
self.set_text(text=f"Команд в очереди {self.command_count}")
time.sleep(0.5)
@ -252,9 +257,10 @@ class SocketRobotArm:
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)
if True:
for i in range(num_joints):
p.resetJointState(self.urdf_manager.body_id, i, jointPoses[i])
p.setJointMotorControl2(self.urdf_manager.body_id, i, p.POSITION_CONTROL, jointPoses[i])
# p.setJointMotorControlArray(self.urdf_manager.body_id, p.POSITION_CONTROL, jointPoses)
time.sleep(1)
def convert_point_to_free(self):
@ -276,7 +282,11 @@ class SocketRobotArm:
jointPoses = self.convert_to_joint_base(
[float(p) * 0.001 for p in points], angles
)
res.append(self.prepare_data.make_step('free', np.degrees(jointPoses), [0,0,0,0,0,0]))
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):

24
robot/urdf_manager.py
View File

@ -14,8 +14,10 @@ import pybullet_industrial as pi
from logger import logger
class UrdfManager:
urdf_filename = None
urdf_filename_tool = "welding_gun"
physics_client = None
body_id = None
@ -39,7 +41,27 @@ class UrdfManager:
urdf_path = os.path.join(
self.application_path, "urdf", f"{self.urdf_filename}.urdf"
)
self.body_id = p.loadURDF(urdf_path, self.robot_start_position)
self.body_id = p.loadURDF(
urdf_path, self.robot_start_position, useFixedBase=True
)
urdf_path_tool = os.path.join(
self.application_path, "urdf", f"{self.urdf_filename_tool}.urdf"
)
num_joints = p.getNumJoints(self.body_id)
end_state = p.getLinkState(self.body_id, num_joints - 1)
self.tool_id = p.loadURDF(urdf_path_tool)
p.createConstraint(
self.body_id,
num_joints - 1,
self.tool_id,
-1,
p.JOINT_FIXED,
[0, 0, 0],
[0, 0, 0],
[0, 0, 0],
)
time.sleep(1)

11
urdf/coord.py
View File

@ -1,11 +0,0 @@
# Мировые координаты
world_coordinates = [
1131.959351, -588.689941, 1277.805054,
6.181231, 54.227802, -100.604988
]
# Поворот осей
axis_rotation = [
-41.612457, 31.759747, -26.773878,
74.869049, -45.417992, -20.86335
]

34
urdf/sample.urdf
View File

@ -26,10 +26,6 @@
<!-- Links: main serial chain -->
<link name="base_link">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
@ -40,10 +36,6 @@
</link>
<link name="link_1">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
@ -54,10 +46,6 @@
</link>
<link name="link_2">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
@ -68,10 +56,6 @@
</link>
<link name="link_3">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -82,12 +66,8 @@
</link>
<link name="link_4">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0.5 0 0.0" />
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
<mesh filename="../urdf_support/sample/5.stl" />
</geometry>
@ -96,10 +76,6 @@
</link>
<link name="link_5">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -110,15 +86,10 @@
</link>
<link name="link_6">
<!-- <inertial>
<mass value="1.0" />
<inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005" />
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
<mesh filename="../urdf_support/sample/7.stl" />
<!-- <box size="0.1 0.1 0.1"/> -->
<mesh filename="../urdf_support/sample/777.stl" />
</geometry>
<material name="black" />
</visual>
@ -172,5 +143,4 @@
<axis xyz="1 0 0" />
<origin rpy="0 0 0" xyz="0.117 0 0" />
</joint>
</robot>

32
urdf/welding_gun.urdf Normal file
View File

@ -0,0 +1,32 @@
<?xml version="1.0"?>
<robot name="welding_gun_robot">
<!-- Основной корпус робота (base_link) -->
<link name="base_link">
<visual>
<geometry>
<mesh filename="../urdf_support/sample/777.stl" />
</geometry>
<material name="gray"/>
</visual>
</link>
<!-- Основной сварочный пистолет (welding_gun_body) -->
<link name="welding_gun_body">
<visual>
<geometry>
<mesh filename="../urdf_support/sample/7.stl" /> <!-- Замените на путь к вашему STL-файлу -->
</geometry>
<material name="metal"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</visual>
</link>
<!-- Соединение сварочного пистолета с роботом (фиксированное соединение) -->
<joint name="welding_gun_fixed_joint" type="fixed">
<origin xyz="-0.02 0 0" rpy="0 0 0"/>
<parent link="base_link"/> <!-- Родительский элемент (например, рука робота или корпус) -->
<child link="welding_gun_body"/> <!-- Сварочный пистолет -->
</joint>
</robot>

21
utils/test_socket_server.py
View File

@ -85,6 +85,21 @@ c_angles = [
"axis": [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479],
},
]
tool_angles = [
{
"world": [1292.602, -119.999, 1023.335, 20.742, 0.367, -91.615],
"axis": [-9.053, 37.962, -47.169, -44.937, 12.879, 44.209],
},
{
"world": [1084.151, 643.621, 644.947, 20.743, 0.367, -91.616],
"axis": [45.855, 19.494, -55.437, 60.337, 55.666, -44.703],
},
{
"world": [768.497, -577.398, 1688.266, 20.738, 0.382, -91.610],
"axis": [-63.241, 38.711, -11.748, -102.888, 66.357, 119.693],
},
]
c_angles = tool_angles
coordinates_index = 0
@ -92,7 +107,7 @@ def handle_client(conn, addr):
print(f"Connected by {addr}")
try:
while True:
data = conn.recv(1024*2)
data = conn.recv(1024 * 2)
if not data:
break
req = json.loads(data)
@ -116,7 +131,9 @@ def handle_client(conn, addr):
if "queryAddr" in req and "world-0" in req["queryAddr"]:
res["queryData"] = c["world"]
if "reqType" in req and (req["reqType"] == "command" or req["reqType"] == "AddRCC"):
if "reqType" in req and (
req["reqType"] == "command" or req["reqType"] == "AddRCC"
):
res["cmdReply"] = ["it is local dear"]
# print(res)
conn.sendall(json.dumps(res).encode())