kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

opt code and fix joint movement jitter

Browse source
This commit is contained in:
wangWking 2023-12-12 18:33:17 +08:00
parent 022ab249e5
commit 84b01b404e
51 changed files with 280 additions and 2012 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
Mercury/mercury_a1/scripts/slider_control.py
View file

@ -8,6 +8,7 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA1'
baud: serial prot baudrate. Defaults is 115200.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -25,12 +26,11 @@ def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = math.degrees(value)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
# mc.send_radians(data_list, 80)
mc.send_angles(data_list, 80)
# time.sleep(0.5)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -42,6 +42,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = Mercury(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止

163
Mercury/mercury_a1_moveit/scripts/obstacle_avoidance_demo.py
View file

@ -1,163 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import tf
class MoveItPlanningDemo:
def __init__(self):
rospy.init_node('moveit_avoid_obstacles', anonymous=True)
# 初始化MoveIt
moveit_commander.roscpp_initialize(sys.argv)
# 创建RobotCommander对象
self.robot = moveit_commander.RobotCommander()
# 创建PlanningSceneInterface对象
self.scene = moveit_commander.PlanningSceneInterface()
# 创建MoveGroupCommander对象
self.arm_group = moveit_commander.MoveGroupCommander("arm_group")
# 获取末端关节的名称
self.end_effector_link = self.arm_group.get_end_effector_link()
# 设置目标位置所使用的坐标参考系
self.reference_frame = 'base'
self.arm_group.set_pose_reference_frame(self.reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm_group.set_goal_position_tolerance(0.01)
self.arm_group.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
self.arm_group.allow_replanning(True)
# 设置规划的最大时间为20秒
self.arm_group.set_planning_time(20)
# 设置规划尝试次数为10次（或者更大的值）
self.arm_group.set_num_planning_attempts(20)
def add_scene(self):
# 添加第一个圆柱作为障碍物（垂直于平面）
cylinder1_pose = geometry_msgs.msg.PoseStamped()
cylinder1_pose.header.frame_id = self.robot.get_planning_frame()
cylinder1_pose.pose.position.x = 0.15
cylinder1_pose.pose.position.y = 0
cylinder1_pose.pose.position.z = 0.30
cylinder1_pose.pose.orientation.w = 1.0
self.scene.add_cylinder("cylinder1", cylinder1_pose, height=0.6, radius=0.01)
# 添加第二个圆柱作为障碍物（水平于平面，构成十字架）
cylinder2_pose = geometry_msgs.msg.PoseStamped()
cylinder2_pose.header.frame_id = self.robot.get_planning_frame()
cylinder2_pose.pose.position.x = 0.15
cylinder2_pose.pose.position.y = 0
cylinder2_pose.pose.position.z = 0.40
cylinder2_pose.pose.orientation.w = 1.0
cylinder2_pose.pose.orientation.x = 0.707 # 围绕x轴旋转90度（水平方向）
cylinder2_pose.pose.orientation.y = 0.0
cylinder2_pose.pose.orientation.z = 0.0
cylinder2_pose.pose.orientation.w = 0.707
self.scene.add_cylinder("cylinder2", cylinder2_pose, height=0.6, radius=0.02)
# 发布当前场景信息
planning_scene = moveit_msgs.msg.PlanningScene()
planning_scene.world.collision_objects.extend(self.scene.get_objects().values())
planning_scene.is_diff = True
planning_scene_publisher = rospy.Publisher('/planning_scene', moveit_msgs.msg.PlanningScene, queue_size=1)
planning_scene_publisher.publish(planning_scene)
rospy.sleep(2)
def robot_move(self):
# 控制机械臂回到初始化位置
self.arm_group.set_named_target('init_pose')
self.arm_group.go()
rospy.sleep(3)
# 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示
target_pose = geometry_msgs.msg.PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.142 # 设置目标点的x坐标
target_pose.pose.position.y = -0.140 # 设置目标点的y坐标
target_pose.pose.position.z = 0.075 # 设置目标点的z坐标
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
# 更新当前的位姿
self.arm_group.set_start_state_to_current_state()
# 获取机械臂当前的关节状态
current_joint_values = self.arm_group.get_current_joint_values()
# 打印当前关节状态
print("Current Joint Values:", current_joint_values)
print("end Joint Values:", self.end_effector_link)
# 设置机械臂的目标姿态
self.arm_group.set_pose_target(target_pose, self.end_effector_link)
# 进行运动规划
plan = self.arm_group.plan()
# print('plan point:', plan[1])
# 执行运动
self.arm_group.execute(plan[1])
rospy.sleep(3)
# 获取末端执行器的姿态
end_effector_pose = self.arm_group.get_current_pose().pose
# 打印末端执行器的坐标位置
print("End Effector Position:", end_effector_pose.position)
print("End Effector Orientation:", end_effector_pose.orientation)
# 控制机械臂末端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
# self.arm_group.shift_pose_target(1, 0.22, self.end_effector_link)
# self.arm_group.go()
# rospy.sleep(5)
# 设置机械臂的目标位置，使用7轴的位置数据进行描述（单位：弧度）
# joint_pose = [0.2967, 0, 0, -1.57000, 0, -1.3439, 0]
# joint_pose = [0.2967, 0, 0, 0, 0, -1.3439, 0]
# arm_group.set_joint_value_target(joint_pose)
# 控制机械臂完成运动
# arm_group.go()
# rospy.sleep(10)
# 控制机械臂回到初始化位置
# arm_group.set_named_target('init_pose')
# arm_group.go()
def run(self):
# 移除所有障碍物
# self.scene.remove_world_object("cylinder1")
# self.scene.remove_world_object("cylinder2")
# 没有障碍物运行一次
# self.robot_move()
# 增加障碍物
self.add_scene()
rospy.sleep(3)
# 获取当前场景中的所有障碍物
current_obstacles = self.scene.get_known_object_names()
rospy.loginfo("Current obstacles in the scene: %s", current_obstacles)
rospy.sleep(2)
# 有障碍物后再运行一次
self.robot_move()
# 关闭MoveIt
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == '__main__':
try:
obstacle = MoveItPlanningDemo()
obstacle.run()
except rospy.ROSInterruptException:
pass

10
Mercury/mercury_a1_moveit/scripts/sync_plan.py
View file

@ -21,17 +21,13 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
# print(data.position)
rounded_data_tuple = tuple(round(value, 2) for value in data.position)
rospy.loginfo(rospy.get_caller_id() + "%s", rounded_data_tuple)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
# mc.send_radians(data_list, 80)
mc.send_angles(data_list, 80)
# time.sleep(0.5)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():

18
Mercury/mercury_b1/scripts/slider_control.py
View file

@ -24,8 +24,7 @@ r = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
rounded_data_tuple = tuple(round(value, 2) for value in data.position)
# print(rounded_data_tuple)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = round(math.degrees(value), 2)
@ -39,15 +38,15 @@ def callback(data):
print('right_arm:', right_arm)
print('middle_arm:', middle_arm)
l.send_angles(left_arm, 50)
l.send_angles(left_arm, 25)
time.sleep(0.02)
r.send_angles(right_arm, 50)
r.send_angles(right_arm, 25)
time.sleep(0.02)
r.send_angle(11, middle_arm[0], 50)
r.send_angle(11, middle_arm[0], 25)
time.sleep(0.02)
r.send_angle(12, middle_arm[1], 50)
r.send_angle(12, middle_arm[1], 25)
time.sleep(0.02)
r.send_angle(13, middle_arm[2], 50)
r.send_angle(13, middle_arm[2], 25)
time.sleep(0.02)
@ -63,7 +62,10 @@ def listener():
print(port2, baud)
l = Mercury(port1, baud)
r = Mercury(port2, baud)
time.sleep(0.05)
l.set_free_mode(1)
r.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止
print("spin ...")

31
Mercury/mercury_b1_moveit/scripts/sync_plan.py
View file

@ -17,16 +17,15 @@ from sensor_msgs.msg import JointState
from pymycobot.mercury import Mercury
# left arm port
cx1 = None
l = None
# right arm port
cx2 = None
r = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
rounded_data_tuple = tuple(round(value, 2) for value in data.position)
# print(rounded_data_tuple)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = round(math.degrees(value), 2)
@ -40,23 +39,20 @@ def callback(data):
print('right_arm:', right_arm)
print('middle_arm:', middle_arm)
cx1.send_angles(left_arm, 50)
l.send_angles(left_arm, 25)
time.sleep(0.02)
cx2.send_angles(right_arm, 50)
r.send_angles(right_arm, 25)
time.sleep(0.02)
cx2.send_angle(11, middle_arm[0], 50)
r.send_angle(11, middle_arm[0], 25)
time.sleep(0.02)
cx2.send_angle(12, middle_arm[1], 50)
r.send_angle(12, middle_arm[1], 25)
time.sleep(0.02)
cx2.send_angle(13, middle_arm[2], 50)
r.send_angle(13, middle_arm[2], 25)
time.sleep(0.02)
# mc.send_radians(data_list, 80)
# mc.send_angles(data_list, 80)
# time.sleep(0.5)
def listener():
global cx1, cx2
global l, r
rospy.init_node("control_slider", anonymous=True)
rospy.Subscriber("joint_states", JointState, callback)
@ -65,9 +61,12 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port1, baud)
print(port2, baud)
cx1 = Mercury(port1, baud)
cx2 = Mercury(port2, baud)
l = Mercury(port1, baud)
r = Mercury(port2, baud)
time.sleep(0.05)
l.set_free_mode(1)
r.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止
print("spin ...")

21
mecharm/mecharm/scripts/slider_control.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python
# -*- coding:utf-8 -*-
"""[summary]
@ -9,7 +9,8 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import math
import time
import rospy
from sensor_msgs.msg import JointState
@ -22,14 +23,13 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
# print(data_list)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -37,10 +37,13 @@ def listener():
rospy.init_node("control_slider", anonymous=True)
rospy.Subscriber("joint_states", JointState, callback)
port = rospy.get_param("~port", "/dev/ttyUSB0") # Select connected device. 选择连接设备
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin() 只是阻止python退出，直到该节点停止

1
mecharm/mecharm_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

4
mecharm/mecharm_moveit/launch/moveit.rviz
View file

@ -41,7 +41,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -193,7 +193,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

132
mecharm/mecharm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,132 +0,0 @@
#!/usr/bin/env python
# -*- 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 listen for 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 = "joint1"
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)
traj = self.arm.plan()
# 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 end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents 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")
self.moving()
# Add the 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 again if there is 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()

16
mecharm/mecharm_moveit/scripts/sync_plan.py
View file

@ -1,5 +1,6 @@
#!/usr/bin/env python2
#!/usr/bin/env python3
# encoding=utf-8
import math
import time
import rospy
from sensor_msgs.msg import JointState
@ -15,11 +16,11 @@ def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
# print("data_list:",data_list)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -30,6 +31,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)

18
mecharm/mecharm_pi/scripts/slider_control.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python
# -*- coding:utf-8 -*-
"""[summary]
@ -9,7 +9,8 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA0'
baud: serial prot baudrate. Defaults is 1000000.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -25,11 +26,11 @@ def callback(data):
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
# print(data_list)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -41,6 +42,9 @@ def listener():
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin() 只是阻止python退出，直到该节点停止

1
mecharm/mecharm_pi_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

4
mecharm/mecharm_pi_moveit/launch/moveit.rviz
View file

@ -41,7 +41,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -193,7 +193,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

132
mecharm/mecharm_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,132 +0,0 @@
#!/usr/bin/env python
# -*- 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 listen for 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 = "joint1"
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)
traj = self.arm.plan()
# 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 end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents 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")
self.moving()
# Add the 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 again if there is 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()

19
mecharm/mecharm_pi_moveit/scripts/sync_plan.py Normal file → Executable file
View file

@ -1,10 +1,10 @@
#!/usr/bin/env python2
#!/usr/bin/env python
import math
import time
import rospy
from sensor_msgs.msg import JointState
from pymycobot.mycobot import MyCobot
# from pymycobot.mypalletizer import MyPalletizer
mc = None
@ -14,11 +14,11 @@ def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
# print("data_list:",data_list)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -29,7 +29,10 @@ def listener():
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

15
myArm/myarm/scripts/slider_control.py
View file

@ -8,6 +8,7 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -20,15 +21,15 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = math.degrees(value)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
# mc.send_radians(data_list, 80)
mc.send_angles(data_list, 80)
# time.sleep(0.5)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -40,7 +41,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyArm(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止
print("spin ...")

6
myArm/myarm_moveit/launch/moveit.rviz
View file

@ -9,7 +9,7 @@ Panels:
- /MotionPlanning1/Planning Metrics1
- /MotionPlanning1/Planned Path1
Splitter Ratio: 0.5
Tree Height: 215
Tree Height: 155
- Class: rviz/Help
Name: Help
- Class: rviz/Views
@ -113,7 +113,7 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
Loop Animation: true
Loop Animation: false
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Show Robot Collision: true
@ -252,7 +252,7 @@ Window Geometry:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000168000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ab000001880000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d0000012c000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000016f000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
Width: 1291

65
myArm/myarm_moveit/launch/mycobot_moveit.launch
View file

@ -1,65 +0,0 @@
<launch>
<!-- By default, we do not start a database (it can be large) -->
<!-- 默认情况下，我们不启动数据库（它可能很大） -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<!-- 允许用户指定数据库位置 -->
<arg name="db_path" default="$(find myarm_moveit)/default_warehouse_mongo_db" />
<!-- By default, we are not in debug mode --> <!-- 默认情况下，我们不处于调试模式 -->
<arg name="debug" default="false" />
<!--
By default, hide joint_state_publisher's GUI
MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
The latter one maintains and publishes the current joint configuration of the simulated robot.
It also provides a GUI to move the simulated robot around "manually".
This corresponds to moving around the real robot without the use of MoveIt.
-->
<arg name="use_gui" default="false" />
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<!-- 加载参数服务器上的URDF、SRDF等.yaml配置文件-->
<include file="$(find myarm_moveit)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- If needed, broadcast static tf for robot root -->
<!-- We do not have a robot connected, so publish fake joint states -->
<!-- 我们没有连接机器人，所以发布假关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="$(arg use_gui)"/>
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<!-- 给定已发布的关节状态，为机器人链接发布 tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
<!-- 运行主MoveIt！ 没有轨迹执行的可执行文件（我们默认没有配置控制器）-->
<include file="$(find myarm_moveit)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<!-- 运行 Rviz 并加载默认配置以查看 move_group 节点的状态 -->
<include file="$(find myarm_moveit)/launch/moveit_rviz.launch">
<arg name="config" value="true"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<!-- 如果启用了数据库加载，也启动 mongodb -->
<include file="$(find myarm_moveit)/launch/default_warehouse_db.launch" if="$(arg db)">
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
</include>
</launch>

15
myArm/myarm_moveit/scripts/sync_plan.py
View file

@ -9,6 +9,7 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -21,15 +22,14 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = math.degrees(value)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
# mc.send_radians(data_list, 80)
mc.send_angles(data_list, 80)
# time.sleep(0.5)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -41,7 +41,10 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyArm(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止
print("spin ...")

18
mycobot_280/mycobot_280/scripts/slider_control.py
View file

@ -8,7 +8,8 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -20,13 +21,15 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
mc.get_system_version()
def listener():
@ -38,6 +41,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止

7
mycobot_280/mycobot_280/scripts/slider_control_gripper.py
View file

@ -9,7 +9,7 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import time
import rospy
from sensor_msgs.msg import JointState
from pymycobot.mycobot import MyCobot
@ -27,7 +27,7 @@ def callback(data):
data_list = data_list[:7]
print("radians:%s"%data_list[:6])
mc.send_radians(data_list[:6], 80)
mc.send_radians(data_list[:6], 25)
gripper_value = int(abs(-0.7-data_list[6])* 117)
print("gripper_value:%s"%gripper_value)
mc.set_gripper_value(gripper_value, 80, 1)
@ -43,6 +43,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止
print("spin ...")

16
mycobot_280/mycobot_280_gripper_moveit/scripts/sync_plan.py
View file

@ -7,25 +7,19 @@ from pymycobot.mycobot import MyCobot
def callback(data):
# port = rospy.get_param("~port", "/dev/ttyUSB0")
# baud = rospy.get_param("~baud", 115200)
# # print(port, baud)
# mc = MyCobot(port, baud)
data_list = []
for index, value in enumerate(data.position):
data_list.append(round(value, 3))
data_list = data_list[:7]
print("radians:%s" % data_list[:6])
# t1 = time.time()
mc.send_radians(data_list[:6], 80)
mc.send_radians(data_list[:6], 25)
# time.sleep(0.02)
gripper_value = int(abs(-0.7 - data_list[6]) * 117)
print("gripper_value:%s\n" % gripper_value)
mc.set_gripper_value(gripper_value, 80)
# t2 = time.time() - t1
# print("cost time:", t2)
# time.sleep(1)
def listener():
@ -35,6 +29,10 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

4
mycobot_280/mycobot_280_moveit/launch/moveit.rviz
View file

@ -41,7 +41,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -193,7 +193,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

17
mycobot_280/mycobot_280_moveit/scripts/sync_plan.py
View file

@ -1,5 +1,6 @@
#!/usr/bin/env python2
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -10,14 +11,13 @@ mc = None
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -25,9 +25,12 @@ def listener():
rospy.init_node("mycobot_reciver", anonymous=True)
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = rospy.get_param("~baud", 1000000)
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)

16
mycobot_280/mycobot_280arduino/scripts/slider_control.py
View file

@ -8,7 +8,8 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -20,13 +21,13 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -38,6 +39,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
print("spin ...")

4
mycobot_280/mycobot_280arduino_moveit/launch/moveit.rviz
View file

@ -43,7 +43,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -194,7 +194,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

18
mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py
View file

@ -1,4 +1,5 @@
#!/usr/bin/env python2
import math
import time
import rospy
from sensor_msgs.msg import JointState
@ -10,14 +11,14 @@ mc = None
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
# rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -28,7 +29,10 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

21
mycobot_280/mycobot_280jn/scripts/slider_control.py
View file

@ -8,7 +8,8 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyTHS1'
baud: serial prot baudrate. Defaults is 1000000.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -21,13 +22,13 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -39,11 +40,9 @@ def listener():
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyCobot(port, baud)
# ip=rospy.get_param("~ip",'192.168.125.226')
# port=rospy.get_param("~port",9000)
# print(ip,port)
# ms=MyCobotSocket(ip,port)
# ms.connect()
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止

4
mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz
View file

@ -43,7 +43,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -194,7 +194,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

20
mycobot_280/mycobot_280jn_moveit/scripts/sync_plan.py
View file

@ -1,5 +1,6 @@
#!/usr/bin/env python3
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -10,15 +11,14 @@ mc = None
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
# rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
global mc
@ -27,8 +27,12 @@ def listener():
port = rospy.get_param("~port", "/dev/ttyTHS1")
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

19
mycobot_280/mycobot_280pi/scripts/slider_control.py
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python2
#!/usr/bin/env python
# encoding=utf-8
"""[summary]
@ -9,7 +9,8 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyAMA0'
baud: serial prot baudrate. Defaults is 1000000.
"""
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -21,14 +22,13 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
global mc
@ -39,6 +39,9 @@ def listener():
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin()只是阻止python退出，直到该节点停止

4
mycobot_280/mycobot_280pi_moveit/launch/moveit.rviz
View file

@ -41,7 +41,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -193,7 +193,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

20
mycobot_280/mycobot_280pi_moveit/scripts/sync_plan.py
View file

@ -1,6 +1,7 @@
#!/usr/bin/env python2
#!/usr/bin/env python
# encoding=utf-8
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -11,14 +12,14 @@ mc = None
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
# rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -29,7 +30,10 @@ def listener():
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyCobot(port, baud)
time.sleep(0.05)
mc.set_fresh_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

32
mycobot_pro/mycobot_600/scripts/slider_600.py
View file

@ -13,42 +13,26 @@ from pymycobot.elephantrobot import ElephantRobot
global mc
old_list = []
def callback(data):
"""callback function,回调函数"""
satrt_time=time.time()
global old_list
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print("position:", data.position)
data_list = []
for index, value in enumerate(data.position):
value = value * 180 / math.pi
data_list.append(value)
print ("data", data_list)
if not old_list:
old_list = data_list
mc.write_angles(data_list, 1999)
elif old_list != data_list:
old_list = data_list
# if mc.check_running():
# mc.task_stop()
# time.sleep(0.05)
mc.write_angles(data_list, 1999)
end_time=time.time()
print('loop_time:',end_time-satrt_time)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.write_angles(data_list, 1000)
def listener():
global mc
rospy.init_node("control_slider", anonymous=True)
ip = rospy.get_param("~ip", "192.168.1.159")
print (ip)
mc = ElephantRobot(ip, 5001)
port = rospy.get_param("~port", 5001)
print (ip, port)
mc = ElephantRobot(ip, int(port))
# START CLIENT,启动客户端
res = mc.start_client()
if res != "":

1
mycobot_pro/mycobot_600_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

137
mycobot_pro/mycobot_600_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,137 +0,0 @@
#!/usr/bin/env python
# -*- 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()
# 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()

32
mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py
View file

@ -13,42 +13,28 @@ from pymycobot.elephantrobot import ElephantRobot
global mc
old_list = []
def callback(data):
"""callback function,回调函数"""
satrt_time=time.time()
global old_list
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print("position:", data.position)
data_list = []
for index, value in enumerate(data.position):
value = value * 180 / math.pi
data_list.append(value)
print ("data", data_list)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.write_angles(data_list, 1000)
if not old_list:
old_list = data_list
mc.write_angles(data_list, 1999)
elif old_list != data_list:
old_list = data_list
# if mc.check_running():
# mc.task_stop()
# time.sleep(0.05)
mc.write_angles(data_list, 1999)
end_time=time.time()
print('loop_time:',end_time-satrt_time)
def listener():
global mc
rospy.init_node("control_slider", anonymous=True)
ip = rospy.get_param("~ip", "192.168.1.159")
print (ip)
mc = ElephantRobot(ip, 5001)
port = rospy.get_param("~port", 5001)
print (ip, port)
mc = ElephantRobot(ip, int(port))
# START CLIENT,启动客户端
res = mc.start_client()
if res != "":

726
mycobot_pro/mycobot_600_moveit/scripts/test.py
View file

@ -1,726 +0,0 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
from socket import *
import math
from multiprocessing import Lock
from operator import imod
from tokenize import Pointfloat
import cv2
import numpy as np
import time
import json
import os,sys
import rospy
from visualization_msgs.msg import Marker
import rospy
from sensor_msgs.msg import JointState
# from moving_utils import Movement
IS_CV_4 = cv2.__version__[0] == '4'
__version__ = "1.0"
global mc
mutex = Lock()
class ElephantRobot(object):
def __init__(self, host, port):
# setup connection
# 建立连接
self.BUFFSIZE = 2048
self.ADDR = (host, port)
self.tcp_client = socket(AF_INET, SOCK_STREAM)
def start_client(self):
try:
self.tcp_client.connect(self.ADDR)
return ""
except error,e:
return e
def stop_client(self):
self.tcp_client.close()
def send_command(self, command):
with mutex:
self.tcp_client.send(command.encode())
recv_data = self.tcp_client.recv(self.BUFFSIZE).decode()
res_str = str(recv_data)
print("recv = " )+ res_str
res_arr = res_str.split(":")
if len(res_arr) == 2:
return res_arr[1]
else:
return ""
def string_to_coords(self, data):
data = data.replace("[", "")
data = data.replace("]", "")
data_arr = data.split(",")
if len(data_arr) == 6:
try:
coords_1 = float(data_arr[0])
coords_2 = float(data_arr[1])
coords_3 = float(data_arr[2])
coords_4 = float(data_arr[3])
coords_5 = float(data_arr[4])
coords_6 = float(data_arr[5])
coords = [coords_1, coords_2, coords_3, coords_4, coords_5, coords_6]
return coords
except:
return invalid_coords()
return invalid_coords()
def string_to_double(self, data):
try:
val = float(data)
return val
except:
return -9999.99
def string_to_int(self, data):
try:
val = int(data)
return val
except:
return -9999
def invalid_coords(self):
coords = [-1, -2, -3, -4, -1, -1]
return coords
def get_angles(self):
command = "get_angles()\n"
res = self.send_command(command)
return self.string_to_coords(res)
def get_coords(self):
command = "get_coords()\n"
res = self.send_command(command)
return self.string_to_coords(res)
def get_speed(self):
command = "get_speed()\n"
res = self.send_command(command)
return self.string_to_double(res)
def power_on(self):
command = "power_on()\n"
res = self.send_command(command)
return True
def power_off(self):
command = "power_off()\n"
res = self.send_command(command)
return True
def check_running(self):
command = "check_running()\n"
res = self.send_command(command)
return res == "1"
def state_check(self):
command = "state_check()\n"
res = self.send_command(command)
return res == "1"
def program_open(self, file_path):
command = "program_open(" + file_path + ")\n"
res = self.send_command(command)
return self.string_to_int(res)
def program_run(self, start_line):
"""run program，运行程序"""
command = "program_run(" + str(start_line) + ")\n"
res = self.send_command(command)
return self.string_to_int(res)
def read_next_error(self):
command = "read_next_error()\n"
res = self.send_command(command)
return res
def write_coords(self, coords, speed):
"""set coords,设置坐标"""
command = "set_coords("
for item in coords:
command += str(item) + ","
command += str(speed) + ")\n"
self.send_command(command)
def write_coord(self, axis, value, speed):
coords = self.get_coords()
if coords != self.invalid_coords():
coords[axis] = value
self.write_coords(coords, speed)
def write_angles(self, angles, speed):
"""set angles,设置角度"""
command = "set_angles("
for item in angles:
command += str(item) + ","
command += str(speed) + ")\n"
self.send_command(command)
def write_angle(self, joint, value, speed):
angles = self.get_angles()
if angles != self.invalid_coords():
angles[joint] = value
self.write_angles(angles, speed)
def set_speed(self, percentage):
command = "set_speed(" + str(percentage) + ")\n"
self.send_command(command)
def set_carte_torque_limit(self, axis_str, value):
command = "set_torque_limit(" + axis_str + "," + str(value) + ")\n"
self.send_command(command)
def set_upside_down(self, up_down):
up = "1"
if up_down:
up = "0"
command = "set_upside_down(" + up + ")\n"
self.send_command(command)
def set_payload(self, payload):
command = "set_speed(" + str(payload) + ")\n"
self.send_command(command)
def state_on(self):
command = "state_on()\n"
self.send_command(command)
def state_off(self):
command = "state_off()\n"
self.send_command(command)
def task_stop(self):
command = "task_stop()\n"
self.send_command(command)
def jog_angle(self, joint_str, direction, speed):
command = (
"jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n"
)
self.send_command(command)
def jog_coord(self, axis_str, direction, speed):
command = (
"jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n"
)
self.send_command(command)
def get_digital_in(self, pin_number):
command = "get_digital_in(" + str(pin_number) + ")\n"
self.send_command(command)
def get_digital_out(self, pin_number):
command = "get_digital_out(" + str(pin_number) + ")\n"
self.send_command(command)
def set_digital_out(self, pin_number, pin_signal):
command = "set_digital_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
self.send_command(command)
def set_analog_out(self, pin_number, pin_signal):
command = "set_analog_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
self.send_command(command)
def get_acceleration(self):
command = "get_acceleration()\n"
res = self.send_command(command)
return self.string_to_int(res)
def set_acceleration(self, acceleration):
command = "set_acceleration(" + str(acceleration) + ")\n"
self.send_command(command)
def command_wait_done(self):
command = "wait_command_done()\n"
self.send_command(command)
def wait(self, seconds):
command = "wait(" + str(seconds) + ")\n"
self.send_command(command)
def assign_variable(self, var_name, var_value):
command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ")\n"
self.send_command(command)
def get_variable(self, var_name):
command = 'get_variable("' + str(var_name) + '")\n'
return self.send_command(command)
class Object_detect(object):
def __init__(self, camera_x = 140, camera_y = 5): # m5
# def __init__(self, camera_x = 140, camera_y = -5): # pi
# inherit the parent class
super(Object_detect, self).__init__()
# get path of file
dir_path = os.path.dirname(__file__)
# declare 600
self.mc = None
# 移动角度
self.move_angles = [
[0, 0, 0, 0, 90, 0], # point to grab
[-33.31, 2.02, -10.72, -0.08, 95, -54.84], # init the point
]
# 移动坐标
self.move_coords = [
[92.3, -104.9, 211.4, -179.6, 28.91, 131.29], # above the red bucket
[165.0, -93.6, 201.4, -173.43, 46.23, 160.65], # above the green bucket
[88.1, 126.3, 193.4, 162.15, 2.23, 156.02], # above the blue bucket
[-5.4, 120.6, 204.6, 162.66, -6.96, 159.93], # above the gray bucket
]
# which robot: USB* is m5; ACM* is wio; AMA* is raspi
self.robot_m5 = os.popen("ls /dev/ttyUSB*").readline()[:-1]
self.robot_wio = os.popen("ls /dev/ttyACM*").readline()[:-1]
self.robot_raspi = os.popen("ls /dev/ttyAMA*").readline()[:-1]
self.robot_jes = os.popen("ls /dev/ttyTHS1").readline()[:-1]
self.raspi = False
if "dev" in self.robot_m5:
self.Pin = [2, 5]
# self.Pin = [5]
elif "dev" in self.robot_wio:
self.Pin = [20, 21]
for i in self.move_coords:
i[2] -= 20
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
import RPi.GPIO as GPIO
GPIO.setwarnings(False)
self.GPIO = GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(20, GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
GPIO.output(20, 1)
GPIO.output(21, 1)
self.raspi = True
if self.raspi:
self.gpio_status(False)
# choose place to set cube
self.color = 0
# parameters to calculate camera clipping parameters
self.x1 = self.x2 = self.y1 = self.y2 = 0
# set cache of real coord
self.cache_x = self.cache_y = 0
# set color HSV
self.HSV = {
"yellow": [np.array([11, 115, 70]), np.array([40, 255, 245])],
"red": [np.array([0, 43, 46]), np.array([8, 255, 255])],
"green": [np.array([35, 43, 46]), np.array([77, 255, 255])],
"blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
"cyan": [np.array([78, 43, 46]), np.array([99, 255, 255])],
}
# use to calculate coord between cube and 600
self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
# The coordinates of the grab center point relative to the 600
self.camera_x, self.camera_y = camera_x, camera_y
# The coordinates of the cube relative to the 600
self.c_x, self.c_y = 0, 0
# The ratio of pixels to actual values
self.ratio = 0
# Get ArUco marker dict that can be detected.
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
# Get ArUco marker params.
self.aruco_params = cv2.aruco.DetectorParameters_create()
# init a node and a publisher
rospy.init_node("marker", anonymous=True)
self.pub = rospy.Publisher('/cube', Marker, queue_size=1)
# init a Marker
self.marker = Marker()
self.marker.header.frame_id = "/joint1"
self.marker.ns = "cube"
self.marker.type = self.marker.CUBE
self.marker.action = self.marker.ADD
self.marker.scale.x = 0.04
self.marker.scale.y = 0.04
self.marker.scale.z = 0.04
self.marker.color.a = 1.0
self.marker.color.g = 1.0
self.marker.color.r = 1.0
# marker position initial
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 0.03
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1.0
# publish marker
def pub_marker(self, x, y, z=0.03):
self.marker.header.stamp = rospy.Time.now()
self.marker.pose.position.x = x
self.marker.pose.position.y = y
self.marker.pose.position.z = z
self.marker.color.g = self.color
self.pub.publish(self.marker)
# pump_control pi
def gpio_status(self, flag):
if flag:
self.GPIO.output(20, 0)
self.GPIO.output(21, 0)
else:
self.GPIO.output(20, 1)
self.GPIO.output(21, 1)
# 开启吸泵 m5
def pump_on(self):
# 让2号位工作
self.mc.set_basic_output(2, 0)
# 让5号位工作
self.mc.set_basic_output(5, 0)
# 停止吸泵 m5
def pump_off(self):
# 让2号位停止工作
self.mc.set_basic_output(2, 1)
# 让5号位停止工作
self.mc.set_basic_output(5, 1)
# Grasping motion
def move(self, x, y, color):
# send Angle to move 600
print(color)
self.mc.send_angles(self.move_angles[0], 30)
time.sleep(4)
# send coordinates to move 600
self.mc.send_coords([x, y, 140, 179.12, -0.18, 179.46], 30, 0)
time.sleep(3)
self.pub_marker(x/1000.0, y/1000.0, 140/1000.0)
self.mc.send_coords([x, y, 95, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
# self.mc.send_coords([x, y, 90, 179.12, -0.18, 179.46], 30, 0) # -178.77, -2.69, 40.15
time.sleep(3)
self.pub_marker(x/1000.0, y/1000.0, 90/1000.0)
# open pump
if "dev" in self.robot_m5:
self.pump_on()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(True)
time.sleep(1.5)
tmp = []
while True:
if not tmp:
tmp = self.mc.get_angles()
else:
break
time.sleep(0.5)
# print(tmp)
self.mc.send_angles([tmp[0], 17.22, -32.51, tmp[3], 97, tmp[5]],30)
time.sleep(3)
self.mc.send_coords(self.move_coords[color], 30, 1)
self.pub_marker(self.move_coords[color][0]/1000.0,
self.move_coords[color][1]/1000.0,
self.move_coords[color][2]/1000.0)
time.sleep(3)
# close pump
if "dev" in self.robot_m5:
self.pump_off()
elif "dev" in self.robot_raspi or "dev" in self.robot_jes:
self.gpio_status(False)
time.sleep(6)
if color == 1:
self.pub_marker(
self.move_coords[color][0]/1000.0+0.04, self.move_coords[color][1]/1000.0-0.02)
elif color == 0:
self.pub_marker(
self.move_coords[color][0]/1000.0+0.03, self.move_coords[color][1]/1000.0)
self.mc.send_angles(self.move_angles[1], 30)
time.sleep(1.5)
# decide whether grab cube
def decide_move(self, x, y, color):
print(x, y, self.cache_x, self.cache_y)
# detect the cube status move or run
if (abs(x - self.cache_x) + abs(y - self.cache_y)) / 2 > 5: # mm
self.cache_x, self.cache_y = x, y
return
else:
self.cache_x = self.cache_y = 0
# 调整吸泵吸取位置，y增大,向左移动;y减小,向右移动;x增大,前方移动;x减小,向后方移动
self.move(x, y, color)
# init 600
def run(self):
if "dev" in self.robot_m5:
self.mc = MyCobot(self.robot_m5, 115200)
elif "dev" in self.robot_raspi:
self.mc = MyCobot(self.robot_raspi, 1000000)
if not self.raspi:
self.pub_pump(False, self.Pin)
self.mc.send_angles([-33.31, 2.02, -10.72, -0.08, 95, -54.84], 30)
time.sleep(3)
# draw aruco
def draw_marker(self, img, x, y):
# draw rectangle on img
cv2.rectangle(
img,
(x - 20, y - 20),
(x + 20, y + 20),
(0, 255, 0),
thickness=2,
lineType=cv2.FONT_HERSHEY_COMPLEX,
)
# add text on rectangle
cv2.putText(img, "({},{})".format(x, y), (x, y),
cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (243, 0, 0), 2,)
# get points of two aruco
def get_calculate_params(self, img):
# Convert the image to a gray image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Detect ArUco marker.
corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params
)
"""
Two Arucos must be present in the picture and in the same order.
There are two Arucos in the Corners, and each aruco contains the pixels of its four corners.
Determine the center of the aruco by the four corners of the aruco.
"""
if len(corners) > 0:
if ids is not None:
if len(corners) <= 1 or ids[0] == 1:
return None
x1 = x2 = y1 = y2 = 0
point_11, point_21, point_31, point_41 = corners[0][0]
x1, y1 = int((point_11[0] + point_21[0] + point_31[0] + point_41[0]) / 4.0), int(
(point_11[1] + point_21[1] + point_31[1] + point_41[1]) / 4.0)
point_1, point_2, point_3, point_4 = corners[1][0]
x2, y2 = int((point_1[0] + point_2[0] + point_3[0] + point_4[0]) / 4.0), int(
(point_1[1] + point_2[1] + point_3[1] + point_4[1]) / 4.0)
return x1, x2, y1, y2
return None
# set camera clipping parameters
def set_cut_params(self, x1, y1, x2, y2):
self.x1 = int(x1)
self.y1 = int(y1)
self.x2 = int(x2)
self.y2 = int(y2)
print(self.x1, self.y1, self.x2, self.y2)
# set parameters to calculate the coords between cube and 600
def set_params(self, c_x, c_y, ratio):
self.c_x = c_x
self.c_y = c_y
self.ratio = 220.0/ratio
# calculate the coords between cube and 600
def get_position(self, x, y):
return ((y - self.c_y)*self.ratio + self.camera_x), ((x - self.c_x)*self.ratio + self.camera_y)
"""
Calibrate the camera according to the calibration parameters.
Enlarge the video pixel by 1.5 times, which means enlarge the video size by 1.5 times.
If two ARuco values have been calculated, clip the video.
"""
def transform_frame(self, frame):
# enlarge the image by 1.5 times
fx = 1.5
fy = 1.5
frame = cv2.resize(frame, (0, 0), fx=fx, fy=fy,
interpolation=cv2.INTER_CUBIC)
if self.x1 != self.x2:
# the cutting ratio here is adjusted according to the actual situation
frame = frame[int(self.y2*0.2):int(self.y1*1.15),
int(self.x1*0.7):int(self.x2*1.15)]
return frame
# detect cube color
def color_detect(self, img):
# set the arrangement of color'HSV
x = y = 0
for mycolor, item in self.HSV.items():
# print("mycolor:",mycolor)
redLower = np.array(item[0])
redUpper = np.array(item[1])
# transfrom the img to model of gray
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# print("hsv",hsv)
# wipe off all color expect color in range
mask = cv2.inRange(hsv, item[0], item[1])
# a etching operation on a picture to remove edge roughness
erosion = cv2.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
# the image for expansion operation, its role is to deepen the color depth in the picture
dilation = cv2.dilate(erosion, np.ones(
(1, 1), np.uint8), iterations=2)
# adds pixels to the image
target = cv2.bitwise_and(img, img, mask=dilation)
# the filtered image is transformed into a binary image and placed in binary
ret, binary = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
# get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
contours, hierarchy = cv2.findContours(
dilation, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
# do something about misidentification
boxes = [
box
for box in [cv2.boundingRect(c) for c in contours]
if min(img.shape[0], img.shape[1]) / 10
< min(box[2], box[3])
< min(img.shape[0], img.shape[1]) / 1
]
if boxes:
for box in boxes:
x, y, w, h = box
# find the largest object that fits the requirements
c = max(contours, key=cv2.contourArea)
# get the lower left and upper right points of the positioning object
x, y, w, h = cv2.boundingRect(c)
# locate the target by drawing rectangle
cv2.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
# calculate the rectangle center
x, y = (x*2+w)/2, (y*2+h)/2
# calculate the real coordinates of 600 relative to the target
if mycolor == "red":
self.color = 0
elif mycolor == "green":
self.color = 1
elif mycolor == "cyan":
self.color = 2
else:
self.color = 3
if abs(x) + abs(y) > 0:
return x, y
else:
return None
if __name__ == "__main__":
# open the camera
cap_num = 0
cap = cv2.VideoCapture(cap_num, cv2.CAP_V4L)
if not cap.isOpened():
cap.open()
# init a class of Object_detect
detect = Object_detect()
# init 600
detect.run()
_init_ = 20
init_num = 0
nparams = 0
num = 0
real_sx = real_sy = 0
while cv2.waitKey(1) < 0:
# read camera
_, frame = cap.read()
# deal img
frame = detect.transform_frame(frame)
if _init_ > 0:
_init_ -= 1
continue
# calculate the parameters of camera clipping
if init_num < 20:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
init_num += 1
continue
elif init_num == 20:
detect.set_cut_params(
(detect.sum_x1)/20.0,
(detect.sum_y1)/20.0,
(detect.sum_x2)/20.0,
(detect.sum_y2)/20.0,
)
detect.sum_x1 = detect.sum_x2 = detect.sum_y1 = detect.sum_y2 = 0
init_num += 1
continue
# calculate params of the coords between cube and 600
if nparams < 10:
if detect.get_calculate_params(frame) is None:
cv2.imshow("figure", frame)
continue
else:
x1, x2, y1, y2 = detect.get_calculate_params(frame)
detect.draw_marker(frame, x1, y1)
detect.draw_marker(frame, x2, y2)
detect.sum_x1 += x1
detect.sum_x2 += x2
detect.sum_y1 += y1
detect.sum_y2 += y2
nparams += 1
continue
elif nparams == 10:
nparams += 1
# calculate and set params of calculating real coord between cube and 600
detect.set_params(
(detect.sum_x1+detect.sum_x2)/20.0,
(detect.sum_y1+detect.sum_y2)/20.0,
abs(detect.sum_x1-detect.sum_x2)/10.0 +
abs(detect.sum_y1-detect.sum_y2)/10.0
)
print("ok")
continue
# get detect result
detect_result = detect.color_detect(frame)
if detect_result is None:
cv2.imshow("figure", frame)
continue
else:
x, y = detect_result
# calculate real coord between cube and 600
real_x, real_y = detect.get_position(x, y)
if num == 20:
detect.pub_marker(real_sx/20.0/1000.0, real_sy/20.0/1000.0)
detect.decide_move(real_sx/20.0, real_sy/20.0, detect.color)
num = real_sx = real_sy = 0
else:
num += 1
real_sy += real_y
real_sx += real_x
cv2.imshow("figure", frame)
# close the window
if cv2.waitKey(1) & 0xFF == ord('q'):
cap.release()
cv2.destroyAllWindows()
sys.exit()

19
mypalletizer_260/mypalletizer_260/scripts/slider_control.py
View file

@ -9,11 +9,11 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import math
import time
import rospy
from sensor_msgs.msg import JointState
# from pymycobot.mycobot import MyCobot
from pymycobot.mypalletizer import MyPalletizer
@ -22,15 +22,14 @@ mc = None
def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
del data_list[3] # delete the angle of joint3 to joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！
# data_list[3] = data_list[4]
# print(data_list)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -42,7 +41,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyPalletizer(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin() 只是阻止python退出，直到该节点停止
print("spin ...")

1
mypalletizer_260/mypalletizer_260_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

132
mypalletizer_260/mypalletizer_260_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,132 +0,0 @@
#!/usr/bin/env python
# -*- 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 listen for 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 = "joint1"
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)
traj = self.arm.plan()
# 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 end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents 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")
self.moving()
# Add the 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 again if there is 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()

15
mypalletizer_260/mypalletizer_260_moveit/scripts/sync_plan.py
View file

@ -1,4 +1,5 @@
#!/usr/bin/env python2
import math
import time
import rospy
from sensor_msgs.msg import JointState
@ -14,11 +15,11 @@ def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
# print("data_list:",data_list)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
del data_list[3]
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -29,7 +30,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyPalletizer(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

18
mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py
View file

@ -9,11 +9,11 @@ Passable parameters:
port: serial prot string. Defaults is '/dev/ttyUSB0'
baud: serial prot baudrate. Defaults is 115200.
"""
import math
import time
import rospy
from sensor_msgs.msg import JointState
# from pymycobot.mycobot import MyCobot
from pymycobot.mypalletizer import MyPalletizer
@ -25,12 +25,10 @@ def callback(data):
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(value)
# del data_list[3] # delete the angle of joint3 to joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！
# data_list[3] = data_list[4]
# print(data_list)
mc.send_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -42,7 +40,9 @@ def listener():
baud = rospy.get_param("~baud", 1000000)
print(port, baud)
mc = MyPalletizer(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
# spin() simply keeps python from exiting until this node is stopped
# spin() 只是阻止python退出，直到该节点停止
print("spin ...")

1
mypalletizer_260/mypalletizer_260_pi_moveit/CMakeLists.txt
View file

@ -14,7 +14,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

4
mypalletizer_260/mypalletizer_260_pi_moveit/launch/moveit.rviz
View file

@ -41,7 +41,7 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 1
- Acceleration_Scaling_Factor: 0.5
Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
@ -183,7 +183,7 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: true
Velocity_Scaling_Factor: 1
Velocity_Scaling_Factor: 0.5
Enabled: true
Global Options:
Background Color: 48; 48; 48

132
mypalletizer_260/mypalletizer_260_pi_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,132 +0,0 @@
#!/usr/bin/env python
# -*- 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 listen for 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 = "joint1"
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)
traj = self.arm.plan()
# 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 end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents 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")
self.moving()
# Add the 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 again if there is 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()

16
mypalletizer_260/mypalletizer_260_pi_moveit/scripts/sync_plan.py
View file

@ -1,6 +1,7 @@
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import time
import math
import rospy
from sensor_msgs.msg import JointState
@ -12,14 +13,13 @@ mc = None
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data)
# rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
# if index != 2:
# value *= -1
data_list.append(value)
# print("data_list:",data_list)
mc.send_radians(data_list, 80)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.send_angles(data_list, 25)
def listener():
@ -30,7 +30,9 @@ def listener():
baud = rospy.get_param("~baud", 115200)
print(port, baud)
mc = MyPalletizer(port, baud)
time.sleep(0.05)
mc.set_free_mode(1)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped

10
ultraArm/ultraarm/scripts/slider_control.py
View file

@ -24,11 +24,11 @@ def callback(data):
print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(round(value,3))
print('data_list:',data_list)
ua.set_radians(data_list, 80)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
ua.set_angles(data_list, 25)
def listener():

1
ultraArm/ultraarm_moveit/CMakeLists.txt
View file

@ -13,7 +13,6 @@ catkin_package()
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/sync_plan.py
scripts/path_planning_and_obstacle_avoidance_demo.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

132
ultraArm/ultraarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py
View file

@ -1,132 +0,0 @@
#!/usr/bin/env python
# -*- 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 listen for 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 = "joint1"
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)
traj = self.arm.plan()
# 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 end of the robotic arm to rotate 90 degrees in reverse, 0,1,2,3,4,5 represents 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")
self.moving()
# Add the 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 again if there is 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()

11
ultraArm/ultraarm_moveit/scripts/sync_plan.py
View file

@ -1,4 +1,5 @@
#!/usr/bin/env python3
import math
import rospy
from sensor_msgs.msg import JointState
from pymycobot.ultraArm import ultraArm
@ -12,11 +13,11 @@ def callback(data):
# print(data.position)
data_list = []
for index, value in enumerate(data.position):
data_list.append(round(value,3))
print('data_list:',data_list)
ua.set_radians(data_list, 50)
# time.sleep(0.5)
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
ua.set_angles(data_list, 25)
def listener():