logs

robot/client_socket.py

@@ -272,13 +272,14 @@ class SocketRobotArm:
             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
         )
-        logger.info(calcWorldPosition)
         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]}")
 
@@ -452,8 +453,15 @@ class SocketRobotArm:
             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],
+            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,
@@ -487,11 +495,12 @@ class SocketRobotArm:
                         ],
                     )
                 )
-
+                points = np.degrees(pj)
+                # points[1] = points[1] + 60
+                # points[2] = points[2] - 100
+                # points[4] = points[4] - 40
                 result.append(
-                    self.prepare_data.make_step(
-                        "free", np.degrees(pj), [0, 0, 0, 0, 0, 0]
-                    )
+                    self.prepare_data.make_step("free", points, [0, 0, 0, 0, 0, 0])
                 )
         return result