2025-1-6

ren_yuan/ren_yuan_pointcloud/pcd2binary.py

@@ -0,0 +1,23 @@
+import open3d as o3d
+import numpy as np
+
+
+def pcd_to_binary(pcd_file, output_file):
+    pcd = o3d.io.read_point_cloud(pcd_file)
+    points = np.asarray(pcd.points)
+    binary_data = points[:, :3]
+    with open(output_file, 'wb') as f:
+        f.write(binary_data.tobytes())
+
+
+# def binary_to_nparray(binary_file, output_file):
+#     with open(binary_file, 'rb') as f:
+#         binary_data = f.read()
+#     # 将二进制数据转换为 NumPy 数组
+#     point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
+#     np.savetxt(output_file, point_cloud)
+
+
+if __name__ == '__main__':
+    pcd_to_binary(r'/home/robot/ROS/ren_yuan/ren_yuan_pointcloud/123.pcd', r'/home/robot/ROS/ren_yuan/ren_yuan_pointcloud/pointcloud.txt')
+    #binary_to_nparray(r'./pointcloud.txt', r'./111123123123.txt')

ren_yuan/ren_yuan_pointcloud/说明.txt

@@ -0,0 +1,3 @@
+需要将相机排出的PCD点云手动裁切以及移动后 导出为txt 再打开txt然后导出为PCD 再用这里的代码转为2进制
+
+x往负方向1300   z 640   且绕z轴转一下（摆放问题）

ren_yuan/src/CMakeLists.txt

@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

ren_yuan/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

ren_yuan/src/lst_robot_r/scripts/moveitServer2.py

@@ -25,7 +25,7 @@ import traceback
 from lst_robot_r.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
-from redis_publisher import Publisher
+#from redis_publisher import Publisher
 
 pi = np.pi
 
@@ -500,7 +500,7 @@ def ROS2PY_msgs(msgs, m_sr):
         
 
 if __name__ =="__main__":
-    publisher = Publisher()
+    #publisher = Publisher()
 
     folder_path = rospy.get_param("folder_path")
     rospy.init_node('moveit_control_server', anonymous=False)   
@@ -521,6 +521,8 @@ if __name__ =="__main__":
     # #ROS2_msgs_write(trajectory_with_type_merge,moveit_server)
     # rospy.loginfo("合并后轨迹执行完毕")
     
-    message = ROS2PY_msgs(trajectory_with_type_merge, moveit_server)
-    publisher.pub_plan_result(message)
-    print("路径规划信息已发布....")
+    # message = ROS2PY_msgs(trajectory_with_type_merge, moveit_server)
+    # publisher.pub_plan_result(message)
+    # print("路径规划信息已发布....")
+
+    rospy.set_param("sign_control",0)

ren_yuan/src/lst_robot_r/scripts/start.py

@@ -1,5 +1,4 @@
 #! /usr/bin/env python3
-import sys
 import rospy
 import os
 import command
@@ -8,14 +7,16 @@ import hjsx
 import threading
 import numpy as np
 import multiprocessing
-import actionlib_msgs.msg._GoalStatusArray
 from moveit_msgs.msg import PlanningScene
 from dynamic_reconfigure.client import Client
 from rospy.exceptions import ROSException
 from std_srvs.srv import Empty
 from rospy.service import ServiceException
-from redis_publisher import Publisher
 
+import actionlib_msgs.msg._GoalStatusArray
+import sys
+
+#from redis_publisher import Publisher
 
 def wait_for_topic(topic_name, message_type):
     try:
@@ -34,37 +35,43 @@ def clear_octomap():
         rospy.loginfo("Failed to call /clear_octomap service or it is not available. Skipping operation.")
 
 waited_once = False
-tim_list = []
+tim_list=[]
 
 if __name__ == "__main__":
     command.load_rviz()
     rospy.init_node("start_node")
 
     # 初始化各种类型参数
-    rospy.set_param("ping_sta", 45)#用于判断平缝的角度
-    # rospy.set_param("sign_control",1)
-    rospy.set_param("sign_pointcloud", 0)
-    rospy.set_param("folder_path", "/home/robot/ROS/ren_yuan/data/123")
-
+    rospy.set_param("ping_sta",45)#用于判断平缝的角度
+    
+    rospy.set_param("sign_pointcloud",0)
+    rospy.set_param("folder_path","/home/robot/ROS/ren_yuan/data/123")
 
     rospy.loginfo("等待rviz启动")
-    wait_for_topic("/execute_trajectory/status", actionlib_msgs.msg.GoalStatusArray)
-    
-    publisher = Publisher()
-    publisher.rds.set('sign_control', 0)
+    wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
     
+    rospy.set_param("sign_control",1)
+    # publisher = Publisher()
+    # publisher.rds.set('sign_control', 0)
+
     while not rospy.is_shutdown():
-        # sign_control = str(rospy.get_param("sign_control"))
-        sign_control = publisher.rds.get('sign_control').decode('ascii')
-        
+        #sign_control = publisher.rds.get('sign_control').decode('ascii')
+        sign_control = str(rospy.get_param("sign_control"))
         if sign_control == "0":
             if not waited_once:
-                print("等待点云数据准备完成...")
+                #print("等待点云数据准备完成...")
+                sys.stdin.flush()#清空键盘缓冲区
+                aaa=input("请选择 1-2 默认为2: ")
+                if aaa=="":
+                    aaa='2'
+                #aaa='2'
+                if(aaa =='1' or aaa=='2'):
+                    rospy.set_param("sign_control",aaa)
+                else:
+                    command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch yunhua_1006a_moveit_config demo.launch")
+                    exit(0)
                 waited_once = True
-                
         elif sign_control == "1":
-            publisher.rds.set('sign_control', 0)
-            
             # 清除场景   点云计算并发布
             clear_octomap()
             process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
@@ -77,7 +84,7 @@ if __name__ == "__main__":
             rospy.loginfo("等待场景地图加载完毕...")
             wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
             rospy.loginfo(f"场景地图已加载完毕")
-            rospy.set_param("sign_pointcloud", 1)
+            rospy.set_param("sign_pointcloud",1)
 
             command.load_visual()
 
@@ -85,28 +92,29 @@ if __name__ == "__main__":
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
+
             while rospy.get_param("sign_control"):
                 pass
 
             waited_once = False      #运行结束，重置参数
  
         elif sign_control == "2":
-            publisher.rds.set('sign_control', 0)
-            
+
+            #publisher.rds.set('sign_control', 0)
+
             hjsx.run()
             command.load_visual()
 
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
+
             while rospy.get_param("sign_control"):
                 pass
             waited_once = False
-            
-        elif sign_control == "3":
-            rospy.loginfo("正在关闭规划器....")
-            command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch yunhua_1006a_moveit_config demo.launch")
-            sys.exit(0)
 
-    
+        # elif sign_control == "3":
+        #     rospy.loginfo("正在关闭规划器....")
+        #     command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch yunhua_1006a_moveit_config demo.launch")
+        #     sys.exit(0)
     

ren_yuan/src/yunhua_1006a_moveit_config/config/fake_controllers.yaml

@@ -10,4 +10,4 @@ controller_list:
       - joint6
 initial:  # Define initial robot poses per group
   - group: manipulator
-    pose: zero
+    pose: home

ren_yuan/src/yunhua_1006a_moveit_config/config/yunhua_ryhj.srdf

@@ -13,22 +13,23 @@
         <chain base_link="base_link" tip_link="link_end"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="zero" group="manipulator">
+    <group_state name="home" group="manipulator">
         <joint name="joint1" value="0"/>
         <joint name="joint2" value="0"/>
         <joint name="joint3" value="0"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="0"/>
+        <joint name="joint5" value="1.57"/>
         <joint name="joint6" value="0"/>
     </group_state>
-    <group_state name="home" group="manipulator">
+    <group_state name="zero" group="manipulator">
         <joint name="joint1" value="0"/>
         <joint name="joint2" value="0"/>
         <joint name="joint3" value="0"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="1.57"/>
+        <joint name="joint5" value="0"/>
         <joint name="joint6" value="0"/>
     </group_state>
+
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link2" reason="Never"/>

ren_yuan/src/yunhua_1006a_moveit_config/launch/moveit.rviz

@@ -343,7 +343,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.6265817284584045
+      Distance: 2.882205009460449
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -351,17 +351,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.2947564125061035
-        Y: 0.001691619516350329
-        Z: 0.49094098806381226
+        X: 1.2886135578155518
+        Y: 0.007511036470532417
+        Z: 0.44920703768730164
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.07499924302101135
+      Pitch: 0.15000008046627045
       Target Frame: base_link
-      Yaw: 0.02673092484474182
+      Yaw: 0.18173374235630035
     Saved: ~
 Window Geometry:
   Displays:

ren_yuan/src/yunhua_ryhj/CMakeLists.txt

@@ -1,14 +1,14 @@
-cmake_minimum_required(VERSION 2.8.3)
-
-project(yunhua_ryhj)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-find_package(roslaunch)
-
-foreach(dir config launch meshes urdf)
-	install(DIRECTORY ${dir}/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
-endforeach(dir)
+cmake_minimum_required(VERSION 2.8.3)
+
+project(yunhua_ryhj)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

ren_yuan/src/yunhua_ryhj/config/joint_names_yunhua_ryhj.yaml

@@ -1 +1 @@
-controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', ]
+controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', ]

ren_yuan/src/yunhua_ryhj/launch/display.launch

@@ -1,20 +1,20 @@
-<launch>
-  <arg
-    name="model" />
-  <param
-    name="robot_description"
-    textfile="$(find yunhua_ryhj)/urdf/yunhua_ryhj.urdf" />
-  <node
-    name="joint_state_publisher_gui"
-    pkg="joint_state_publisher_gui"
-    type="joint_state_publisher_gui" />
-  <node
-    name="robot_state_publisher"
-    pkg="robot_state_publisher"
-    type="robot_state_publisher" />
-  <node
-    name="rviz"
-    pkg="rviz"
-    type="rviz"
-    args="-d $(find yunhua_ryhj)/urdf.rviz" />
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find yunhua_ryhj)/urdf/yunhua_ryhj.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find yunhua_ryhj)/urdf.rviz" />
 </launch>

ren_yuan/src/yunhua_ryhj/launch/gazebo.launch

@@ -1,20 +1,20 @@
-<launch>
-  <include
-    file="$(find gazebo_ros)/launch/empty_world.launch" />
-  <node
-    name="tf_footprint_base"
-    pkg="tf"
-    type="static_transform_publisher"
-    args="0 0 0 0 0 0 base_link base_footprint 40" />
-  <node
-    name="spawn_model"
-    pkg="gazebo_ros"
-    type="spawn_model"
-    args="-file $(find yunhua_ryhj)/urdf/yunhua_ryhj.urdf -urdf -model yunhua_ryhj"
-    output="screen" />
-  <node
-    name="fake_joint_calibration"
-    pkg="rostopic"
-    type="rostopic"
-    args="pub /calibrated std_msgs/Bool true" />
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find yunhua_ryhj)/urdf/yunhua_ryhj.urdf -urdf -model yunhua_ryhj"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
 </launch>

ren_yuan/src/yunhua_ryhj/package.xml

@@ -1,21 +1,21 @@
-<package format="2">
-  <name>yunhua_ryhj</name>
-  <version>1.0.0</version>
-  <description>
-    <p>URDF Description package for yunhua_ryhj</p>
-    <p>This package contains configuration data, 3D models and launch files
-for yunhua_ryhj robot</p>
-  </description>
-  <author>TODO</author>
-  <maintainer email="TODO@email.com" />
-  <license>BSD</license>
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>roslaunch</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>rviz</depend>
-  <depend>joint_state_publisher_gui</depend>
-  <depend>gazebo</depend>
-  <export>
-    <architecture_independent />
-  </export>
+<package format="2">
+  <name>yunhua_ryhj</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for yunhua_ryhj</p>
+    <p>This package contains configuration data, 3D models and launch files
+for yunhua_ryhj robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
 </package>

ren_yuan/src/yunhua_ryhj/urdf/yunhua_ryhj.csv

@@ -1,8 +1,8 @@
-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.073946,-7.5783E-05,0.079686,0,0,0,26.825,0.29447,-0.00068906,-0.0018702,0.70624,8.3009E-05,0.88713,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,0.1451,0.1451,0.1451,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,,HY1020A-180-1-1,基坐标系,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
-link1,0.040145,-0.0017665,-0.17281,0,0,0,39.487,0.98418,-0.16112,-0.47642,1.5184,-0.10144,1.2525,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,,HY1020A-180-2-1;送丝盘SLDPRT-1,坐标系1,轴1,joint1,revolute,0,0,0.511,0,0,0,base_link,0,0,-1,0,0,-3.14,3.14,,,,,,,,
-link2,-0.030086,0.33235,0.14338,0,0,0,11.602,0.54948,0.0072392,0.0022966,0.040699,-0.0016648,0.56667,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,,HY1010A-180-3-1,坐标系2,轴2,joint2,revolute,0.225,0,0,1.5708,0,0,link1,0,0,-1,0,0,-3.14,3.14,,,,,,,,
-link3,0.080981,0.12571,-9.0574E-05,0,0,0,13.961,0.19012,-0.11007,0.0065658,0.19149,0.011087,0.28951,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,,HY1010A-180-4-1;送丝机-1,坐标系3,轴3,joint3,revolute,0,0.76,0,0,0,0,link2,0,0,1,0,0,-3.14,3.14,,,,,,,,
-link4,-0.0016575,0.026244,0.35186,0,0,0,10.585,0.16278,0.00030377,0.0018777,0.18729,-0.010668,0.051504,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,,HY1006A-200-1-1;HY1006A-144-5-1,坐标系4,轴4,joint4,revolute,0.9915,0.215,0,0,-1.5708,0,link3,0,0,-1,0,0,-3.14,3.14,,,,,,,,
-link5,-5.2388E-05,-0.061382,-0.0003515,0,0,0,1.0093,0.0042625,6.072E-07,-2.4533E-07,0.0023268,-0.00020667,0.0032649,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,,HY1006A-138-6-1,坐标系5,轴5,joint5,revolute,0,0,0,0,1.5708,0,link4,0,0,1,0,0,-3.14,3.14,,,,,,,,
-link6,-0.073417,-0.00013522,0.2336,0,0,0,0.62764,0.0028394,1.5554E-06,0.00027709,0.0011602,-4.9029E-08,0.0030715,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,,焊枪_LST-1,坐标系6,轴6,joint6,revolute,0,0,0,1.5708,0,0,link5,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+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.073946,-7.5783E-05,0.079686,0,0,0,26.825,0.29447,-0.00068906,-0.0018702,0.70624,8.3009E-05,0.88713,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,0.1451,0.1451,0.1451,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,,HY1020A-180-1-1,基坐标系,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+link1,0.040145,-0.0017665,-0.17281,0,0,0,39.487,0.98418,-0.16112,-0.47642,1.5184,-0.10144,1.2525,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,,HY1020A-180-2-1;送丝盘SLDPRT-1,坐标系1,轴1,joint1,revolute,0,0,0.511,0,0,0,base_link,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link2,-0.030086,0.33235,0.14338,0,0,0,11.602,0.54948,0.0072392,0.0022966,0.040699,-0.0016648,0.56667,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,,HY1010A-180-3-1,坐标系2,轴2,joint2,revolute,0.225,0,0,1.5708,0,0,link1,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link3,0.080981,0.12571,-9.0574E-05,0,0,0,13.961,0.19012,-0.11007,0.0065658,0.19149,0.011087,0.28951,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,,HY1010A-180-4-1;送丝机-1,坐标系3,轴3,joint3,revolute,0,0.76,0,0,0,0,link2,0,0,1,0,0,-3.14,3.14,,,,,,,,
+link4,-0.0016575,0.026244,0.35186,0,0,0,10.585,0.16278,0.00030377,0.0018777,0.18729,-0.010668,0.051504,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,,HY1006A-200-1-1;HY1006A-144-5-1,坐标系4,轴4,joint4,revolute,0.9915,0.215,0,0,-1.5708,0,link3,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link5,-5.2388E-05,-0.061382,-0.0003515,0,0,0,1.0093,0.0042625,6.072E-07,-2.4533E-07,0.0023268,-0.00020667,0.0032649,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,,HY1006A-138-6-1,坐标系5,轴5,joint5,revolute,0,0,0,0,1.5708,0,link4,0,0,1,0,0,-3.14,3.14,,,,,,,,
+link6,-0.073417,-0.00013522,0.2336,0,0,0,0.62764,0.0028394,1.5554E-06,0.00027709,0.0011602,-4.9029E-08,0.0030715,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,,焊枪_LST-1,坐标系6,轴6,joint6,revolute,0,0,0,1.5708,0,0,link5,0,0,-1,0,0,-3.14,3.14,,,,,,,,

ren_yuan/src/yunhua_ryhj/urdf/yunhua_ryhj.urdf

@@ -1,420 +1,420 @@
-<?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-4-g7f85cfe  Build Version: 1.6.7995.38578
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="yunhua_ryhj">
-  <link
-    name="base_link">
-    <inertial>
-      <origin
-        xyz="-0.073946 -7.5783E-05 0.079686"
-        rpy="0 0 0" />
-      <mass
-        value="26.825" />
-      <inertia
-        ixx="0.29447"
-        ixy="-0.00068906"
-        ixz="-0.0018702"
-        iyy="0.70624"
-        iyz="8.3009E-05"
-        izz="0.88713" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.1451 0.1451 0.1451 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link1">
-    <inertial>
-      <origin
-        xyz="0.040145 -0.0017665 -0.17281"
-        rpy="0 0 0" />
-      <mass
-        value="39.487" />
-      <inertia
-        ixx="0.98418"
-        ixy="-0.16112"
-        ixz="-0.47642"
-        iyy="1.5184"
-        iyz="-0.10144"
-        izz="1.2525" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link1.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://yunhua_ryhj/meshes/link1.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint1"
-    type="revolute">
-    <origin
-      xyz="0 0 0.511"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="link1" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.617995"
-      upper="2.617995"
-      effort="0"
-      velocity="2.827" />
-  </joint>
-  <link
-    name="link2">
-    <inertial>
-      <origin
-        xyz="-0.030086 0.33235 0.14338"
-        rpy="0 0 0" />
-      <mass
-        value="11.602" />
-      <inertia
-        ixx="0.54948"
-        ixy="0.0072392"
-        ixz="0.0022966"
-        iyy="0.040699"
-        iyz="-0.0016648"
-        izz="0.56667" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link2.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 0 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link2.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint2"
-    type="revolute">
-    <origin
-      xyz="0.225 0 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link1" />
-    <child
-      link="link2" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-1.448624"
-      upper="1.570797"
-      effort="0"
-      velocity="2.827" />
-  </joint>
-  <link
-    name="link3">
-    <inertial>
-      <origin
-        xyz="0.080981 0.12571 -9.0574E-05"
-        rpy="0 0 0" />
-      <mass
-        value="13.961" />
-      <inertia
-        ixx="0.19012"
-        ixy="-0.11007"
-        ixz="0.0065658"
-        iyy="0.19149"
-        iyz="0.011087"
-        izz="0.28951" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link3.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://yunhua_ryhj/meshes/link3.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint3"
-    type="revolute">
-    <origin
-      xyz="0 0.76 0"
-      rpy="0 0 0" />
-    <parent
-      link="link2" />
-    <child
-      link="link3" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-1.308998"
-      upper="1.186824"
-      effort="0"
-      velocity="3.463" />
-  </joint>
-  <link
-    name="link4">
-    <inertial>
-      <origin
-        xyz="-0.0016575 0.026244 0.35186"
-        rpy="0 0 0" />
-      <mass
-        value="10.585" />
-      <inertia
-        ixx="0.16278"
-        ixy="0.00030377"
-        ixz="0.0018777"
-        iyy="0.18729"
-        iyz="-0.010668"
-        izz="0.051504" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link4.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 0 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link4.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint4"
-    type="revolute">
-    <origin
-      xyz="0.9915 0.215 0"
-      rpy="0 -1.5708 0" />
-    <parent
-      link="link3" />
-    <child
-      link="link4" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-2.356196"
-      upper="2.356196"
-      effort="0"
-      velocity="7.106" />
-  </joint>
-  <link
-    name="link5">
-    <inertial>
-      <origin
-        xyz="-5.2388E-05 -0.061382 -0.0003515"
-        rpy="0 0 0" />
-      <mass
-        value="1.0093" />
-      <inertia
-        ixx="0.0042625"
-        ixy="6.072E-07"
-        ixz="-2.4533E-07"
-        iyy="0.0023268"
-        iyz="-0.00020667"
-        izz="0.0032649" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link5.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 0 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link5.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint5"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="-1.5708 3.1416 -1.5708" />
-    <parent
-      link="link4" />
-    <child
-      link="link5" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-1.570797"
-      upper="2.268929"
-      effort="0"
-      velocity="5.639" />
-  </joint>
-  <link
-    name="link6">
-    <inertial>
-      <origin
-        xyz="-0.073417 -0.00013522 0.2336"
-        rpy="0 0 0" />
-      <mass
-        value="0.62764" />
-      <inertia
-        ixx="0.0028394"
-        ixy="1.5554E-06"
-        ixz="0.00027709"
-        iyy="0.0011602"
-        iyz="-4.9029E-08"
-        izz="0.0030715" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link6.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://yunhua_ryhj/meshes/link6.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint6"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link5" />
-    <child
-      link="link6" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.839726"
-      upper="3.839726"
-      effort="0"
-      velocity="0" />
-  </joint>
-
-    <link name="link_flp">
-    <collision>
-      <geometry>
-        <box size="0.0001 0.0001 0.0001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-  </link>
-
-  <joint name="joint_flp" type="fixed">
-    <parent link="link6"/>
-    <child link="link_flp"/>
-    <origin rpy="0 0 3.1415926" xyz="0 0 0.140"/>
-  </joint>
-
-  <link name="link_end">
-  </link>
-
-  <joint name="joint_end" type="fixed">
-    <parent link="link_flp"/>
-    <child link="link_end"/>
-    <origin rpy="0.0 0.0 0.0" xyz="-0.1385635  0.0032697  0.3679281"/>
-  </joint>
-
-</robot>
+<?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-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="yunhua_ryhj">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="-0.073946 -7.5783E-05 0.079686"
+        rpy="0 0 0" />
+      <mass
+        value="26.825" />
+      <inertia
+        ixx="0.29447"
+        ixy="-0.00068906"
+        ixz="-0.0018702"
+        iyy="0.70624"
+        iyz="8.3009E-05"
+        izz="0.88713" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.1451 0.1451 0.1451 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link1">
+    <inertial>
+      <origin
+        xyz="0.040145 -0.0017665 -0.17281"
+        rpy="0 0 0" />
+      <mass
+        value="39.487" />
+      <inertia
+        ixx="0.98418"
+        ixy="-0.16112"
+        ixz="-0.47642"
+        iyy="1.5184"
+        iyz="-0.10144"
+        izz="1.2525" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link1.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://yunhua_ryhj/meshes/link1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.511"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.617995"
+      upper="2.617995"
+      effort="0"
+      velocity="2.827" />
+  </joint>
+  <link
+    name="link2">
+    <inertial>
+      <origin
+        xyz="-0.030086 0.33235 0.14338"
+        rpy="0 0 0" />
+      <mass
+        value="11.602" />
+      <inertia
+        ixx="0.54948"
+        ixy="0.0072392"
+        ixz="0.0022966"
+        iyy="0.040699"
+        iyz="-0.0016648"
+        izz="0.56667" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 0 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint2"
+    type="revolute">
+    <origin
+      xyz="0.225 0 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link1" />
+    <child
+      link="link2" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-1.448624"
+      upper="1.570797"
+      effort="0"
+      velocity="2.827" />
+  </joint>
+  <link
+    name="link3">
+    <inertial>
+      <origin
+        xyz="0.080981 0.12571 -9.0574E-05"
+        rpy="0 0 0" />
+      <mass
+        value="13.961" />
+      <inertia
+        ixx="0.19012"
+        ixy="-0.11007"
+        ixz="0.0065658"
+        iyy="0.19149"
+        iyz="0.011087"
+        izz="0.28951" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link3.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://yunhua_ryhj/meshes/link3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint3"
+    type="revolute">
+    <origin
+      xyz="0 0.76 0"
+      rpy="0 0 0" />
+    <parent
+      link="link2" />
+    <child
+      link="link3" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-1.308998"
+      upper="1.186824"
+      effort="0"
+      velocity="3.463" />
+  </joint>
+  <link
+    name="link4">
+    <inertial>
+      <origin
+        xyz="-0.0016575 0.026244 0.35186"
+        rpy="0 0 0" />
+      <mass
+        value="10.585" />
+      <inertia
+        ixx="0.16278"
+        ixy="0.00030377"
+        ixz="0.0018777"
+        iyy="0.18729"
+        iyz="-0.010668"
+        izz="0.051504" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 0 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint4"
+    type="revolute">
+    <origin
+      xyz="0.9915 0.215 0"
+      rpy="0 -1.5708 0" />
+    <parent
+      link="link3" />
+    <child
+      link="link4" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-2.356196"
+      upper="2.356196"
+      effort="0"
+      velocity="7.106" />
+  </joint>
+  <link
+    name="link5">
+    <inertial>
+      <origin
+        xyz="-5.2388E-05 -0.061382 -0.0003515"
+        rpy="0 0 0" />
+      <mass
+        value="1.0093" />
+      <inertia
+        ixx="0.0042625"
+        ixy="6.072E-07"
+        ixz="-2.4533E-07"
+        iyy="0.0023268"
+        iyz="-0.00020667"
+        izz="0.0032649" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 0 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint5"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 3.1416 -1.5708" />
+    <parent
+      link="link4" />
+    <child
+      link="link5" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-1.570797"
+      upper="2.268929"
+      effort="0"
+      velocity="5.639" />
+  </joint>
+  <link
+    name="link6">
+    <inertial>
+      <origin
+        xyz="-0.073417 -0.00013522 0.2336"
+        rpy="0 0 0" />
+      <mass
+        value="0.62764" />
+      <inertia
+        ixx="0.0028394"
+        ixy="1.5554E-06"
+        ixz="0.00027709"
+        iyy="0.0011602"
+        iyz="-4.9029E-08"
+        izz="0.0030715" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_ryhj/meshes/link6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link5" />
+    <child
+      link="link6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.839726"
+      upper="3.839726"
+      effort="0"
+      velocity="0" />
+  </joint>
+
+    <link name="link_flp">
+    <collision>
+      <geometry>
+        <box size="0.0001 0.0001 0.0001"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+  </link>
+
+  <joint name="joint_flp" type="fixed">
+    <parent link="link6"/>
+    <child link="link_flp"/>
+    <origin rpy="0 0 3.1415926" xyz="0 0 0.140"/>
+  </joint>
+
+  <link name="link_end">
+  </link>
+
+  <joint name="joint_end" type="fixed">
+    <parent link="link_flp"/>
+    <child link="link_end"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.1385635  0.0032697  0.3679281"/>
+  </joint>
+
+</robot>