test model
This commit is contained in:
parent
a992e30bba
commit
a2059f72ba
|
|
@ -256,7 +256,7 @@ class SocketRobotArm:
|
|||
self.urdf_manager.body_id, i, math.radians(axisJointPoses[i])
|
||||
)
|
||||
|
||||
time.sleep(2)
|
||||
time.sleep(1)
|
||||
|
||||
coordinates = self.world_coordinates[:3]
|
||||
angles = [
|
||||
|
|
@ -266,29 +266,32 @@ class SocketRobotArm:
|
|||
]
|
||||
|
||||
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):
|
||||
p.resetJointState(self.urdf_manager.body_id, i, worldJointPoses[i])
|
||||
|
||||
calcWorldPosition = p.getLinkState(
|
||||
self.urdf_manager.body_id, num_joints - 1,
|
||||
# computeForwardKinematics=True
|
||||
self.urdf_manager.body_id,
|
||||
num_joints - 1,
|
||||
computeForwardKinematics=True
|
||||
)
|
||||
calcWorldPosition = list(
|
||||
[p * 1000 for p in calcWorldPosition[0]]
|
||||
+ [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
|
||||
)
|
||||
logger.info(calcWorldPosition)
|
||||
for i, pos in enumerate(zip(calcWorldPosition, self.world_coordinates)):
|
||||
logger.info(f"delta by WORLD {i} {pos[0] - pos[1]}")
|
||||
# logger.info(calcWorldPosition)
|
||||
# for i, pos in enumerate(zip(calcWorldPosition, self.world_coordinates)):
|
||||
# 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('-------')
|
||||
|
||||
# for i, pos in enumerate(zip(axisJointPoses, worldJointPoses)):
|
||||
# logger.info(f"delta by JOINT {i} {pos[0] - pos[1]}")
|
||||
|
||||
time.sleep(2)
|
||||
# time.sleep(1)
|
||||
|
||||
self._get_command_count()
|
||||
self.set_text(text=f"Команд в очереди {self.remote_command_count}")
|
||||
|
|
@ -451,20 +454,17 @@ class SocketRobotArm:
|
|||
endEffectorLinkIndex=num_joints - 1,
|
||||
targetPosition=[float(c) for c in coordinates],
|
||||
targetOrientation=angles,
|
||||
lowerLimits=lower_limits,
|
||||
upperLimits=upper_limits,
|
||||
jointRanges=[
|
||||
math.radians(-17.409513 - (-17.596806)),
|
||||
math.radians(10.897068 - (-48.161888)),
|
||||
math.radians(-33.003525 - 66.406982),
|
||||
math.radians(176.892288 - 177.113632),
|
||||
math.radians(38.957874 - (-2.829787)),
|
||||
math.radians(-159.934479 - (-159.933365)),
|
||||
],
|
||||
restPoses=[0, 0, 0, 0, 0, 0, 0, 0],
|
||||
# solver=0,
|
||||
# maxNumIterations=100,
|
||||
# residualThreshold=0.001,
|
||||
# lowerLimits=lower_limits,
|
||||
# upperLimits=upper_limits,
|
||||
# jointRanges=[4, 4, 4, 4, 4, 4],
|
||||
# restPoses=[
|
||||
# math.radians(0.1873),
|
||||
# math.radians(59.059),
|
||||
# math.radians(-99.4105),
|
||||
# math.radians(-0.2214),
|
||||
# math.radians(41.7877),
|
||||
# math.radians(-0.0011),
|
||||
# ],
|
||||
)
|
||||
|
||||
return joint_angles
|
||||
|
|
|
|||
|
|
@ -138,7 +138,7 @@
|
|||
<parent link="link_1" />
|
||||
<child link="link_2" />
|
||||
<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 name="joint_3" type="revolute">
|
||||
|
|
@ -146,7 +146,7 @@
|
|||
<parent link="link_2" />
|
||||
<child link="link_3" />
|
||||
<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 name="joint_4" type="revolute">
|
||||
|
|
@ -154,7 +154,7 @@
|
|||
<parent link="link_3" />
|
||||
<child link="link_4" />
|
||||
<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 name="joint_5" type="revolute">
|
||||
|
|
@ -162,7 +162,7 @@
|
|||
<parent link="link_4" />
|
||||
<child link="link_5" />
|
||||
<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 name="joint_6" type="revolute">
|
||||
|
|
@ -170,7 +170,7 @@
|
|||
<parent link="link_5" />
|
||||
<child link="link_6" />
|
||||
<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>
|
||||
|
||||
</robot>
|
||||
Loading…
Reference in New Issue