test el
This commit is contained in:
parent
0dd0b01744
commit
f3bb5bd8fc
|
|
@ -72,7 +72,7 @@ class SocketRobotArm:
|
|||
f"urdf/{self.urdf_filename}.urdf",
|
||||
[0, 0, 0],
|
||||
useFixedBase=True,
|
||||
globalScaling=2,
|
||||
globalScaling=1,
|
||||
)
|
||||
|
||||
while True:
|
||||
|
|
@ -101,7 +101,7 @@ 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(self.start_world_coordinates[0:3]))
|
||||
|
||||
self.get_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"]]]
|
||||
)
|
||||
self.set_joint(angles)
|
||||
time.sleep(0.5)
|
||||
|
||||
def convert_to_joint(self, coordinates):
|
||||
num_joints = p.getNumJoints(self.body_id)
|
||||
|
|
@ -167,8 +168,8 @@ class SocketRobotArm:
|
|||
def set_joint(self, coordinates):
|
||||
num_joints = p.getNumJoints(self.body_id)
|
||||
for joint_index in range(0, num_joints):
|
||||
if not joint_index in coordinates:
|
||||
return
|
||||
# if not joint_index in coordinates:
|
||||
# return
|
||||
p.setJointMotorControl2(
|
||||
bodyUniqueId=self.body_id,
|
||||
jointIndex=joint_index,
|
||||
|
|
|
|||
|
|
@ -1,4 +1,9 @@
|
|||
X0 Y100
|
||||
X100 Y100
|
||||
X100 Y0
|
||||
X0 Y0
|
||||
X0 Y50
|
||||
X0 Y100
|
||||
X50 Y100
|
||||
X100 Y100
|
||||
X100 Y50
|
||||
X100 Y0
|
||||
X50 Y0
|
||||
X0 Y0
|
||||
|
|
@ -18,8 +18,8 @@ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
|||
res = {"queryData": ["ok"]}
|
||||
|
||||
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"] = [-60, 40, -10, -60, -75, 0]
|
||||
# res["queryData"] = [-59.696, 39.438, -7.478, -58.198, -76.606, 0.0]
|
||||
res["queryData"] = [-60, 40, -10, -60, -75, 0]
|
||||
|
||||
if "queryAddr" in req and "world-0" in req["queryAddr"]:
|
||||
res["queryData"] = [
|
||||
|
|
|
|||
165
urdf/sample.urdf
165
urdf/sample.urdf
|
|
@ -2,144 +2,151 @@
|
|||
<robot name="sample_robot">
|
||||
<!-- 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.1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.1" />
|
||||
<geometry>
|
||||
<box size="0.2 0.2 0.2"/>
|
||||
<box size="0.2 0.2 0.2" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.0 0 0 1.0"/>
|
||||
<material name="grey">
|
||||
<color rgba="0.0 0 0 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</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.2473"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2473" />
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.4946"/>
|
||||
<box size="0.1 0.1 0.4946" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1.0 0.0 0.0 1.0"/>
|
||||
<material name="red">
|
||||
<color rgba="1.0 0.0 0.0 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</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.3649605"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.3649605" />
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.729921"/>
|
||||
<box size="0.1 0.1 0.729921" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0 0.0 1.0 1.0"/>
|
||||
<material name="blue">
|
||||
<color rgba="0 0.0 1.0 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</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.049"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.049" />
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.098277"/>
|
||||
<box size="0.1 0.1 0.098277" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0. 0.22 1.00 1.0"/>
|
||||
<material name="lightblue">
|
||||
<color rgba="0. 0.22 1.00 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</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 0 0.5545195"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.5545195" />
|
||||
<geometry>
|
||||
<box size="0.1 0.1 1.109039"/>
|
||||
<box size="0.1 0.1 1.109039" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.96 0.76 0.13 1.0"/>
|
||||
<material name="yellow">
|
||||
<color rgba="0.96 0.76 0.13 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</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.0585"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0585" />
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.117"/>
|
||||
<box size="0.1 0.1 0.117" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1.0 0.5 0.0 1.0"/>
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.5 0.0 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</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.05"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.05" />
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.1"/>
|
||||
<box size="0.1 0.1 0.1" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.15 0.15 0.15 1.0"/>
|
||||
<material name="black">
|
||||
<color rgba="0.15 0.15 0.15 1.0" />
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<!-- joints: main serial chain -->
|
||||
<joint name="joint_1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.2" />
|
||||
<parent link="base_link" />
|
||||
<child link="link_1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415" />
|
||||
</joint>
|
||||
<joint name="joint_2" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0170450 0 0.494600"/>
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415"/>
|
||||
<origin rpy="0 0 0" xyz="0.0170450 0 0.494600" />
|
||||
<parent link="link_1" />
|
||||
<child link="link_2" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415" />
|
||||
</joint>
|
||||
<joint name="joint_3" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.729921"/>
|
||||
<parent link="link_2"/>
|
||||
<child link="link_3"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.729921" />
|
||||
<parent link="link_2" />
|
||||
<child link="link_3" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269" />
|
||||
</joint>
|
||||
<joint name="joint_4" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.098277"/>
|
||||
<parent link="link_3"/>
|
||||
<child link="link_4"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.098277" />
|
||||
<parent link="link_3" />
|
||||
<child link="link_4" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813" />
|
||||
</joint>
|
||||
<joint name="joint_5" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0 0 1.109039"/>
|
||||
<parent link="link_4"/>
|
||||
<child link="link_5"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0 1.109039" />
|
||||
<parent link="link_4" />
|
||||
<child link="link_5" />
|
||||
<axis xyz="0 -1 0" />
|
||||
<limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595" />
|
||||
</joint>
|
||||
<joint name="joint_6" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0.117"/>
|
||||
<parent link="link_5"/>
|
||||
<child link="link_6"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<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"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0 0.117" />
|
||||
<parent link="link_5" />
|
||||
<child link="link_6" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="0" lower="-6.2831" upper="6.2831" velocity="8.3775" />
|
||||
</joint>
|
||||
</robot>
|
||||
Loading…
Reference in New Issue