add the Rviz part

harvest_arm_rm/config/config.yaml

@@ -14,8 +14,8 @@ harvest_arm_rm:
     max_joint: [155.0, 115.0, 172.0, 130.0, 175.0, 125.0, 179.0]
     min_joint: [-155.0, -30.0, -172.0, -130.0, -175.0, -125.0, -179.0]
 
-    max_joint_speed: 180.0      # deg/s
-    max_joint_acc: 600.0
+    max_joint_speed: 300.0      # deg/s
+    max_joint_acc: 10000.0
 
     max_cartesian_speed: 1.0          # m/s
     max_cartesian_angular_speed: 1.5 # rad/s
@@ -55,6 +55,8 @@ harvest_arm_rm:
 
     action_topic: "/action/arm"
     observation_topic: "/observation/arm"
+    action_rviz_topic: "/action/arm_rviz"
+    observation_rviz_topic: "/observation/arm_rviz"
 
     interval: 0.02
     kp_j : 1.0

harvest_arm_rm/harvest_arm_rm/arm_cfg.py

@@ -1,6 +0,0 @@
-def main():
-    print('Hi from harvest_arm_rm.')
-
-
-if __name__ == '__main__':
-    main()

harvest_arm_rm/harvest_arm_rm/run_realman_arm.py

@@ -15,7 +15,7 @@ from Robotic_Arm.rm_robot_interface import *
 
 
 class RM_Arm(Node):
-    def __init__(self,  port=8080, level=3, mode=2, node_name="harvest_arm_rm"):
+    def __init__(self,  port=8080, level=3, mode=2, node_name="realman_run_node"):
         """
         Initialize and connect to the robotic arm.
         Args:
@@ -24,7 +24,7 @@ class RM_Arm(Node):
             level (int, optional): Connection level. Defaults to 3.
             mode (int, optional): Thread mode (0: single, 1: dual, 2: triple). Defaults to 2.
         """
-        super().__init__('harvest_arm_rm')
+        super().__init__('realman_run_node')
         self.config, self.sts, self.arm_arg = self.get_arg()
 
 
@@ -50,6 +50,9 @@ class RM_Arm(Node):
         #   Create subcriber for target joint
         self.action_sub = self.create_subscription(JointState, self.config["action_topic"], self.arm_action_callback, 10)
 
+        self.observation_rviz_sub = self.create_subscription(JointState, self.config["observation_rviz_topic"], self.arm_observation_rviz_callback, 10)
+        self.action_rviz_pub = self.create_publisher(JointState, self.config["action_rviz_topic"], 10)
+
         self.timer = self.create_timer(
             self.config["interval"],
             self.rm_timer
@@ -101,6 +104,8 @@ class RM_Arm(Node):
             "interval": interval,
             "action_topic": self.declare_parameter('action_topic', "/action/arm").value,
             "observation_topic": self.declare_parameter('observation_topic', "/observation/arm").value,
+            "action_rviz_topic": self.declare_parameter('action_rviz_topic', "/action/arm_rviz").value,
+            "observation_rviz_topic": self.declare_parameter('observation_rviz_topic', "/observation/arm_rviz").value,
 
             "arm_ctrl_voltage": self.declare_parameter('arm_ctrl_voltage', 2).value,
             "arm_ctrl_io_mode": self.declare_parameter('arm_ctrl_io_mode', [0, 0, 1, 1]).value,
@@ -127,7 +132,15 @@ class RM_Arm(Node):
 
         return config, sts, arm_arg
     
-
+    def arm_observation_rviz_callback(self, msg):
+        """
+        Callback function for joint observation from RViz.
+        Args:
+            msg (JointState): Message containing the joint observation.
+        """
+        if self.arm_arg['arm_handle'].id == -1:
+            self.sts["joint_pos_sts"] = list(msg.position)
+            self.sts["joint_speed_sts"] = list(msg.velocity)
 
     def arm_action_callback(self, msg):
         """
@@ -135,6 +148,7 @@ class RM_Arm(Node):
         Args:
             msg (JointState): Message containing the joint command.
         """
+
         self.sts["tool_cmd"][1] = self.sts["tool_cmd"][0]
 
         total_seconds = msg.header.stamp.sec + (msg.header.stamp.nanosec * 1e-9)
@@ -147,15 +161,26 @@ class RM_Arm(Node):
             self.sts["tool_cmd"][0] = int(action_list[8])
         
 
-    def arm_observation_pub(self):
+    def arm_observation_relay_pub(self):
         """
         Publish the current state of the robotic arm.
         """
+        msg0= JointState()
+        msg0.header.stamp = self.get_clock().now().to_msg()
+        msg0.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_cmd"]) + [float(self.sts["tool_cmd"][0])]
+        msg0.velocity = []
+        msg0.effort = []
+        self.action_rviz_pub.publish(msg0)
+
+
+
         msg = JointState()
         msg.header.stamp = self.get_clock().now().to_msg()
-        msg.position = [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"][0])] + [float(self.sts["timer_cnt"])]
+        arm_real = self.arm_arg['arm_handle'].id
+        msg.position = [float(arm_real)] + [float(self.sts["arm_enable"])] + list(self.sts["joint_pos_sts"]) + [float(self.sts["tool_cmd"][0])] + [float(self.sts["timer_cnt"])]
         msg.velocity = self.sts["joint_speed_sts"]
         msg.effort = []
+
         self.observation_pub.publish(msg)
 
 
@@ -208,7 +233,7 @@ class RM_Arm(Node):
             if len(joint_position) != 7:
                 self.get_logger().warn(f"Warning: Expected 7 joint positions, got {len(joint_position)}: {joint_position}")
                 # Pad with zeros or truncate to 7
-            self.sts["joint_sts"] = joint_position
+            self.sts["joint_pos_sts"] = joint_position
             self.sts["joint_speed_sts"] = joint_speed
 
         except Exception as e:
@@ -260,7 +285,7 @@ class RM_Arm(Node):
         #############       movej --> arm
         if self.sts["arm_enable"] == 1:
             dis_max_vt = self.config["max_joint_speed"] * self.config["interval"]
-            dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"]
+            dis_vt = np.array(self.sts["joint_speed_sts"]) * self.config["interval"]  * 0.75
             d_j = np.clip(a=(np.array(self.sts["joint_target"]) - np.array(self.sts["joint_pos_sts"])) * self.config["kp_j"], 
                             a_min= dis_vt - self.config["lmt_05att"] , 
                             a_max=dis_vt + self.config["lmt_05att"])
@@ -270,11 +295,11 @@ class RM_Arm(Node):
 
         #############       tool 
         if self.sts["tool_cmd"][0] == 1 and self.sts["tool_cmd"][1] == 0:
-            self.set_tool_position(self.arm_arg['arm'], self.config, percent=1, device=1, tool_name=self.config["tool_name"])
+            self.set_tool_position( percent=1, device=1, tool_name=self.config["tool_name"])
         elif self.sts["tool_cmd"][0] == 0 and self.sts["tool_cmd"][1] == 1:
-            self.set_tool_position(self.arm_arg['arm'], self.config, percent=0, device=1, tool_name=self.config["tool_name"])
+            self.set_tool_position( percent=0, device=1, tool_name=self.config["tool_name"])
 
-        self.arm_observation_pub()
+        self.arm_observation_relay_pub()
 
         self.sts["timer_cnt"] += 1
 

harvest_arm_rm/harvest_arm_rm/run_realman_rviz.py

@@ -0,0 +1,143 @@
+#!/usr/bin/env python3
+
+import math
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+import numpy as np
+
+
+class RealmanRvizNode(Node):
+    def __init__(self):
+        super().__init__("realman_rviz_node")
+
+        self.config, self.sts = self.get_arg()
+
+        self.joint_pub = self.create_publisher(
+            JointState,
+            self.config["joint_state_topic"],
+            10
+        )
+        
+
+        self.action_sub = self.create_subscription(JointState, self.config["action_topic"], self.arm_action_callback, 10)
+        self.observation_pub = self.create_publisher(JointState, self.config["observation_topic"], 10)
+
+        self.timer = self.create_timer(0.02, self.timer_callback)
+
+
+    def get_arg(self):
+        config = {
+            "action_topic": self.declare_parameter('action_rviz_topic', "/action/arm_rviz").value,
+            "observation_topic": self.declare_parameter('observation_rviz_topic', "/observation/arm_rviz").value,
+
+            "joint_names": self.declare_parameter("joint_names", [
+                            "joint_1",
+                            "joint_2",
+                            "joint_3",
+                            "joint_4",
+                            "joint_5",
+                            "joint_6",
+                            "joint_7",
+                            ]).value,
+            "joint_state_topic": self.declare_parameter("joint_state_topic", "/joint_states").value,
+            "interval": self.declare_parameter("interval", 0.02).value,
+        }
+        sts = {
+            "joint_cmd": [-10.0] * 7,
+            "joint_velocity": [0.0] * 7,
+            "joint_target": [10.0] * 7,
+            "arm_enable":  int(0),
+            "tool_cmd":  int(0),
+            "pose_sts": [0.0] * 6,
+            "timer_cnt": 0,
+            "action_timestamp": 0.0,
+        }
+        return config, sts
+
+    def timer_callback(self):
+        """
+        Timer callback function to publish the current state of the robotic arm.
+        """
+        
+
+        self.position_regulation()
+
+        out = JointState()
+        out.header.stamp = self.get_clock().now().to_msg()
+        out.name = list(self.config["joint_names"])
+        out.position = [math.radians(x) for x in self.sts["joint_cmd"]]
+        out.velocity = []
+        out.effort = []
+
+        self.joint_pub.publish(out)
+
+        self.arm_observation_pub()
+
+
+    def arm_action_callback(self, msg):
+        """
+        Callback function for joint command.
+        Args:def observation_callback(self, msg):
+        """
+        self.get_logger().debug(f"Received action message: {msg}")
+
+        total_seconds = msg.header.stamp.sec + (msg.header.stamp.nanosec * 1e-9)
+        if total_seconds > self.sts["action_timestamp"]:
+            self.sts["action_timestamp"] = total_seconds
+
+            action_list = list(msg.position)
+            self.sts["arm_enable"] = int(action_list[0])
+            self.sts["joint_target"] = action_list[1:8]
+            self.sts["tool_cmd"] = int(action_list[8])
+
+            
+
+
+            self.sts["timer_cnt"] += 1
+
+    def arm_observation_pub(self):
+        """
+        Publish the current state of the robotic arm.
+        """
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.position =  list(self.sts["joint_cmd"]) 
+        msg.velocity = list(self.sts["joint_velocity"])
+        msg.effort = []
+        self.observation_pub.publish(msg)
+
+    def position_regulation(self):
+        """
+        Regulates the joint positions to ensure they are within the range [-pi, pi].
+        """
+        j_target = np.array(self.sts["joint_target"])
+        j_cmd = np.array(self.sts["joint_cmd"])
+        d_j = j_target - j_cmd
+        j_cmd += d_j
+
+        v = d_j / self.config["interval"]
+        v_p = np.array(self.sts["joint_velocity"])
+        alpha = 0.15
+        self.sts["joint_velocity"] = (alpha * v + (1-alpha) * v_p).tolist()
+        self.sts["joint_cmd"] = j_cmd.tolist()
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RealmanRvizNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

harvest_arm_rm/launch/rm_arm.launch.py

@@ -17,8 +17,8 @@ def generate_launch_description():
     return LaunchDescription([
         Node(
             package="harvest_arm_rm",
-            executable="realman_run",
-            name="harvest_arm_rm",
+            executable="realman_arm",
+            name="realman_arm_node",
             output="screen",
             parameters=[config_file],
         )

harvest_arm_rm/launch/rm_arm_rviz.launch.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import os
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    package_share_dir = get_package_share_directory("harvest_arm_rm")
+
+    config_file = os.path.join(
+        package_share_dir,
+        "config",
+        "config.yaml"
+    )
+
+    urdf_file = os.path.join(
+        package_share_dir,
+        "urdf",
+        "urdf_rm75",
+        "RM75.urdf"
+    )
+
+    with open(urdf_file, "r") as f:
+        robot_description = f.read()
+
+    realman_arm_node = Node(
+        package="harvest_arm_rm",
+        executable="realman_arm",
+        name="realman_arm_node",
+        output="screen",
+        parameters=[config_file],
+    )
+
+    realman_rviz_node = Node(
+        package="harvest_arm_rm",
+        executable="realman_rviz",
+        name="realman_rviz_node",
+        output="screen",
+        parameters=[config_file],
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[
+            {
+                "robot_description": robot_description,
+            }
+        ],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+    )
+
+    return LaunchDescription([
+        robot_state_publisher_node,
+        realman_arm_node,
+        realman_rviz_node,
+        rviz_node,
+    ])

harvest_arm_rm/launch/rm_rviz.launch.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import os
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    package_share_dir = get_package_share_directory("harvest_arm_rm")
+
+    config_file = os.path.join(
+        package_share_dir,
+        "config",
+        "config.yaml"
+    )
+
+    urdf_file = os.path.join(
+        package_share_dir,
+        "urdf",
+        "urdf_rm75",
+        "RM75.urdf"
+    )
+    print(f"Loading URDF file from: {urdf_file}")
+    with open(urdf_file, "r") as f:
+        robot_description = f.read()
+
+    rviz_bridge_node = Node(
+        package="harvest_arm_rm",
+        executable="realman_rviz",
+        name="realman_rviz_node",
+        output="screen",
+        parameters=[config_file],
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[
+            {
+                "robot_description": robot_description,
+            }
+        ],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+    )
+
+    return LaunchDescription([
+        robot_state_publisher_node,
+        rviz_bridge_node,
+        rviz_node,
+    ])

harvest_arm_rm/setup.py

@@ -14,7 +14,13 @@ setup(
 
     (os.path.join("share", "harvest_arm_rm", "config"), glob("config/*.yaml")),
     (os.path.join("share", "harvest_arm_rm", "launch"), glob("launch/*.launch.py")),
-],
+
+    (os.path.join("share", "harvest_arm_rm", "urdf", "urdf_rm75"),
+    glob("urdf/urdf_rm75/*.urdf") + glob("urdf/urdf_rm75/*.csv")),
+
+    (os.path.join("share", "harvest_arm_rm", "urdf", "urdf_rm75", "meshes"),
+    glob("urdf/urdf_rm75/meshes/*.STL")),
+            ],
     install_requires=['setuptools'],
     zip_safe=True,
     maintainer='zl',
@@ -28,8 +34,8 @@ setup(
     },
     entry_points={
         'console_scripts': [
-            'arm_cfg = harvest_arm_rm.arm_cfg:main',
-            'realman_run = harvest_arm_rm.run_realman_arm:main',
+            'realman_arm = harvest_arm_rm.run_realman_arm:main',
+            'realman_rviz = harvest_arm_rm.run_realman_rviz:main',
         ],
     },
 )

harvest_arm_rm/urdf/urdf_rm75/RM75-B.csv

@@ -0,0 +1,9 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,0.00049987,5.2709E-05,0.060019,0,0,0,0.83887,0.0017232,-3.1058E-06,-3.7924E-05,0.0017051,1.3691E-06,0.00090158,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,,连杆1-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+link_1,1.4803E-07,-0.021108,-0.025186,0,0,0,0.59354,0.0012661,6.0354E-09,-6.3788E-09,0.0011817,-0.00021121,0.00056132,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,,连杆2-1,link_1,joint_1,joint_1,revolute,0,0,0.2405,0,0,0,base_link,0,0,1,60,3.14,-3.106,3.106,,,,,,,,
+link_2,4.2145E-07,-0.076129,0.011078,0,0,0,0.43285,0.0012584,1.4694E-09,-5.7413E-09,0.00031747,0.000279,0.0012225,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,,连杆3-1,link_2,joint_2,joint_2,revolute,0,0,0,-1.5708,0,0,link_1,0,0,1,60,3.14,-2.2689,2.2689,,,,,,,,
+link_3,-3.2093E-07,-0.023545,-0.027347,0,0,0,0.43132,0.00079433,1.02E-09,1.3908E-08,0.00073037,-0.00014262,0.00031507,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,,连杆4-1,link_3,joint_3,joint_3,revolute,0,-0.256,0,1.5708,0,0,link_2,0,0,1,30,3.14,-3.106,3.106,,,,,,,,
+link_4,5.0722E-06,-0.059593,0.010569,0,0,0,0.28963,0.00063737,7.0681E-08,3.8708E-08,0.00015648,0.00014461,0.00061418,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,,连杆5-1,link_4,joint_4,joint_4,revolute,0,0,0,-1.5708,0,0,link_3,0,0,1,30,3.14,-2.356,2.356,,,,,,,,
+link_5,2.7551E-07,-0.018042,-0.02154,0,0,0,0.23942,0.00028595,1.9823E-09,-1.192E-09,0.00026273,-4.424E-05,0.0001199,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,,连杆6-1,link_5,joint_5,joint_5,revolute,0,-0.21,0,1.5708,0,0,link_4,0,0,1,10,3.14,-3.106,3.106,,,,,,,,
+link_6,3.4947E-06,-0.059381,0.0073681,0,0,0,0.2188,0.00035054,3.4456E-08,1.7975E-08,0.00010493,7.8243E-05,0.00033448,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,,连杆7-1,link_6,joint_6,joint_6,revolute,0,0,0,-1.5708,0,0,link_5,0,0,1,10,3.14,-2.234,2.234,,,,,,,,
+link_7,0.00081557,1.3323E-05,-0.012705,0,0,0,0.065037,2.1144E-05,2.2774E-08,2.5471E-08,1.8109E-05,1.019E-08,3.19E-05,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,,末端法兰件 方案一-1,link_7,joint_7,joint_7,revolute,0,-0.144,0,1.5708,0,0,link_6,0,0,1,10,3.14,-6.28,6.28,,,,,,,,

harvest_arm_rm/urdf/urdf_rm75/RM75.urdf

@@ -0,0 +1,453 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="RM75-B">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="0.00049987 5.2709E-05 0.060019"
+        rpy="0 0 0" />
+      <mass
+        value="1.862" />
+      <inertia
+        ixx="0.0017232"
+        ixy="-3.1058E-06"
+        ixz="-3.7924E-05"
+        iyy="0.0017051"
+        iyz="1.3691E-06"
+        izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link_1">
+    <inertial>
+      <origin
+        xyz="0.000241 -0.013273 -0.00995"
+        rpy="0 0 0" />
+      <mass
+        value="1.574" />
+      <inertia
+        ixx="0.002487573"
+        ixy="0.000009663"
+        ixz="-0.000007909"
+        iyy="0.002321038"
+        iyz="0.000179393"
+        izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.2405"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_2">
+    <inertial>
+      <origin
+        xyz="-0.000357 -0.106789 0.005329"
+        rpy="0 0 0" />
+      <mass
+        value="1.217" />
+      <inertia
+        ixx="0.003494121"
+        ixy="0.000002921"
+        ixz="-0.000005613"
+        iyy="0.000892721"
+        iyz="-0.000583884"
+        izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_1" />
+    <child
+      link="link_2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.2689"
+      upper="2.2689"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_3">
+    <inertial>
+      <origin
+        xyz="0.000003 -0.01398 -0.011324"
+        rpy="0 0 0" />
+      <mass
+        value="1.11" />
+      <inertia
+        ixx="0.001836663"
+        ixy="0.000002259"
+        ixz="-0.000004216"
+        iyy="0.001498875"
+        iyz="0.000037167"
+        izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="0 -0.256 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_2" />
+    <child
+      link="link_3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_4">
+    <inertial>
+      <origin
+        xyz="-0.000005 -0.084658 0.004747"
+        rpy="0 0 0" />
+      <mass
+        value="0.685" />
+      <inertia
+        ixx="0.001282444"
+        ixy="-0.000000551"
+        ixz="-0.000000630"
+        iyy="0.000373013"
+        iyz="-0.000232084"
+        izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_3" />
+    <child
+      link="link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.356"
+      upper="2.356"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_5">
+    <inertial>
+      <origin
+        xyz="0.000078 -0.012937 -0.008781"
+        rpy="0 0 0" />
+      <mass
+        value="0.619" />
+      <inertia
+        ixx="0.000627336"
+        ixy="0.000001636"
+        ixz="-0.000001345"
+        iyy="0.000542455"
+        iyz="0.000034970"
+        izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0 -0.21 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_4" />
+    <child
+      link="link_5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_6">
+    <inertial>
+      <origin
+        xyz="-0.000014 -0.078524 0.002819"
+        rpy="0 0 0" />
+      <mass
+        value="0.602" />
+      <inertia
+        ixx="0.000780774"
+        ixy="-0.000000121"
+        ixz="-0.000000469"
+        iyy="0.000289973"
+        iyz="-0.000120513"
+        izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_5" />
+    <child
+      link="link_6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.234"
+      upper="2.234"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_7">
+    <inertial>
+      <origin
+        xyz="0.001094 -0.000077 -0.010119"
+        rpy="0 0 0" />
+      <mass
+        value="0.107" />
+      <inertia
+        ixx="0.000044123"
+        ixy="-0.000000064"
+        ixz="0.0000003"
+        iyy="0.000035078"
+        iyz="-0.000000029"
+        izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://harvest_arm_rm/urdf/urdf_rm75/meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="revolute">
+    <origin
+      xyz="0 -0.144 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_6" />
+    <child
+      link="link_7" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-6.28"
+      upper="6.28"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+</robot>