renyuan

reyuan/lst_robot_r/scripts/joint2ee.py

@@ -1,76 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import moveit_commander
-import rospy
-
-"""根据轨迹点的六轴角度值，计算末端执行器的位姿"""
-"""1.roslaunch lstrobot_moveit_config demo.launch"""
-"""2.rosrun lstrobot_planning joint2ee.py"""
-
-class MoveitPlanner:
-    def __init__(self, group_name):
-        # 初始化 moveit_commander
-        moveit_commander.roscpp_initialize(sys.argv)
-        rospy.init_node('1111_node', anonymous=True)
-        self.robot = moveit_commander.RobotCommander()
-        self.group = moveit_commander.MoveGroupCommander(group_name)
-        self.planning_scene_interface = moveit_commander.PlanningSceneInterface()
-        self.end_effector_link = self.group.get_end_effector_link()
-
-    def get_robot_state(self):
-        # 获取当前机械臂状态
-        return self.robot.get_current_state()
-
-    def get_end_effector_pose(self):
-        # 获取末端执行器的位姿
-        return self.group.get_current_pose(self.end_effector_link)
-
-        
-#从文件中读取关节角度值
-def read_joint_value(input_filename):
-    joint_value_list = []
-    with open(input_filename, 'r') as file:
-        for line in file:
-            #将每行的关节值转换为浮点数并去掉换行符
-            joint_value=[float(val) for val in line.strip().split()]
-            joint_value_list.append(joint_value)
-            
-    return joint_value_list
-
-#写入末端执行器位姿到目标文件
-def write_ee_pose(joint_value_list, planner, output_filename):
-    with open(output_filename, 'w') as file:
-       for idx,joint_value in enumerate(joint_value_list):
-           #设置关节值为当前轨迹点的关节位置
-           planner.group.set_joint_value_target(joint_value)
-           
-           plan=planner.group.plan()
-           
-           if plan:
-                planner.group.execute(plan[1], wait=True)
-                
-                fk_result = planner.get_end_effector_pose()
-                position=fk_result.pose.position
-                orientation=fk_result.pose.orientation
-           
-                #格式化位姿数据
-                pose_data = f"Point {idx + 1}:\n"
-                pose_data += f"Position: x={position.x}, y={position.y}, z={position.z}\n"
-                pose_data += f"Orientation (Quaternion): x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}\n"
-                pose_data += "-" * 50 + "\n"
-                
-                file.write(pose_data)
-                
-    print(f"End effector pose saved to {output_filename}.")
-    
-def main():
-    planner = MoveitPlanner("manipulator")
-    
-    input_filename = '/home/tong/moveir_ws/data/123/outtt.txt'
-    joint_value_list = read_joint_value(input_filename)
-    
-    output_filename = '/home/tong/moveir_ws/data/123/ee_pose.txt'
-    write_ee_pose(joint_value_list, planner, output_filename)
-    
-if __name__ == '__main__':
-    main()

reyuan/lst_robot_r/scripts/moveitServer2.py

@@ -36,7 +36,7 @@ class MoveIt_Control:
         self.home_pose=[]
         self.hf_num=0#焊缝计数 从0开始
         self.hf_fail=[]
-        # Init ros config
+        # Init ros configs
         moveit_commander.roscpp_initialize(sys.argv)
         self.robot = moveit_commander.RobotCommander()
         
@@ -141,6 +141,7 @@ class MoveIt_Control:
         #self.hf_num=self.hf_num+1#焊缝计数
         return waypoints,trajectory,trajectory_with_type
     
+    #TODO retime_plan
     def retime_plan(self,traj):
         current_state = self.arm.get_current_state()
         
@@ -170,7 +171,7 @@ class MoveIt_Control:
         return pose
     
     #TODO move_p_path_constraints    
-    def create_path_constraints2(self,start_point,end_point,r):#创建路径约束
+    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  #高度延长16cm
@@ -222,13 +223,13 @@ class MoveIt_Control:
             #起点位置设置为规划组最后一个点
             state = self.arm.get_current_state()
             state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
-            path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
+            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_constraints2(self.home_pose,point,rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
+            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)#设定目标点为传入的点
@@ -260,9 +261,9 @@ class MoveIt_Control:
                 rrr=rrr+0.2#每次增加20厘米半径
                 rospy.loginfo("R值：{}".format(rrr))
                 if trajectory:
-                    path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                    path_constraints = self.create_path_constraints(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
                 else:
-                    path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
+                    path_constraints = self.create_path_constraints(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
                 self.arm.set_path_constraints(path_constraints)#设定约束
                 if(i>=(b-2)):
                     rospy.loginfo("所有移动规划尝试失败 取消路径约束")
@@ -289,6 +290,7 @@ class MoveIt_Control:
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
         
+    #TODO go_ready    
     def go_ready(self,a=1,v=1):
         rospy.loginfo("go_ready start")
         self.arm.set_max_acceleration_scaling_factor(a)
@@ -339,7 +341,7 @@ class MoveIt_Control:
             rospy.loginfo("*******************")
             
             #向上移动末端执行器
-            if i<len(all_data)-1:
+            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 = self.plan_cartesian_path_LLL(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)

reyuan/lst_robot_r/scripts/rotation_pcd.py

@@ -1,52 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import pcl
-import numpy as np
-
-def main():
-    # 初始化ROS节点
-    rospy.init_node('transform_pcd_node', anonymous=True)
-
-    # 定义点云地图的路径和文件名
-    pcd_path = "/home/tong/renyuan_pointcloud/4/trans/LazerData.pcd"
-
-    # 读取点云地图
-    cloud = pcl.load(pcd_path)
-    
-    points = cloud.to_array()
-    centroid = np.mean(points, axis=0)
-    points_centered = points - centroid
-    
-    theta = np.pi
-    
-    #旋转点云
-    transform_matrix = np.array([[1,  0,          0],
-                                 [0, np.cos(theta), -np.sin(theta)],
-                                 [0, np.sin(theta),  np.cos(theta)]], dtype=np.float32)
-
-
-
-    # 手动变换点云
-    transformed_points = np.dot(points_centered[:, :3],transform_matrix.T)
-    transformed_points += centroid
-    
-    # #定义位移向量(沿Z轴方向移动一个单位长度)
-    
-    # print("Transformed Points (before translation):", transformed_points)
-    
-    translation_vector = np.array([-1000, 0, 0], dtype=np.float32)
-    transformed_points += translation_vector
-    
-    # print("Transformed Points (after translation):", transformed_points)
-    
-    # 创建新的点云对象
-    transformed_cloud = pcl.PointCloud()
-    transformed_cloud.from_array(transformed_points.astype(np.float32))
-
-    # 保存旋转后的点云地图到PCD文件中
-    pcl.save(transformed_cloud, "/home/tong/renyuan_pointcloud/4/trans/trans.pcd")
-
-if __name__ == "__main__":
-    main()
-