def __init__(self, pose):
self.check_collision = False
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 将场景中的颜色设置发布
self.sendColors()
rospy.sleep(5)
joint_positions = pose
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
if arm.go():
pass
else:
self.check_collision = True
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
# arm.set_goal_position_tolerance(0.00001)
# arm.set_goal_orientation_tolerance(0.00001)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(1.0)
arm.set_planning_time(0.5) # 规划时间限制为2秒
arm.allow_replanning(True) # 当运动规划失败后，是否允许重新规划
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.listener = tf.TransformListener()
def callback(states):
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
arm.set_planner_id("RRTConnectkConfigDefault")
arm.set_num_planning_attempts(20)
arm.allow_replanning(True)
pose = copy.deepcopy(states.pose[-1])
temp = tf.transformations.euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pose.position.z += 0.22
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
print pose
arm.set_pose_target(pose)
move_plan = arm.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = arm.plan()
i += 1
if (i == 5): break
state = arm.execute(move_plan)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('kinect_ur3_move', anonymous=True)
self.listener = tf.TransformListener()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.initialPose = PoseStamped()
self.Pose_goal = PoseStamped()
self.start_goal = PoseStamped()
self.current_goal = PoseStamped()
self.next_pose_goal = PoseStamped()
self.waypoints = []
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.allow_replanning(True)
self.robot_arm.set_goal_position_tolerance(0.01)
self.robot_arm.set_goal_orientation_tolerance(0.1)
def printPose(self):
self.start_goal = self.robot_arm.get_current_pose("ee_link")
print("Start pose:%s",self.start_goal)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('get_camera_pose_image_auto')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.5) # 规划时间限制为2秒
arm.allow_replanning(False) # 当运动规划失败后，是否允许重新规划
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
# 开始图像接收
image_receiver = ImageReceiver()
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.listener = tf.TransformListener()
self.save_cnt = 0
print "\n\n[INFO] Pose image saver started."
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('start_pos')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.pose_goal = Pose()
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to up ======")
rospy.sleep(0.5)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def move_TF(self):
self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538]
self.pose_goal.position.x = self.ee_TF_list[0]
self.pose_goal.position.y = self.ee_TF_list[1]
self.pose_goal.position.z = self.ee_TF_list[2]
self.pose_goal.orientation.x = self.ee_TF_list[3]
self.pose_goal.orientation.y = self.ee_TF_list[4]
self.pose_goal.orientation.z = self.ee_TF_list[5]
self.pose_goal.orientation.w = self.ee_TF_list[6]
self.robot_arm.set_pose_target(self.pose_goal)
self.robot_arm.go(True)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_ik', anonymous=True)
robot = moveit_commander.RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
# right_arm.set_goal_position_tolerance(0.01)
# right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
# end_effector_link = right_arm.get_end_effector_link()
# Get the current pose so we can add it as a waypoint
# start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
valid_pose = np.load("valid_pose.npy")
for i in valid_pose:
waypoints.append(i.pose)
no_pros_data = np.load("dmp_euler_traj.npy")
pose_list = []
for idx in no_pros_data:
pose = Pose()
pose.position.x = idx[0]
pose.position.y = idx[1]
pose.position.z = idx[2]
pose.orientation.x = idx[3]
pose.orientation.y = idx[4]
pose.orientation.z = idx[5]
pose.orientation.w = idx[6]
pose_list.append(pose)
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
(plan, fraction) = right_arm.compute_cartesian_path(
pose_list, # waypoint poses
0.01, # eef_step
0.0 # jump_threshold
) # avoid_collisions
print "fraction is %s" % fraction
right_arm.execute(plan)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.08) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.scene = scene
self.scene_pub = scene_pub
self.colors = dict()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m
self.approach_distance = 0.06
self.back_distance = 0.05
# sub and pub point cloud
self.point_cloud = None
self.update_cloud_flag = False
rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.callback_pointcloud)
thread.start_new_thread(self.publish_pointcloud, ())
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_speed_demo')
# Initialize the move group for the right arm
arm = MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# Get back the planned trajectory
arm.set_joint_value_target(joint_positions)
traj = arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
arm.execute(new_traj)
rospy.sleep(1)
arm.set_named_target('home')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.Pose_goal = Pose()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
# robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
# self.robot_arm.set_named_target("up") #go to goal state.
# self.robot_arm.go()
# print("====== move plan go to home 1 ======")
# rospy.sleep(2)
# print("====== move plan go to up ======")
# self.robot_arm.set_named_target("zeros") #go to goal state.
# self.robot_arm.go(wait=True)
# print("====== move plan go to zeros ======")
# rospy.sleep(1)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state.
self.robot_arm.go(True)
print("====== move plan go to Pose_goal ======")
#rospy.sleep(2)
robot_state = self.robot_arm.get_current_pose();
robot_angle = self.robot_arm.get_current_joint_values();
print(robot_state)
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
self.Pose_goal = data
self.move_code()
def listener(self):
rospy.Subscriber("chatter", Pose, self.callback)
def arm_pose():
arm = MoveGroupCommander('arm')
arm.allow_replanning(True)
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.03)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_footprint'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
curr_pose = arm.get_current_pose(end_effector_link).pose.position
return curr_pose.x, curr_pose.y, curr_pose.z
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('pick_6')
# arm.set_named_target('home')
# arm.go()
self.box_name = ''
self.scene = scene
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to zero ======")
rospy.sleep(0.5)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home2 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(1) # 规划时间限制为2秒
arm.set_num_planning_attempts(2) # 规划两次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print arm.get_current_pose(eef_link).pose
sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects',
Float64MultiArray, self.callback)
self.juggle_rects = Float64MultiArray()
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('rest')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('wave')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('rest')
# Get back the planned trajectory
traj = right_arm.plan()
# Set the trajectory speed
new_traj = create_tracking_trajectory(traj, 1.0)
rospy.loginfo(new_traj)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def talker_by13():
#init
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo')
#cartesian = rospy.get_param('~cartesian', True)
arm = MoveGroupCommander('manipulator')
arm.set_pose_reference_frame('base_link')
arm.allow_replanning(True)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# arm.set_max_acceleration_scaling_factor(0.5)
#arm.set_max_velocity_scaling_factor(0.5)
end_effector_link = arm.get_end_effector_link()
#arm.set_named_target('home')
arm.set_named_target('up')
arm.go()
rospy.sleep(2)
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_link'
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.86
target_pose.pose.position.y = 0.25
target_pose.pose.position.z = 0.02832
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
#next: find workspace
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
#arm.shift_pose_target(2,-0.05,end_effector_link)
#arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
rospy.sleep(2)
pose = PoseStamped().pose
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
i = 1
while i <= 10:
waypoints = list()
j = 1
while j <= 5:
# random pose
rand_pose = arm.get_random_pose(arm.get_end_effector_link())
pose.position.x = rand_pose.pose.position.x
pose.position.y = rand_pose.pose.position.y
pose.position.z = rand_pose.pose.position.z
waypoints.append(copy.deepcopy(pose))
j += 1
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
i += 1
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.4)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.1) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.082 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.075=0.085m
self.approach_distance = 0.05
self.back_distance = 0.05
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test')
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.allow_replanning(True)
arm.set_planning_time(5)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_init_orientation = Quaternion()
target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))
current_pose = arm.get_current_pose(end_effector_link)
self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
arm.set_pose_target(current_pose)
arm.go()
rospy.sleep(2)
constraints = Constraints()
orientation_constraint = OrientationConstraint()
constraints.name = 'gripper constraint'
orientation_constraint.header = target_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = target_init_orientation[0]
orientation_constraint.orientation.y = target_init_orientation[1]
orientation_constraint.orientation.z = target_init_orientation[2]
orientation_constraint.orientation.w = target_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
arm.set_path_constraints(constraints)
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('resting')
# Get back the planned trajectory
traj = right_arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def setUpClass(self):
rospy.init_node('test_moveit')
rospy.sleep(5) # intentinally wait for starting up hrpsys
self.robot = RobotCommander()
# TODO: Read groups from SRDF file ideally.
for mg_attr in self._MOVEGROUP_ATTRS:
mg = MoveGroupCommander(mg_attr[0])
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
mg.set_planner_id(self._KINEMATICSOLVER_SAFE)
# Allow replanning to increase the odds of a solution
mg.allow_replanning(True)
# increase planning time
mg.set_planning_time(30.0)
# Append MoveGroup instance to the MoveGroup attribute list.
mg_attr.append(mg)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
arm.set_planning_time(10)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
print "======== create new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
# Test Position
start_pose.pose.position.x = 0.2 # 20 cm
start_pose.pose.position.y = -0.11 # -11 cm
start_pose.pose.position.z = 0.6 # 60 cm
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print start_pose
print "====== move to position ======="
# KDL
# arm.set_joint_value_target(start_pose, True)
# IK Fast
arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
arm.get_end_effector_link())
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
self.arm = arm
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# max planning time
arm.set_planning_time(10)
# start pose
arm.set_start_state_to_current_state()
end_effector = arm.get_end_effector_link()
rospy.sleep(1)
print "======== create 100 new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
i = 1
while i <= 1000:
# random position
start_pose = arm.get_random_pose(end_effector)
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print "====== move to position", i, "======="
# KDL
arm.set_joint_value_target(start_pose, True)
# IK Fast
# arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y,
# start_pose.pose.position.z], end_effector)
i += 1
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_pick_and_place_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(1.0)
arm.set_planning_time(0.05) # 规划时间限制
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
arm.set_named_target('work')
arm.go()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# arm.set_goal_position_tolerance(0.01)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
waypoints = list()
pose = PoseStamped().pose
# start with the current pose
waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose))
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
# first pose
pose.position.x = 0.35
pose.position.y = 0.25
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# second pose
pose.position.x = 0.25
pose.position.y = -0.3
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# third pose
pose.position.x += 0.15
waypoints.append(copy.deepcopy(pose))
# fourth pose
pose.position.y += 0.15
waypoints.append(copy.deepcopy(pose))
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class ArmTracker:
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the right arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the right arm reference frame accordingly
self.right_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.right_arm.allow_replanning(False)
# Set a position tolerance in meters
self.right_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.right_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the right_arm shoulder lift frame
#target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('right_arm_base_link', target)
# Convert the position values to a Python list
p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.right_arm.set_named_target('resting')
self.right_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame, target)
# Compute the distance between the current target and the last target
p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.right_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame, target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.right_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.right_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.right_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def update_target_pose(self, target):
self.target = target
def relax_all_servos(self):
command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
args = shlex.split(command)
subprocess.Popen(args)
def shutdown(self):
# Stop any further target messages from being processed
self.target_subscriber.unregister()
# Stop any current arm movement
self.right_arm.stop()
# Move to the resting position
self.right_arm.set_named_target('resting')
self.right_arm.go()
# Relax the servos
self.relax_all_servos()
os._exit(0)
class Arm_Controller:
def __init__(self):
# Give the launch a chance to catch up
# rospy.sleep(5)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Arm_Controller')
rospy.loginfo("Launched Arm Controller")
# constants
self.GROUP_NAME_ARM = 'arm'
self.GRIPPER_FRAME = 'gripper_link'
self.REFERENCE_FRAME = 'base_link'
self.ARM_BASE_FRAME = 'arm_base_link'
self.done = True
self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped)
rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1)
self.robot_name = "gatlin"
move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm)
# We need a tf listener to convert poses into arm reference base
self.tfl = tf.TransformListener()
# Initialize the move group for the right arm
self.arm = MoveGroupCommander(self.GROUP_NAME_ARM)
self.gripper = Gripper()
self.robot = moveit_commander.RobotCommander()
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Set the planner
self.arm.set_planner_id("RRTConnectkConfigDefault")
# Set the right arm reference frame
self.arm.set_pose_reference_frame(self.REFERENCE_FRAME)
# Give the scene a chance to catch up
rospy.sleep(1)
# Allow some leeway in position (meters) and orientation (radians)
# USELESS; do not work on pick and place! Explained on this issue:
# https://github.com/ros-planning/moveit_ros/issues/577
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.05)
# Allow 2 seconds per planning attempt
self.arm.set_planning_time(2.0)
# Create a quaternion from the Euler angles
#q = quaternion_from_euler(0, pitch, yaw)
# horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555)
# deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861)
down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
# back_pos = Point(-0.03, 0.0313, 0.476)
# init rest pose
self.rest_pose = PoseStamped()
self.rest_pose.header.frame_id = self.REFERENCE_FRAME
self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35)
self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981)
# init place_upper pose
self.place_upper_pose = PoseStamped()
self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME
self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998)
self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889)
# init current pose
self.current_pose = PoseStamped()
self.current_pose.header.frame_id = self.REFERENCE_FRAME
self.current_pose.pose.position = Point(0,0,0)
self.current_pose.pose.orientation = down
# Open the gripper
rospy.loginfo("Set Gripper: open")
self.gripper.set(1.0)
# self.arm.set_pose_target(self.rest_pose)
# self.arm.go()
# rospy.sleep(1)
# self.ar = ArbotixROS()
# rate = rospy.Rate(30)
# while not rospy.is_shutdown():
# # rospy.logerr(self.ar.getVoltage(4))
# # rospy.logerr(self.ar.getSpeed(5))
# rospy.logerr(self.ar.getPosition(1))
# rate.sleep()
rospy.spin()
def MoveToPoseWithIntermediate(self, ps, offsets) :
success = False
for offset in offsets:
# interpose = getOffsetPose(hand_pose, offset)
interpose = getOffsetPose(ps, offset)
success = self.MoveToPose(interpose, "MoveToIntermediatePose")
rospy.sleep(1)
success = self.MoveToPose(ps, "MoveToPose")
return success
def MoveToPose(self, ps, name) :
newpose = self.transform_pose(self.REFERENCE_FRAME, ps)
down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
newpose.pose.orientation = down
if self.move_arm_to_pose(newpose) :
rospy.loginfo("SUCCEEDED: %s" % name)
return True
else :
rospy.logerr("FAILED %s" % name)
return False
def move_arm_to_pose(self, ps):
arm_target_pose = deepcopy(ps)
arm_target_pose.header.stamp = rospy.Time.now()
self.test_pose_publisher.publish(arm_target_pose)
self.arm.set_pose_target(arm_target_pose)
success = self.arm.go()
return success
def handle_move_arm(self, req):
success = True
gripper = self.gripper
if req.action == "OPEN_GRIPPER":
rospy.loginfo("Beginning to open gripper")
gripper.open(block=True)
rospy.loginfo("Opened Gripper")
elif req.action == "CLOSE_GRIPPER" :
rospy.loginfo("Beginning to close Gripper")
gripper.close(block=True)
rospy.loginfo("Closed Gripper")
elif req.action == "MOVE_TO_POSE_INTERMEDIATE" :
rospy.loginfo("Trying to Move To Pose With Intermediate")
offsets = [Vector3(0,0,.07)]
success = self.MoveToPoseWithIntermediate(req.ps, offsets)
elif req.action == "MOVE_TO_POSE" :
rospy.loginfo("Trying to Move To Pose")
success = self.MoveToPose(req.ps, "FAILED MoveToPose")
elif req.action == "RESET_ARM" :
rospy.loginfo("Trying to Move To Rest Pose")
success = self.move_arm_to_pose(self.rest_pose)
elif req.action == "PLACE_UPPER" :
rospy.loginfo("Trying to Move To Place Upper Pose")
success = self.move_arm_to_pose(self.place_upper_pose)
# success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose")
# elif req.action == "MOVE_TO_POS" :
# rospy.loginfo("Trying to Move To Pos")
# new_pose = Pose()
# if req.limb == 'left':
# try:
# self.initial_left
# new_pose = deepcopy(self.initial_left)
# except AttributeError:
# new_pose = deepcopy(self.hand_pose_left)
# self.initial_left = deepcopy(self.hand_pose_left)
# elif req.limb == 'right':
# try:
# self.initial_right
# new_pose = deepcopy(self.initial_right)
# except AttributeError:
# new_pose = deepcopy(self.hand_pose_right)
# self.initial_right = deepcopy(self.hand_pose_right)
# new_pose.position = deepcopy(req.pose.position)
# # success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose")
# success = self.MoveToPoseWithIntermediate(req.limb, new_pose)
# rospy.loginfo("Moved to pos: %r" % success)
else :
success = False
rospy.logerr("invalid action")
return MoveRobotResponse(success)
def orientation_cb(self, data):
if(data.x == -1.0 and data.y == -2.0):
print "################################################"
print "################### Orientation! #############"
print "################################################"
deg = data.z*15
rad = -deg * pi/180 + pi/2
print rad
q = quaternion_from_euler(0,rad,0)
self.current_pose.pose.orientation.x = q[0]
self.current_pose.pose.orientation.y = q[1]
self.current_pose.pose.orientation.z = q[2]
self.current_pose.pose.orientation.w = q[3]
else:
return
# transform the pose stamped to the new frame
def transform_pose(self, new_frame, pose):
if pose.header.frame_id == new_frame:
return pose
try:
ps = deepcopy(pose)
ps.header.stamp = rospy.Time(0)
self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0))
new_pose = self.tfl.transformPose(new_frame, ps)
new_pose.header.stamp = deepcopy(pose.header.stamp)
return new_pose
except Exception as e:
rospy.logerr(e)
rospy.logerr("no transform")
return None
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('box1')
scene.remove_world_object('box2')
scene.remove_world_object('target')
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table
table_size = [0.2, 0.7, 0.01]
# Set the length, width and height of the object to attach
tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
scene.attach_mesh
# Place the end of the object within the grasp of the gripper
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_footprint'
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
# Update the current state
right_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Return the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('TCP_Move', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('xarm6')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('link_base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 角度弧度转换
j1 = 90.0 / 180 * math.pi
j2 = -18.6 / 180 * math.pi
j3 = -28.1 / 180 * math.pi
j4 = 1.0 / 180 * math.pi
j5 = 47.6 / 180 * math.pi
j6 = -0.9 / 180 * math.pi
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [j1, j2, j3, j4, j5, j6]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
# 向下按压门把手
current_pose = arm.get_current_joint_values()
current_pose[4] += (20.0 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
#推开门
current_pose = arm.get_current_joint_values()
current_pose[0] -= (42.0 / 180.0) * math.pi
current_pose[1] += (2.0 / 180.0) * math.pi
current_pose[2] -= (11.4 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.2)
arm.set_goal_orientation_tolerance(0.1)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
print(end_effector_link)
# 控制机械臂先回到初始化位置
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
print(start_pose)
# 初始化路点列表
waypoints = []
os.system("rosrun pick_test send_gripper.py --value 0.0")
# 将初始位姿加入路点列表
# arm.set_named_target('test5')
# arm.go()
# rospy.sleep(1)
######
marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot)
listener = tf.TransformListener()
r = rospy.Rate(10)
while not rospy.is_shutdown():
try:
(trans,
rot) = listener.lookupTransform('/base_link', '/ar_marker_4',
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
######
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
print carrot
wpose.position.z -= carrot.z
waypoints.append(deepcopy(wpose))
# wpose.position.z -= carrot.z+0.1
# wpose.position.y += carrot.y
# waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 控制机械臂先回到初始化位置
waypoints = []
test_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(test_pose)
wpose.position.x -= carrot.x + 0.03
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.02, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
########
os.system("rosrun pick_test send_gripper.py --value 0.8")
########
waypoints = []
pick_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(pick_pose)
wpose.position.z += 0.1
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.02, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
eef = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
# right_arm.set_goal_position_tolerance(0.05)
# right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
#grasp_pose.header.frame_id = 'gazebo_wolrd'
# Shift the grasp pose by half the width of the target to center it
# grasp_pose.pose.position.y -= target_size[1] / 2.0
# grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0
# grasp_pose.pose.position.x = target_pose.pose.position.x -0.07
# grasp_pose.pose.position.z += 0.18
#Allowed touch object list
# ato = [target_id, 'r_forearm_link']
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id]
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
# print grasp.grasp_pose
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
#Allowed touch link list
atl = ['r_forearm_link']
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_pick_attempts:
success = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(0.2)
if success:
self.ac.send_goal(g_close)
rospy.sleep(3)
## If the pick was successful, attempt the place operation
#if success:
#success = False
#n_attempts = 0
## Generate valid place poses
#places = self.make_places(place_pose)
## Repeat until we succeed or run out of attempts
#while success == False and n_attempts < max_place_attempts:
#for place in places:
#success = right_arm.place(target_id, place)
#if success:
#break
#n_attempts += 1
#rospy.loginfo("Place attempt: " + str(n_attempts))
#rospy.sleep(0.2)
#if not success:
#rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
#else:
#rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
## Return the arm to the "resting" pose stored in the SRDF file
##right_arm.set_named_target('resting')
##right_arm.go()
## Open the gripper to the neutral position
#right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
#right_gripper.go()
#rospy.sleep(1)
#rospy.spin()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.55
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.54
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.60
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.25
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path (
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault");
left_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#left_arm.set_named_target('left_start')
#left_arm.go()
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.56
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.51
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.49
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class Pick_Place:
def __init__(self, planArm="left_arm"):
# Initialize the move_group API
#moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('pick_place', anonymous=True)
# Connect to the arm move group
if planArm == "right_arm":
self.arm = MoveGroupCommander('right_arm')
self.hand = Gripper(1)
else:
self.arm = MoveGroupCommander('left_arm')
self.hand = Gripper(0)
rospy.sleep(1)
self.hand.release()
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
#self.arm.set_goal_position_tolerance(0.001)
#self.arm.set_goal_orientation_tolerance(0.01)
def pick(self):
self.graspFromSide()
def graspFromSide(self):
targetPose=Pose()
targetPose.position.x = 0.96
targetPose.position.y = 0.10
targetPose.position.z = -0.0
targetPose.orientation.x = 0.044
targetPose.orientation.y = 0.702
targetPose.orientation.z = 0.045
targetPose.orientation.w = 0.708
# right side grasp
# Position
#x = 0.636
#y = 0.832
#z = 0.192
# Orientation
#x = -0.382
#y = 0.922
#z = 0.021
#w= 0.049
#xyz 0.871 0.428 0.038
#orientation 0.119 0.698 -0.083 0.700
self.moveToTargetPose(targetPose)
while self.hand.range.state() == 65535.0:
targetPose.position.x += 0.05
self.moveToTargetPose(targetPose)
while self.hand.range.state() > 70:
targetPose.position.x += 0.02
self.moveToTargetPose(targetPose)
self.hand.grasp()
rospy.sleep(1)
targetPose.position.x -= 0.05
targetPose.position.y -= 0.05
targetPose.position.z += 0.1
self.moveToTargetPose(targetPose)
self.hand.release()
rospy.sleep(1)
def moveToTargetPose(self, targetPose):
# move to the defined pose
self.arm.set_start_state_to_current_state()
self.arm.set_pose_target(targetPose)
self.arm.go()
rospy.sleep(0.5)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.3
box1_pose.pose.position.y = 0
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.3
box2_pose.pose.position.y = 0.25
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.2
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.31
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.29
box2_pose.pose.position.y = -0.4
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.04, 0.04, 0.05]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set an initial position for the arm
start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)
# Plan and execute a trajectory to the goal configuration
arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Move end effector to the right 0.3 meters
wpose.position.y -= 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Move end effector up and back
wpose.position.x -= 0.2
wpose.position.z += 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses
0.025, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class ArmTracker:
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the left arm
self.left_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the left gripper
left_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the left arm reference frame accordingly
self.left_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.left_arm.allow_replanning(False)
# Set a position tolerance in meters
self.left_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.left_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.left_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
left_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
left_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose',
PoseStamped,
self.update_target_pose,
queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the left_arm shoulder lift frame
#target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('left_rotate', target)
# Convert the position values to a Python list
p0 = [
target_arm.pose.position.x, target_arm.pose.position.y,
target_arm.pose.position.z
]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.left_arm.set_named_target('l_start')
self.left_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame,
target)
# Compute the distance between the current target and the last target
p1 = [
target_base.pose.position.x, target_base.pose.position.y,
target_base.pose.position.z
]
p2 = [
self.last_target_pose.pose.position.x,
self.last_target_pose.pose.position.y,
self.last_target_pose.pose.position.z
]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.left_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [
ee_pose.pose.position.x, ee_pose.pose.position.y,
ee_pose.pose.position.z
]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame,
target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.left_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.left_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.left_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def update_target_pose(self, target):
self.target = target
def relax_all_servos(self):
command = 'rosrun rbx2_dynamixels arbotix_relax_all_servos.py'
args = shlex.split(command)
subprocess.Popen(args)
def shutdown(self):
# Stop any further target messages from being processed
self.target_subscriber.unregister()
# Stop any current arm movement
self.left_arm.stop()
# Move to the r_start position
self.left_arm.set_named_target('r_start')
self.left_arm.go()
# Relax the servos
self.relax_all_servos()
os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface("base_link")
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
the_object = None
the_object_dist = 1.0
count = -1
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
right_arm.set_support_surface_name(support_surface)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = target_pose.pose.position.x
place_pose.pose.position.y = 0.03
place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Set the start state to the current state
#right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
# Give the servos a rest
arbotix_relax_all_servos()
rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
