云华2024-12-18

ren_yuan/src/fanuc_ry/config/joint_names_fanuc_ry.yaml

@@ -1 +0,0 @@
-controller_joint_names: ['', 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', ]

ren_yuan/src/fanuc_ry/urdf/fanuc_ry.csv

@@ -1,8 +0,0 @@
-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.0067459,0.00048961,0.12827,0,0,0,13.511,0.1373,0.00087636,0.003867,0.14276,4.9465E-05,0.11338,0,0,0,0,0,0,package://fanuc_ry/meshes/base_link.STL,0.26667,0.26667,0.26667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/base_link.STL,,底座-1,基坐标系,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
-Link_1,0.046629,0.040325,0.11902,0,0,0,10.873,0.1054,-0.0045186,-0.021996,0.13842,-0.018604,0.12811,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_1.STL,0.26667,0.26667,0.26667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_1.STL,,轴1-1,Origin_joint_1,Axis_joint_1,joint_1,revolute,0,0,0.277,0,0,0,base_link,0,0,1,0,0,-3,3,,,,,,,,
-Link_2,0.079218,0.046804,0.19413,0,0,0,37.391,11.84,0.35759,-5.9826,16.808,0.31317,5.2925,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_2.STL,0.26667,0.26667,0.26667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_2.STL,,轴2-1,Origin_joint_2,Axis_joint_2,joint_2,revolute,0.15,-0.042195,0.173,0,0,0,Link_1,0,1,0,0,0,-3,3,,,,,,,,
-Link_3,0.0819232659965815,0.0488957476294858,0.0997479026407966,0,0,0,5.31125443623239,0.0377023601596469,-0.00198499744122145,-0.00823986510994731,0.0380505516425425,-0.00237816508450241,0.0202238320497431,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_3.STL,0.266666666666667,0.266666666666667,0.266666666666667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_3.STL,,轴3-1,Origin_joint_3,Axis_joint_3,joint_3,revolute,0,0,0.78,0,0,0,Link_2,0,-1,0,0,0,-3,3,,,,,,,,
-Link_4,0.475870558811332,-0.0337629939092132,-0.00126165368026476,0,0,0,6.90672216285316,0.0193668843618062,0.048070034103511,-0.000520432981432283,0.433691694631167,0.000101052935278791,0.439756920518466,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_4.STL,0.266666666666667,0.266666666666667,0.266666666666667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_4.STL,,轴4-1,Origin_joint_4,Axis_joint_4,joint_4,revolute,0.18449,0.042195,0.2,0,0,0,Link_3,-1,0,0,0,0,-3,3,,,,,,,,
-Link_5,-1.00678870289315,0.00460908463366021,-0.785340384700437,0,0,0,37.3909058444352,11.8404562839242,0.357586794323293,-5.9825947327725,16.8076547081665,0.313167595336379,5.29248954036271,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_5.STL,0.266666666666667,0.266666666666667,0.266666666666667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_5.STL,,轴5-1,Origin_joint_5,Axis_joint_5,joint_5,revolute,0.90151,0,-0.00052875,0,0,0,Link_4,0,-1,0,0,0,-3,3,,,,,,,,
-Link_6,-0.0165412016845266,0.0951891558056374,0.119534910446276,0,0,0,0.789056328743544,0.00166815677862568,-0.000330967965728139,6.08102317073045E-06,0.00102524254386583,-1.68914833733875E-06,0.0013292952791531,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_6.STL,0.266666666666667,0.266666666666667,0.266666666666667,1,0,0,0,0,0,0,package://fanuc_ry/meshes/Link_6.STL,,轴6-1;相机支架总成-1,Origin_joint_6,Axis_joint_6,joint_6,revolute,0.093994,0,0.0005287,0,0,0,Link_5,0,0,1,0,0,-3,3,,,,,,,,

ren_yuan/src/fanuc_ry/urdf/fanuc_ry.urdf

@@ -1,413 +0,0 @@
-<?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="fanuc_ry">
-  
-  <link name="world_link" />
-  <joint name="world_joint" type="fixed">
-    <parent link="world_link" />
-    <child link="base_link" />
-    <origin xyz="0 0 2.17" rpy="0 3.14 0" />
-  </joint>
-  
-  <link
-    name="base_link">
-    <!-- <inertial>
-      <origin
-        xyz="0.0067459 0.00048961 0.12827"
-        rpy="0 0 0" />
-      <mass
-        value="13.511" />
-      <inertia
-        ixx="0.1373"
-        ixy="0.00087636"
-        ixz="0.003867"
-        iyy="0.14276"
-        iyz="4.9465E-05"
-        izz="0.11338" />
-    </inertial> -->
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.26667 0.26667 0.26667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="Link_1">
-    <inertial>
-      <origin
-        xyz="0.046629 0.040325 0.11902"
-        rpy="0 0 0" />
-      <mass
-        value="10.873" />
-      <inertia
-        ixx="0.1054"
-        ixy="-0.0045186"
-        ixz="-0.021996"
-        iyy="0.13842"
-        iyz="-0.018604"
-        izz="0.12811" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_1.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.26667 0.26667 0.26667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_1.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_1"
-    type="revolute">
-    <origin
-      xyz="0 0 0.277"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="Link_1" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3"
-      upper="3"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_2">
-    <inertial>
-      <origin
-        xyz="0.079218 0.046804 0.19413"
-        rpy="0 0 0" />
-      <mass
-        value="37.391" />
-      <inertia
-        ixx="11.84"
-        ixy="0.35759"
-        ixz="-5.9826"
-        iyy="16.808"
-        iyz="0.31317"
-        izz="5.2925" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_2.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.26667 0.26667 0.26667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_2.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_2"
-    type="revolute">
-    <origin
-      xyz="0.15 -0.042195 0.173"
-      rpy="0 0 0" />
-    <parent
-      link="Link_1" />
-    <child
-      link="Link_2" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-3"
-      upper="3"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_3">
-    <inertial>
-      <origin
-        xyz="0.0819232659965815 0.0488957476294858 0.0997479026407966"
-        rpy="0 0 0" />
-      <mass
-        value="5.31125443623239" />
-      <inertia
-        ixx="0.0377023601596469"
-        ixy="-0.00198499744122145"
-        ixz="-0.00823986510994731"
-        iyy="0.0380505516425425"
-        iyz="-0.00237816508450241"
-        izz="0.0202238320497431" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_3.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.266666666666667 0.266666666666667 0.266666666666667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_3.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_3"
-    type="revolute">
-    <origin
-      xyz="0 0 0.78"
-      rpy="0 0 0" />
-    <parent
-      link="Link_2" />
-    <child
-      link="Link_3" />
-    <axis
-      xyz="0 -1 0" />
-    <limit
-      lower="-3"
-      upper="3"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_4">
-    <inertial>
-      <origin
-        xyz="0.475870558811332 -0.0337629939092132 -0.00126165368026476"
-        rpy="0 0 0" />
-      <mass
-        value="6.90672216285316" />
-      <inertia
-        ixx="0.0193668843618062"
-        ixy="0.048070034103511"
-        ixz="-0.000520432981432283"
-        iyy="0.433691694631167"
-        iyz="0.000101052935278791"
-        izz="0.439756920518466" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_4.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.266666666666667 0.266666666666667 0.266666666666667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_4.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_4"
-    type="revolute">
-    <origin
-      xyz="0.18449 0.042195 0.2"
-      rpy="0 0 0" />
-    <parent
-      link="Link_3" />
-    <child
-      link="Link_4" />
-    <axis
-      xyz="-1 0 0" />
-    <limit
-      lower="-3"
-      upper="3"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_5">
-    <inertial>
-      <origin
-        xyz="-1.00678870289315 0.00460908463366021 -0.785340384700437"
-        rpy="0 0 0" />
-      <mass
-        value="37.3909058444352" />
-      <inertia
-        ixx="11.8404562839242"
-        ixy="0.357586794323293"
-        ixz="-5.9825947327725"
-        iyy="16.8076547081665"
-        iyz="0.313167595336379"
-        izz="5.29248954036271" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_5.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.266666666666667 0.266666666666667 0.266666666666667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_5.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_5"
-    type="revolute">
-    <origin
-      xyz="0.90151 0 -0.00052875"
-      rpy="0 0 0" />
-    <parent
-      link="Link_4" />
-    <child
-      link="Link_5" />
-    <axis
-      xyz="0 -1 0" />
-    <limit
-      lower="-3"
-      upper="3"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_6">
-    <inertial>
-      <origin
-        xyz="-0.0165412016845266 0.0951891558056374 0.119534910446276"
-        rpy="0 0 0" />
-      <mass
-        value="0.789056328743544" />
-      <inertia
-        ixx="0.00166815677862568"
-        ixy="-0.000330967965728139"
-        ixz="6.08102317073045E-06"
-        iyy="0.00102524254386583"
-        iyz="-1.68914833733875E-06"
-        izz="0.0013292952791531" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_6.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.266666666666667 0.266666666666667 0.266666666666667 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://fanuc_ry/meshes/Link_6.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_6"
-    type="revolute">
-    <origin
-      xyz="0.093994 0 0.0005287"
-      rpy="0 -1.57 3.14" />
-    <parent
-      link="Link_5" />
-    <child
-      link="Link_6" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-3"
-      upper="3"
-      effort="0"
-      velocity="0" />
-  </joint>
-
-  <link name="link_tool">
-  </link>
-
-  <joint name="joint_tool" type="fixed">
-    <parent link="Link_6"/>
-    <child link="link_tool"/>
-    <origin xyz="0.051 0.1209 0.3147" rpy="0 0 -0.400"/>
-  </joint>
-
-</robot>

ren_yuan/src/fanuc_ry_moveit_config/.setup_assistant

@@ -1,11 +0,0 @@
-moveit_setup_assistant_config:
-  URDF:
-    package: fanuc_ry
-    relative_path: urdf/fanuc_ry.urdf
-    xacro_args: ""
-  SRDF:
-    relative_path: config/fanuc_ry.srdf
-  CONFIG:
-    author_name: ryrobot
-    author_email: ryrobot@123.com
-    generated_timestamp: 1733531055

ren_yuan/src/fanuc_ry_moveit_config/config/fanuc_ry.srdf

@@ -1,43 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!--This does not replace URDF, and is not an extension of URDF.
-    This is a format for representing semantic information about the robot structure.
-    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="fanuc_ry">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="manipulator">
-        <chain base_link="world_link" tip_link="link_tool"/>
-    </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">
-        <joint name="joint_1" value="0"/>
-        <joint name="joint_2" value="0"/>
-        <joint name="joint_3" value="0"/>
-        <joint name="joint_4" value="0"/>
-        <joint name="joint_5" value="0"/>
-        <joint name="joint_6" value="0"/>
-    </group_state>
-    <group_state name="home" group="manipulator">
-        <joint name="joint_1" value="0"/>
-        <joint name="joint_2" value="-1.5182"/>
-        <joint name="joint_3" value="-1.5265"/>
-        <joint name="joint_4" value="0"/>
-        <joint name="joint_5" value="1.75"/>
-        <joint name="joint_6" 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="Link_1" link2="Link_2" reason="Adjacent"/>
-    <disable_collisions link1="Link_1" link2="Link_3" reason="Never"/>
-    <disable_collisions link1="Link_1" link2="base_link" reason="Adjacent"/>
-    <disable_collisions link1="Link_2" link2="Link_3" reason="Adjacent"/>
-    <disable_collisions link1="Link_2" link2="Link_5" reason="Never"/>
-    <disable_collisions link1="Link_3" link2="Link_4" reason="Adjacent"/>
-    <disable_collisions link1="Link_3" link2="Link_5" reason="Never"/>
-    <disable_collisions link1="Link_3" link2="Link_6" reason="Never"/>
-    <disable_collisions link1="Link_4" link2="Link_5" reason="Adjacent"/>
-    <disable_collisions link1="Link_5" link2="Link_6" reason="Adjacent"/>
-</robot>

ren_yuan/src/lst_robot_r/scripts/command.py

@@ -25,7 +25,7 @@ def load_visual():
     command = "rosrun visual visual_node"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
 def load_rviz():
-    command = "roslaunch fanuc_ry_moveit_config demo.launch"
+    command = "roslaunch yunhua_1006a_moveit_config demo.launch"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
 def launch_publish_pointcloud():
     file_name = "dycl_0506.py"

ren_yuan/src/lst_robot_r/scripts/dycl_0506.py

@@ -112,7 +112,7 @@ ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2)
 def build_pointcloud2_msg(points):
     msg = PointCloud2()
     msg.header.stamp = rospy.Time(0)
-    msg.header.frame_id = "world_link"
+    msg.header.frame_id = "base_link"
 
     if len(points.shape) == 3:
         msg.height = points.shape[1]

ren_yuan/src/lst_robot_r/scripts/hjsx.py

@@ -129,7 +129,7 @@ def compute_pose_R(fangxiang, hanfeng):
                     [0, np.sin(np.pi), np.cos(np.pi)]])
 
     R_mat = np.matmul(A, np.matmul(B, np.matmul(C, D)))
-    #R_mat = np.matmul(A, np.matmul(B, D))
+    #R_mat = np.matmul(A, np.matmul(B, C))
     #A*(B*(C*D))              x*y*z*x(180)
     return R_mat
 
@@ -193,12 +193,12 @@ def run():
     faxiang/= np.linalg.norm(faxiang)  # 单位化
     shuzhi /= np.linalg.norm(shuzhi)  # 单位化
     
-    fangxiang=faxiang+shuzhi*4 #arctan(4)=76度
+    fangxiang=faxiang-shuzhi*4 #arctan(4)=76度
     mat=compute_pose_R(fangxiang,hanfeng1)
     q = R.from_matrix(mat).as_quat()#应该是RPY转四元数
     result.append(("s", point1[0],point1[1], q, q))
 
-    fangxiang=-faxiang+shuzhi*4 #arctan(4)=76度
+    fangxiang=-faxiang-shuzhi*4 #arctan(4)=76度
     mat=compute_pose_R(fangxiang,hanfeng1)
     q = R.from_matrix(mat).as_quat()#应该是RPY转四元数
     result.append(("s", point2[0],point2[1], q, q))

ren_yuan/src/lst_robot_r/scripts/moveitServer2.py

@@ -129,7 +129,9 @@ class MoveIt_Control:
             moveit_server.arm.execute(retimed_planed)
             rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
+            err=0
         else:
+            err=1
             rospy.loginfo("fraction=" + str(fraction) + " success after " + str(maxtries))   
             
             # rospy.loginfo("本次焊缝规划失败")
@@ -138,7 +140,7 @@ class MoveIt_Control:
             # trajectory_with_type.pop()
             # self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
         #self.hf_num=self.hf_num+1#焊缝计数
-        return waypoints,trajectory,trajectory_with_type
+        return waypoints,trajectory,trajectory_with_type,err
     
     #TODO retime_plan
     def retime_plan(self,traj,v):
@@ -324,21 +326,27 @@ class MoveIt_Control:
 
             point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0],q1[1],q1[2],q1[3]]
             point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
-            way_points,trajectory,trajectory_with_type = self.plan_cartesian_path_LLL(point11,point22,way_points,trajectory,trajectory_with_type,v=speed_v)
+            way_points,trajectory,trajectory_with_type,error = self.plan_cartesian_path_LLL(point11,point22,way_points,trajectory,trajectory_with_type,v=speed_v)
             
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
             rospy.loginfo("*******************")
             
             #向上移动末端执行器
             if i<len(all_data):
-                up_value=0.1
+                up_value=-0.1
                 point_up=[point22[0], point22[1], point22[2]-up_value, point22[3], point22[4],point22[5],point22[6]]
-                way_points, trajectory, trajectory_with_type = self.plan_cartesian_path_LLL(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
+                way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_LLL(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
                 rospy.loginfo("末端执行器上移完毕")
                 rospy.loginfo("*******************")
             
-        rospy.loginfo("All of The trajectory Plan successfully")
-        rospy.loginfo("*******************")
+            if(error==1):
+                rospy.loginfo("焊缝规划出现失败 清空路径列表")
+                rospy.loginfo("*******************")
+                trajectory.clear()
+                trajectory_with_type.clear()
+                break
+        #rospy.loginfo("All of The trajectory Plan successfully")
+        #rospy.loginfo("*******************")
         if gohome:
             #trajectory,trajectory_with_type= self.move_p_flexible(trajectory,trajectory_with_type)
             trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)

ren_yuan/src/lst_robot_r/scripts/start.py

@@ -62,7 +62,7 @@ if __name__ == "__main__":
                 if(aaa =='1' or aaa=='2'):
                     rospy.set_param("sign_control",aaa)
                 else:
-                    command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch fanuc_ry_moveit_config demo.launch")
+                    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":

ren_yuan/src/visual/CMakeLists.txt

@@ -5,7 +5,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
-  fanuc_ry_moveit_config
+  yunhua_1006a_moveit_config
 )
 
 catkin_package(

ren_yuan/src/fanuc_ry/CMakeLists.txt → ren_yuan/src/yunhua_1006a/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 
-project(fanuc_ry)
+project(yunhua_1006a)
 
 find_package(catkin REQUIRED)
 

ren_yuan/src/yunhua_1006a/config/joint_names_yunhua_1006a.yaml

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

ren_yuan/src/fanuc_ry/launch/display.launch → ren_yuan/src/yunhua_1006a/launch/display.launch

@@ -3,7 +3,7 @@
     name="model" />
   <param
     name="robot_description"
-    textfile="$(find fanuc_ry)/urdf/fanuc_ry.urdf" />
+    textfile="$(find yunhua_1006a)/urdf/yunhua_1006a.urdf" />
   <node
     name="joint_state_publisher_gui"
     pkg="joint_state_publisher_gui"
@@ -16,5 +16,5 @@
     name="rviz"
     pkg="rviz"
     type="rviz"
-    args="-d $(find fanuc_ry)/urdf.rviz" />
+    args="-d $(find yunhua_1006a)/launch/display.rviz" />
 </launch>

ren_yuan/src/yunhua_1006a/launch/display.rviz

@@ -0,0 +1,217 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 721
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/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
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      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
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_tool:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: false
+        link1:
+          Value: false
+        link2:
+          Value: false
+        link3:
+          Value: false
+        link4:
+          Value: false
+        link5:
+          Value: false
+        link6:
+          Value: false
+        link_tool:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.10000000149011612
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          link1:
+            link2:
+              link3:
+                link4:
+                  link5:
+                    link6:
+                      link_tool:
+                        {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.0788538455963135
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.8500401377677917
+        Y: -0.14005500078201294
+        Z: 0.6948269009590149
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.530398428440094
+      Target Frame: <Fixed Frame>
+      Yaw: 2.070430278778076
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1018
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 28

ren_yuan/src/fanuc_ry/launch/gazebo.launch → ren_yuan/src/yunhua_1006a/launch/gazebo.launch

@@ -10,7 +10,7 @@
     name="spawn_model"
     pkg="gazebo_ros"
     type="spawn_model"
-    args="-file $(find fanuc_ry)/urdf/fanuc_ry.urdf -urdf -model fanuc_ry"
+    args="-file $(find yunhua_1006a)/urdf/yunhua_1006a.urdf -urdf -model yunhua_1006a"
     output="screen" />
   <node
     name="fake_joint_calibration"

ren_yuan/src/fanuc_ry/package.xml → ren_yuan/src/yunhua_1006a/package.xml

@@ -1,10 +1,10 @@
 <package format="2">
-  <name>fanuc_ry</name>
+  <name>yunhua_1006a</name>
   <version>1.0.0</version>
   <description>
-    <p>URDF Description package for fanuc_ry</p>
+    <p>URDF Description package for yunhua_1006a</p>
     <p>This package contains configuration data, 3D models and launch files
-for fanuc_ry robot</p>
+for yunhua_1006a robot</p>
   </description>
   <author>TODO</author>
   <maintainer email="TODO@email.com" />

ren_yuan/src/yunhua_1006a/urdf/yunhua_1006a.csv

@@ -0,0 +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_1006a/meshes/base_link.STL,0.1451,0.1451,0.1451,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/base_link.STL,,HY1020A-180-1-1,基坐标系,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+link1,0.040145,-0.0017665,0.17119,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_1006a/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/link1.STL,,送丝盘SLDPRT-1;HY1020A-180-2-1,frame1,轴1,joint1,revolute,0,0,0.167,0,0,0,base_link,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link2,-0.029712,-0.33095,-0.054381,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_1006a/meshes/link2.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/link2.STL,,HY1010A-180-3-1,frame2,轴2,joint2,revolute,0.22463,-0.089,0.3454,-1.5708,0,0,link1,0,0,1,0,0,-3.14,3.14,,,,,,,,
+link3,0.080981,-0.12571,0.11109,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_1006a/meshes/link3.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/link3.STL,,HY1010A-180-4-1;送丝机-1,frame3,轴3,joint3,revolute,0.00037384,-0.7586,-0.022,0,0,0,link2,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link4,0.0017864,-0.026244,-0.31714,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_1006a/meshes/link4.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/link4.STL,,HY1006A-200-1-1;HY1006A-144-5-1,frame4,轴4,joint4,revolute,0.3225,-0.215,0.11087,3.1416,-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_1006a/meshes/link5.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/link5.STL,,HY1006A-138-6-1,frame5,轴5,joint5,revolute,0.00012894,0,-0.669,3.1416,-1.5708,0,link4,0,0,1,0,0,-3.14,3.14,,,,,,,,
+link6,0.073417,0.0001354,0.093611,0,0,0,0.62765,0.0028394,1.5554E-06,-0.00027709,0.0011602,4.9005E-08,0.0030715,0,0,0,0,0,0,package://yunhua_1006a/meshes/link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://yunhua_1006a/meshes/link6.STL,,焊枪_LST-1,法兰盘坐标系,轴6,joint6,revolute,0,0.14,0,-1.5708,0,0,link5,0,0,-1,0,0,-3.14,3.14,,,,,,,,

ren_yuan/src/yunhua_1006a/urdf/yunhua_1006a.urdf

@@ -0,0 +1,404 @@
+<?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_1006a">
+  <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_1006a/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_1006a/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link1">
+    <inertial>
+      <origin
+        xyz="0.040145 -0.0017665 0.17119"
+        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_1006a/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_1006a/meshes/link1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.167"
+      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.029712 -0.33095 -0.054381"
+        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_1006a/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_1006a/meshes/link2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint2"
+    type="revolute">
+    <origin
+      xyz="0.22463 -0.089 0.3454"
+      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 0.11109"
+        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_1006a/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_1006a/meshes/link3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint3"
+    type="revolute">
+    <origin
+      xyz="0.00037384 -0.7586 -0.022"
+      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.0017864 -0.026244 -0.31714"
+        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_1006a/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_1006a/meshes/link4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint4"
+    type="revolute">
+    <origin
+      xyz="0.3225 -0.215 0.11087"
+      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_1006a/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_1006a/meshes/link5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint5"
+    type="revolute">
+    <origin
+      xyz="0.00012894 0 -0.669"
+      rpy="0 -1.5708 0" />
+    <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.0001354 0.093611"
+        rpy="0 0 0" />
+      <mass
+        value="0.62765" />
+      <inertia
+        ixx="0.0028394"
+        ixy="1.5554E-06"
+        ixz="-0.00027709"
+        iyy="0.0011602"
+        iyz="4.9005E-08"
+        izz="0.0030715" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://yunhua_1006a/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_1006a/meshes/link6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint6"
+    type="revolute">
+    <origin
+      xyz="0 0.14 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="17.453" />
+  </joint>
+
+  <link name="link_tool">
+  </link>
+  <joint name="joint_tool" type="fixed">
+    <parent link="link6"/>
+    <child link="link_tool"/>
+    <origin xyz="0.085387 0.007948 0.525332" rpy="0 0.736255 0"/>
+  </joint>
+
+</robot>

ren_yuan/src/yunhua_1006a_moveit_config/.setup_assistant

@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: yunhua_1006a
+    relative_path: urdf/yunhua_1006a.urdf
+    xacro_args: ""
+  SRDF:
+    relative_path: config/yunhua_1006a.srdf
+  CONFIG:
+    author_name: lstrobot
+    author_email: lstrobot@123.com
+    generated_timestamp: 1733188818

ren_yuan/src/fanuc_ry_moveit_config/CMakeLists.txt → ren_yuan/src/yunhua_1006a_moveit_config/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.1.3)
-project(fanuc_ry_moveit_config)
+project(yunhua_1006a_moveit_config)
 
 find_package(catkin REQUIRED)
 

ren_yuan/src/fanuc_ry_moveit_config/config/fake_controllers.yaml → ren_yuan/src/yunhua_1006a_moveit_config/config/fake_controllers.yaml

@@ -2,12 +2,12 @@ controller_list:
   - name: fake_manipulator_controller
     type: $(arg fake_execution_type)
     joints:
-      - joint_1
-      - joint_2
-      - joint_3
-      - joint_4
-      - joint_5
-      - joint_6
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
 initial:  # Define initial robot poses per group
   - group: manipulator
     pose: home

ren_yuan/src/fanuc_ry_moveit_config/config/gazebo_fanuc_ry.urdf → ren_yuan/src/yunhua_1006a_moveit_config/config/gazebo_yunhua_1006a.urdf

@@ -2,41 +2,35 @@
 <!-- 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="fanuc_ry">
-    <link name="world_link" />
-    <joint name="world_joint" type="fixed">
-        <parent link="world_link" />
-        <child link="base_link" />
-        <origin xyz="0 0 2.17" rpy="0 3.14 0" />
-    </joint>
+<robot name="yunhua_1006a">
     <link name="base_link">
         <!-- <inertial>
       <origin
-        xyz="0.0067459 0.00048961 0.12827"
+        xyz="-0.073946 -7.5783E-05 0.079686"
         rpy="0 0 0" />
       <mass
-        value="13.511" />
+        value="26.825" />
       <inertia
-        ixx="0.1373"
-        ixy="0.00087636"
-        ixz="0.003867"
-        iyy="0.14276"
-        iyz="4.9465E-05"
-        izz="0.11338" />
+        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://fanuc_ry/meshes/base_link.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/base_link.STL" />
             </geometry>
             <material name="">
-                <color rgba="0.26667 0.26667 0.26667 1" />
+                <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://fanuc_ry/meshes/base_link.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/base_link.STL" />
             </geometry>
         </collision>
         <inertial>
@@ -45,161 +39,161 @@
             <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
         </inertial>
     </link>
-    <link name="Link_1">
+    <link name="link1">
         <inertial>
-            <origin xyz="0.046629 0.040325 0.11902" rpy="0 0 0" />
-            <mass value="10.873" />
-            <inertia ixx="0.1054" ixy="-0.0045186" ixz="-0.021996" iyy="0.13842" iyz="-0.018604" izz="0.12811" />
+            <origin xyz="0.040145 -0.0017665 0.17119" 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://fanuc_ry/meshes/Link_1.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link1.STL" />
             </geometry>
             <material name="">
-                <color rgba="1 1 0 1" />
+                <color rgba="1 1 1 1" />
             </material>
         </visual>
         <collision>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_1.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link1.STL" />
             </geometry>
         </collision>
     </link>
-    <joint name="joint_1" type="revolute">
-        <origin xyz="0 0 0.277" rpy="0 0 0" />
+    <joint name="joint1" type="revolute">
+        <origin xyz="0 0 0.167" rpy="0 0 0" />
         <parent link="base_link" />
-        <child link="Link_1" />
+        <child link="link1" />
         <axis xyz="0 0 1" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-2.617995" upper="2.617995" effort="0" velocity="2.827" />
     </joint>
-    <link name="Link_2">
+    <link name="link2">
         <inertial>
-            <origin xyz="0.079218 0.046804 0.19413" rpy="0 0 0" />
-            <mass value="37.391" />
-            <inertia ixx="11.84" ixy="0.35759" ixz="-5.9826" iyy="16.808" iyz="0.31317" izz="5.2925" />
+            <origin xyz="-0.029712 -0.33095 -0.054381" 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://fanuc_ry/meshes/Link_2.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link2.STL" />
             </geometry>
             <material name="">
-                <color rgba="1 1 0 1" />
+                <color rgba="1 0 0 1" />
             </material>
         </visual>
         <collision>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_2.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link2.STL" />
             </geometry>
         </collision>
     </link>
-    <joint name="joint_2" type="revolute">
-        <origin xyz="0.15 -0.042195 0.173" rpy="0 0 0" />
-        <parent link="Link_1" />
-        <child link="Link_2" />
-        <axis xyz="0 1 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+    <joint name="joint2" type="revolute">
+        <origin xyz="0.22463 -0.089 0.3454" 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="Link_3">
+    <link name="link3">
         <inertial>
-            <origin xyz="0.0819232659965815 0.0488957476294858 0.0997479026407966" rpy="0 0 0" />
-            <mass value="5.31125443623239" />
-            <inertia ixx="0.0377023601596469" ixy="-0.00198499744122145" ixz="-0.00823986510994731" iyy="0.0380505516425425" iyz="-0.00237816508450241" izz="0.0202238320497431" />
+            <origin xyz="0.080981 -0.12571 0.11109" 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://fanuc_ry/meshes/Link_3.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link3.STL" />
             </geometry>
             <material name="">
-                <color rgba="1 1 0 1" />
+                <color rgba="1 1 1 1" />
             </material>
         </visual>
         <collision>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_3.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link3.STL" />
             </geometry>
         </collision>
     </link>
-    <joint name="joint_3" type="revolute">
-        <origin xyz="0 0 0.78" rpy="0 0 0" />
-        <parent link="Link_2" />
-        <child link="Link_3" />
-        <axis xyz="0 -1 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+    <joint name="joint3" type="revolute">
+        <origin xyz="0.00037384 -0.7586 -0.022" 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="Link_4">
+    <link name="link4">
         <inertial>
-            <origin xyz="0.475870558811332 -0.0337629939092132 -0.00126165368026476" rpy="0 0 0" />
-            <mass value="6.90672216285316" />
-            <inertia ixx="0.0193668843618062" ixy="0.048070034103511" ixz="-0.000520432981432283" iyy="0.433691694631167" iyz="0.000101052935278791" izz="0.439756920518466" />
+            <origin xyz="0.0017864 -0.026244 -0.31714" 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://fanuc_ry/meshes/Link_4.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link4.STL" />
             </geometry>
             <material name="">
-                <color rgba="1 1 0 1" />
+                <color rgba="1 0 0 1" />
             </material>
         </visual>
         <collision>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_4.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link4.STL" />
             </geometry>
         </collision>
     </link>
-    <joint name="joint_4" type="revolute">
-        <origin xyz="0.18449 0.042195 0.2" rpy="0 0 0" />
-        <parent link="Link_3" />
-        <child link="Link_4" />
-        <axis xyz="-1 0 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+    <joint name="joint4" type="revolute">
+        <origin xyz="0.3225 -0.215 0.11087" 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="Link_5">
+    <link name="link5">
         <inertial>
-            <origin xyz="-1.00678870289315 0.00460908463366021 -0.785340384700437" rpy="0 0 0" />
-            <mass value="37.3909058444352" />
-            <inertia ixx="11.8404562839242" ixy="0.357586794323293" ixz="-5.9825947327725" iyy="16.8076547081665" iyz="0.313167595336379" izz="5.29248954036271" />
+            <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://fanuc_ry/meshes/Link_5.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link5.STL" />
             </geometry>
             <material name="">
-                <color rgba="1 1 0 1" />
+                <color rgba="1 0 0 1" />
             </material>
         </visual>
         <collision>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_5.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link5.STL" />
             </geometry>
         </collision>
     </link>
-    <joint name="joint_5" type="revolute">
-        <origin xyz="0.90151 0 -0.00052875" rpy="0 0 0" />
-        <parent link="Link_4" />
-        <child link="Link_5" />
-        <axis xyz="0 -1 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+    <joint name="joint5" type="revolute">
+        <origin xyz="0.00012894 0 -0.669" rpy="0 -1.5708 0" />
+        <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="Link_6">
+    <link name="link6">
         <inertial>
-            <origin xyz="-0.0165412016845266 0.0951891558056374 0.119534910446276" rpy="0 0 0" />
-            <mass value="0.789056328743544" />
-            <inertia ixx="0.00166815677862568" ixy="-0.000330967965728139" ixz="6.08102317073045E-06" iyy="0.00102524254386583" iyz="-1.68914833733875E-06" izz="0.0013292952791531" />
+            <origin xyz="0.073417 0.0001354 0.093611" rpy="0 0 0" />
+            <mass value="0.62765" />
+            <inertia ixx="0.0028394" ixy="1.5554E-06" ixz="-0.00027709" iyy="0.0011602" iyz="4.9005E-08" izz="0.0030715" />
         </inertial>
         <visual>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_6.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link6.STL" />
             </geometry>
             <material name="">
                 <color rgba="0.75294 0.75294 0.75294 1" />
@@ -208,79 +202,79 @@
         <collision>
             <origin xyz="0 0 0" rpy="0 0 0" />
             <geometry>
-                <mesh filename="package://fanuc_ry/meshes/Link_6.STL" />
+                <mesh filename="package://yunhua_1006a/meshes/link6.STL" />
             </geometry>
         </collision>
     </link>
-    <joint name="joint_6" type="revolute">
-        <origin xyz="0.093994 0 0.0005287" rpy="0 -1.57 3.14" />
-        <parent link="Link_5" />
-        <child link="Link_6" />
+    <joint name="joint6" type="revolute">
+        <origin xyz="0 0.14 0" rpy="-1.5708 0 0" />
+        <parent link="link5" />
+        <child link="link6" />
         <axis xyz="0 0 -1" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.839726" upper="3.839726" effort="0" velocity="17.453" />
     </joint>
     <link name="link_tool" />
     <joint name="joint_tool" type="fixed">
-        <parent link="Link_6" />
+        <parent link="link6" />
         <child link="link_tool" />
-        <origin xyz="0.051 0.1209 0.3107" rpy="0 0 0" />
+        <origin xyz="0.085387 0.007948 0.525332" rpy="0 0.736255 0" />
     </joint>
-    <transmission name="trans_joint_1">
+    <transmission name="trans_joint1">
         <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint_1">
+        <joint name="joint1">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        <actuator name="joint_1_motor">
+        <actuator name="joint1_motor">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
     </transmission>
-    <transmission name="trans_joint_2">
+    <transmission name="trans_joint2">
         <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint_2">
+        <joint name="joint2">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        <actuator name="joint_2_motor">
+        <actuator name="joint2_motor">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
     </transmission>
-    <transmission name="trans_joint_3">
+    <transmission name="trans_joint3">
         <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint_3">
+        <joint name="joint3">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        <actuator name="joint_3_motor">
+        <actuator name="joint3_motor">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
     </transmission>
-    <transmission name="trans_joint_4">
+    <transmission name="trans_joint4">
         <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint_4">
+        <joint name="joint4">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        <actuator name="joint_4_motor">
+        <actuator name="joint4_motor">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
     </transmission>
-    <transmission name="trans_joint_5">
+    <transmission name="trans_joint5">
         <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint_5">
+        <joint name="joint5">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        <actuator name="joint_5_motor">
+        <actuator name="joint5_motor">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
     </transmission>
-    <transmission name="trans_joint_6">
+    <transmission name="trans_joint6">
         <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint_6">
+        <joint name="joint6">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        <actuator name="joint_6_motor">
+        <actuator name="joint6_motor">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>

ren_yuan/src/fanuc_ry_moveit_config/config/joint_limits.yaml → ren_yuan/src/yunhua_1006a_moveit_config/config/joint_limits.yaml

@@ -8,33 +8,33 @@ default_acceleration_scaling_factor: 0.1
 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
-  joint_1:
-    has_velocity_limits: false
-    max_velocity: 0
+  joint1:
+    has_velocity_limits: true
+    max_velocity: 2.827
     has_acceleration_limits: false
     max_acceleration: 0
-  joint_2:
-    has_velocity_limits: false
-    max_velocity: 0
+  joint2:
+    has_velocity_limits: true
+    max_velocity: 2.827
     has_acceleration_limits: false
     max_acceleration: 0
-  joint_3:
-    has_velocity_limits: false
-    max_velocity: 0
+  joint3:
+    has_velocity_limits: true
+    max_velocity: 3.463
     has_acceleration_limits: false
     max_acceleration: 0
-  joint_4:
-    has_velocity_limits: false
-    max_velocity: 0
+  joint4:
+    has_velocity_limits: true
+    max_velocity: 7.106
     has_acceleration_limits: false
     max_acceleration: 0
-  joint_5:
-    has_velocity_limits: false
-    max_velocity: 0
+  joint5:
+    has_velocity_limits: true
+    max_velocity: 5.639
     has_acceleration_limits: false
     max_acceleration: 0
-  joint_6:
-    has_velocity_limits: false
-    max_velocity: 0
+  joint6:
+    has_velocity_limits: true
+    max_velocity: 17.453
     has_acceleration_limits: false
     max_acceleration: 0

ren_yuan/src/fanuc_ry_moveit_config/config/kinematics.yaml → ren_yuan/src/yunhua_1006a_moveit_config/config/kinematics.yaml

@@ -4,4 +4,4 @@ manipulator:
   kinematics_solver_timeout: 0.005
   goal_joint_tolerance: 0.0001
   goal_position_tolerance: 0.0001
-  goal_orientation_tolerance: 0.001
+  goal_orientation_tolerance: 0.001

ren_yuan/src/fanuc_ry_moveit_config/config/ompl_planning.yaml → ren_yuan/src/yunhua_1006a_moveit_config/config/ompl_planning.yaml

@@ -194,4 +194,4 @@ manipulator:
     - ABITstar
     - BITstar
   projection_evaluator: joints(joint1,joint2)
-  longest_valid_segment_fraction: 0.001
+  longest_valid_segment_fraction: 0.001

ren_yuan/src/yunhua_1006a_moveit_config/config/yunhua_1006a.srdf

@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="yunhua_1006a">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="manipulator">
+        <chain base_link="base_link" tip_link="link_tool"/>
+    </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="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="joint6" value="0"/>
+    </group_state>
+    <group_state name="test" group="manipulator">
+        <joint name="joint1" value="0"/>
+        <joint name="joint2" value="-0.3907"/>
+        <joint name="joint3" value="0.3811"/>
+        <joint name="joint4" value="-0.4824"/>
+        <joint name="joint5" value="0.2282"/>
+        <joint name="joint6" value="-1.0128"/>
+    </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"/>
+    <disable_collisions link1="base_link" link2="link3" reason="Never"/>
+    <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
+    <disable_collisions link1="link1" link2="link3" reason="Never"/>
+    <disable_collisions link1="link1" link2="link4" reason="Never"/>
+    <disable_collisions link1="link1" link2="link5" reason="Never"/>
+    <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
+    <disable_collisions link1="link2" link2="link4" reason="Never"/>
+    <disable_collisions link1="link2" link2="link5" reason="Never"/>
+    <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
+    <disable_collisions link1="link3" link2="link5" reason="Never"/>
+    <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
+    <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
+</robot>

ren_yuan/src/fanuc_ry_moveit_config/launch/chomp_planning_pipeline.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/chomp_planning_pipeline.launch.xml

@@ -17,5 +17,5 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-  <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/chomp_planning.yaml" />
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/launch/default_warehouse_db.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/default_warehouse_db.launch

@@ -2,7 +2,7 @@
 
   <arg name="reset" default="false"/>
   <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find fanuc_ry_moveit_config)/default_warehouse_mongo_db" />
+  <arg name="moveit_warehouse_database_path" default="$(find yunhua_1006a_moveit_config)/default_warehouse_mongo_db" />
 
   <!-- Launch the warehouse with the configured database location -->
   <include file="$(dirname)/warehouse.launch">

ren_yuan/src/fanuc_ry_moveit_config/launch/demo.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/demo.launch

@@ -6,7 +6,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find fanuc_ry_moveit_config)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find yunhua_1006a_moveit_config)/default_warehouse_mongo_db" />
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />

ren_yuan/src/fanuc_ry_moveit_config/launch/fake_moveit_controller_manager.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/fake_moveit_controller_manager.launch.xml

@@ -7,6 +7,6 @@
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam subst_value="true" file="$(find fanuc_ry_moveit_config)/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find yunhua_1006a_moveit_config)/config/fake_controllers.yaml"/>
 
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/launch/gazebo.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/gazebo.launch

@@ -5,7 +5,7 @@
   <arg name="paused" default="false" doc="Start Gazebo paused"/>
   <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
   <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
-  <arg name="initial_joint_positions" default=" -J joint_1 0 -J joint_2 0 -J joint_3 0 -J joint_4 0 -J joint_5 0 -J joint_6 0" doc="Initial joint configuration of the robot"/>
+  <arg name="initial_joint_positions" default=" -J joint1 0 -J joint2 0 -J joint3 0 -J joint4 0 -J joint5 0 -J joint6 0" doc="Initial joint configuration of the robot"/>
 
   <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
@@ -13,7 +13,7 @@
   </include>
 
   <!-- Set the robot urdf on the parameter server -->
-  <param name="robot_description" textfile="$(find fanuc_ry_moveit_config)/config/gazebo_fanuc_ry.urdf" />
+  <param name="robot_description" textfile="$(find yunhua_1006a_moveit_config)/config/gazebo_yunhua_1006a.urdf" />
 
   <!-- Unpause the simulation after loading the robot model -->
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
@@ -23,7 +23,7 @@
     respawn="false" output="screen" />
 
   <!-- Load the controller parameters onto the parameter server -->
-  <rosparam file="$(find fanuc_ry_moveit_config)/config/gazebo_controllers.yaml" />
+  <rosparam file="$(find yunhua_1006a_moveit_config)/config/gazebo_controllers.yaml" />
   <include file="$(dirname)/ros_controllers.launch"/>
 
   <!-- Spawn the Gazebo ROS controllers -->

ren_yuan/src/fanuc_ry_moveit_config/launch/move_group.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/move_group.launch

@@ -77,7 +77,7 @@
 
   <!-- Sensors Functionality -->
   <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_sensor_manager" value="fanuc_ry" />
+    <arg name="moveit_sensor_manager" value="yunhua_1006a" />
   </include>
 
   <!-- Start the actual move_group node/action server -->

ren_yuan/src/fanuc_ry_moveit_config/launch/moveit.rviz → ren_yuan/src/yunhua_1006a_moveit_config/launch/moveit.rviz

@@ -4,10 +4,7 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /MotionPlanning1/Scene Geometry1
         - /TF1/Frames1
-        - /TF1/Tree1
-        - /Axes1
       Splitter Ratio: 0.5
     Tree Height: 808
   - Class: rviz/Help
@@ -40,7 +37,7 @@ Visualization Manager:
         Z: 0
       Plane: XY
       Plane Cell Count: 10
-      Reference Frame: world_link
+      Reference Frame: base_link
       Value: true
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
@@ -74,37 +71,37 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          Link_1:
+          base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_2:
+          link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_3:
+          link2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_4:
+          link3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_5:
+          link4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_6:
+          link5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          base_link:
+          link6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -113,10 +110,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -146,7 +139,7 @@ Visualization Manager:
         Show Workspace: false
         Start State Alpha: 1
         Start State Color: 0; 255; 0
-      Planning Scene Topic: /move_group/monitored_planning_scene
+      Planning Scene Topic: move_group/monitored_planning_scene
       Robot Description: robot_description
       Scene Geometry:
         Scene Alpha: 1
@@ -163,37 +156,37 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          Link_1:
+          base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_2:
+          link1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_3:
+          link2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_4:
+          link3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_5:
+          link4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link_6:
+          link5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          base_link:
+          link6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -202,72 +195,11 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 0.5
         Show Robot Collision: false
         Show Robot Visual: false
       Value: true
       Velocity_Scaling_Factor: 0.1
-    - Class: rviz/TF
-      Enabled: true
-      Filter (blacklist): ""
-      Filter (whitelist): ""
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        Link_1:
-          Value: false
-        Link_2:
-          Value: false
-        Link_3:
-          Value: false
-        Link_4:
-          Value: false
-        Link_5:
-          Value: false
-        Link_6:
-          Value: false
-        base_link:
-          Value: true
-        link_tool:
-          Value: false
-        world_link:
-          Value: false
-      Marker Alpha: 1
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: false
-      Show Axes: true
-      Show Names: false
-      Tree:
-        world_link:
-          base_link:
-            Link_1:
-              Link_2:
-                Link_3:
-                  Link_4:
-                    Link_5:
-                      Link_6:
-                        link_tool:
-                          {}
-      Update Interval: 0
-      Value: true
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /rviz_visual_tools
-      Name: MarkerArray
-      Namespaces:
-        Path: true
-        Text: true
-        point0  1.260560  -0.018740  1.496050: true
-        point0  1.334610  -0.019910  1.499610: true
-        point1  1.260620  0.017340  1.498150: true
-        point1  1.334480  0.017110  1.498570: true
-      Queue Size: 100
-      Value: true
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -278,37 +210,37 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        Link_1:
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link_2:
+        link1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link_3:
+        link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link_4:
+        link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link_5:
+        link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link_6:
+        link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        base_link:
+        link6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -317,30 +249,71 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true
-    - Alpha: 1
-      Class: rviz/Axes
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: false
+        link1:
+          Value: false
+        link2:
+          Value: false
+        link3:
+          Value: false
+        link4:
+          Value: false
+        link5:
+          Value: false
+        link6:
+          Value: false
+        link_tool:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 0.10000000149011612
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        base_link:
+          link1:
+            link2:
+              link3:
+                link4:
+                  link5:
+                    link6:
+                      link_tool:
+                        {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/MarkerArray
       Enabled: true
-      Length: 0.07999999821186066
-      Name: Axes
-      Radius: 0.009999999776482582
-      Reference Frame: link_tool
-      Show Trail: false
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        Path: true
+        Text: true
+        point0  1.260560  -0.018740  0.496050: true
+        point0  1.334610  -0.019910  0.499610: true
+        point1  1.260620  0.017340  0.498150: true
+        point1  1.334480  0.017110  0.498570: true
+      Queue Size: 100
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: world_link
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -352,7 +325,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.2017788290977478
+      Distance: 1.9398404359817505
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -360,17 +333,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -1.2660410404205322
-        Y: 0.010858980938792229
-        Z: 0.6525150537490845
+        X: 0.7921029925346375
+        Y: 0.2473401576280594
+        Z: 0.18657992780208588
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.07520197331905365
-      Target Frame: world_link
-      Yaw: 3.3168013095855713
+      Pitch: 0.460000216960907
+      Target Frame: base_link
+      Yaw: 5.764949798583984
     Saved: ~
 Window Geometry:
   Displays:
@@ -384,7 +357,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000254000001a20000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001ee0000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1920

ren_yuan/src/fanuc_ry_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml

@@ -1,6 +1,6 @@
 <launch>
   <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
-  <include file="$(find fanuc_ry_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
+  <include file="$(find yunhua_1006a_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
     <arg name="planning_adapters"
          default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
                   default_planner_request_adapters/AddTimeParameterization
@@ -13,7 +13,7 @@
   </include>
 
   <!-- load chomp config -->
-  <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/chomp_planning.yaml" />
 
   <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
   <param name="trajectory_initialization_method" value="fillTrajectory"/>

ren_yuan/src/fanuc_ry_moveit_config/launch/ompl_planning_pipeline.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/ompl_planning_pipeline.launch.xml

@@ -19,6 +19,6 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-  <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/ompl_planning.yaml"/>
 
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

@@ -7,7 +7,7 @@
   <param name="request_adapters" value="$(arg planning_adapters)" />
 
   <!-- Define default planner (for all groups) -->
-  <param name="default_planner_config" value="PTP" />
+  <param name="default_planner_config" value="RRTConnect" />
 
   <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
   <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction

ren_yuan/src/fanuc_ry_moveit_config/launch/planning_context.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/planning_context.launch

@@ -6,20 +6,20 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find fanuc_ry)/urdf/fanuc_ry.urdf"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find yunhua_1006a)/urdf/yunhua_1006a.urdf"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find fanuc_ry_moveit_config)/config/fanuc_ry.srdf" />
+  <param name="$(arg robot_description)_semantic" textfile="$(find yunhua_1006a_moveit_config)/config/yunhua_1006a.srdf" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/joint_limits.yaml"/>
-    <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/cartesian_limits.yaml"/>
+    <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/cartesian_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/kinematics.yaml"/>
 
   </group>
 

ren_yuan/src/fanuc_ry_moveit_config/launch/ros_controllers.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/ros_controllers.launch

@@ -2,7 +2,7 @@
 <launch>
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find fanuc_ry_moveit_config)/config/ros_controllers.yaml" command="load"/>
+  <rosparam file="$(find yunhua_1006a_moveit_config)/config/ros_controllers.yaml" command="load"/>
 
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

ren_yuan/src/fanuc_ry_moveit_config/launch/run_benchmark_ompl.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/run_benchmark_ompl.launch

@@ -15,7 +15,7 @@
 
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/ompl_planning.yaml"/>
+    <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/ompl_planning.yaml"/>
   </node>
 
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/launch/sensor_manager.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/sensor_manager.launch.xml

@@ -3,15 +3,15 @@
   <!-- This file makes it easy to include the settings for sensor managers -->
 
   <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/sensors_3d.yaml" />
+  <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/sensors_3d.yaml" />
 
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.002" />
+  <param name="octomap_resolution" type="double" value="0.004" />
   <param name="max_range" type="double" value="5.0" />
 
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
-  <arg name="moveit_sensor_manager" default="fanuc_ry" />
+  <arg name="moveit_sensor_manager" default="yunhua_1006a" />
   <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
 
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/launch/setup_assistant.launch → ren_yuan/src/yunhua_1006a_moveit_config/launch/setup_assistant.launch

@@ -8,7 +8,7 @@
 
   <!-- Run -->
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-        args="--config_pkg=fanuc_ry_moveit_config"
+        args="--config_pkg=yunhua_1006a_moveit_config"
         launch-prefix="$(arg launch_prefix)"
         required="true"
         output="screen" />

ren_yuan/src/fanuc_ry_moveit_config/launch/simple_moveit_controller_manager.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/simple_moveit_controller_manager.launch.xml

@@ -3,6 +3,6 @@
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
   <!-- Load controller list to the parameter server -->
-  <rosparam file="$(find fanuc_ry_moveit_config)/config/simple_moveit_controllers.yaml" />
-  <rosparam file="$(find fanuc_ry_moveit_config)/config/ros_controllers.yaml" />
+  <rosparam file="$(find yunhua_1006a_moveit_config)/config/simple_moveit_controllers.yaml" />
+  <rosparam file="$(find yunhua_1006a_moveit_config)/config/ros_controllers.yaml" />
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/launch/stomp_planning_pipeline.launch.xml → ren_yuan/src/yunhua_1006a_moveit_config/launch/stomp_planning_pipeline.launch.xml

@@ -19,5 +19,5 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-  <rosparam command="load" file="$(find fanuc_ry_moveit_config)/config/stomp_planning.yaml"/>
+  <rosparam command="load" file="$(find yunhua_1006a_moveit_config)/config/stomp_planning.yaml"/>
 </launch>

ren_yuan/src/fanuc_ry_moveit_config/package.xml → ren_yuan/src/yunhua_1006a_moveit_config/package.xml

@@ -1,12 +1,12 @@
 <package>
 
-  <name>fanuc_ry_moveit_config</name>
+  <name>yunhua_1006a_moveit_config</name>
   <version>0.3.0</version>
   <description>
-     An automatically generated package with all the configuration and launch files for using the fanuc_ry with the MoveIt Motion Planning Framework
+     An automatically generated package with all the configuration and launch files for using the yunhua_1006a with the MoveIt Motion Planning Framework
   </description>
-  <author email="ryrobot@123.com">ryrobot</author>
-  <maintainer email="ryrobot@123.com">ryrobot</maintainer>
+  <author email="lstrobot@123.com">lstrobot</author>
+  <maintainer email="lstrobot@123.com">lstrobot</maintainer>
 
   <license>BSD</license>
 
@@ -35,7 +35,7 @@
   <!-- <run_depend>gazebo_ros_control</run_depend> -->
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
-  <run_depend>fanuc_ry</run_depend>
+  <run_depend>yunhua_1006a</run_depend>
 
 
 </package>

ren_yuan/说明.txt

@@ -3,7 +3,9 @@
 
 二、修改焊接姿态规划，将其移动到hjsx.py中，节省一次读写，且姿态的规划不再是基于世界坐标系的，而是与焊缝绑定（待测试）
 
-三、修改关节限位（未完成）
+三、修改关节限位
 
-四、修改moveit代码  加入规划失败检测 一条焊缝失败则两条都不焊
+四、修改moveit代码  加入规划失败检测 一条焊缝失败则两条都不焊 （未完成）
+
+五、手搓数据 进行测试