feat: finish removing old ikpy-based IK

arm_launch.py

@@ -1,91 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
-from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch.conditions import IfCondition
-
-
-
-#Prevent making __pycache__ directories
-from sys import dont_write_bytecode
-dont_write_bytecode = True
-
-def launch_setup(context, *args, **kwargs):
-    # Retrieve the resolved value of the launch argument 'mode'
-    mode = LaunchConfiguration('mode').perform(context)
-    nodes = []
-
-    # Arm
-    # package_dir = get_package_share_directory('arm_pkg')
-    package_dir = "/home/david/repos/rover-ros2/src/arm_pkg"  # TODO: copy files to share and point there
-    urdf_file_name = 'arm12.urdf'
-    urdf = os.path.join(package_dir, "urdf", urdf_file_name)
-    # urdf = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/" + urdf_file_name
-    with open(urdf, 'r') as infp:
-        robot_desc = infp.read()
-
-    rviz_config_path = os.path.join(package_dir, 'viz.rviz')
-
-    if mode == 'anchor':
-        nodes.append(
-            Node(
-                package='robot_state_publisher',
-                executable='robot_state_publisher',
-                name='robot_state_publisher',
-                output='both',
-                parameters=[{'robot_description': robot_desc}],
-                arguments=[urdf],
-                on_exit=Shutdown()
-            )
-        )
-        nodes.append(
-            Node(
-                package='arm_pkg',
-                executable='arm',  # change as needed
-                name='arm',
-                output='both',
-                parameters=[{'launch_mode': 'anchor'}],
-                on_exit=Shutdown()
-            )
-        )
-        nodes.append(
-            Node(
-                package='rviz2',
-                executable='rviz2',
-                name='rviz2',
-                output='both',
-                arguments=['-d', rviz_config_path],
-                on_exit=Shutdown()
-            )
-        )
-        # nodes.append(
-        #     Node(
-        #         package='anchor_pkg',
-        #         executable='anchor',  # change as needed
-        #         name='anchor',
-        #         output='both',
-        #         parameters=[{'launch_mode': 'anchor'}],
-        #         on_exit=Shutdown()
-        #     )
-        # )
-    else:
-        # If an invalid mode is provided, print an error.
-        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
-
-    return nodes
-
-def generate_launch_description():
-    declare_arg = DeclareLaunchArgument(
-        'mode',
-        default_value='anchor',
-        description='Launch mode: arm, core, bio, anchor, or ptz'
-    )
-
-    return LaunchDescription([
-        declare_arg,
-        OpaqueFunction(function=launch_setup)
-    ])

src/anchor_pkg/launch/rover.launch.py

@@ -6,7 +6,6 @@ from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoi
 from launch_ros.actions import Node
 
 
-
 #Prevent making __pycache__ directories
 from sys import dont_write_bytecode
 dont_write_bytecode = True
@@ -18,18 +17,14 @@ def launch_setup(context, *args, **kwargs):
 
     if mode == 'anchor':
         # Launch every node and pass "anchor" as the parameter
-        
+
-        # Arm
-        urdf_file = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/arm12.urdf"
-        robot_description = {'robot_description': open(urdf_file).read()}
         nodes.append(
             Node(
                 package='arm_pkg',
                 executable='arm',  # change as needed
                 name='arm',
                 output='both',
-                parameters=[{'launch_mode': mode},
+                parameters=[{'launch_mode': mode}],
-                            {'robot_description': robot_description}],
                 on_exit=Shutdown()
             )
         )

src/arm_pkg/arm_pkg/arm_node.py

@@ -12,7 +12,6 @@ from ros2_interfaces_pkg.msg import ArmManual
 from ros2_interfaces_pkg.msg import SocketFeedback
 from ros2_interfaces_pkg.msg import DigitFeedback
 from sensor_msgs.msg import JointState
-from tf2_ros import TransformBroadcaster, TransformStamped
 import math
 
 # control_qos = qos.QoSProfile(
@@ -55,7 +54,6 @@ class SerialRelay(Node):
 
         # New messages
         self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
-        self.tf_broadcaster = TransformBroadcaster(self)
         self.joint_state = JointState()
         self.joint_state.name = [
             "Axis_0_Joint",
@@ -66,15 +64,7 @@ class SerialRelay(Node):
             "Wrist-EF_Roll_Joint",
             "Gripper_Slider_Left",
         ]
-        self.joint_state.position = [0.0] * len(
+        self.joint_state.position = [0.0] * len(self.joint_state.name)  # Initialize with zeros
-            self.joint_state.name
-        )  # Initialize with zeros
-        self.odom_trans = TransformStamped()
-        self.odom_trans.header.frame_id = "odom"
-        self.odom_trans.child_frame_id = "base_link"
-        self.odom_trans.transform.translation.x = 0.0
-        self.odom_trans.transform.translation.y = 0.0
-        self.odom_trans.transform.translation.z = 0.0
 
         self.joint_command_sub = self.create_subscription(
             JointState, "/joint_commands", self.joint_command_callback, 10
@@ -179,10 +169,8 @@ class SerialRelay(Node):
 
         # Set target angles for each arm axis for embedded IK PID to handle
         command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
-        # Wrist yaw
+        # Wrist yaw and roll
-        command += f"can_relay_tovic,digit,36,0,{positions[4]}\n"
+        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
-        # Wrist roll
-        command += f"can_relay_tovic,digit,35,{positions[5]}\n"
         # Gripper IK does not have adequate hardware yet
         self.send_cmd(command)
 
@@ -193,36 +181,18 @@ class SerialRelay(Node):
         axis3 = msg.axis3
 
         # Send controls for arm
-        command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n"
+        command = f"can_relay_tovic,arm,18,{int(msg.brake)}\n"
-        command += (
+        command += f"can_relay_tovic,arm,39,{axis0},{axis1},{axis2},{axis3}\n"
-            "can_relay_tovic,arm,39,"
-            + str(axis0)
-            + ","
-            + str(axis1)
-            + ","
-            + str(axis2)
-            + ","
-            + str(axis3)
-            + "\n"
-        )
 
         # Send controls for end effector
 
-        # command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
+        command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
-        # command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-        command += (
-            "can_relay_tovic,digit,39,"
-            + str(msg.effector_yaw)
-            + ","
-            + str(msg.effector_roll)
-            + "\n"
-        )
 
-        command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
 
-        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        command += f"can_relay_tovic,digit,28,{msg.laser}\n"
 
-        command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
+        command += f"can_relay_tovic,digit,34,{msg.linear_actuator}\n"
 
         self.send_cmd(command)
 
@@ -263,6 +233,8 @@ class SerialRelay(Node):
             if len(parts) >= 4:
                 self.digit_feedback.wrist_angle = float(parts[3])
                 # self.digit_feedback.wrist_roll = float(parts[4])
+                self.joint_state.position[4] = math.radians(float(parts[4]))  # Wrist roll
+                self.joint_state.position[5] = math.radians(float(parts[3]))  # Wrist yaw
         else:
             return
 
@@ -280,18 +252,7 @@ class SerialRelay(Node):
             angles_in = parts[3:7]
             # Convert the angles to floats divide by 10.0
             angles = [float(angle) / 10.0 for angle in angles_in]
-            #
+
-            #
-            # THIS NEEDS TO BE REMOVED LATER
-            # PLACEHOLDER FOR WRIST VALUE
-            #
-            #
-            angles.append(0.0)  # placeholder for wrist_continuous
-            angles.append(0.0)  # placeholder for wrist
-            #
-            #
-            # # Update the arm's current angles
-            self.arm.update_angles(angles)
             self.arm_feedback.axis0_angle = angles[0]
             self.arm_feedback.axis1_angle = angles[1]
             self.arm_feedback.axis2_angle = angles[2]
@@ -302,13 +263,10 @@ class SerialRelay(Node):
             self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
             self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
             self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
-            self.joint_state.position[4] = math.radians(angles[4])  # Wrist roll
+            # Wrist is handled by digit feedback
-            self.joint_state.position[5] = math.radians(angles[5])  # Wrist yaw
             self.joint_state.header.stamp = self.get_clock().now().to_msg()
             self.joint_state_pub.publish(self.joint_state)
 
-            # self.odom_trans.header.stamp = self.get_clock().now().to_msg()
-            self.tf_broadcaster.sendTransform(self.odom_trans)
         else:
             self.get_logger().info("Invalid angle feedback input format")
 

src/arm_pkg/package.xml

@@ -11,8 +11,6 @@
   <depend>common_interfaces</depend>
   <depend>python3-numpy</depend>
   <depend>ros2_interfaces_pkg</depend>
-  <!-- TODO: remove after refactored out -->
-  <exec_depend>python3-ikpy-pip</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.py

@@ -6,14 +6,12 @@ package_name = 'arm_pkg'
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version='1.0.0',
     packages=find_packages(exclude=['test']),
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['package.xml'])
-        (os.path.join('share', package_name, 'launch'), glob('launch/*')),
-  	    (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

src/arm_pkg/urdf/arm11.urdf

@@ -1,239 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!--MX64trntbl + MX106:  ? + 0.153 -->
-  <!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
-  <!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
-  <material name="omni/Blue">
-    <color rgba="0 0 0.8 1"/>
-  </material>
-  <material name="omni/Red">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <material name="omni/Green">
-    <color rgba="0 1 0 1"/>
-  </material>
-  <material name="omni/Yellow">
-    <color rgba="1 1 0 1"/>
-  </material>
-  <material name="omni/LightGrey">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <material name="omni/DarkGrey">
-    <color rgba="0.4 0.4 0.4 1"/>
-  </material>
-  <!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
-  <!-- The first use of a macro.  This one was defined in youbot_base/base.urdf.xacro above.
-       A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
-       that equals "base", and uses it to generate names for its component links and joints
-       (e.g., base_link).  The xacro:included origin block is also an argument to the macro.  By convention,
-       the origin block defines where the component is w.r.t its parent (in this case the parent
-       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
-  <!-- foot for arm-->
-  <link name="base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="10.0"/>
-      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-    </inertial>
-  </link>
-  <!-- joint between base_link and arm_0_link -->
-  <joint name="arm_joint_0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="arm_link_0"/>
-  </joint>
-  <link name="arm_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-      <material name="omni/LightGrey"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <mass value="0.2"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_1" type="revolute">
-    <parent link="arm_link_0"/>
-    <child link="arm_link_1"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="arm_link_1">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <mass value="0.190421352"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_2" type="revolute">
-    <parent link="arm_link_1"/>
-    <child link="arm_link_2"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_2">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-      <material name="omni/Red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <mass value="0.29302326"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_3" type="revolute">
-    <parent link="arm_link_2"/>
-    <child link="arm_link_3"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.471"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_3">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-      <material name="omni/Yellow"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <mass value="0.21931466"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_4" type="revolute">
-    <parent link="arm_link_3"/>
-    <child link="arm_link_4"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.377"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_4">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-      <material name="omni/Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_5" type="revolute">
-    <parent link="arm_link_4"/>
-    <child link="arm_link_5"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.132"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <link name="arm_link_5">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_6" type="fixed">
-    <parent link="arm_link_5"/>
-    <child link="arm_link_6"/>
-    <origin rpy="0 0 0" xyz="0 0 0.248"/>
-  </joint>
-  <link name="arm_link_6">
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="1e-12"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
-    </inertial>
-  </link>
-  <!-- END OF ARM LINKS/JOINTS -->
-</robot>

src/arm_pkg/urdf/arm12.urdf

@@ -1,340 +0,0 @@
-<robot name="robot">
-
-  <link name="base_footprint"></link>
-
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint" />
-    <child link="base_link" />
-    <origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
-  </joint>
-
-  <link name="base_link">
-    <visual>
-      <origin xyz="-0.175 0 0" rpy="0 0 0" />
-      <geometry>
-        <!-- <box size="0.5 0.5 0.01" /> -->
-        <mesh filename="package://arm_pkg/urdf/Arm Base Plate.STL" scale="1 1 1"/>
-      </geometry>
-      <material name="base_link-material">
-        <color rgba="0 0.6038273388475408 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.175 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_0" type="revolute">
-    <parent link="base_link" />
-    <child link="Axis_0_Plate" />
-    <origin xyz="0 0 0.035" rpy="0 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
-  </joint>
-
-  <link name="Axis_0_Plate">
-    <visual>
-      <origin xyz="-0.175 0 0" rpy="0 0 0" />
-      <geometry>
-        <!-- <cylinder radius="0.15" length="0.059" /> -->
-        <mesh filename="package://arm_pkg/urdf/Axis_0_Plate.STL" scale="1 1 1"/>
-      </geometry>
-      <material name="Axis_0-material">
-        <color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.175 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_1_Joint" type="revolute">
-    <parent link="Axis_0_Plate" />
-    <child link="Axis_1" />
-    <origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/>
-  </joint>
-
-  <link name="Axis_1">
-    <visual>
-      <origin xyz="0 -0.11 0.005" rpy="-1.6 0 0" />
-      <geometry>
-        <!-- <cylinder radius="0.082" length="0.1" /> -->
-        <mesh filename="package://arm_pkg/urdf/Axis 1.STL" scale="1 1 1"/>
-      </geometry>
-      <material name="Axis_1-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_1_to_Segment_1" type="fixed">
-    <parent link="Axis_1" />
-    <child link="Segment_1" />
-    <origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-
-  <link name="Segment_1">
-    <visual>
-      <origin xyz="-0.005 0 -0.35" rpy="0 0 0" />
-      <geometry>
-        <!-- <cylinder radius="0.025" length="0.473" /> -->
-        <mesh filename="package://arm_pkg/urdf/Segment 1.STL" scale="1 1 1"/>
-      </geometry>
-      <material name="Segment_1-material">
-        <color rgba="0.2 0.2 0.2 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_2_Joint" type="revolute">
-    <parent link="Segment_1" />
-    <child link="Axis_2" />
-    <origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/>
-  </joint>
-
-  <link name="Axis_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-      <material name="Axis_2-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_2_to_Segment_2" type="fixed">
-    <parent link="Axis_2" />
-    <child link="Segment_2" />
-    <origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-
-  <link name="Segment_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-      <material name="Segment_2-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_3_Joint" type="revolute">
-    <parent link="Segment_2" />
-    <child link="Axis_3" />
-    <origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>
-  </joint>
-
-  <link name="Axis_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-      <material name="Axis_3-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_3_to_Segment_3" type="fixed">
-    <parent link="Axis_3" />
-    <child link="Segment_3" />
-    <origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-
-  <link name="Segment_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-      <material name="Segment_3-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Wrist_Joint" type="revolute">
-    <parent link="Segment_3" />
-    <child link="Axis_4" />
-    <origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>
-  </joint>
-
-  <link name="Axis_4">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-      <material name="Axis_4-material">
-        <color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Continuous_Joint" type="revolute">
-    <parent link="Axis_4" />
-    <child link="Axis_4_C" />
-    <origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
-  </joint>
-
-  <link name="Axis_4_C">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-      <material name="Axis_4_C-material">
-        <color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-
-  <joint name="Axis_4_C_to_Effector" type="fixed">
-    <parent link="Axis_4_C" />
-    <child link="Effector" />
-    <origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
-  </joint>
-
-  <link name="Effector">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.05 0.01 0.135" />
-      </geometry>
-      <material name="Effector-material">
-        <color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.01 0.01 0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
-    </inertial>
-  </link>
-
-</robot>

src/arm_pkg/viz.rviz

@@ -1,234 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /TF1
-        - /RobotModel1
-        - /RobotModel1/Description Topic1
-      Splitter Ratio: 0.5
-    Tree Height: 717
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-        arm_link_0:
-          Value: true
-        arm_link_1:
-          Value: true
-        arm_link_2:
-          Value: true
-        arm_link_3:
-          Value: true
-        arm_link_4:
-          Value: true
-        arm_link_5:
-          Value: true
-        arm_link_6:
-          Value: true
-        base_link:
-          Value: true
-        odom:
-          Value: true
-      Marker Scale: 0.44999998807907104
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: false
-      Tree:
-        odom:
-          base_link:
-            arm_link_0:
-              arm_link_1:
-                arm_link_2:
-                  arm_link_3:
-                    arm_link_4:
-                      arm_link_5:
-                        arm_link_6:
-                          {}
-      Update Interval: 0
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        arm_link_0:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        arm_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        arm_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        arm_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        arm_link_4:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        arm_link_5:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        arm_link_6:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: odom
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 3.1002085208892822
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6997972726821899
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 5.6954216957092285
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: true
-  Height: 945
-  Hide Left Dock: true
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001630000036afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002c0000036a000000ab00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002effc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c000002ef000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e80000036a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1512
-  X: 0
-  Y: 37