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

test joint coordinates

This commit is contained in:
user 2024-11-19 09:01:33 +03:00
parent 865f8fe1b4
commit 065284106c
4 changed files with 8053 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7937
app.log
View File

File diff suppressed because it is too large Load Diff

45
delta.json
View File

@ -1,30 +1,37 @@
[
{
"position": [
{
"point": 562.854492,
"calc": 1649.3684579350195,
"delta": -1086.5139659350193
},
{
"point": 1527.264526,
"calc": -530.2399496718566,
"delta": 2057.5044756718567
},
{
"point": 1250.14856,
"calc": 995.1876181234434,
"delta": 254.96094187655672
}
],
"orientation": [
{
"point": 0.002,
"calc": 3807.450915022899,
"delta": -3807.448915022899
"point": 136.124298,
"calc": 11.411812786329795,
"delta": 124.71248521367022
},
{
"point": -0.001,
"calc": -2489.5691806111413,
"delta": 2489.568180611141
"point": 73.102768,
"calc": 12.07935084339071,
"delta": 61.023417156609284
},
{
"point": 89.999,
"calc": 4489.547537744236,
"delta": -4399.5485377442365
},
{
"point": 0.002,
"calc": -5537.059631276998,
"delta": 5537.061631276998
},
{
"point": 0.0,
"calc": -5951.370542882227,
"delta": 5951.370542882227
"point": -170.128082,
"calc": 52.02530054645324,
"delta": -222.15338254645326
}
]
}

95
robot/client_socket.py
View File

@ -21,7 +21,7 @@ from logger import logger
class PrepareRobotData:
line_speed = 100.0
line_smooth = 0
line_tool = 1
line_tool = 0
def make_step(self, type, point, start_coordinates):
step = {
@ -155,6 +155,7 @@ class UrdfManager:
height=500,
viewMatrix=p.computeViewMatrix(
cameraEyePosition=[4, -4, 1.5],
# cameraEyePosition=[4,-4,4],
cameraTargetPosition=[0, 0, 0], # Центр фокусировки камеры
cameraUpVector=[0, 0, 1], # Направление вверх
),
@ -174,7 +175,7 @@ class SocketRobotArm:
# laser_id = 15
laser_id = 14
filename = "test"
filename = "axis"
urdf_filename = "sample"
pass_size = 4
@ -239,7 +240,7 @@ class SocketRobotArm:
def upd_model_func(self):
# logger.info("UPDATE MODEL FUNC")\
mode = "by_joint"
mode = "by_world"
# mode = "by_world"
self.get_axis()
self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
@ -247,16 +248,20 @@ class SocketRobotArm:
self.get_world()
self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
if mode == 'by_joint':
if mode == "by_joint":
self.urdf_manager.robot.set_joint_position(
{
f"joint_{i+1}": math.radians(c)
for i, c in enumerate(self.start_axis_coordinates)
}
)
if mode == 'by_world':
if mode == "by_world":
coordinates = self.start_world_coordinates[:3]
angles = [self.start_world_coordinates[1],self.start_world_coordinates[2],self.start_world_coordinates[3]]
angles = [
self.start_world_coordinates[1],
self.start_world_coordinates[2],
self.start_world_coordinates[3],
]
self.urdf_manager.robot.set_endeffector_pose(
[a * 0.001 for a in coordinates], p.getQuaternionFromEuler(angles)
@ -464,10 +469,18 @@ class SocketRobotArm:
point["m4"],
point["m5"],
]
self.urdf_manager.robot.set_endeffector_pose(
[(float(point) * 0.001) for point in points],
p.getQuaternionFromEuler([float(point) for point in angles]),
# self.urdf_manager.robot.set_endeffector_pose(
# [(float(point) * 0.001) for point in points],
# p.getQuaternionFromEuler([float(point) for point in angles]),
# )
jointPoses = self.convert_to_joint_base(
[float(p) * 0.001 * 0.001 * -1 for p in points], angles
)
body_id = 0
num_joints = p.getNumJoints(body_id)
for i in range(num_joints):
p.resetJointState(body_id, i, jointPoses[i])
time.sleep(1)
def convert_to_joint(self, coordinates, angles=[]):
@ -478,13 +491,49 @@ class SocketRobotArm:
)
else:
target_orientation = None
self.urdf_manager.robot.set_endeffector_pose(coordinates, target_orientation)
logger.info(coordinates)
logger.info(target_orientation)
self.urdf_manager.robot
self.urdf_manager.robot.set_endeffector_pose(
[c * 1000 for c in coordinates], target_orientation
)
[end_effector_position, end_effector_orientation] = (
self.urdf_manager.robot.get_endeffector_pose()
)
time.sleep(2)
return list(end_effector_position) + list(end_effector_orientation)
def convert_to_joint_base(self, coordinates, angles):
body_id = 0
num_joints = p.getNumJoints(body_id)
logger.info(coordinates)
lower_limits = []
upper_limits = []
for i in range(num_joints):
limit = p.getJointInfo(body_id, i)
# logger.info(limit)
# logger.info(limit[8])
lower_limits.append(limit[8])
upper_limits.append(limit[9])
logger.info(lower_limits)
joint_angles = p.calculateInverseKinematics(
body_id,
endEffectorLinkIndex=num_joints - 1,
targetPosition=[float(c) for c in coordinates],
targetOrientation=angles,
lowerLimits=lower_limits,
upperLimits=upper_limits,
jointRanges=[0, 0, 0, 0, 0, 0],
restPoses=[0, 0, 0, 0, 0, 0, -math.pi * 0.5, 0],
# solver=0,
# maxNumIterations=100,
# residualThreshold=0.001,
)
# self.urdf_manager.robot.
return joint_angles
def convert_file_to_joint(self):
result = []
with open(f"data/{self.filename}.nc.result", "r") as fp:
@ -495,22 +544,28 @@ class SocketRobotArm:
prep[item[:1]] = float(item[1:])
pj = list(
self.convert_to_joint(
coordinates=(
prep.get("X", 0),
prep.get("Y", 0),
prep.get("Z", 0),
),
self.convert_to_joint_base(
coordinates=[
f
for f in (
prep.get("X", 0) + self.start_world_coordinates[0],
prep.get("Y", 0) + self.start_world_coordinates[1],
prep.get("Z", 0) + self.start_world_coordinates[2],
)
],
angles=[
prep.get("U", 0),
prep.get("V", 0),
prep.get("W", 0),
prep.get("U", 0) + self.start_world_coordinates[3],
prep.get("V", 0) + self.start_world_coordinates[4],
prep.get("W", 0) + self.start_world_coordinates[5],
],
)
)
logger.info(pj)
logger.info(np.degrees(pj))
# pj = np.degrees(pj)
result.append(
self.prepare_data.make_step(
"free", tuple(pj), self.start_axis_coordinates
"free", np.degrees(pj), [0, 0, 0, 0, 0, 0]
)
)
return result

30
urdf/sample.urdf
View File

@ -26,10 +26,10 @@
<!-- Links: main serial chain -->
<link name="base_link">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
@ -40,10 +40,10 @@
</link>
<link name="link_1">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
@ -54,10 +54,10 @@
</link>
<link name="link_2">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -68,10 +68,10 @@
</link>
<link name="link_3">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -82,10 +82,10 @@
</link>
<link name="link_4">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -96,10 +96,10 @@
</link>
<link name="link_5">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -110,10 +110,10 @@
</link>
<link name="link_6">
<inertial>
<!-- <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>
</inertial> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0" />
<geometry>
@ -127,7 +127,7 @@
<!-- Joints: main serial chain -->
<joint name="joint_1" type="revolute">
<limit lower="-2.705" upper="2.705" effort="0" velocity="0"/>
<origin rpy="0 0 0" xyz="0.014359931 0.001723413 0.261326995" />
<origin rpy="0 0 0" xyz="0 0 0" />
<parent link="base_link" />
<child link="link_1" />
<axis xyz="0 0 1" />