basic working with stl but some domain issues

robot/rospackages/src/arm_ik/arm_ik/IKNode.py

@@ -43,7 +43,8 @@ def __init__(self):
     # Joint lengths
     self.L1 = 1.354
     self.L2 = 1.333
-    self.L3 = 0.1
+    # self.L3 = 0.4992
+    self.L3 = 1.250
 
     # Cylindrical coordinates and angles
     self.calculate_cylindical()
@@ -67,6 +68,12 @@ def calculate_angles(self):
     """ Performs IK calculation and stores values in self.angles.
         Returns True if within range, False otherwise"""
     try:
+      # if self.pitch >= math.pi / 2:
+      #   pitch2 = (math.pi / 2) - self.pitch
+      #   cu = self.L3 * math.sin(pitch2) - self.u
+      #   cv = self.L3 * math.cos(pitch2) - self.v
+      #   self.get_logger().warn(f"in special case for end effector pointing up")
+      # else:
       cu = self.u - self.L3 * math.sin(self.pitch)
       cv = self.v - self.L3 * math.cos(self.pitch)
       if cu == 0 or cv == 0:
@@ -87,8 +94,9 @@ def calculate_angles(self):
       b3 = math.acos(B3)
 
       # contains Shoulder Swivel, Shoulder Flex, Elbow Flex, Wrist Flex (in that order)
-      self.angles = [float(self.phi), (math.pi / 2) - (b1 + a1), 
-                     math.pi - b2, math.pi - (self.pitch + a2 + b3)]
+      self.angles = [float(self.phi), (math.pi / 2) - (b1 + a1),
+                      math.pi - b2, math.pi - (self.pitch + a2 + b3)]
+      self.get_logger().info(f"angles: {self.angles}")
       return True
 
     except ValueError as e:
@@ -102,12 +110,12 @@ def joy_callback(self, message: Joy):
     self.y += (-y / 30000) # forward-back of mouse
     self.z += (-z / 30000) # vertical axis of mouse
     self.th += (yaw / 150) # use rotation axis that is activated by spinning the top
-    self.pitch += (roll / 150)
+    self.pitch += (roll / 30000)
     self.calculate_cylindical()
     # Perform IK. If out of range, restore point where it was
     if not self.calculate_angles():
       self.x, self.y, self.z, self.th, self.pitch = old_values
-    # self.get_logger().info(f"Current location: {self.x} {self.y} {self.z} roll {self.th}")
+    # self.get_logger().info(f"Current location: {self.x} {self.y} {self.z} roll {self.th} pitch {self.pitch}")
     # self.get_logger().info(f"Cylindical coordinates: u {self.u} v {self.v} phi {self.phi}")
 
   def calculate_cylindical(self):
@@ -116,7 +124,10 @@ def calculate_cylindical(self):
     if self.x == 0:
       self.phi = 0
     else:
-      self.phi = math.atan(self.y / self.x)
+      if self.x >= 0:
+        self.phi = math.atan(self.y / self.x)
+      else:
+        self.phi = math.pi + math.atan(self.y / self.x)
 
 
 def main(args=None):

robot/rospackages/src/arm_ik/rviz/urdf.rviz

@@ -0,0 +1,35 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Value: true
+    - Alpha: 0.8
+      Class: rviz_default_plugins/RobotModel
+      Description Topic:
+        Value: /robot_description
+      Name: RobotModel
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Value: true
+  Global Options:
+    Fixed Frame: base_link
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 1.7
+      Name: Current View
+      Pitch: 0.33
+      Value: Orbit (rviz)
+      Yaw: 5.5
+Window Geometry:
+  Height: 800
+  Width: 1200

robot/rospackages/src/arm_ik/setup.py

@@ -18,6 +18,7 @@
         # (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
         (os.path.join('share', package_name), glob('urdf/*')),
         # ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        (os.path.join('share', package_name), glob('meshes/*'))
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -30,6 +31,7 @@
         'console_scripts': [
             'IKNode = arm_ik.IKNode:main',
             'CadMouseJoyNode = arm_ik.CadMouseJoyNode:main',
+            'state_publisher = arm_ik.state_publisher:main'
         ],
     },
 )

robot/rospackages/src/arm_ik/urdf/astro_arm.urdf

@@ -25,9 +25,8 @@
         xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
-        <!-- <mesh
-          filename="package://arm_ik/meshes/base_link.STL" /> -->
-        <mesh filename="file://$(find arm_ik)/meshes/base_linkABC.STL" />
+        <mesh
+          filename="package://arm_ik/meshes/base_link.STL" />
       </geometry>
       <material
         name="">