diff --git a/myArm/myarm/scripts/slider_control.py b/myArm/myarm/scripts/slider_control.py index a5f6ce70..59b5284c 100755 --- a/myArm/myarm/scripts/slider_control.py +++ b/myArm/myarm/scripts/slider_control.py @@ -8,7 +8,7 @@ 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) diff --git a/myArm/myarm_moveit/launch/moveit.rviz b/myArm/myarm_moveit/launch/moveit.rviz index 7c688ca0..d57286c6 100644 --- a/myArm/myarm_moveit/launch/moveit.rviz +++ b/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: diff --git a/myArm/myarm_moveit/scripts/obstacle_test.py b/myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py similarity index 90% rename from myArm/myarm_moveit/scripts/obstacle_test.py rename to myArm/myarm_moveit/scripts/obstacle_avoidance_demo.py index 8df8f879..813b4d19 100755 --- a/myArm/myarm_moveit/scripts/obstacle_test.py +++ b/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 @@ def __init__(self): # 创建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 @@ def robot_move(self): # 控制机械臂回到初始化位置 self.arm_group.set_named_target('init_pose') self.arm_group.go() - rospy.sleep(5) + rospy.sleep(3) # 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示 @@ -100,10 +94,6 @@ def robot_move(self): 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 @@ def robot_move(self): # 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 @@ def robot_move(self): 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() diff --git a/myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100755 index 56dfe126..00000000 --- a/myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -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() diff --git a/myArm/myarm_moveit/scripts/sync_plan.py b/myArm/myarm_moveit/scripts/sync_plan.py index a5f6ce70..736425c6 100755 --- a/myArm/myarm_moveit/scripts/sync_plan.py +++ b/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 @@ 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 ...")