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

test el

This commit is contained in:
Kseninia Mikhaylova 2024-10-10 16:35:50 +03:00
parent 0dd0b01744
commit f3bb5bd8fc
4 changed files with 100 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
client_socket.py
View File

@ -72,7 +72,7 @@ class SocketRobotArm:
f"urdf/{self.urdf_filename}.urdf", f"urdf/{self.urdf_filename}.urdf",
[0, 0, 0], [0, 0, 0],
useFixedBase=True, useFixedBase=True,
globalScaling=2, globalScaling=1,
) )
while True: while True:
@ -101,7 +101,7 @@ class SocketRobotArm:
self.get_world() self.get_world()
self.set_text(text=f"Мировые координаты {self.start_world_coordinates}") self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
time.sleep(0.5) time.sleep(0.5)
self.set_joint(self.convert_to_joint(self.start_world_coordinates[0:3])) # self.set_joint(self.convert_to_joint(self.start_world_coordinates[0:3]))
self.get_command_count() self.get_command_count()
self.set_text(text=f"Команд в очереди {self.remote_command_count}") self.set_text(text=f"Команд в очереди {self.remote_command_count}")
@ -151,6 +151,7 @@ class SocketRobotArm:
[float(p) for p in [p["m0"], p["m1"], p["m2"]]] [float(p) for p in [p["m0"], p["m1"], p["m2"]]]
) )
self.set_joint(angles) self.set_joint(angles)
time.sleep(0.5)
def convert_to_joint(self, coordinates): def convert_to_joint(self, coordinates):
num_joints = p.getNumJoints(self.body_id) num_joints = p.getNumJoints(self.body_id)
@ -167,8 +168,8 @@ class SocketRobotArm:
def set_joint(self, coordinates): def set_joint(self, coordinates):
num_joints = p.getNumJoints(self.body_id) num_joints = p.getNumJoints(self.body_id)
for joint_index in range(0, num_joints): for joint_index in range(0, num_joints):
if not joint_index in coordinates: # if not joint_index in coordinates:
return # return
p.setJointMotorControl2( p.setJointMotorControl2(
bodyUniqueId=self.body_id, bodyUniqueId=self.body_id,
jointIndex=joint_index, jointIndex=joint_index,

11
data/test.nc.result
View File

@ -1,4 +1,9 @@
X0 Y100 X0 Y0
X100 Y100 X0 Y50
X100 Y0 X0 Y100
X50 Y100
X100 Y100
X100 Y50
X100 Y0
X50 Y0
X0 Y0 X0 Y0

4
test_socket_server.py
View File

@ -18,8 +18,8 @@ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
res = {"queryData": ["ok"]} res = {"queryData": ["ok"]}
if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]: if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
res["queryData"] = [-59.696, 39.438, -7.478, -58.198, -76.606, 0] # res["queryData"] = [-59.696, 39.438, -7.478, -58.198, -76.606, 0.0]
# res["queryData"] = [-60, 40, -10, -60, -75, 0] res["queryData"] = [-60, 40, -10, -60, -75, 0]
if "queryAddr" in req and "world-0" in req["queryAddr"]: if "queryAddr" in req and "world-0" in req["queryAddr"]:
res["queryData"] = [ res["queryData"] = [

165
urdf/sample.urdf
View File

@ -2,144 +2,151 @@
<robot name="sample_robot"> <robot name="sample_robot">
<!-- links: main serial chain --> <!-- links: main serial chain -->
<link name="base_link"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.1"/> <origin rpy="0 0 0" xyz="0 0 0.1" />
<geometry> <geometry>
<box size="0.2 0.2 0.2"/> <box size="0.2 0.2 0.2" />
</geometry> </geometry>
<material name=""> <material name="grey">
<color rgba="0.0 0 0 1.0"/> <color rgba="0.0 0 0 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<link name="link_1"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.2473"/> <origin rpy="0 0 0" xyz="0 0 0.2473" />
<geometry> <geometry>
<box size="0.1 0.1 0.4946"/> <box size="0.1 0.1 0.4946" />
</geometry> </geometry>
<material name=""> <material name="red">
<color rgba="1.0 0.0 0.0 1.0"/> <color rgba="1.0 0.0 0.0 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<link name="link_2"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.3649605"/> <origin rpy="0 0 0" xyz="0 0 0.3649605" />
<geometry> <geometry>
<box size="0.1 0.1 0.729921"/> <box size="0.1 0.1 0.729921" />
</geometry> </geometry>
<material name=""> <material name="blue">
<color rgba="0 0.0 1.0 1.0"/> <color rgba="0 0.0 1.0 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<link name="link_3"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.049"/> <origin rpy="0 0 0" xyz="0 0 0.049" />
<geometry> <geometry>
<box size="0.1 0.1 0.098277"/> <box size="0.1 0.1 0.098277" />
</geometry> </geometry>
<material name=""> <material name="lightblue">
<color rgba="0. 0.22 1.00 1.0"/> <color rgba="0. 0.22 1.00 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<link name="link_4"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.5545195"/> <origin rpy="0 0 0" xyz="0 0 0.5545195" />
<geometry> <geometry>
<box size="0.1 0.1 1.109039"/> <box size="0.1 0.1 1.109039" />
</geometry> </geometry>
<material name=""> <material name="yellow">
<color rgba="0.96 0.76 0.13 1.0"/> <color rgba="0.96 0.76 0.13 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<link name="link_5"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.0585"/> <origin rpy="0 0 0" xyz="0 0 0.0585" />
<geometry> <geometry>
<box size="0.1 0.1 0.117"/> <box size="0.1 0.1 0.117" />
</geometry> </geometry>
<material name=""> <material name="orange">
<color rgba="1.0 0.5 0.0 1.0"/> <color rgba="1.0 0.5 0.0 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<link name="link_6"> <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> <visual>
<origin rpy="0 0 0" xyz="0 0 0.05"/> <origin rpy="0 0 0" xyz="0 0 0.05" />
<geometry> <geometry>
<box size="0.1 0.1 0.1"/> <box size="0.1 0.1 0.1" />
</geometry> </geometry>
<material name=""> <material name="black">
<color rgba="0.15 0.15 0.15 1.0"/> <color rgba="0.15 0.15 0.15 1.0" />
</material> </material>
</visual> </visual>
</link> </link>
<!-- joints: main serial chain --> <!-- joints: main serial chain -->
<joint name="joint_1" type="revolute"> <joint name="joint_1" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.2"/> <origin rpy="0 0 0" xyz="0 0 0.2" />
<parent link="base_link"/> <parent link="base_link" />
<child link="link_1"/> <child link="link_1" />
<axis xyz="0 0 1"/> <axis xyz="0 0 1" />
<limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415"/> <limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415" />
</joint> </joint>
<joint name="joint_2" type="revolute"> <joint name="joint_2" type="revolute">
<origin rpy="0 0 0" xyz="0.0170450 0 0.494600"/> <origin rpy="0 0 0" xyz="0.0170450 0 0.494600" />
<parent link="link_1"/> <parent link="link_1" />
<child link="link_2"/> <child link="link_2" />
<axis xyz="0 1 0"/> <axis xyz="0 1 0" />
<limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415"/> <limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415" />
</joint> </joint>
<joint name="joint_3" type="revolute"> <joint name="joint_3" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.729921"/> <origin rpy="0 0 0" xyz="0 0 0.729921" />
<parent link="link_2"/> <parent link="link_2" />
<child link="link_3"/> <child link="link_3" />
<axis xyz="0 -1 0"/> <axis xyz="0 -1 0" />
<limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269"/> <limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269" />
</joint> </joint>
<joint name="joint_4" type="revolute"> <joint name="joint_4" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.098277"/> <origin rpy="0 0 0" xyz="0 0 0.098277" />
<parent link="link_3"/> <parent link="link_3" />
<child link="link_4"/> <child link="link_4" />
<axis xyz="-1 0 0"/> <axis xyz="-1 0 0" />
<limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813"/> <limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813" />
</joint> </joint>
<joint name="joint_5" type="revolute"> <joint name="joint_5" type="revolute">
<origin rpy="0 0 0" xyz="0.0 0 1.109039"/> <origin rpy="0 0 0" xyz="0.0 0 1.109039" />
<parent link="link_4"/> <parent link="link_4" />
<child link="link_5"/> <child link="link_5" />
<axis xyz="0 -1 0"/> <axis xyz="0 -1 0" />
<limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595"/> <limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595" />
</joint> </joint>
<joint name="joint_6" type="revolute"> <joint name="joint_6" type="revolute">
<origin rpy="0 0 0" xyz="0.0 0 0.117"/> <origin rpy="0 0 0" xyz="0.0 0 0.117" />
<parent link="link_5"/> <parent link="link_5" />
<child link="link_6"/> <child link="link_6" />
<axis xyz="-1 0 0"/> <axis xyz="-1 0 0" />
<limit effort="0" lower="-6.2831" upper="6.2831" velocity="8.3775"/> <limit effort="0" lower="-6.2831" upper="6.2831" velocity="8.3775" />
</joint>
<!-- ROS base_link to Fanuc World Coordinates transform -->
<link name="base"/>
<joint name="base_link-base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.350"/>
<parent link="base_link"/>
<child link="base"/>
</joint>
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
<link name="flange"/>
<joint name="joint_6-flange" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="link_6"/>
<child link="flange"/>
</joint>
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
<link name="tool0"/>
<joint name="link_6-tool0" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="link_6"/>
<child link="tool0"/>
</joint> </joint>
</robot> </robot>