机械臂代码优化

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -174,25 +174,25 @@ class MoveIt_Control:
                     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_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                    break
                 else:
-                    path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
-                self.arm.set_path_constraints(path_constraints)#设定约束
-                if(i>=(b-5)):
-                    rospy.loginfo("所有移动规划尝试失败 取消路径约束")
+                    rospy.loginfo("check failed! 移动轨迹无效")
+                    rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
                     self.arm.clear_path_constraints()
-                if(i==(b-1)):
-                    rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
-                    er=1
-                    break
+                    rrr=rrr+0.2#每次增加20厘米半径
+                    rospy.loginfo("R值：{}".format(rrr))
+                    if trajectory:
+                        path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                    else:
+                        path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
+                    self.arm.set_path_constraints(path_constraints)#设定约束
+                    if(i>=(b-5)):
+                        rospy.loginfo("所有移动规划尝试失败 取消路径约束")
+                        self.arm.clear_path_constraints()
+                    if(i==(b-1)):
+                        rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
+                        er=1
+                        break  
         #清除约束
         self.arm.clear_path_constraints()
         
@@ -392,7 +392,7 @@ class MoveIt_Control:
                 continue
 
             point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
-            points,trajectory,trajectory_with_type = self.move_pl_check(point22,points,trajectory,trajectory_with_type,v=speed_v)
+            points,trajectory,trajectory_with_type = self.move_pl(point22,points,trajectory,trajectory_with_type,v=speed_v)
 
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
             rospy.loginfo("*******************")