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

test model

This commit is contained in:
Kseninia Mikhaylova 2024-11-20 17:59:32 +03:00
parent a992e30bba
commit a2059f72ba
3 changed files with 1487 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1460
app.log
View File

File diff suppressed because it is too large Load Diff

44
robot/client_socket.py
View File

@ -256,7 +256,7 @@ class SocketRobotArm:
self.urdf_manager.body_id, i, math.radians(axisJointPoses[i]) self.urdf_manager.body_id, i, math.radians(axisJointPoses[i])
) )
time.sleep(2) time.sleep(1)
coordinates = self.world_coordinates[:3] coordinates = self.world_coordinates[:3]
angles = [ angles = [
@ -266,29 +266,32 @@ class SocketRobotArm:
] ]
worldJointPoses = self.convert_to_joint_base( worldJointPoses = self.convert_to_joint_base(
[float(p) * 0.001 for p in coordinates], angles [float(p) * 0.001 for p in coordinates], [math.radians(p) for p in angles]
) )
for i in range(num_joints): for i in range(num_joints):
p.resetJointState(self.urdf_manager.body_id, i, worldJointPoses[i]) p.resetJointState(self.urdf_manager.body_id, i, worldJointPoses[i])
calcWorldPosition = p.getLinkState( calcWorldPosition = p.getLinkState(
self.urdf_manager.body_id, num_joints - 1, self.urdf_manager.body_id,
# computeForwardKinematics=True num_joints - 1,
computeForwardKinematics=True
) )
calcWorldPosition = list( calcWorldPosition = list(
[p * 1000 for p in calcWorldPosition[0]] [p * 1000 for p in calcWorldPosition[0]]
+ [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])] + [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
) )
logger.info(calcWorldPosition) # logger.info(calcWorldPosition)
for i, pos in enumerate(zip(calcWorldPosition, self.world_coordinates)): # for i, pos in enumerate(zip(calcWorldPosition, self.world_coordinates)):
logger.info(f"delta by WORLD {i} {pos[0] - pos[1]}") # logger.info(f"delta by WORLD {i} {pos[0] - pos[1]}")
position_diff = np.linalg.norm(self.world_coordinates[:3] - np.array(calcWorldPosition[:3]))
print("Position Difference:", position_diff)
# logger.info('-------') # logger.info('-------')
# for i, pos in enumerate(zip(axisJointPoses, worldJointPoses)): # for i, pos in enumerate(zip(axisJointPoses, worldJointPoses)):
# logger.info(f"delta by JOINT {i} {pos[0] - pos[1]}") # logger.info(f"delta by JOINT {i} {pos[0] - pos[1]}")
time.sleep(2) # time.sleep(1)
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}")
@ -451,20 +454,17 @@ class SocketRobotArm:
endEffectorLinkIndex=num_joints - 1, endEffectorLinkIndex=num_joints - 1,
targetPosition=[float(c) for c in coordinates], targetPosition=[float(c) for c in coordinates],
targetOrientation=angles, targetOrientation=angles,
lowerLimits=lower_limits, # lowerLimits=lower_limits,
upperLimits=upper_limits, # upperLimits=upper_limits,
jointRanges=[ # jointRanges=[4, 4, 4, 4, 4, 4],
math.radians(-17.409513 - (-17.596806)), # restPoses=[
math.radians(10.897068 - (-48.161888)), # math.radians(0.1873),
math.radians(-33.003525 - 66.406982), # math.radians(59.059),
math.radians(176.892288 - 177.113632), # math.radians(-99.4105),
math.radians(38.957874 - (-2.829787)), # math.radians(-0.2214),
math.radians(-159.934479 - (-159.933365)), # math.radians(41.7877),
], # math.radians(-0.0011),
restPoses=[0, 0, 0, 0, 0, 0, 0, 0], # ],
# solver=0,
# maxNumIterations=100,
# residualThreshold=0.001,
) )
return joint_angles return joint_angles

10
urdf/sample.urdf
View File

@ -138,7 +138,7 @@
<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" />
<origin rpy="0 0 0" xyz="0.168621 -0.103 0.23445" /> <origin rpy="0 0 0" xyz="0.168621 -0.103 0.4946" />
</joint> </joint>
<joint name="joint_3" type="revolute"> <joint name="joint_3" type="revolute">
@ -146,7 +146,7 @@
<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" />
<origin rpy="0 0 0" xyz="0.006871 0.002321 0.729516" /> <origin rpy="0 0 0" xyz="0.006871 0.002321 0.729921" />
</joint> </joint>
<joint name="joint_4" type="revolute"> <joint name="joint_4" type="revolute">
@ -154,7 +154,7 @@
<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" />
<origin rpy="0 0 0" xyz="0.240065 0.101766 0.097739" /> <origin rpy="0 0 0" xyz="0.240065 0.101766 0.098277" />
</joint> </joint>
<joint name="joint_5" type="revolute"> <joint name="joint_5" type="revolute">
@ -162,7 +162,7 @@
<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" />
<origin rpy="0 0 0" xyz="0.867755 0.037912 0.000177" /> <origin rpy="0 0 0" xyz="1.109039 0.037912 0.000177" />
</joint> </joint>
<joint name="joint_6" type="revolute"> <joint name="joint_6" type="revolute">
@ -170,7 +170,7 @@
<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" />
<origin rpy="0 0 0" xyz="0.098817 -0.039029 -0.000419" /> <origin rpy="0 0 0" xyz="0.117 -0.039029 -0.000419" />
</joint> </joint>
</robot> </robot>