Random joint limits, microstepping

src/ar3_config/config/joint_limits.yaml

@@ -0,0 +1,40 @@
+joint_limits:
+  joint1:
+    has_velocity_limits: true
+    max_velocity: 40.1750
+    has_acceleration_limits: true
+    max_acceleration: 50.75
+  joint2:
+    has_velocity_limits: true
+    max_velocity: 40.1750
+    has_acceleration_limits: true
+    max_acceleration: 30.875
+  joint3:
+    has_velocity_limits: true
+    max_velocity: 40.1750
+    has_acceleration_limits: true
+    max_acceleration: 40.5
+  joint4:
+    has_velocity_limits: true
+    max_velocity: 40.1750
+    has_acceleration_limits: true
+    max_acceleration: 50.125
+  joint5:
+    has_velocity_limits: true
+    max_velocity: 40.6100
+    has_acceleration_limits: true
+    max_acceleration: 50.75
+  joint6:
+    has_velocity_limits: true
+    max_velocity: 40.6100
+    has_acceleration_limits: true
+    max_acceleration: 70.0
+
+cartesian_limits:
+  max_trans_vel: 10
+  max_trans_acc: 20.25
+  max_trans_dec: -50
+  max_rot_vel: 10.57
+
+default_acceleration_scaling_factor: 1.0
+default_velocity_scaling_factor: 1.0

src/ar3_config/launch/dev.launch.py

@@ -76,6 +76,7 @@ def generate_launch_description():
                 get_yaml(join(config_root, 'trajectory.yaml')),
                 get_yaml(join(config_root, 'planning_scene.yaml')),
                 get_yaml(join(config_root, "ar3_controllers.yaml")),
+                get_yaml(join(config_root, "joint_limits.yaml")),
             ],
         ),
 

src/ar3_description/urdf/ar3.urdf

@@ -92,6 +92,7 @@
       link="link_1" />
     <axis
       xyz="0 0 1" />
+    <limit effort="0" velocity="-20.0" lower="-3.14" upper="3.14" />
   </joint>
   <link
     name="link_2">
@@ -145,6 +146,7 @@
       link="link_2" />
     <axis
       xyz="0 0 -1" />
+    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
   </joint>
   <link
     name="link_3">
@@ -198,6 +200,7 @@
       link="link_3" />
     <axis
       xyz="0 0 -1" />
+    <limit effort="0" velocity="200.0" lower="-3.14" upper="3.14" />
   </joint>
   <link
     name="link_4">
@@ -251,6 +254,7 @@
       link="link_4" />
     <axis
       xyz="0 0 -1" />
+    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
   </joint>
   <link
     name="link_5">
@@ -304,6 +308,7 @@
       link="link_5" />
     <axis
       xyz="-1 0 0" />
+    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
   </joint>
   <link
     name="link_6">
@@ -357,6 +362,7 @@
       link="link_6" />
     <axis
       xyz="0 0 1" />
+    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
   </joint>
 
 

src/cobot_platform/cobot_platform/move.py

@@ -46,11 +46,11 @@ class CobotMove(Node):
         print(self._serial.readline())
 
     def _move_j1_cb(self, msg):
-        self._cur[0] = int(msg.data / pi / 2. * 8000)
+        self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
         self._move_servo()
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 10000)
+        self._cur[1] = int(msg.data / pi / 2. * 10000 * 4)
         self._move_servo()
 
     def _move_j3_cb(self, msg):
@@ -74,10 +74,11 @@ class CobotMove(Node):
 
 
         line = self._serial.readline().decode('UTF-8')
+        # print(line)
 
         blocks = re.findall(r'[ABCDEF]-*\d+', line)
-        a = float(blocks[0][1:]) / 2000. * pi * 2.
-        b = float(blocks[1][1:]) / 3840.64 * pi * 2.
+        a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
+        b = float(blocks[1][1:]) / 10000. / 4. * pi * 2.
         c = float(blocks[2][1:]) / 10000. * pi * 2.
         d = float(blocks[3][1:]) / 2000. * pi * 2.
         e = float(blocks[4][1:]) / 2000. * pi * 2.