2024-1-6

ren_yuan/src/CMakeLists.txt

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

ren_yuan/src/CMakeLists.txt

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

ren_yuan/src/lst_robot_r/scripts/hjsx.py

@@ -1,7 +1,7 @@
 import numpy as np
 import os
 import rospy
-from math import acos, degrees,tan,atan,sqrt,sin,cos
+from math import acos, degrees,tan,atan,sqrt,sin,cos,radians
 from scipy.spatial.transform import Rotation as R
 
 #求向量的模
@@ -193,8 +193,9 @@ def run():
 
 
     faxiang=[middle2[0]-middle1[0],middle2[1]-middle1[1],middle2[2]-middle1[2]]
-    shuzhi=np.cross(faxiang,hanfeng1)#向量叉乘 
-
+    #shuzhi=np.cross(faxiang,hanfeng1)#向量叉乘 
+    shuzhi=[0,0,1]
+    
     faxiang/= np.linalg.norm(faxiang)  # 单位化
     shuzhi /= np.linalg.norm(shuzhi)  # 单位化
     
@@ -205,10 +206,8 @@ def run():
     # else:
     #     fangxiang=faxiang-shuzhi*5.6 #arctan(4)=76度
 
-    if(point1[0][0]<0 and point2[0][0]<0):
-        fangxiang=faxiang+shuzhi*5.6 #arctan(4)=76度
-    else:
-        fangxiang=faxiang-shuzhi*5.6 #arctan(4)=76度
+    ang_deg=30
+    fangxiang=-faxiang-shuzhi*tan(radians(90-ang_deg))#arctan(4)=76度
 
     mat=compute_pose_R(fangxiang,hanfeng1)
     q = R.from_matrix(mat).as_quat()#应该是RPY转四元数
@@ -220,10 +219,7 @@ def run():
     # else:
     #     fangxiang=-faxiang-shuzhi*5.6 #arctan(4)=76度
 
-    if(point1[0][0]<0 and point2[0][0]<0):
-        fangxiang=faxiang+shuzhi*5.6 #arctan(4)=76度
-    else:
-        fangxiang=-faxiang-shuzhi*5.6 #arctan(4)=76度
+    fangxiang=faxiang-shuzhi*tan(radians(90-ang_deg)) #arctan(4)=76度
         
     mat=compute_pose_R(fangxiang,hanfeng1)
     q = R.from_matrix(mat).as_quat()#应该是RPY转四元数

ren_yuan/src/lst_robot_r/scripts/moveitServer2.py

@@ -1,18 +1,18 @@
 # 导入基本ros和moveit库
-from logging.handlers import RotatingFileHandler
 import os
 import moveit_msgs.msg
 import rospy, sys
 import moveit_commander      
 import moveit_msgs
+import tf.transformations
 
+from logging.handlers import RotatingFileHandler
 from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
 from moveit_msgs.msg import  PlanningScene, ObjectColor,CollisionObject, AttachedCollisionObject,RobotTrajectory
 from moveit_msgs.msg import RobotState
 from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint
 from shape_msgs.msg import SolidPrimitive
 from sensor_msgs.msg import JointState
-import tf.transformations
 from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint
 from geometry_msgs.msg import PoseStamped, Pose,Point,Vector3
 
@@ -25,16 +25,16 @@ import traceback
 from lst_robot_r.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
-
+from redis_publisher import Publisher
 
 pi = np.pi
 
 class MoveIt_Control:
     # 初始化程序
     def __init__(self, is_use_gripper=False):
-        self.home_pose=[]
-        self.hf_num=0#焊缝计数 从0开始
-        self.hf_fail=[]
+        self.home_pose = []
+        self.hf_num = 0#焊缝计数 从0开始
+        self.hf_fail = []
         # Init ros configs
         moveit_commander.roscpp_initialize(sys.argv)
         self.robot = moveit_commander.RobotCommander()
@@ -61,12 +61,12 @@ class MoveIt_Control:
         # 机械臂初始姿态
         #self.move_j()
         self.go_home()
-        self.home_pose=self.get_now_pose()
+        self.home_pose = self.get_now_pose()
         #self.go_ready()
 
 
     #TODO plan_cartesian_path
-    def plan_cartesian_path_LLL(self,point_s,point_e,waypoints,trajectory,trajectory_with_type,v=1):
+    def plan_cartesian_path_LLL(self, point_s, point_e, waypoints, trajectory, trajectory_with_type, v=1):
         fraction = 0.0  # 路径规划覆盖率
         maxtries = 50  # 最大尝试规划次数
         attempts = 0  # 已经尝试规划次数
@@ -74,36 +74,36 @@ class MoveIt_Control:
         #起点位置设置为规划组最后一个点
         start_pose = self.arm.get_current_pose(self.end_effector_link).pose
         wpose = deepcopy(start_pose)
-        waypoint=[]
+        waypoint = []
         #print(waypoint)
         if waypoints:
-            wpose.position.x=waypoints[-1].position.x
-            wpose.position.y=waypoints[-1].position.y
-            wpose.position.z=waypoints[-1].position.z
-            wpose.orientation.x=waypoints[-1].orientation.x
-            wpose.orientation.y=waypoints[-1].orientation.y
-            wpose.orientation.z=waypoints[-1].orientation.z
-            wpose.orientation.w=waypoints[-1].orientation.w
+            wpose.position.x = waypoints[-1].position.x
+            wpose.position.y = waypoints[-1].position.y
+            wpose.position.z = waypoints[-1].position.z
+            wpose.orientation.x = waypoints[-1].orientation.x
+            wpose.orientation.y = waypoints[-1].orientation.y
+            wpose.orientation.z = waypoints[-1].orientation.z
+            wpose.orientation.w = waypoints[-1].orientation.w
             waypoint.append(deepcopy(wpose))
 
         #wpose = deepcopy(pose)
-        wpose.position.x=point_s[0]
-        wpose.position.y=point_s[1]
-        wpose.position.z=point_s[2]
-        wpose.orientation.x=point_s[3]
-        wpose.orientation.y=point_s[4]
-        wpose.orientation.z=point_s[5]
-        wpose.orientation.w=point_s[6]
+        wpose.position.x = point_s[0]
+        wpose.position.y = point_s[1]
+        wpose.position.z = point_s[2]
+        wpose.orientation.x = point_s[3]
+        wpose.orientation.y = point_s[4]
+        wpose.orientation.z = point_s[5]
+        wpose.orientation.w = point_s[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
-        wpose.position.x=point_e[0]
-        wpose.position.y=point_e[1]
-        wpose.position.z=point_e[2]
-        wpose.orientation.x=point_e[3]
-        wpose.orientation.y=point_e[4]
-        wpose.orientation.z=point_e[5]
-        wpose.orientation.w=point_e[6]
+        wpose.position.x = point_e[0]
+        wpose.position.y = point_e[1]
+        wpose.position.z = point_e[2]
+        wpose.orientation.x = point_e[3]
+        wpose.orientation.y = point_e[4]
+        wpose.orientation.z = point_e[5]
+        wpose.orientation.w = point_e[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
@@ -120,8 +120,8 @@ class MoveIt_Control:
             rospy.loginfo("Path computed successfully.")
             #traj = plan
             trajj = plan #取出规划的信息
-            retimed_planed=self.retime_plan(trajj,v)
-            trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
+            retimed_planed = self.retime_plan(trajj, v)
+            trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
             trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
             trajj_with_type.points[-1].type = "end"
             trajectory.append(trajj)
@@ -129,58 +129,52 @@ class MoveIt_Control:
             moveit_server.arm.execute(retimed_planed)
             rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
-            err=0
+            err = 0
         else:
-            err=1
+            err = 1
             rospy.loginfo("fraction=" + str(fraction) + " failed after " + str(maxtries))   
             
-            # rospy.loginfo("本次焊缝规划失败")
-            # points.pop()#将本条焊缝的起点从规划中删除
-            # trajectory.pop()
-            # 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,err
+        return waypoints, trajectory, trajectory_with_type, err
     
-    #TODO plan_cartesian_path
-    def plan_cartesian_path_CCC(self,point_s,point_m,point_e,waypoints,trajectory,trajectory_with_type,v=1):
+    
+    def plan_cartesian_path_point(self, point_ss, point_ee, waypoints, trajectory, trajectory_with_type, v=1):
         fraction = 0.0  # 路径规划覆盖率
         maxtries = 50  # 最大尝试规划次数
         attempts = 0  # 已经尝试规划次数
-        
+
         #起点位置设置为规划组最后一个点
         start_pose = self.arm.get_current_pose(self.end_effector_link).pose
         wpose = deepcopy(start_pose)
-        waypoint=[]
+        waypoint = []
         #print(waypoint)
         if waypoints:
-            wpose.position.x=waypoints[-1].position.x
-            wpose.position.y=waypoints[-1].position.y
-            wpose.position.z=waypoints[-1].position.z
-            wpose.orientation.x=waypoints[-1].orientation.x
-            wpose.orientation.y=waypoints[-1].orientation.y
-            wpose.orientation.z=waypoints[-1].orientation.z
-            wpose.orientation.w=waypoints[-1].orientation.w
+            wpose.position.x = waypoints[-1].position.x
+            wpose.position.y = waypoints[-1].position.y
+            wpose.position.z = waypoints[-1].position.z
+            wpose.orientation.x = waypoints[-1].orientation.x
+            wpose.orientation.y = waypoints[-1].orientation.y
+            wpose.orientation.z = waypoints[-1].orientation.z
+            wpose.orientation.w = waypoints[-1].orientation.w
             waypoint.append(deepcopy(wpose))
 
         #wpose = deepcopy(pose)
-        wpose.position.x=point_s[0]
-        wpose.position.y=point_s[1]
-        wpose.position.z=point_s[2]
-        wpose.orientation.x=point_s[3]
-        wpose.orientation.y=point_s[4]
-        wpose.orientation.z=point_s[5]
-        wpose.orientation.w=point_s[6]
+        wpose.position.x = point_ss[0]
+        wpose.position.y = point_ss[1]
+        wpose.position.z = point_ss[2]
+        wpose.orientation.x = point_ss[3]
+        wpose.orientation.y = point_ss[4]
+        wpose.orientation.z = point_ss[5]
+        wpose.orientation.w = point_ss[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
-        wpose.position.x=point_e[0]
-        wpose.position.y=point_e[1]
-        wpose.position.z=point_e[2]
-        wpose.orientation.x=point_e[3]
-        wpose.orientation.y=point_e[4]
-        wpose.orientation.z=point_e[5]
-        wpose.orientation.w=point_e[6]
+        wpose.position.x = point_ee[0]
+        wpose.position.y = point_ee[1]
+        wpose.position.z = point_ee[2]
+        wpose.orientation.x = point_ee[3]
+        wpose.orientation.y = point_ee[4]
+        wpose.orientation.z = point_ee[5]
+        wpose.orientation.w = point_ee[6]
         waypoint.append(deepcopy(wpose))
         waypoints.append(deepcopy(wpose))
 
@@ -197,29 +191,23 @@ class MoveIt_Control:
             rospy.loginfo("Path computed successfully.")
             #traj = plan
             trajj = plan #取出规划的信息
-            retimed_planed=self.retime_plan(trajj,v)
-            trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
-            trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
-            trajj_with_type.points[-1].type = "end"
+            retimed_planed = self.retime_plan(trajj, v)
+            trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
             trajectory.append(trajj)
             rospy.loginfo("开始执行")
             moveit_server.arm.execute(retimed_planed)
             rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
-            err=0
+            err = 0
         else:
-            err=1
+            err = 1
             rospy.loginfo("fraction=" + str(fraction) + " failed after " + str(maxtries))   
             
-            # rospy.loginfo("本次焊缝规划失败")
-            # points.pop()#将本条焊缝的起点从规划中删除
-            # trajectory.pop()
-            # 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, err
+     
      
     #TODO retime_plan
-    def retime_plan(self,traj,v):
+    def retime_plan(self, traj, v):
         current_state = self.arm.get_current_state()
         
         retimed_traj = self.arm.retime_trajectory(
@@ -230,10 +218,11 @@ class MoveIt_Control:
         
         return retimed_traj
     
+    
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
-        pose=[]
+        pose = []
         pose.append(current_pose.position.x)
         pose.append(current_pose.position.y)
         pose.append(current_pose.position.z)
@@ -245,130 +234,20 @@ class MoveIt_Control:
         # 打印位姿信息       
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
-    
-    def zhu_shi():#折叠注释代码用的
-        
-    # def create_path_constraints(self,start_point,end_point,r):
-    #     #计算起点指向终点的向量
-    #     vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
-    #     height = np.linalg.norm(vector)+0.16  
-    #     radius = r
-        
-    #     # 位置约束
-    #     position_constraint = PositionConstraint()
-    #     position_constraint.header.frame_id = "base_link"
-    #     position_constraint.link_name = self.end_effector_link
-    #     position_constraint.target_point_offset = Vector3(0, 0, 0)
-    #     position_constraint.weight = 1.0
-
-    #     #构建 shape_msgs/SolidPrimitive 消息
-    #     bounding_volume = SolidPrimitive()
-    #     bounding_volume.type = SolidPrimitive.CYLINDER#圆柱
-    #     # bounding_volume.dimensions = [0.003,1]
-    #     bounding_volume.dimensions = [height,radius]
-    #     position_constraint.constraint_region.primitives.append(bounding_volume)
-
-    #     #构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
-    #     pose = Pose()
-    #     pose.position.x = start_point[0] + vector[0] / 2#定义位置（找了个几何中心）
-    #     pose.position.y = start_point[1] + vector[1] / 2 
-    #     pose.position.z = start_point[2] + vector[2] / 2
-    #     # 计算圆柱体的姿态
-    #     z_axis = np.array([0, 0, 1])
-    #     axis = np.cross(z_axis, vector) #计算两个向量（向量数组）的叉乘  叉乘返回的数组既垂直于a，又垂直于b
-    #     angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))#反余弦求角度
-    #     q = tf.transformations.quaternion_about_axis(angle, axis)#通过旋转轴和旋转角返回四元数
-    #     pose.orientation.x = q[0]
-    #     pose.orientation.y = q[1]
-    #     pose.orientation.z = q[2]
-    #     pose.orientation.w = q[3]
-    #     position_constraint.constraint_region.primitive_poses.append(pose)
-
-    #     constraints = Constraints()
-    #     constraints.position_constraints.append(position_constraint)
-
-    #     return constraints
-   
 
-    # def move_p_flexible(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
-    #     self.arm.set_max_acceleration_scaling_factor(a)
-    #     self.arm.set_max_velocity_scaling_factor(v)
-    #     rrr=0.09    #初始允许半径 9cm
-    #     er=0
-    #     if trajectory:
-    #         #起点位置设置为规划组最后一个点
-    #         state = self.arm.get_current_state()
-    #         state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
-    #         path_constraints = self.create_path_constraints(points[-1],point,rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
-    #     else:
-    #         #刚开始规划 起点位置设定为当前状态  按理来说是home点 
-    #         self.go_home()
-    #         self.home_pose=self.get_now_pose()
-    #         state = self.arm.get_current_state()
-    #         path_constraints = self.create_path_constraints(self.home_pose,point,rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
-
-    #     self.arm.set_path_constraints(path_constraints)#设定约束
-    #     self.arm.set_pose_target(point, self.end_effector_link)#设定目标点为传入的点
-    #     self.arm.set_start_state(state)#起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
-        
-    #     #尝试规划次数
-    #     b = 4 #尝试规划5次  无约束5次
-    #     for i in range(b):
-    #         traj = self.arm.plan()#调用plan进行规划
-    #         error=traj[3]
-    #         if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
-    #             trajj = traj[1] #取出规划的信息
-    #             rospy.loginfo("正在检查移动轨迹有效性...")
-    #             error_c, limit_margin=check.check_joint_positions(trajj)
-    #             if not error_c:       
-    #                 rospy.loginfo("本次移动OK")
-    #                 rospy.loginfo("*******************")
-    #                 trajj_with_type = mark_the_traj(trajj,"during-p",welding_sequence)
-    #                 trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
-    #                 trajj_with_type.points[-1].type = "end"      
-    #                 trajectory_with_type.append(trajj_with_type)
-    #                 points.append(point)
-    #                 trajectory.append(trajj)  
-    #                 break  
-    #         else:
-    #             rospy.loginfo("check failed! 移动轨迹无效")
-    #             rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
-    #             self.arm.clear_path_constraints()
-    #             rrr=rrr+0.2#每次增加20厘米半径
-    #             rospy.loginfo("R值：{}".format(rrr))
-    #             if trajectory:
-    #                 path_constraints = self.create_path_constraints(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
-    #             else:
-    #                 path_constraints = self.create_path_constraints(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
-    #             self.arm.set_path_constraints(path_constraints)#设定约束
-    #             if(i>=(b-2)):
-    #                 rospy.loginfo("所有移动规划尝试失败 取消路径约束")
-    #                 self.arm.clear_path_constraints()
-    #             if(i==(b-1)):
-    #                 rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
-    #                 er=1
-    #                 break
-    #     #清除约束
-    #     self.arm.clear_path_constraints()
-        
-    #     return points,trajectory,trajectory_with_type,er
-    
-        pass
 
-
-    def go_home(self,a=1,v=1):
+    def go_home(self, a=1, v=1):
         rospy.loginfo("go_home start")
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
         self.arm.set_named_target('home')
-        rospy.loginfo("get_home end")
+        rospy.loginfo("get home_point")
         self.arm.go()
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
         
-    
 
-    def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
+    def go_home_justplan(self, trajectory, trajectory_with_type, a=1, v=1):
         rospy.loginfo("go_home start")
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
@@ -381,46 +260,48 @@ class MoveIt_Control:
         trajj = traj[1]
         moveit_server.arm.execute(trajj)
         rospy.loginfo("go_home end")
-        traj_with_type = mark_the_traj(trajj,"go-home",welding_sequence)
+        traj_with_type = mark_the_traj(trajj, "go-home", welding_sequence)
         trajectory.append(trajj)
         trajectory_with_type.append(traj_with_type)
         return trajectory,trajectory_with_type
 
-    #TODO path_planning
 
-    def path_planning(self,folder_path,gohome=True):
+    #TODO path_planning
+    def path_planning(self, folder_path, gohome=True):
         file_path_result = os.path.join(folder_path, 'result.txt')
         all_data = process_welding_data(file_path_result)
 
-        way_points,trajectory,trajectory_with_type = [],[],[]
+        way_points,trajectory,trajectory_with_type = [], [], []
         for i in range(len(all_data)):
-            rospy.loginfo("共读取到%d条焊缝，开始规划第%d条",len(all_data),i+1)
+            rospy.loginfo("共读取到%d条焊缝，开始规划第%d条", len(all_data), i+1)
             start_point = all_data[i][0]
             end_point = all_data[i][1]
             q1 = all_data[i][2]
             q2 = all_data[i][3]
 
-            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]]
+            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]]
             
-            up_value=-0.1
-            point_up=[point11[0], point11[1], point11[2]-up_value, point11[3], point11[4],point11[5],point11[6]]
-            way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_LLL(point_up,point11,way_points,trajectory,trajectory_with_type,v=speed_v)
+            up_value = -0.1
+            point_up = [point11[0], point11[1], point11[2]-up_value, point11[3], point11[4], point11[5], point11[6]]
+            # 点到点规划
+            way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_point(point_up, point11, way_points, trajectory, trajectory_with_type, v=speed_v)
             rospy.loginfo("末端执行器上移完毕")
             rospy.loginfo("*******************")
             
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
                 trajectory_with_type.clear()
                 break
             
-            way_points,trajectory,trajectory_with_type,error = 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("第%d条焊缝规划完毕", i+1)
             rospy.loginfo("*******************")
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
@@ -428,13 +309,14 @@ class MoveIt_Control:
                 break
             #向上移动末端执行器
             if i<len(all_data):
-                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,error = self.plan_cartesian_path_LLL(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
+                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, error = self.plan_cartesian_path_point(point22, point_up, way_points,trajectory,trajectory_with_type,v=speed_v)
                 rospy.loginfo("末端执行器上移完毕")
                 rospy.loginfo("*******************")
             
-            if(error==1):
+            if(error == 1):
                 rospy.loginfo("焊缝规划出现失败 清空路径列表")
                 rospy.loginfo("*******************")
                 trajectory.clear()
@@ -444,11 +326,11 @@ class MoveIt_Control:
         #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)
+            trajectory, trajectory_with_type = self.go_home_justplan(trajectory, trajectory_with_type)
 
-        traj_merge= merge_trajectories(trajectory)
-        trajectory_with_type_merge= merge_trajectories_with_type(trajectory_with_type)
-        return trajectory,traj_merge,trajectory_with_type_merge
+        traj_merge = merge_trajectories(trajectory)
+        trajectory_with_type_merge = merge_trajectories_with_type(trajectory_with_type)
+        return trajectory, traj_merge, trajectory_with_type_merge
 
 ###########################################################class end#############################################################
 def process_welding_data(filename):
@@ -475,16 +357,17 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     traj_with_type.joint_names = traj.joint_trajectory.joint_names
     traj_with_type.points = [
         JointTrajectoryPoint_ex(
-            positions=point.positions,
-            velocities=point.velocities,
-            accelerations=point.accelerations,
-            effort=point.effort,
-            type=TYPE,
-            sequence=SEQUENCE
+            positions = point.positions,
+            velocities = point.velocities,
+            accelerations = point.accelerations,
+            effort = point.effort,
+            type = TYPE,
+            sequence = SEQUENCE
         ) for point in traj.joint_trajectory.points
     ]
     return traj_with_type
 
+
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -544,12 +427,14 @@ def merge_trajectories_with_type(trajectory_with_type):
 
 
 class py_msgs():
-    fial=[]
-    shun_xv=[]
+    fial = []
+    shun_xv = []
     class point():
-        xyz_list=[]
-        type=[]
+        xyz_list = []
+        type = []
+
 
+#redis消息格式
 def ROS2PY_msgs(msgs, m_sr):
     for i in range(len(msgs.points)):
         py_msgs.point.xyz_list.append(msgs.points[i].positions)
@@ -615,19 +500,18 @@ def ROS2PY_msgs(msgs, m_sr):
         
 
 if __name__ =="__main__":
-    # from redis_publisher import Publisher  #注意 可能是史前redis 
-    # publisher = Publisher()
+    publisher = Publisher()
 
     folder_path = rospy.get_param("folder_path")
     rospy.init_node('moveit_control_server', anonymous=False)   
     moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
 
-    speed_v=0.001
+    speed_v = 0.001
     # speed_v=get_user_input()
     # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
-    trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
+    trajectory, trajectory_merge, trajectory_with_type_merge = moveit_server.path_planning(folder_path)
 
     #moveit_server.go_ready()       #合并后的轨迹也需要从ready点开始执行
 
@@ -637,4 +521,6 @@ if __name__ =="__main__":
     # #ROS2_msgs_write(trajectory_with_type_merge,moveit_server)
     # rospy.loginfo("合并后轨迹执行完毕")
     
-    rospy.set_param("sign_control",0)
+    message = ROS2PY_msgs(trajectory_with_type_merge, moveit_server)
+    publisher.pub_plan_result(message)
+    print("路径规划信息已发布....")

ren_yuan/src/lst_robot_r/scripts/redis_publisher.py

@@ -1,47 +1,44 @@
 import time
 import redis
 
+def redis_init(max_retries=5, retry_delay=1):
+    pool = redis.ConnectionPool(host='192.168.100.133', port=6379, db=0)    #ip地址和端口号
+    retries = 0
+    while retries < max_retries:
+        try:
+            rds = redis.Redis(connection_pool=pool)
+            if rds.ping():
+                print("Connected to Redis successfully.")
+                return rds
+            else:
+                print("Redis 连接失败，正在尝试重新连接...")
+        except redis.ConnectionError as e:
+            print(f"Connection error (retry {retries + 1} of {max_retries}): {e}")
+        except Exception as e:
+            print(f"An error occurred (retry {retries + 1} of {max_retries}): {e}")
+
+        # 增加延迟时间，防止过于频繁的重试
+        time.sleep(retry_delay * (1 ** retries))
+        retries += 1
+
+    print("多次尝试重连失败,检查redis服务是否开启.")
+    return None
 
 class Publisher:
     def __init__(self):
-        print('redis初始化中..')
-        if not self.redis_init():
+        self.rds = redis_init()
+        if self.rds is None:
             raise Exception("Redis 初始化失败")
-        self.rds = None
-
-    def redis_init(self, max_retries=5, retry_delay=1):
-        pool = redis.ConnectionPool(host='localhost', port=6379, db=0)
-        retries = 0
-        while retries < max_retries:
-            try:
-                self.rds = redis.Redis(connection_pool=pool)
-                if self.rds.ping():
-                    print("Redis 连接成功.")
-                    return True
-                else:
-                    print("Redis 连接失败，正在尝试重新连接...")
-            except redis.ConnectionError as e:
-                print(f"Connection error (retry {retries + 1} of {max_retries}): {e}")
-            except Exception as e:
-                print(f"An error occurred (retry {retries + 1} of {max_retries}): {e}")
-
-            # 增加延迟时间，防止过于频繁的重试
-            time.sleep(retry_delay * (1 ** retries))
-            retries += 1
-
-        print("多次尝试重连失败,检查redis服务是否开启.")
-        return False
 
     def pub_plan_result(self, result_dict):
         """
         发布路径规划结果
         """
         if self.rds is not None:
-            self.rds.publish('plan_result', result_dict)
+            self.rds.publish('ros_plan_result', result_dict)
         else:
             self.redis_init()
-            self.rds.publish('plan_result', result_dict)
-
+            self.rds.publish('ros_plan_result', result_dict)
 
 if __name__ == '__main__':
     Publisher()

ren_yuan/src/lst_robot_r/scripts/start.py

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

ren_yuan/src/yunhua_ryhj/CMakeLists.txt

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

ren_yuan/src/yunhua_ryhj/config/joint_names_yunhua_ryhj.yaml

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

ren_yuan/src/yunhua_ryhj/launch/display.launch

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

ren_yuan/src/yunhua_ryhj/launch/gazebo.launch

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

ren_yuan/src/yunhua_ryhj/package.xml

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

ren_yuan/src/yunhua_ryhj/urdf/yunhua_ryhj.csv

@@ -1,8 +1,8 @@
-Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
-base_link,-0.073946,-7.5783E-05,0.079686,0,0,0,26.825,0.29447,-0.00068906,-0.0018702,0.70624,8.3009E-05,0.88713,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,0.1451,0.1451,0.1451,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,,HY1020A-180-1-1,基坐标系,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
-link1,0.040145,-0.0017665,-0.17281,0,0,0,39.487,0.98418,-0.16112,-0.47642,1.5184,-0.10144,1.2525,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,,HY1020A-180-2-1;送丝盘SLDPRT-1,坐标系1,轴1,joint1,revolute,0,0,0.511,0,0,0,base_link,0,0,-1,0,0,-3.14,3.14,,,,,,,,
-link2,-0.030086,0.33235,0.14338,0,0,0,11.602,0.54948,0.0072392,0.0022966,0.040699,-0.0016648,0.56667,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,,HY1010A-180-3-1,坐标系2,轴2,joint2,revolute,0.225,0,0,1.5708,0,0,link1,0,0,-1,0,0,-3.14,3.14,,,,,,,,
-link3,0.080981,0.12571,-9.0574E-05,0,0,0,13.961,0.19012,-0.11007,0.0065658,0.19149,0.011087,0.28951,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,,HY1010A-180-4-1;送丝机-1,坐标系3,轴3,joint3,revolute,0,0.76,0,0,0,0,link2,0,0,1,0,0,-3.14,3.14,,,,,,,,
-link4,-0.0016575,0.026244,0.35186,0,0,0,10.585,0.16278,0.00030377,0.0018777,0.18729,-0.010668,0.051504,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,,HY1006A-200-1-1;HY1006A-144-5-1,坐标系4,轴4,joint4,revolute,0.9915,0.215,0,0,-1.5708,0,link3,0,0,-1,0,0,-3.14,3.14,,,,,,,,
-link5,-5.2388E-05,-0.061382,-0.0003515,0,0,0,1.0093,0.0042625,6.072E-07,-2.4533E-07,0.0023268,-0.00020667,0.0032649,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,,HY1006A-138-6-1,坐标系5,轴5,joint5,revolute,0,0,0,0,1.5708,0,link4,0,0,1,0,0,-3.14,3.14,,,,,,,,
-link6,-0.073417,-0.00013522,0.2336,0,0,0,0.62764,0.0028394,1.5554E-06,0.00027709,0.0011602,-4.9029E-08,0.0030715,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,,焊枪_LST-1,坐标系6,轴6,joint6,revolute,0,0,0,1.5708,0,0,link5,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,-0.073946,-7.5783E-05,0.079686,0,0,0,26.825,0.29447,-0.00068906,-0.0018702,0.70624,8.3009E-05,0.88713,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,0.1451,0.1451,0.1451,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/base_link.STL,,HY1020A-180-1-1,基坐标系,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+link1,0.040145,-0.0017665,-0.17281,0,0,0,39.487,0.98418,-0.16112,-0.47642,1.5184,-0.10144,1.2525,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link1.STL,,HY1020A-180-2-1;送丝盘SLDPRT-1,坐标系1,轴1,joint1,revolute,0,0,0.511,0,0,0,base_link,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link2,-0.030086,0.33235,0.14338,0,0,0,11.602,0.54948,0.0072392,0.0022966,0.040699,-0.0016648,0.56667,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link2.STL,,HY1010A-180-3-1,坐标系2,轴2,joint2,revolute,0.225,0,0,1.5708,0,0,link1,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link3,0.080981,0.12571,-9.0574E-05,0,0,0,13.961,0.19012,-0.11007,0.0065658,0.19149,0.011087,0.28951,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,1,1,1,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link3.STL,,HY1010A-180-4-1;送丝机-1,坐标系3,轴3,joint3,revolute,0,0.76,0,0,0,0,link2,0,0,1,0,0,-3.14,3.14,,,,,,,,
+link4,-0.0016575,0.026244,0.35186,0,0,0,10.585,0.16278,0.00030377,0.0018777,0.18729,-0.010668,0.051504,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link4.STL,,HY1006A-200-1-1;HY1006A-144-5-1,坐标系4,轴4,joint4,revolute,0.9915,0.215,0,0,-1.5708,0,link3,0,0,-1,0,0,-3.14,3.14,,,,,,,,
+link5,-5.2388E-05,-0.061382,-0.0003515,0,0,0,1.0093,0.0042625,6.072E-07,-2.4533E-07,0.0023268,-0.00020667,0.0032649,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,1,0,0,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link5.STL,,HY1006A-138-6-1,坐标系5,轴5,joint5,revolute,0,0,0,0,1.5708,0,link4,0,0,1,0,0,-3.14,3.14,,,,,,,,
+link6,-0.073417,-0.00013522,0.2336,0,0,0,0.62764,0.0028394,1.5554E-06,0.00027709,0.0011602,-4.9029E-08,0.0030715,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://yunhua_ryhj/meshes/link6.STL,,焊枪_LST-1,坐标系6,轴6,joint6,revolute,0,0,0,1.5708,0,0,link5,0,0,-1,0,0,-3.14,3.14,,,,,,,,

ren_yuan/src/yunhua_ryhj/urdf/yunhua_ryhj.urdf

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