update myarm ros and add moveit avoidance script

myArm/myarm/scripts/slider_control.py

@@ -8,7 +8,7 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -23,9 +23,11 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        radians_to_angles = math.degrees(value)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
+    # mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 80)
     # time.sleep(0.5)
 
 

myArm/myarm_moveit/launch/moveit.rviz

@@ -236,9 +236,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5
+      Pitch: 0.4847961962223053
       Target Frame: base
-      Yaw: -0.6232355833053589
+      Yaw: 5.471311569213867
     Saved: ~
 Window Geometry:
   Displays:

myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 
 import sys
 import rospy
@@ -22,14 +23,7 @@ class MoveItPlanningDemo:
 
         # 创建MoveGroupCommander对象
         self.arm_group = moveit_commander.MoveGroupCommander("arm_group")
-        # 切换规划器为ompl
-        # self.arm_group.set_planner_id("ompl")
-        # planner_params = {"clearance": 0.1}  # 设置障碍物清除参数为0.1米（可以根据实际情况调整值）
-        # arm_group.set_planner_params("ompl", planner_params)
-
-        # 设置 OMPL 规划器的名称
-        # self.arm_group.set_planner_id("RRTConnectkConfigDefault")
-
+        
         # 获取末端关节的名称
         self.end_effector_link = self.arm_group.get_end_effector_link()
         
@@ -85,7 +79,7 @@ class MoveItPlanningDemo:
         # 控制机械臂回到初始化位置
         self.arm_group.set_named_target('init_pose')
         self.arm_group.go()
-        rospy.sleep(5)
+        rospy.sleep(3)
 
         
         # 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示
@@ -100,10 +94,6 @@ class MoveItPlanningDemo:
         target_pose.pose.orientation.z = 0.0
         target_pose.pose.orientation.w = 0.014
         
-        # 移除所有障碍物
-        # scene.remove_world_object("cylinder1")
-        # scene.remove_world_object("cylinder2")
-
         # 更新当前的位姿
         self.arm_group.set_start_state_to_current_state()
 
@@ -120,7 +110,7 @@ class MoveItPlanningDemo:
         # print('plan point:', plan[1])
         # 执行运动
         self.arm_group.execute(plan[1])
-        rospy.sleep(5)
+        rospy.sleep(3)
         # 获取末端执行器的姿态
         end_effector_pose = self.arm_group.get_current_pose().pose
 
@@ -146,8 +136,8 @@ class MoveItPlanningDemo:
         
     def run(self):
         # 移除所有障碍物
-        self.scene.remove_world_object("cylinder1")
-        self.scene.remove_world_object("cylinder2")
+        # self.scene.remove_world_object("cylinder1")
+        # self.scene.remove_world_object("cylinder2")
         # 没有障碍物运行一次
         # self.robot_move()
         

myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,136 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-
-import rospy, roslib, sys
-import moveit_commander
-from moveit_msgs.msg import RobotTrajectory
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from geometry_msgs.msg import PoseStamped, Pose
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
-
-
-class MoveItPlanningDemo:
-    def __init__(self):
-        # API to initialize move_group,初始化move_group的API
-        moveit_commander.roscpp_initialize(sys.argv)
-
-        # Initialize the ROS node,初始化ROS节点
-        rospy.init_node("moveit_ik_demo")
-
-        # Initialize the scene object to monitor changes in the external environment
-        # 初始化场景对象，用来监听外部环境的变化
-        self.scene = moveit_commander.PlanningSceneInterface()
-        rospy.sleep(1)
-
-        # Initialize self.arm group in the robotic arm that needs to be controlled by move group
-        # 初始化需要使用move group控制的机械臂中的self.arm group
-        self.arm = moveit_commander.MoveGroupCommander("arm_group")
-
-        # Get the name of the terminal link,获取终端link的名称
-        self.end_effector_link = self.arm.get_end_effector_link()
-
-        # Set the reference coordinate system used for the target position
-        # 设置目标位置所使用的参考坐标系
-        self.reference_frame = "base"
-        self.arm.set_pose_reference_frame(self.reference_frame)
-
-        # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划
-        self.arm.allow_replanning(True)
-
-        # Set the allowable error of position (unit: meter) and attitude (unit: radian)
-        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
-        self.arm.set_goal_position_tolerance(0.01)
-        self.arm.set_goal_orientation_tolerance(0.05)
-
-    def moving(self):
-        # # Control the robotic arm to return to the initialization position first
-        # 控制机械臂先回到初始化位置
-        self.arm.set_named_target("init_pose")
-        self.arm.go()
-        rospy.sleep(2)
-
-        # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates,
-        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
-        # Pose is described by quaternion, based on base_link coordinate system
-        # 姿态使用四元数描述，基于base_link坐标系
-        target_pose = PoseStamped()
-        target_pose.header.frame_id = self.reference_frame
-        target_pose.header.stamp = rospy.Time.now()
-        target_pose.pose.position.x = 0.132
-        target_pose.pose.position.y = -0.150
-        target_pose.pose.position.z = 0.075
-        target_pose.pose.orientation.x = 0.026
-        target_pose.pose.orientation.y = 1.0
-        target_pose.pose.orientation.z = 0.0
-        target_pose.pose.orientation.w = 0.014
-
-        # Set the current state of the robot arm as the initial state of motion
-        # 设置机器臂当前的状态作为运动初始状态
-        self.arm.set_start_state_to_current_state()
-
-        # Set the target pose of the terminal motion of the robotic arm
-        # 设置机械臂终端运动的目标位姿
-        self.arm.set_pose_target(target_pose, self.end_effector_link)
-        # Plan the movement path,规划运动路径
-        traj = self.arm.plan()
-        print('traj:', traj)
-        # Control the motion of the robotic arm according to the planned motion path
-        # 按照规划的运动路径控制机械臂运动
-        self.arm.execute(traj)
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy
-        # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
-        self.arm.shift_pose_target(1, 0.12, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        self.arm.shift_pose_target(1, 0.1, self.end_effector_link)
-        self.arm.go()
-        rospy.sleep(1)
-
-        # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy
-        # 控制机械臂终端反向旋转90度  0,1,2,3,4,5 代表xyzrpy
-        # self.arm.shift_pose_target(3, -1.57, end_effector_link)
-        # self.arm.go()
-        # rospy.sleep(1)
-
-    def run(self):
-        self.scene.remove_world_object("suit")
-
-        # Run once without obstacles,没有障碍物运行一次
-        self.moving()
-
-        # Add environment,添加环境
-        quat = quaternion_from_euler(3.1415, 0, -1.57)
-
-        suit_post = PoseStamped()
-        suit_post.header.frame_id = self.reference_frame
-        suit_post.pose.position.x = 0.0
-        suit_post.pose.position.y = 0.0
-        suit_post.pose.position.z = -0.02
-        suit_post.pose.orientation.x = quat[0]
-        suit_post.pose.orientation.y = quat[1]
-        suit_post.pose.orientation.z = quat[2]
-        suit_post.pose.orientation.w = quat[3]
-
-        suit_path = (
-            roslib.packages.get_pkg_dir("mycobot_description")
-            + "/urdf/mycobot/suit_env.dae"
-        )
-        # need `pyassimp==3.3`
-        self.scene.add_mesh("suit", suit_post, suit_path)
-        rospy.sleep(2)
-
-        # Run it again if there is an environmental impact,有环境影响后再运行一次
-        self.moving()
-
-        # close and exit moveit，关闭并退出moveit
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
-
-
-if __name__ == "__main__":
-    o = MoveItPlanningDemo()
-    o.run()

myArm/myarm_moveit/scripts/sync_plan.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+# -*- coding: utf-8 -*-
 
 """[summary]
 This file obtains the joint angle of the manipulator in ROS,
@@ -8,7 +9,7 @@ Passable parameters:
     port: serial prot string. Defaults is '/dev/ttyUSB0'
     baud: serial prot baudrate. Defaults is 115200.
 """
-
+import math
 import rospy
 from sensor_msgs.msg import JointState
 
@@ -23,9 +24,11 @@ def callback(data):
     print(data.position)
     data_list = []
     for index, value in enumerate(data.position):
-        data_list.append(value)
+        radians_to_angles = math.degrees(value)
+        data_list.append(radians_to_angles)
 
-    mc.send_radians(data_list, 80)
+    # mc.send_radians(data_list, 80)
+    mc.send_angles(data_list, 80)
     # time.sleep(0.5)
 
 
@@ -38,7 +41,7 @@ def listener():
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyArm(port, baud)
-
+  
     # spin() simply keeps python from exiting until this node is stopped
     # spin()只是阻止python退出，直到该节点停止
     print("spin ...")