def init():
#Wake up Baxter
baxter_interface.RobotEnable().enable()
rospy.sleep(0.25)
print "Baxter is enabled"
print "Intitializing clients for services"
global ik_service_left
ik_service_left = rospy.ServiceProxy(
"ExternalTools/left/PositionKinematicsNode/IKService",
SolvePositionIK)
global ik_service_right
ik_service_right = rospy.ServiceProxy(
"ExternalTools/right/PositionKinematicsNode/IKService",
SolvePositionIK)
global obj_loc_service
obj_loc_service = rospy.ServiceProxy(
"object_location_service", ObjLocation)
global stopflag
stopflag = False
#Taken from the MoveIt Tutorials
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
global scene
scene = moveit_commander.PlanningSceneInterface()
#Activate Left Arm to be used with MoveIt
global left_group
left_group = MoveGroupCommander("left_arm")
left_group.set_goal_position_tolerance(0.01)
left_group.set_goal_orientation_tolerance(0.01)
global right_group
right_group = MoveGroupCommander("right_arm")
pose_right = Pose()
pose_right.position = Point(0.793, -0.586, 0.329)
pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0)
request_pose(pose_right, "right", right_group)
global left_gripper
left_gripper = baxter_interface.Gripper('left')
left_gripper.calibrate()
print left_gripper.parameters()
global end_effector_subs
end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback)
rospy.sleep(1)
global pubpic
pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
rospy.sleep(1)
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)
class Robot:
def __init__(self, config, debug=0):
print "[INFO] Initialise Robot"
self.DEBUG = debug
# configuration
self.config = config
# initialise move groups
self.arm = MoveGroupCommander(ARM_GROUP)
self.gripper = MoveGroupCommander(GRIPPER_GROUP)
# initialise pick and place manager
if self.DEBUG is 1:
# verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
else:
# non verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)
# tolerance: position (in m), orientation (in rad)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
self.place_manager = None
self.pick_manager = None
self.initialise_move_actions()
self.start_position()
"""Initialise the place and pick manager.
"""
def initialise_move_actions(self):
self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG)
self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG)
"""Move robot arm to "start position".
"""
def start_position(self):
self.arm.set_named_target(START_POSITION)
self.arm.go()
rospy.sleep(2)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") ]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") ]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral") ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
# 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('target_pose', PoseStamped, queue_size=10)
# 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.04)
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)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 1
rospy.loginfo("Scaling for MoveIt timeout=" + str(rospy.get_param('/move_group/trajectory_execution/allowed_execution_duration_scaling')))
# 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 "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the closed position
rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed ) )
gripper.set_joint_value_target(self.gripper_closed)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the neutral position
rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral) )
gripper.set_joint_value_target(self.gripper_neutral)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.34
# 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.005, 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.36
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 = table_pose.pose.position.x - 0.04
#box1_pose.pose.position.y = 0.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 = table_pose.pose.position.x - 0.06
#box2_pose.pose.position.y = 0.2
#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 = table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
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
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 = table_pose.pose.position.x - 0.03
place_pose.pose.position.y = -0.15
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], [target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
#print arm.pick(target_id, grasps)
result = arm.pick(target_id, grasps)
print 'hello'
#print MoveItErrorCodes.SUCCESS
#if result == MoveItErrorCodes.SUCCESS:
result = MoveItErrorCodes.SUCCESS
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
print arm.pick(target_id, grasps)
print 'kill-me'
print MoveItErrorCodes.SUCCESS
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
success = False
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts:
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
#if success:
success=1
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def cartesian_move_to(pose_, execute=False):
goal_pose = deepcopy(pose_)
######################################## experiment start
# for hardcoded aruco pose
# some known arm positions
# aruco pose in simulation: 0.52680940312, -0.0490591337742, 0.921301848054, 0.504516550338, 0.506271228009, 0.497290765692, 0.49178693403
# in base_footprint: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0
# pick pose: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0
# final hand pose in simulation: 0.47653800831, -0.396454309047, 1.05656705145, -0.630265833017, -0.318648344327, 0.582106845777, 0.402963810396
# goal_pose = PoseStamped()
# goal_pose.header.frame_id = 'base_footprint'
# goal_pose.pose.position.x = 0.52680940312
# goal_pose.pose.position.y = -0.0490591337742
# goal_pose.pose.position.z = 0.871301848054
# goal_pose.pose.orientation.x = 0
# goal_pose.pose.orientation.y = 0
# goal_pose.pose.orientation.z = 0
# goal_pose.pose.orientation.w = 1
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)
rospy.sleep(2)
start_pose=arm.get_current_pose(end_effector_link).pose
rospy.loginfo("End effector start pose: " + str(start_pose))
rospy.loginfo("Aruco pose: " + str(goal_pose))
waypoints=[]
waypoints.append(start_pose)
# goal_pose.pose.orientation.x = -0.5
# goal_pose.pose.orientation.y = -0.5
# goal_pose.pose.orientation.z = -0.5
# goal_pose.pose.orientation.w = 1
# goal_pose.pose.orientation.y -= 0.1*(1.0/2.0)
waypoints.append(deepcopy(goal_pose.pose))
fraction=0.0
maxtries=100
attempts=0
while fraction<1.0 and attempts<maxtries:
(plan,fraction)=arm.compute_cartesian_path(waypoints,0.02,0.0,True)
attempts+=1
if attempts %20==0:
if (attempts<100):
rospy.loginfo("still trying after"+str(attempts)+" attempts...")
else :
rospy.loginfo("Finished after "+str(attempts)+" attempts...")
if fraction>0.89:
rospy.loginfo("path compute successfully. Arm is moving.")
print(plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1].positions)
print(plan)
#for item in plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1]
if execute:
arm.execute(plan)
rospy.loginfo("path execution complete. ")
rospy.sleep(2)
if (attempts %100==0 and fraction<0.9):
rospy.loginfo("path planning failed with only " + str(fraction*100)+ "% success after "+ str(maxtries)+" attempts")
return fraction
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'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_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()
rospy.sleep(1)
gripper.set_named_target('finger_home')
gripper.go()
rospy.sleep(3)
# 设置桌面的高度
table_ground = 0.1
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [1, 2, 0.01]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 1.5
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)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.5, 0.05, 0.2]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 1.5
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 = 1.5
place_pose.pose.position.y = 0.3
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.set_named_target('finger_home')
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class BaseArmNavigation(object):
def __init__(self, node_name):
self.node_name = node_name
rospy.init_node(node_name)
rospy.loginfo("Starting node " + str(node_name))
rospy.on_shutdown(self.shutdown)
#Initialize necessary variables
self.arm_group_name = rospy.get_param("~arm_group_name", "ur5_arm")
self.stow_group_state = rospy.get_param("~stow_group_state", "stow")
self.work_group_state = rospy.get_param("work_group_state", "work")
self.reference_frame = rospy.get_param("~reference_frame", "map")
self.robot_base_frame = rospy.get_param("~robot_base_frame", "odom")
self.initial_location_topic = rospy.get_param(
"~initial_location_topic", "/initial_locations")
self.poi_topic = rospy.get_param("~poi_topic", "/poi")
self.error_topic = rospy.get_param("~error_topic", "/error")
self.allow_replanning = rospy.get_param("~allow_replanning", True)
self.position_tolerance = rospy.get_param("~position_tolerance", .0001)
self.orientation_tolerance = rospy.get_param("~orientation_tolerance",
.0001)
self.desired_orientation = rospy.get_param("~desired_orientation",
Quaternion(0, 0, 0, 1))
self.cartestian_path = rospy.get_param("~cartestian_path", False)
self.cp_max_attempts = rospy.get_param("~cp_max_attempts", 100)
self.workface_offset = rospy.get_param("~workface_offset", 1.0)
# Initialize a number of global variables
self.poi = None
self.error = None
# Arm Navigation Initialization
# Initialize the move group for the right arm
self.arm = MoveGroupCommander(self.arm_group_name)
# Set the arm reference frame accordingly
self.arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.arm.allow_replanning(self.allow_replanning)
# Set a position tolerance in meters
self.arm.set_goal_position_tolerance(self.position_tolerance)
# Set an orientation tolerance in radians
self.arm.set_goal_orientation_tolerance(self.orientation_tolerance)
# What is the end effector link?
self.end_effector_link = self.arm.get_end_effector_link()
# Move Base Initialization
# Publisher to manually control the robot
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# Initialize Tf
# Create the transform listener
self.listener = tf.TransformListener()
# Queue Tf data
rospy.sleep(3)
rospy.loginfo("Listening to Tfs")
# Subscribe to necessary topics
# Subscribe to the initial locations topic and store them as self.initial_locations
self.initial_locations = rospy.wait_for_message(
self.initial_location_topic, PoseArray)
# Subscribe to the poi topic
rospy.wait_for_message(self.poi_topic, PointStamped)
# Use queue_size=1 so outdated poi and error messages are not piled up
self.poi_subscriber = rospy.Subscriber(self.poi_topic,
PointStamped,
self.update_poi,
queue_size=1)
self.error_subscriber = rospy.Subscriber(self.error_topic,
PointStamped,
self.update_error,
queue_size=1)
rospy.loginfo("Subscribed to necessary topics")
rospy.loginfo("Ready to perform task")
# Perform desired task
self.perform_task()
def perform_task(self):
rospy.loginfo("Performing specified task")
return
def update_poi(self, poi):
self.poi = poi
def update_error(self, error):
self.error = error
def convert_point_to_arm_goal(self, point, initial_location):
#Create Pose Msgs
pose = PoseStamped()
pose.header.frame_id = self.reference_frame
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = initial_location.pose.position.x
pose.pose.position.y = point.point.y
pose.pose.position.z = point.point.z
pose.pose.orientation = initial_location.pose.orientation
return pose
def convert_pose_to_move_base_goal(self, goal):
# Initialize the move_base_goal
move_base_goal = MoveBaseGoal()
# Use the map frame to define goal poses
move_base_goal.target_pose.header.frame_id = self.reference_frame
# Set the time stamp to "now"
move_base_goal.target_pose.header.stamp = rospy.Time.now()
# Set the pose
move_base_goal.target_pose.pose = Pose(
Point(goal.pose.position.x - self.workface_offset,
goal.pose.position.y, 0.0), goal.pose.orientation)
return move_base_goal
def plan_cartesian_path(self, end_pose):
#Initialize necessary variables
fraction = 0.0
attempts = 0
start_pose = self.arm.get_current_pose(self.end_effector_link)
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the start point and goal
while fraction < 1.0 and attempts < self.cp_max_attempts:
(plan, fraction) = self.arm.compute_cartesian_path(
[start_pose, end_pose], # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# If we have a complete plan, return that plan
if fraction == 1.0:
return plan
else:
return None
def plan_regular_path(self, end_pose):
# Set the start state to the current state
self.arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
self.arm.set_pose_target(end_pose, self.end_effector_link)
# Plan the trajectory to the goal
plan = self.arm.plan()
return plan
def arm_nav_predicition(self, goal):
# Initialize the outcome to False
outcome = False
if self.cartestian_path:
plan = self.plan_cartesian_path(goal)
else:
plan = self.plan_regular_path(goal)
if plan is not None:
# Execute the planned trajectory
self.arm.execute(plan)
outcome = True
return outcome
def arm_nav_predicition_with_move_base(self, goal):
# Initialize the outcome to False
outcome = False
if self.cartestian_path:
plan = self.plan_cartesian_path(goal)
else:
plan = self.plan_regular_path(goal)
if plan is not None:
# Execute the planned trajectory
self.arm.execute(plan)
outcome = True
else:
move_base_goal = self.convert_pose_to_move_base_goal(goal)
base_move_outcome = self.base_move(move_base_goal)
if base_move_outcome:
if self.cartestian_path:
plan = self.plan_cartesian_path(goal)
else:
plan = self.plan_regular_path(goal)
if plan is not None:
# Execute the planned trajectory
self.arm.execute(plan)
outcome = True
return outcome
def arm_nav_correction(self, goal):
# Initialize the outcome to False
outcome = False
if self.cartestian_path:
plan = self.plan_cartesian_path(goal)
else:
plan = self.plan_regular_path(goal)
if plan is not None:
# Execute the planned trajectory
self.arm.execute(plan)
outcome = True
return outcome
def arm_nav_correction_with_move_base(self, goal):
# Initialize the outcome to False
outcome = False
if self.cartestian_path:
plan = self.plan_cartesian_path(goal)
else:
plan = self.plan_regular_path(goal)
if plan is not None:
# Execute the planned trajectory
self.arm.execute(plan)
outcome = True
else:
move_base_goal = self.convert_pose_to_move_base_goal(goal)
base_move_outcome = self.base_move(move_base_goal)
if base_move_outcome:
if self.cartestian_path:
plan = self.plan_cartesian_path(goal)
else:
plan = self.plan_regular_path(goal)
if plan is not None:
# Execute the planned trajectory
self.arm.execute(plan)
outcome = True
return outcome
def base_move(self, goal):
# Initialize the outcome to False
base_move_outcome = False
# Get the current position of the system's base
if self.listener.canTransform(self.reference_frame,
self.robot_base_frame, rospy.Time.now()):
try:
start_position = PoseStamped()
start_position.header.frame_id = self.robot_base_frame
start_position.header.stamp = rospy.Time.now()
start_position.pose = Pose(Point(0.0, 0.0, 0.0),
Quaternion(0.0, 0.0, 0.0, 1))
start_position = self.listener.transformPose(
self.reference_frame, start_position)
except:
print "TF execption in base_arm_nav.base_move"
else:
return
# Send the goal pose to the MoveBaseAction server
self.move_base.send_goal(goal)
# Allow 1 minute for the system to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(60))
# If the system doesn't get there in time, abort the goal and return
# to the start position
if not finished_within_time:
# Cancel current goal
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
# Move back to start_position
self.move_base.send_goal(start_position)
else:
# The system made it to the goal position
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
#Set the outcome to True
base_move_outcome = True
else:
self.move_base.send_goal(start_position)
return base_move_outcome
def shutdown(self):
rospy.loginfo("Stopping the robot")
# Stop any further target messages from being processed
self.poi_subscriber.unregister()
self.error_subscriber.unregister()
# Stop any current arm movement
self.arm.stop()
# Cancel any active move_base goals
self.move_base.cancel_goal()
rospy.sleep(2)
# Stop the robot from moving
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
# Move the arm to the stow position
self.arm.set_named_target(self.stow_group_state)
self.arm.go()
#Shut down MoveIt! cleanly
rospy.loginfo("Shutting down 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')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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('target_pose', PoseStamped, queue_size=10)
# 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(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
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")
rospy.sleep(1)
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
goal.plan_grasps = False
find_objects.send_goal(goal)
find_objects.wait_for_result(rospy.Duration(5.0))
find_result = find_objects.get_result()
rospy.loginfo("Found %d objects" %len(find_result.objects))
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
self.scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_id = 'target'
target_size = None
the_object = None
the_object_dist = 0.30
the_object_dist_min = 0.10
count = -1
for obj in find_result.objects:
count +=1
dx = obj.object.primitive_poses[0].position.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist and d > the_object_dist_min:
#if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
# if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
rospy.loginfo("object is in the working zone")
the_object_dist = d
the_object = count
target_size = [0.02, 0.02, 0.05]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
target_pose.pose.position.z = 0.04
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
# wait for the scene to sync
self.scene.waitForSync()
self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
self.scene.sendColors()
grasp_pose = target_pose
grasp_pose.pose.position.y -= target_size[1] / 2.0
grasp_pose.pose.position.x += target_size[0]
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
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
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 = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
arm.set_named_target('right_up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
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)
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):
# 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 placeSrvCallback(self, req):
rospy.loginfo("Received the service call!")
rospy.loginfo(req)
temp_dbdata = Tmsdb()
target = Tmsdb()
temp_dbdata.id = req.object_id
rospy.wait_for_service('tms_db_reader')
try:
tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData)
res = tms_db_reader(temp_dbdata)
target = res.tmsdb[0]
except rospy.ServiceException as e:
print "Service call failed: %s" % e
self.shutdown()
print(target.name)
scene = PlanningSceneInterface()
# 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.1)
arm.set_goal_orientation_tolerance(0.3)
# 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)
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(0.05)
target_id = str(req.object_id)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = req.x
target_pose.pose.position.y = req.y
target_pose.pose.position.z = req.z
# q = quaternion_from_euler(target.rr, target.rp, target.ry)
q = quaternion_from_euler(req.roll, req.pitch, req.yaw)
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]
print(target_pose.pose.position.x)
print(target_pose.pose.position.y)
print(target_pose.pose.position.z)
print(target_pose.pose.orientation.x)
print(target_pose.pose.orientation.y)
print(target_pose.pose.orientation.z)
print(target_pose.pose.orientation.w)
# Initialize the grasp pose to the target pose
place_pose = target_pose
# Generate a list of grasps
places = self.make_places(place_pose)
result = None
n_attempts = 0
# 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 = arm.place(target_id, place)
print(result)
if result == MoveItErrorCodes.SUCCESS:
break
# rospy.sleep(0.2)
scene.remove_world_object(str(req.object_id))
ret = rp_placeResponse()
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success the place operation")
ret.result = True
else:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
ret.result = False
return ret
class PickAndPlace:
def setColor(self,name,r,g,b,a=0.9):
color=ObjectColor()
color.id=name
color.color.r=r
color.color.g=g
color.color.b=b
color.color.a=a
self.colors[name]=color
def sendColors(self):
p=PlanningScene()
p.is_diff=True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self,traj, time, positions, velocities=None):
point= trajectory_msgs.msg.JointTrajectoryPoint()
point.positions= copy.deepcopy(positions)
if velocities is not None:
point.velocities= copy.deepcopy(velocities)
point.time_from_start= rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self,q_traj, t_traj):
assert(len(q_traj)==len(t_traj))
#Insert current position to beginning.
if t_traj[0]>1.0e-2:
t_traj.insert(0,0.0)
q_traj.insert(0,self.Q(arm=arm))
self.dq_traj=self.QTrajToDQTraj(q_traj, t_traj)
def QTrajToDQTraj(self,q_traj, t_traj):
dof= len(q_traj[0])
#Modeling the trajectory with spline.
splines= [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)]
splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj= []
for t in t_traj:
dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self,q,t,dq=None):
jp= trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions= q
jp.time_from_start= rospy.Duration(t)
if dq is not None: jp.velocities= dq
return jp
def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None):
assert(len(q_traj)==len(t_traj))
if dq_traj is not None: (len(dq_traj)==len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names= joint_names
if dq_traj is not None:
self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)]
else:
self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)]
self.traj.header.stamp= rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self,q_traj):
if len(q_traj)==0: return
q_prev= np.array(q_traj[0])
q_offset= np.array([0]*len(q_prev))
for q in q_traj:
q_diff= np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d]<-math.pi: q_offset[d]+=1
elif q_diff[d]>math.pi: q_offset[d]-=1
q_prev= copy.deepcopy(q)
q[:]= q+q_offset*2.0*math.pi
def add_target(self,target_pose,target_size,frame,x,y,o1,o2,o3,o4):
target_pose.header.frame_id=frame
target_pose.pose.position.x=x
target_pose.pose.position.y=y
target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=o1
target_pose.pose.orientation.y=o2
target_pose.pose.orientation.z=o3
target_pose.pose.orientation.w=o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self,start_pose,end_pose,maxtries,exe_signal=False):
waypoints=[]
fraction=0.0
attempts=0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction!=1 and attempts<maxtries:
(plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True)
attempts+=1
if (attempts %maxtries==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
return 0,0
continue
elif fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.")
if exe_signal:
q_traj=[self.arm.get_current_joint_values()]
t_traj=[0.0]
for i in range(2,len(plan.joint_trajectory.points)):
q_traj.append(plan.joint_trajectory.points[i].positions)
t_traj.append(t_traj[-1]+0.08)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(5)
end_joint_state=plan.joint_trajectory.points[-1].positions
signal=1
return signal,end_joint_state
#move and rotate
def cts_rotate(self,start_pose,end_pose,angle_r,maxtries,exe_signal=False):
angle=angle_r*3.14/180.0
waypoints=[]
fraction=0.0
attempts=0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction!=1 and attempts<maxtries:
(plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True)
attempts+=1
if (attempts %maxtries==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
return 0,0.0
continue
elif fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.")
if exe_signal:
q_traj=[self.arm.get_current_joint_values()]
t_traj=[0.0]
per_angle=angle/(len(plan.joint_trajectory.points)-2)
for i in range(2,len(plan.joint_trajectory.points)):
joint_inc_list = [j for j in plan.joint_trajectory.points[i].positions]
#plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
joint_inc_list[6]-=per_angle*(i-1)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1]+0.05)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(3.5)
end_joint_state=plan.joint_trajectory.points[-1].positions
signal=1
return signal,end_joint_state
def cts_rotate_delta(self,start_pose,end_pose,angle_pre,angle_r,maxtries,exe_signal=False):
angle=(angle_r-angle_pre)*3.14/180.0
waypoints=[]
fraction=0.0
attempts=0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction!=1 and attempts<maxtries:
(plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True)
attempts+=1
if (attempts %maxtries==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
return 0,0.0
continue
elif fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.")
if exe_signal:
end_joint_state=plan.joint_trajectory.points[-1].positions
q_traj=[self.arm.get_current_joint_values()]
t_traj=[0.0]
per_angle=angle/(len(plan.joint_trajectory.points)-2)
for i in range(2,len(plan.joint_trajectory.points)):
joint_inc_list = [j for j in plan.joint_trajectory.points[i].positions]
#~ plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
#~ if i==len(plan.joint_trajectory.points)-1:
#~ joint_inc_list[6]-=angle
#~ q_traj.append(joint_inc_list)
#~ t_traj.append(1.5)
joint_inc_list[6]-=per_angle*(i-1)+(angle_pre*3.14/180.0)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1]+0.1)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(5)
signal=1
return signal,end_joint_state
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self,initial_state,end_joint_state_f,end_joint_state_b,freq,times):
q_traj=[initial_state]
t_traj=[0.0]
for i in range(times):
q_traj.append(end_joint_state_f)
t_traj.append(t_traj[-1]+0.5/freq)
q_traj.append(end_joint_state_b)
t_traj.append(t_traj[-1]+0.5/freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1]+0.5/freq)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(5)
def shake_a(self,pre_p_angle,r_angle,amp):
start_shake_pose=self.arm.get_current_pose(self.arm_end_effector_link)# for trajectory of shaking
start_joint_state=self.arm.get_current_joint_values() # for state[6] to rotate
shift_pose=deepcopy(start_shake_pose)
r_angle=(r_angle/180.0)*3.14
pre_p_angle=(pre_p_angle/180.0)*3.14
shift_pose.pose.position.x+=amp*math.sin(r_angle)*math.cos(pre_p_angle)# in verticle direction
shift_pose.pose.position.y-=amp*math.sin(r_angle)*math.sin(pre_p_angle)
shift_pose.pose.position.z+=amp*math.cos(r_angle)#...
signal,end_joint_state=self.cts(start_shake_pose,shift_pose,300)
return signal,end_joint_state
def shake_axis(self,pre_p_angle,r_angle,amp,axis_angle):
start_shake_pose=self.arm.get_current_pose(self.arm_end_effector_link)# for trajectory of shaking
start_joint_state=self.arm.get_current_joint_values() # for state[6] to rotate
shift_pose_b=deepcopy(start_shake_pose)
shift_pose_f=deepcopy(start_shake_pose)
r_angle=(r_angle/180.0)*3.14
axis_angle=(axis_angle/180.0)*3.14
pre_p_angle=(pre_p_angle/180.0)*3.14
shift_pose_b.pose.position.x-=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.cos(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.cos(pre_p_angle)
shift_pose_b.pose.position.y+=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.sin(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.sin(pre_p_angle)
shift_pose_b.pose.position.z+=0.5*amp*math.sin(axis_angle)*math.sin(r_angle)+0.5*amp*math.cos(axis_angle)*math.cos(r_angle)#...
signal,end_joint_state_1=self.cts(start_shake_pose,shift_pose_b,300)
shift_pose_f.pose.position.x+=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.cos(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.cos(pre_p_angle)
shift_pose_f.pose.position.y-=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.sin(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.sin(pre_p_angle)
shift_pose_f.pose.position.z-=0.5*amp*math.sin(axis_angle)*math.sin(r_angle)+0.5*amp*math.cos(axis_angle)*math.cos(r_angle)#...
signal,end_joint_state_2=self.cts(start_shake_pose,shift_pose_f,300)
return signal,end_joint_state_1,end_joint_state_2
def setupSence(self):
r_tool_size=[0.03,0.02,0.18]
l_tool_size=[0.03,0.02,0.18]
#real scene table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=-0.052
table_pose.pose.position.y=0.65
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=self.arm_end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.057
l_p.pose.position.z=0.09
l_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=self.arm_end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.057
r_p.pose.position.z=0.09
r_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size)
#Params: pose of bottle, grasp_radius, grasp_height, grasp_theta
def get_grasp_pose(self,pose,r,theta):
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=pose.pose.position.x+r*math.sin(theta)
g_p.pose.position.y=pose.pose.position.y-r*math.cos(theta)
g_p.pose.position.z=pose.pose.position.z
g_p.pose.orientation.w=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta))
g_p.pose.orientation.x=-0.5*(math.cos(0.5*theta)+math.sin(0.5*theta))
g_p.pose.orientation.y=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta))
g_p.pose.orientation.z=0.5*(math.sin(0.5*theta)+math.cos(0.5*theta))
return g_p
def get_pour_pose(self,pose,h,r,theta):
p_p=PoseStamped()
p_p.header.frame_id=self.arm_end_effector_link
p_p.pose.position.x=pose.pose.position.x-r*math.cos(theta)
p_p.pose.position.y=pose.pose.position.y+r*math.sin(theta)
p_p.pose.position.z=pose.pose.position.z+h
theta*=-1
p_p.pose.orientation.w=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta))
p_p.pose.orientation.x=-0.5*(math.cos(0.5*theta)+math.sin(0.5*theta))
p_p.pose.orientation.y=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta))
p_p.pose.orientation.z=0.5*(math.sin(0.5*theta)+math.cos(0.5*theta))
return p_p
def pour_rotate(self,initial_pose,angle_pre,angle_r,r,maxtries):
angle_pre=(angle_pre/180.0)*3.14
angle_r_1=(angle_r/180.0)*3.14
cur_pose=self.arm.get_current_pose(self.arm_end_effector_link)
final_pose=deepcopy(initial_pose)
final_pose.pose.position.x+=r*(1-math.cos(angle_r_1))*math.cos(angle_pre)
final_pose.pose.position.y+=r*(1-math.cos(angle_r_1))*math.sin(angle_pre)
final_pose.pose.position.z+=r*math.sin(angle_r_1)
#~ signal,e_j_s=self.cts_rotate_delta(cur_pose,final_pose,angle_r_pre,angle_r,maxtries,True)
signal,e_j_s=self.cts_rotate(cur_pose,final_pose,angle_r,maxtries,True)
return signal
def p_r_trail(self,angle_pre,angle_r,r,maxtries):
angle_pre=(angle_pre/180.0)*3.14
angle_r_1=(angle_r/180.0)*3.14
initial_pose=self.arm.get_current_pose(self.arm_end_effector_link)
final_pose=deepcopy(initial_pose)
final_pose.pose.position.x+=r*(1-math.cos(angle_r_1))*math.cos(angle_pre)
final_pose.pose.position.y+=r*(1-math.cos(angle_r_1))*math.sin(angle_pre)
final_pose.pose.position.z+=r*math.sin(angle_r_1)
signal,e_j_s=self.cts_rotate(initial_pose,final_pose,angle_r,maxtries,True)
return signal
def move_back(self,back_pose):
current_pose=self.arm.get_current_pose(self.arm_end_effector_link)
signal,end_j=self.cts(current_pose,back_pose,300,True)
return signal
def pg_g_pp(self,pose,r):
start_pose=self.arm.get_current_pose(self.arm_end_effector_link)
p_i_radian=np.arctan(abs(pose.pose.position.x/pose.pose.position.y))
p_i_angle=p_i_radian*180.0/3.14
pick_list=[p_i_angle,5.0,25.0,45.0,65.0,15.0,35.0,55.0,75.0,10.0,20.0,30.0,40.0,50.0,60.0]
for i in range(len(pick_list)):
theta=(pick_list[i]/180.0)*3.14
if pose.pose.position.x>0:
theta*=-1.0
grasp_pose=self.get_grasp_pose(pose,r,theta)
#pick up
signal,e_j_s=self.cts(start_pose,grasp_pose,300,True)
if signal==0: continue
break
rospy.sleep(1)
self.gripperCtrl(0)
rospy.sleep(2)
#move to ori_pose
current_pose=self.arm.get_current_pose(self.arm_end_effector_link)
signal,e_j_s=self.cts(current_pose,start_pose,300,True)
rospy.sleep(2)
rospy.loginfo("Grasping done.")
return start_pose,grasp_pose
def pp_ps_old(self,target_pose,pour_angle,r_pour,h_pour):
for i in range(len(pour_angle)):
maxtries=300
start_pose=self.arm.get_current_pose(self.arm_end_effector_link)
theta=(pour_angle[i]/180.0)*3.14
pour_pose=self.get_pour_pose(target_pose,h_pour,r_pour,theta)
#move to pose
signal_1,e_j_s=self.cts_rotate(start_pose,pour_pose,90.0,maxtries,True)
if signal_1==0: continue
pre_pour_angle=pour_angle[i]
rospy.loginfo("Ready for pouring.")
return pre_pour_angle
def pp_ps(self,target_pose,pour_angle,r_pour,h_pour):
maxtries=300
start_pose=self.arm.get_current_pose(self.arm_end_effector_link)
theta=(pour_angle/180.0)*3.14
pour_pose=self.get_pour_pose(target_pose,h_pour,r_pour,theta)
#move to pose
signal_1,e_j_s=self.cts_rotate(start_pose,pour_pose,90.0,maxtries,True)
return signal_1
def go_back(self,ori_pose,pre_grasp_pose):
cur_pose=self.arm.get_current_pose(self.arm_end_effector_link)
signal,e1=self.cts(cur_pose,ori_pose,300,True)
rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
rospy.sleep(1)
signal_1,e2=self.cts(ori_pose,pre_grasp_pose,300,True)
rospy.loginfo("back to grasp pose, open gripper..")
rospy.sleep(1)
self.gripperCtrl(255)
rospy.sleep(1)
signal_2,e3=self.cts(pre_grasp_pose,ori_pose,300,True)
rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")
def rotate_back(self,joint_state):
q_traj=[self.arm.get_current_joint_values()]
t_traj=[0.0]
q_traj.append(joint_state)
t_traj.append(t_traj[-1]+3)
self.FollowQTraj(q_traj,t_traj)
self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj))
rospy.sleep(3)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
self.colors=dict()
rospy.sleep(1)
self.robot=moveit_commander.robot.RobotCommander()
self.arm=MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian=rospy.get_param('~cartesian',True)
self.arm_end_effector_link=self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame='base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names= [[]]
self.joint_names[0]= rospy.get_param('controller_joint_names')
self.traj= trajectory_msgs.msg.JointTrajectory()
self.sub_jpc= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory,queue_size=10)
#scene planning
self.l_id='l_tool'
self.r_id='r_tool'
self.table_id='table'
self.target1_id='target1_object'
self.target2_id='target2_object'
#~ self.target3_id='target3_object'
#~ self.target4_id='target4_object'
self.f_target_id='receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
#~ self.scene.remove_world_object(self.target3_id)
#~ self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground=0.13
self.table_size=[0.9,0.6,0.018]
self.setupSence()
target1_size=[0.035,0.035,0.19]
target2_size=target1_size
#~ target3_size=target1_size
#~ target4_size=target1_size
self.f_target_size=[0.2,0.2,0.04]
f_target_pose=PoseStamped()
pre_pour_pose=PoseStamped()
target1_pose=PoseStamped()
target2_pose=PoseStamped()
#~ target3_pose=PoseStamped()
#~ target4_pose=PoseStamped()
joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
joint_names= rospy.get_param('controller_joint_names')
traj= trajectory_msgs.msg.JointTrajectory()
traj.joint_names= joint_names
#final target
l_gripper=0.18
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
#~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
#~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
msg1 = rospy.wait_for_message('/aruco_simple/pose',Pose)
rospy.sleep(1)
msg2 = rospy.wait_for_message('/aruco_simple/pose2',Pose)
rospy.sleep(1)
self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.1-l_gripper,0,0,0,1)
self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
self.add_target(target1_pose,target1_size,self.reference_frame,-msg1.position.y-0.257,-msg1.position.x+0.81-0.065,0,0,0,1)
self.scene.add_box(self.target1_id,target1_pose,target1_size)
#~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
#~ self.scene.add_box(self.target2_id,target2_pose,target2_size)
#~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
#~ self.scene.add_box(self.target3_id,target3_pose,target3_size)
#~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
#~ self.scene.add_box(self.target4_id,target4_pose,target4_size)
#attach_pose
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y=-0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
#set color
self.setColor(self.target1_id,0.8,0,0,1.0)
self.setColor(self.target2_id,0.8,0,0,1.0)
#~ self.setColor(self.target3_id,0.8,0,0,1.0)
#self.setColor(self.target4_id,0.8,0,0,1.0)
self.setColor(self.f_target_id,0.8,0.3,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state=[-1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
tar_num=1
maxtries=300
r_grasp=0.16
#~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
#~ save_data=["carrot.txt","pepper.txt","cucumber.txt"]
#ratio_table=[0.03,0.025,0.04]
for i in range(0,tar_num):
#grasp target
rospy.loginfo("Choosing source...")
if i==0:
target_pose=target1_pose
target_id=self.target1_id
target_size=target1_size
#~ amp=0.13
#~ freq=2.75
#~ r_angle=40.0
elif i==1:
target_pose=target2_pose
target_id=self.target2_id
target_size=target2_size
#~ amp=0.15
#~ freq=2.5
#~ r_angle=45.0
elif i==2:
target_pose=target3_pose
target_id=self.target3_id
target_size=target3_size
amp=0.15
freq=2.75
r_angle=40.0
elif i==3:
target_pose=target4_pose
target_id=self.target4_id
target_size=target4_size
amp=0.1
freq=2.75
r_angle=45.0
r_pour=0.14
r_bottle=0.1
#~ h_pour=0.1
pour_angle=[0.0,-15.0,15.0,-30.0,30.0,-45.0,45.0]
h_pour_list=[0.12]
#~ tip_angle=[30.0,40.0,50,60.0,70.0,80.0]
tip_angle=[30.0,40.0,50.0]
s_axis=[0.0]
shake_freq=[2.5,2.75]
#~ shake_freq=[2.50,2.75]
shake_amp=[0.15,0.18]
shake_per_times=4
shake_times_tgt=2
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: "+str(i+1)+" ")
#grasp and back
ori_pose,g_pose=self.pg_g_pp(target_pose,r_grasp)
ori_joint_state=self.arm.get_current_joint_values()
#move to target position for pouring
#~ for m in range(len(pour_angle)):
for m in range(len(h_pour_list)):
dfile=open("moyashi_0_shake_1","a")
#~ for x in range(len(s_axis)):
for x in range(len(s_axis)):
for j in range(len(tip_angle)):
#~ for x in range(len(s_axis)):
for k in range(len(shake_freq)):
#~ dfile=open("carrot_h_"+str(h_pour_list[m])+"_ta_"+str(tip_angle[j])+"_axis_"+str(s_axis[x])"_shaking_freq_"+str(shake_freq[k])+"_1","a")
for n in range(len(shake_amp)):
pp_ps_sgn=self.pp_ps(f_target_pose,pour_angle[0],r_pour,h_pour_list[m])
if pp_ps_sgn==0:
rospy.loginfo("pre-Pouring angle not correct, back for replanning...")
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
initial_pose=self.arm.get_current_pose(self.arm_end_effector_link)
pour_signal=self.pour_rotate(initial_pose,pour_angle[0],tip_angle[j],r_bottle,maxtries)
rospy.sleep(2)
if pour_signal !=1:
rospy.loginfo("Pouring angle not correct, back for replanning...")
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
back_signal=0
ratio=[0]
ratio_signal=0
shake_times=0
for n_shake in range(10):
#~ for x in range(len(s_axis)):
if back_signal==1:
pp_ps_sgn=self.pp_ps(f_target_pose,pour_angle[0],r_pour,h_pour_list[m])
if pp_ps_sgn==0:
rospy.loginfo("pre-Pouring angle not correct, back for replanning...")
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
initial_pose=self.arm.get_current_pose(self.arm_end_effector_link)
pour_signal=self.pour_rotate(initial_pose,pour_angle[0],tip_angle[j],r_bottle,maxtries)
rospy.sleep(2)
if pour_signal !=1:
rospy.loginfo("Pouring angle not correct, back for replanning...")
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
back_signal=0
amp=shake_amp[n]
freq=shake_freq[k]
shake_times+=1
rospy.loginfo("shaking degree : "+str(tip_angle[j])+", axis degree: "+str(s_axis[x])+", shaking amp : "+str(amp)+ ", shaking freq : "+str(freq)+", times :"+str(shake_times))
signal=True
start_joint_state=self.arm.get_current_joint_values()
signal,end_joint_state_b,end_joint_state_f=self.shake_axis(pour_angle[0],tip_angle[j],amp,s_axis[x])
self.shaking(start_joint_state,end_joint_state_f,end_joint_state_b,freq,shake_per_times)
rospy.sleep(0.5)
area_ratio= rospy.wait_for_message('/ratio_carrot',Float64)
dfile.write("%f %f %f %f %f %f \r\n" % (h_pour_list[m],tip_angle[j],s_axis[x],shake_freq[k],shake_amp[n],area_ratio.data))
#~ ratio.append(area_ratio)
if (area_ratio.data-ratio[-1]<0.01):
ratio_signal+=1
if ratio_signal==2:
ratio_signal=0
self.rotate_back(ori_joint_state)
back_signal=1
rospy.sleep(1)
ratio.append(area_ratio.data)
self.rotate_back(ori_joint_state)
rospy.sleep(3)
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
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')
# 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
right_arm = MoveGroupCommander('arm')
# 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
right_arm.set_named_target('contract')
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.4
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.40
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.40
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()
rospy.sleep(2)
# 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.40
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.08
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('contract')
right_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('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)
print("====== move plan go to initial pose ======")
self.initialPose.header.frame_id = 'leftee_link'
self.initialPose.pose.position.x = 0.103190426853 # red line 0.2 0.2
self.initialPose.pose.position.y = -0.827689781681 # green line 0.15 0.15
self.initialPose.pose.position.z = 0.978219026059 # blue line # 0.35 0.6
# Pose_goal.pose.orientation = start_pose.orientation
self.initialPose.pose.orientation.x =-0.556104592788
self.initialPose.pose.orientation.y = 0.448401851544
self.initialPose.pose.orientation.z = -0.495185387101
self.initialPose.pose.orientation.w = 0.494444024955
self.start_goal = self.robot_arm.get_current_pose("leftee_link")
self.Pose_goal = PoseStamped()
t = rospy.Time(0)
self.listener.waitForTransform('/leftbase_link','/world',t,rospy.Duration(5))
if self.listener.canTransform('/leftbase_link','/world',t):
self.Pose_goal = self.listener.transformPose('/leftbase_link',self.start_goal)
print(self.Pose_goal)
else:
rospy.logerr('Transformation is not possible!')
sys.exit(0)
print("Start pose:%s",self.Pose_goal)
self.listener.waitForTransform('/leftbase_link','/kinect2_rgb_optical_frame',t,rospy.Duration(2))
self.waypoints.append(self.Pose_goal.pose)
# initialJointValues = [4.111435176058169,
# 2.715653621728593,
# 0.7256647920137681,
# 5.983459446005512,
# -5.682231515319553,
# -6.283185179581844]
# self.robot_arm.set_joint_value_target(initialJointValues)
# plan = self.robot_arm.plan()
# self.robot_arm.execute(plan)
# rospy.sleep(3) # wait for robot to move to initial position
#
# # save these positions on run
# self.robot_initialPos = Pose().position
# self.robot_initialPos.x = self.robot_arm.get_current_pose().pose.position.x
# self.robot_initialPos.y = self.robot_arm.get_current_pose().pose.position.z
# self.robot_initialPos.z = self.robot_arm.get_current_pose().pose.position.y
#
# self.maxLeft = self.robot_initialPos.x - 0.200 # x left
# self.maxRight = self.robot_initialPos.x + 0.200 # x right
# self.maxHeight = self.robot_initialPos.y + 0.300 # y height
# self.maxUp = self.robot_initialPos.z + 0.100 # z up
# self.maxDown = self.robot_initialPos.z - 0.100 # z down
#
# 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)
#
# self.index_tip_variation = Point()
# self.indextip = Point()
# self.executing = False
def move_code(self):
# self.robot_arm.set_pose_reference_frame('leftbase_link')
# self.wpose = copy.deepcopy(self.Pose_goal.pose)
# # self.wpose.position.x = 0.22844899063259538
# # self.wpose.position.y = 0.3232940134968078
# self.wpose.position.z += 0.12581195793171412
# self.waypoints.append(copy.deepcopy(self.wpose))
# wpose.position.x = 0.24706361090659945
# wpose.position.y = 0.399076783202562
# wpose.position.z = 0.11524980903243331
# self.waypoints.append(copy.deepcopy(wpose))
# wpose.position.x = 0.2384431148207265
# wpose.position.y = 0.42584037071081504
# wpose.position.z = 0.11603463304830763
# self.waypoints.append(copy.deepcopy(wpose))
# fraction = 0.0
# attempts = 0
# self.robot_arm.set_start_state_to_current_state()
# while fraction < 1.0 and attempts < 100:
# (plan, fraction) = self.robot_arm.compute_cartesian_path(
# self.waypoints, # waypoints to follow
# 0.01, # eef_step
# 0.0) # jump_threshold
# attempts+=1
# print(attempts)
# if fraction == 1.0:
# self.robot_arm.execute(plan, wait=True)
# else:
# print("failed")
self.pose_goal1 = PoseStamped()
self.pose_goal1.header.frame_id = '/leftbase_link'
self.pose_goal1.pose.position.x = 0.22844899063259538
self.pose_goal1.pose.position.y = 0.3232940134968078
self.pose_goal1.pose.position.z = 0.12581195793171412
self.pose_goal1.pose.orientation.x = 0.651452360152262
self.pose_goal1.pose.orientation.y = -0.22289710950191718
self.pose_goal1.pose.orientation.z = 0.29974203617303874
self.pose_goal1.pose.orientation.w = 0.6603646059402177
self.robot_arm.set_pose_target(self.pose_goal1,"leftee_link")
self.robot_arm.go()
# rospy.sleep(1)
# traj = self.robot_arm.plan()
# self.robot_arm.execute(traj,wait=True)
self.pose_goal2 = PoseStamped()
self.pose_goal2.header.frame_id = '/leftbase_link'
self.pose_goal2.pose.position.x = 0.24706361090659945
self.pose_goal2.pose.position.y = 0.399076783202562
self.pose_goal2.pose.position.z = 0.11524980903243331
self.pose_goal2.pose.orientation.x = 0.6965603247710086
self.pose_goal2.pose.orientation.y = -0.3587025266077312
self.pose_goal2.pose.orientation.z = 0.24958274959805474
self.pose_goal2.pose.orientation.w = 0.5690735123542583
self.robot_arm.set_pose_target(self.pose_goal2,"leftee_link")
self.robot_arm.go()
# rospy.sleep(1)
# traj = self.robot_arm.plan()
# self.robot_arm.execute(traj,wait=True)
self.pose_goal3 = PoseStamped()
self.pose_goal3.header.frame_id = '/leftbase_link'
self.pose_goal3.pose.position.x = 0.2384431148207265
self.pose_goal3.pose.position.y = 0.42584037071081504
self.pose_goal3.pose.position.z = 0.11603463304830763
self.pose_goal3.pose.orientation.x = 0.6611332801180229
self.pose_goal3.pose.orientation.y = -0.25768212027385673
self.pose_goal3.pose.orientation.z = 0.31112563888658357
self.pose_goal3.pose.orientation.w = 0.6322211224239263
self.robot_arm.set_pose_target(self.pose_goal3,"leftee_link")
self.robot_arm.go()
# rospy.sleep(1)
# traj = self.robot_arm.plan()
# self.robot_arm.execute(traj,wait=True)
self.pose_goal4 = PoseStamped()
self.pose_goal4.header.frame_id = '/leftbase_link'
self.pose_goal4.pose.position.x = 0.20267968408694
self.pose_goal4.pose.position.y = 0.3807708740746333
self.pose_goal4.pose.position.z = 0.1621807421476515
self.pose_goal4.pose.orientation.x = 0.6500893824643447
self.pose_goal4.pose.orientation.y = -0.17866011127418618
self.pose_goal4.pose.orientation.z = 0.19968845817208114
self.pose_goal4.pose.orientation.w = 0.71104773336217
self.robot_arm.set_pose_target(self.pose_goal4,"leftee_link")
traj = self.robot_arm.plan()
self.robot_arm.execute(traj,wait=True)
self.pose_goal5 = PoseStamped()
self.pose_goal5.header.frame_id = '/leftbase_link'
self.pose_goal5.pose.position.x = 0.19261972875892824
self.pose_goal5.pose.position.y = 0.3487254051054294
self.pose_goal5.pose.position.z = 0.17423391692094703
self.pose_goal5.pose.orientation.x = 0.6623443436827807
self.pose_goal5.pose.orientation.y = -0.20253221354720935
self.pose_goal5.pose.orientation.z = 0.17253708109857815
self.pose_goal5.pose.orientation.w = 0.7003653535927353
self.robot_arm.set_pose_target(self.pose_goal5,"leftee_link")
traj = self.robot_arm.plan()
self.robot_arm.execute(traj,wait=True)
self.pose_goal6 = PoseStamped()
self.pose_goal6.header.frame_id = '/leftbase_link'
self.pose_goal6.pose.position.x = 0.2157980659367808
self.pose_goal6.pose.position.y = 0.36988878402044156
self.pose_goal6.pose.position.z = 0.1431730427943465
self.pose_goal6.pose.orientation.x = 0.6607398125620346
self.pose_goal6.pose.orientation.y = -0.24398334555829662
self.pose_goal6.pose.orientation.z = 0.2786725065244132
self.pose_goal6.pose.orientation.w = 0.6528680274703835
self.robot_arm.set_pose_target(self.pose_goal6,"leftee_link")
traj = self.robot_arm.plan()
self.robot_arm.execute(traj,wait=True)
self.pose_goal7 = PoseStamped()
self.pose_goal7.header.frame_id = '/leftbase_link'
self.pose_goal7.pose.position.x = 0.22114246898738382
self.pose_goal7.pose.position.y = 0.4464193752676767
self.pose_goal7.pose.position.z = 0.13641486607992853
self.pose_goal7.pose.orientation.x = 0.6791504338035828
self.pose_goal7.pose.orientation.y = -0.2962309634157678
self.pose_goal7.pose.orientation.z = 0.2696628694935311
self.pose_goal7.pose.orientation.w = 0.6150478366718206
self.robot_arm.set_pose_target(self.pose_goal7,"leftee_link")
traj = self.robot_arm.plan()
self.robot_arm.execute(traj,wait=True)
self.pose_goal8 = PoseStamped()
self.pose_goal8.header.frame_id = '/leftbase_link'
self.pose_goal8.pose.position.x = 0.2045166261139516
self.pose_goal8.pose.position.y = 0.3927153592410454
self.pose_goal8.pose.position.z = 0.15411342380507587
self.pose_goal8.pose.orientation.x = 0.6693368542084698
self.pose_goal8.pose.orientation.y = -0.2647950292869915
self.pose_goal8.pose.orientation.z = 0.2573395449506767
self.pose_goal8.pose.orientation.w = 0.6447077839360945
self.robot_arm.set_pose_target(self.pose_goal8,"leftee_link")
traj = self.robot_arm.plan()
self.robot_arm.execute(traj,wait=True)
self.pose_goal9 = PoseStamped()
self.pose_goal9.header.frame_id = '/leftbase_link'
self.pose_goal9.pose.position.x = 0.2213576147649789
self.pose_goal9.pose.position.y = 0.32543277234322004
self.pose_goal9.pose.position.z = 0.1435574570513627
self.pose_goal9.pose.orientation.x = 0.6325814784326014
self.pose_goal9.pose.orientation.y = -0.24063914058631417
self.pose_goal9.pose.orientation.z = 0.38283744641817435
self.pose_goal9.pose.orientation.w = 0.6287837201947233
self.robot_arm.set_pose_target(self.pose_goal9,"leftee_link")
traj = self.robot_arm.plan()
self.robot_arm.execute(traj,wait=True)
# self.pose_goal1 = PoseStamped()
# self.pose_goal1.header.frame_id = '/leftbase_link'
# self.pose_goal1.pose.position.x = 0.2053894995328921
# self.pose_goal1.pose.position.y = 0.35793405277574963
# self.pose_goal1.pose.position.z = 0.15759221743998164
# self.pose_goal1.pose.orientation.x = 0.6896423630881793
# self.pose_goal1.pose.orientation.y = -0.3133288084434257
# self.pose_goal1.pose.orientation.z = 0.1543914767126632
# self.pose_goal1.pose.orientation.w = 0.6343356688316201
# self.robot_arm.set_pose_target(self.pose_goal1,"leftee_link")
# # traj = self.robot_arm.plan()
# # self.robot_arm.execute(traj,wait=True)
# self.robot_arm.go()
# # rospy.sleep(1)
def back_initailPose(self):
self.robot_arm.set_pose_target(self.Pose_goal,"leftee_link")
self.robot_arm.go()
rospy.sleep(5)
def callback_left(self, data):
print("left===========")
self.next_pose_goal = data
rospy.loginfo(rospy.get_name() + ": kinect data %s" % data)
# self.move_code()
def callback_right(self, data):
print("right===========")
rospy.loginfo("I heard: [%s]" % data)
self.back_initailPose()
def kinectlistener(self):
# rospy.Subscriber("leapmotion/raw", leap, callback)
rospy.Subscriber("/kinect2/click_point/left", PoseStamped, self.callback_left)
rospy.Subscriber("/kinect2/click_point/right", PoseStamped, self.callback_right)
rospy.spin()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端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)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('target')
# 设置桌面的高度
table_ground = 0.6
# 设置table和tool的三维尺寸
table_size = [0.1, 0.7, 0.01]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = -0.015
p.pose.position.z = 0.0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.25
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)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
0.827228546495185, 0.29496592875743577, 1.1185644936946095,
-0.7987583317769674, -0.18950024740190782, 0.11752152218233858
]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class MoveItObstaclesDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('manipulator')
# 设置机械臂工作空间
self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100])
# 设置机械臂最大速度
self.arm.set_max_velocity_scaling_factor(value=0.1)
# 获取终端link的名称
self.end_effector_link = self.arm.get_end_effector_link()
rospy.loginfo('end effector link: {}'.format(self.end_effector_link))
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
# self.arm.allow_replanning(True)
self.arm.set_num_planning_attempts(10)
# self.arm.allow_looking(True)
# 设置目标位置所使用的参考坐标系
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(3)
# 控制机械臂先回到初始化位置
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(2)
def planning(self, start_point, end_point):
"""
功能：动态避障
:param start_point: 起始点, type: dict
:param end_point: 终点, type: dict
:return: None
"""
# 移动机械臂到指定位置，并获取当前位姿数据为机械臂运动的起始位姿
if start_point:
self.move_arm(p=start_point)
self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中
# way_points = self.get_way_points(start_point, end_point)
while True:
print('set planner id: RRT')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('up')
self.arm.go()
rospy.sleep(5)
print('set planner id: PRM')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(5)
# -------------------------------------------------------------------
def get_way_points(self, a, b):
way_points = []
# plan 1
self.arm.set_named_target('up')
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
else:
rospy.logerr("Trajectory is empty. Planning false!")
self.arm.clear_pose_targets()
# plan 2
# traj = self.moveit_planning(p=b)
for i, p in enumerate(traj.joint_trajectory.points):
# rospy.loginfo('waypoint: {}'.format(i))
if i%2 == 0:
point = {
'x': p.positions[0],
'y': p.positions[1],
'z': p.positions[2],
'ox': p.positions[3],
'oy': p.positions[4],
'oz': p.positions[5]
}
way_points.append(point)
rospy.loginfo('waypoint: \n {}'.format(way_points))
rospy.loginfo(len(way_points))
return way_points
def moveit_planning(self, p):
"""
使用moveit求解器规划路径
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start moveit planning...')
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.pose.position.x = p['x']
target_pose.pose.position.y = p['y']
target_pose.pose.position.z = p['z']
if 'ox' in p.keys() and p['ox']:
target_pose.pose.orientation.x = p['ox']
if 'oy' in p.keys() and p['oy']:
target_pose.pose.orientation.y = p['oy']
if 'oz' in p.keys() and p['oz']:
target_pose.pose.orientation.z = p['oz']
if 'ow' in p.keys() and p['ow']:
target_pose.pose.orientation.w = p['ow']
# 传入一个PoseStamped
# self.arm.set_pose_target(target_pose, self.end_effector_link)
# 尝试直接传入一个列表
self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link)
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
return traj
else:
rospy.logerr("Trajectory is empty. Planning false!")
def move_arm(self, p):
"""
移动机械臂到p点
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start arm moving...')
traj = self.moveit_planning(p)
self.arm.execute(traj)
# 设置当前位置为起始位置
self.arm.set_start_state_to_current_state()
rospy.sleep(1)
def stop_arm(self):
"""
急停
:return:
"""
pass
def exist_danger_obstacle(self):
"""
环境中是否存在危险的障碍物
:return: True or False
"""
return False
def get_obstacle_octomap(self):
"""
获取环境的octomap信息
:return:
"""
pass
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.01)
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()
print(end_effector_link)
# 控制机械臂先回到初始化位置
# 获取当前位姿数据最为机械臂运动的起始位姿
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
# arm.set_named_target('test5')
# arm.go()
# rospy.sleep(1)
######
marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot)
listener = tf.TransformListener()
r = rospy.Rate(100)
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
x = euler_from_quaternion(rot)
print x
quaternion = quaternion_from_euler(x[0] + 1.5707, x[1], x[2] + 3.14)
y = euler_from_quaternion(quaternion)
print y
arm.set_named_target('test5')
arm.go()
rospy.sleep(1)
start_pose = arm.get_current_pose(end_effector_link).pose
######
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
wpose.orientation.x = quaternion[0]
wpose.orientation.y = quaternion[1]
wpose.orientation.z = quaternion[2]
wpose.orientation.w = quaternion[3]
wpose.position.x = 0.33
wpose.position.y = -0.134
wpose.position.z = 1.0484
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 10 #最大尝试规划次数
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.005, # 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)
# 控制机械臂先回到初始化位置
# 关闭并退出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_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('panda_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 = 'panda_link0'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
#box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
#scene.remove_world_object(box2_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
#arm.set_named_target('home')
#arm.go()
#rospy.sleep(2)
# 设置桌面的高度
table_ground = 0.25
# 设置table、box1和box2的三维尺寸
table_size = [0.4, 1.4, 0.02]
box1_size = [0.1, 0.1, 0.1]
#box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.4
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.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.19
#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)
# 将桌子设置成红色，两个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)
# 将场景中的颜色设置发布
self.sendColors()
# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.2
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
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.4
target_pose2.pose.position.y = -0.25
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose2.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose2, end_effector_link)
arm.go()
rospy.sleep(2)
# 控制机械臂回到初始化位置
#arm.set_named_target('home')
#arm.go()
# 关闭并退出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)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(1)
arm.set_max_velocity_scaling_factor(1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
# 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换
(trans,
rot) = listener.lookupTransform('/base_link', '/iron_block',
rospy.Time(0))
object_detect_x = trans[0]
object_detect_y = trans[1]
# object_detect_z = trans[2]
# if(object_detect_x>0.033 or object_detect_x<0.027):
# # print("detect x error!")
# continue
if (object_detect_y > 0.25 or object_detect_y < 0.2):
# print("detect y error!")
continue
# if(trans[2]>-0.04 or trans[2]<-0.08):
# print("detect z error!")
# continue
print(trans)
# print(rot)
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = object_detect_x
target_pose.pose.position.y = object_detect_y - 0.05
target_pose.pose.position.z = 0.15
target_pose.pose.orientation.x = -rot[0]
target_pose.pose.orientation.y = -rot[1]
target_pose.pose.orientation.z = -rot[2]
target_pose.pose.orientation.w = -rot[3]
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
# 规划运动路径
traj = arm.plan()
num_of_points = len(traj.joint_trajectory.points)
d = rospy.Duration.from_sec(1)
print traj.joint_trajectory.points[num_of_points -
1].time_from_start
for index in range(num_of_points):
traj.joint_trajectory.points[index].time_from_start *= \
d/traj.joint_trajectory.points[num_of_points-1].time_from_start
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
# # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# # 姿态使用四元数描述，基于base_link坐标系
# target_pose = PoseStamped()
# target_pose.header.frame_id = reference_frame
# target_pose.header.stamp = rospy.Time.now()
# target_pose.pose.position.x = object_detect_x
# target_pose.pose.position.y = object_detect_y-0.1
# target_pose.pose.position.z = 0.10
# target_pose.pose.orientation.x = -rot[0]
# target_pose.pose.orientation.y = -rot[1]
# target_pose.pose.orientation.z = -rot[2]
# target_pose.pose.orientation.w = -rot[3]
# # 设置机器臂当前的状态作为运动初始状态
# arm.set_start_state_to_current_state()
# # 设置机械臂终端运动的目标位姿
# arm.set_pose_target(target_pose, end_effector_link)
# # 规划运动路径
# traj = arm.plan()
# num_of_points = len(traj.joint_trajectory.points)
# d = rospy.Duration.from_sec(0.2)
# print traj.joint_trajectory.points[num_of_points-1].time_from_start
# for index in range(num_of_points):
# traj.joint_trajectory.points[index].time_from_start *= \
# d/traj.joint_trajectory.points[num_of_points-1].time_from_start
# # 按照规划的运动路径控制机械臂运动
# arm.execute(traj)
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.y -= 0.3
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 10 #最大尝试规划次数
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.1, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# print plan
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo(
"Path computed successfully. Moving the arm.")
# num_of_points = len(plan.joint_trajectory.points)
# d = rospy.Duration.from_sec(6)
# scale_factor = d/plan.joint_trajectory.points[num_of_points-1].time_from_start
# print scale_factor
# for index in range(num_of_points):
# plan.joint_trajectory.points[index].time_from_start *= scale_factor
# print plan
# Scale the trajectory speed by a factor of 0.25
new_traj = self.scale_trajectory_speed(plan, 0.25)
arm.execute(new_traj)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# #rospy.sleep(1)
# # os.system("python ~/MQTT/ros_gripper_control.py 1100")
# # rospy.sleep(1)
# 回起始点
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# rospy.loginfo("go origin position")
# joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
# # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
# os.system("python ~/MQTT/ros_gripper_control.py 0")
# rospy.sleep(1)
# #回起始点
# joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
rospy.sleep(2)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# listener = tf.TransformListener()
# while not rospy.is_shutdown():
# try:
# # 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换
# (trans,rot) = listener.lookupTransform('/camera_color_optical_frame', '/iron_block', rospy.Time(0))
# object_detect_x = trans[0]
# object_detect_y = trans[1]
# if(object_detect_x>0.033 or object_detect_x<0.027):
# # print("detect x error!")
# continue
# if(object_detect_y>0.040 or object_detect_y<0.020):
# # print("detect y error!")
# continue
# # if(trans[2]>-0.04 or trans[2]<-0.08):
# # print("detect z error!")
# # continue
# print(trans)
# # print(rot)
# # 获取当前位姿数据最为机械臂运动的起始位姿
# start_pose = arm.get_current_pose(end_effector_link).pose
# # 初始化路点列表
# waypoints = []
# # 将初始位姿加入路点列表
# waypoints.append(start_pose)
# wpose = deepcopy(start_pose)
# wpose.position.y -= 0.2
# wpose.position.z -= 0.155
# waypoints.append(deepcopy(wpose))
# wpose.position.y -= 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.001, # eef_step，终端步进值
# 0.0, # jump_threshold，跳跃阈值
# True) # avoid_collisions，避障规划
# # 尝试次数累加
# attempts += 1
# # 打印运动规划进程
# if attempts % 10 == 0:
# rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# #print plan
# # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
# if fraction == 1.0:
# rospy.loginfo("Path computed successfully. Moving the arm.")
# num_of_points = len(plan.joint_trajectory.points)
# d = rospy.Duration.from_sec(3)
# print plan.joint_trajectory.points[num_of_points-1].time_from_start
# for index in range(num_of_points):
# plan.joint_trajectory.points[index].time_from_start *= \
# d/plan.joint_trajectory.points[num_of_points-1].time_from_start
# #print plan
# 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)
# # os.system("python ~/MQTT/ros_gripper_control.py 1100")
# # rospy.sleep(1)
# # 回起始点
# rospy.loginfo("go origin position")
# joint_positions = [-0.007717277486910995, -0.9330994764397906, 0.46196335078534034, 7.155322862129146e-05, -1.08595462478185, -0.007682373472949389]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
# # # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# # joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686]
# # arm.set_joint_value_target(joint_positions)
# # # 控制机械臂完成运动
# # arm.go()
# # os.system("python ~/MQTT/ros_gripper_control.py 0")
# # rospy.sleep(1)
# # #回起始点
# # joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05]
# # arm.set_joint_value_target(joint_positions)
# # # 控制机械臂完成运动
# # arm.go()
# rospy.sleep(2)
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# continue
# 关闭并退出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_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):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.008)
arm.set_goal_orientation_tolerance(0.04)
arm.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
joint_state = [
-2.419833894224669, 0.06734230828523968, -0.5274768816844496,
2.0819618074484882, -0.4270016439669033, -0.47903255506397024,
-2.711311334870862
]
#scene planning
table_id = 'table'
#cylinder_id='cylinder'
#box1_id='box1'
box2_id = 'box2'
target_id = 'target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground = 0.5
table_size = [0.5, 1, 0.01]
#box1_size=[0.1,0.05,0.03]
box2_size = [0.05, 0.05, 0.16]
r_tool_size = [0.03, 0.01, 0.06]
l_tool_size = [0.03, 0.01, 0.06]
target_size = [0.05, 0.05, 0.16]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.75
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.7
box1_pose.pose.position.y=-0.2
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.6
box2_pose.pose.position.y = -0.05
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)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.6
target_pose.pose.position.y = 0.05
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
scene.add_box(target_id, target_pose, target_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.04
l_p.pose.position.z = 0.04
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.04
r_p.pose.position.z = 0.04
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
#grasp
g_p = PoseStamped()
g_p.header.frame_id = end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
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)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor('target_object', 0, 1, 0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
grasp_pose = target_pose
grasp_pose.pose.position.x -= 0.13
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = 0.707
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose, end_effector_link)
#arm.set_joint_value_target(joint_state)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(2)
print arm.get_current_joint_values()
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0, 0.11, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
saved_target_pose = arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link, target_id, g_p, target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2, 0.18, end_effector_link)
arm.go()
rospy.sleep(2)
arm.shift_pose_target(1, -0.2, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(2, -0.18, end_effector_link)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, target_id)
rospy.sleep(2)
#arm.set_pose_target(saved_target_pose,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_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('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
self.trajectory_publisher = rospy.Publisher('/path',
path,
queue_size=20)
self.trajectory = path()
# 等待场景准备就绪
rospy.sleep(1)
robot_cmd = RobotCommander()
# 等待场景准备就绪
rospy.sleep(1)
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端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)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
class MoveItCartesianDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node(NODE_NAME, anonymous=True)
self.pub = rospy.Publisher(ON_OFF_SIGNAL_TOPIC, Bool, queue_size=2)
self.rate = rospy.Rate(2)
# 是否需要使用笛卡尔运动规划
self.cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的self.arm group
self.arm = MoveGroupCommander(ARM_GROUP)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
self.arm.set_pose_reference_frame(REFERENCE_FRAME)
# 获取终端link的名称
end_effector_link = 'link_6'
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置位置（单位：米）和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# 环境建模
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
self.colors = dict()
rospy.sleep(1)
# 场景物体设置
table_id = 'table'
scene.remove_world_object(table_id)
self.table_ground = 0.592 - 0.148
table_size = [0.5, 1.5, 0.04]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.6
table_pose.pose.position.y = -0.5
table_pose.pose.position.z = self.table_ground - table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
self.setColor(table_id, 0, 0.8, 0, 1.0)
self.sendColors()
# rospy.loginfo(self.arm.get_current_pose(END_EFFECTOR_LINK).pose)
# # 控制机械臂先回到初始化位置
# self.arm.set_named_target(ALL_ZERO_POSE)
# self.arm.go()
# # rospy.sleep(1)
# rospy.loginfo("已到达准备点")
# rospy.spin()
# 控制机械臂先回到初始化位置
self.arm.set_named_target(HOME_POSE)
self.arm.go()
# rospy.sleep(1)
rospy.loginfo("已到达准备点")
# rospy.spin()
# 第二段运动：移动到停泊点
anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(anchor_pose)
self.arm.go()
rospy.loginfo("已移动到停泊点")
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
# 提前规划：停泊点位置
self.anchor_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
self.anchor_pose.position.x = 0.7
self.anchor_pose.position.y = 0
self.anchor_pose.position.z = 0.764 # self.table_ground + 0.10 + HEIGHT_OF_END_EFFECTOR
self.anchor_pose.orientation.x = 0
self.anchor_pose.orientation.y = 0.707
self.anchor_pose.orientation.z = 0
self.anchor_pose.orientation.w = 0.707
# 用于确定停泊点的关节角情况
# waypoints = []
# waypoints.append(start_pose)
# waypoints.append(self.anchor_pose)
# traj, fraction = self.cartesian_plan(self.arm, waypoints)
# self.arm.set_start_state_to_current_state()
# if fraction == 1.0:
# self.arm.execute(traj)
# rospy.loginfo("已到达吸取点")
# else:
# rospy.spin()
# def callback(data):
# rospy.loginfo(data)
# rospy.Subscriber('joint_states', JointState, callback)
# rospy.spin()
# 提前规划:中间点位置
self.middle_pose = deepcopy(start_pose)
self.middle_pose.position.x = 0
self.middle_pose.position.y = 0.7
self.middle_pose.position.z = self.anchor_pose.position.z
self.middle_pose.orientation.x = -0.5
self.middle_pose.orientation.y = 0.5
self.middle_pose.orientation.z = 0.5
self.middle_pose.orientation.w = 0.5
# 提前规划:下料点位置
self.place_pose = deepcopy(start_pose)
self.place_pose.position.x = 0
self.place_pose.position.y = 0.7
self.place_pose.position.z = 0.05 + HEIGHT_OF_END_EFFECTOR - 0.14
self.place_pose.orientation.x = -0.5
self.place_pose.orientation.y = 0.5
self.place_pose.orientation.z = 0.5
self.place_pose.orientation.w = 0.5
# 用于确定下料点的关节角情况
# waypoints = []
# waypoints.append(start_pose)
# waypoints.append(self.place_pose)
# traj, fraction = self.cartesian_plan(self.arm, waypoints)
# self.arm.set_start_state_to_current_state()
# if fraction == 1.0:
# self.arm.execute(traj)
# rospy.loginfo("已到达吸取点")
# else:
# rospy.spin()
# def callback(data):
# rospy.loginfo(data)
# rospy.Subscriber('joint_states', JointState, callback)
# rospy.spin()
# 提前规划：释放点位置
self.release_pose = deepcopy(start_pose)
self.release_pose.position.x = 0.7
self.release_pose.position.y = 0
self.release_pose.position.z = 0.704 # self.table_ground + 0.10 + HEIGHT_OF_END_EFFECTOR
self.release_pose.orientation.x = 0
self.release_pose.orientation.y = 0.707
self.release_pose.orientation.z = 0
self.release_pose.orientation.w = 0.707
while not rospy.is_shutdown():
# 开启web端action通讯，等待讯号
self.server = actionlib.SimpleActionServer('abb_grasp',
AbbGraspAction,
self.abb_execute, False)
self.server.start()
rospy.spin()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# action callback function
def abb_execute(self, goal):
# 统计参数，与action有关
self.num = 0 # 计算本轮循环中的件数
self.schedule_percent = AbbGraspFeedback() # 统计进度百分比
self.schedule_result = AbbGraspResult() # 统计完成进度数量
self.goal = goal # 设定目标值
self.item_id = 0 # 初始化计件id值
rate = rospy.Rate(25)
rospy.loginfo("Get message from web socket and action!")
while self.num <= self.goal.grasp_id:
self.num += 1
# service 方法 - 接收视觉处理后的数据
rospy.wait_for_service('abb_visual')
rospy.loginfo("Start to get coorporation data from visual PC...")
try:
rospy.loginfo("Service client Start.")
abb_get_visual_data = rospy.ServiceProxy('abb_visual', AbbPose)
res_coordinate = abb_get_visual_data(GET_DATA_SIGNAL)
rospy.loginfo(res_coordinate)
if (res_coordinate.item_id - self.item_id):
# self.arm.set_start_state_to_current_state()
# 执行函数
self.callback(res_coordinate)
self.item_id = res_coordinate.item_id
except rospy.ServiceException as e:
rospy.loginfo("Service call failed: %s" % e)
self.server.set_succeeded(self.schedule_result)
rospy.loginfo(self.schedule_result)
rospy.loginfo("------------Finished------------")
# 获取视觉处理的坐标位置后的执行函数
def callback(self, data):
# 设置机械臂工作空间中的目标位姿，位置使用x,y,z坐标描述
# 姿态使用四元数描述，基于base_link坐标系
rospy.loginfo("目标点位置：\n")
rospy.loginfo(data)
##################### 即时规划 ######################
# 获取当前位姿数据为停泊点位姿
anchor_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
anchor_pose.orientation.x = 0
anchor_pose.orientation.y = 0.707
anchor_pose.orientation.z = 0
anchor_pose.orientation.w = 0.707
self.anchor_pose = anchor_pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if self.cartesian:
waypoints.append(anchor_pose)
# 初始化路点数据
wpose = deepcopy(anchor_pose)
# wpose.orientation.x = 0
# wpose.orientation.y = 0.707
# wpose.orientation.z = 0
# wpose.orientation.w = 0.707
# 第三段路径：停泊点=》吸取点
rospy.loginfo("规划：从停泊点至吸取点")
wpose.position.x = data.position_x
wpose.position.y = data.position_y
#------------------- !!!注意!!! -------------------#
# if data.orientation_x:
# wpose.position.z = 0.699 # data.position_z # 0.714
# else:
# wpose.position.z = 0.692 # data.position_z # 0.714
wpose.position.z = data.position_z
#------------------- !!!修改!!! -------------------#
rad_x = data.orientation_w
rad_y = 1.5708
rad_z = 0
list_orientation = quaternion_from_euler(rad_x, rad_y, rad_z)
wpose.orientation.x = list_orientation[0]
wpose.orientation.y = list_orientation[1]
wpose.orientation.z = list_orientation[2]
wpose.orientation.w = list_orientation[3]
#------------------- !!!修改!!! -------------------#
if self.cartesian:
# 气泵控制，气阀操作，吸盘抓取
on_off_signal = 1
self.pub.publish(on_off_signal)
rospy.loginfo("Open the gas valve.")
self.rate.sleep()
waypoints.append(wpose)
traj, fraction = self.cartesian_plan(self.arm, waypoints)
self.arm.set_start_state_to_current_state()
if fraction == 1.0:
wait_time = data.time / 1000
rospy.sleep(wait_time)
self.arm.set_start_state_to_current_state()
self.arm.execute(traj)
rospy.loginfo("已到达吸取点")
else:
rospy.loginfo("Anchor_to_Grasp_Path Planning failed with only " +
str(fraction) + "success after " + str(100) +
" attempts.")
# def callback(data):
# rospy.loginfo(data)
# rospy.Subscriber('joint_states', JointState, callback)
# rospy.spin()
# # 第3.5段路径：吸取点 =》 反弹点
# self.arm.shift_pose_target(2, 0.05, END_EFFECTOR_LINK)
# self.arm.go()
# rospy.sleep(1)
# 第四段路径：反弹点 =》 停泊点
rospy.loginfo("规划：从吸取点返回停泊点")
# grasp_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
# waypoints = []
# waypoints.append(grasp_pose)
# waypoints.append(anchor_pose)
# traj, fraction = self.cartesian_plan(self.arm, waypoints)
# self.arm.set_start_state_to_current_state()
# if fraction == 1.0:
# self.arm.execute(traj)
# rospy.loginfo("已返回停泊点")
# else:
# rospy.loginfo("Grasp_to_Anchor_Path Planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.")
anchor_pose = [0, 0.597, -0.665, 0, 1.636,
0] # 停泊点 [0, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(anchor_pose)
self.arm.go()
rospy.loginfo("已移动到停泊点")
###################### 即时规划结束 ####################
# rospy.spin()
# anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0] # 停泊点 [0, 0.597, -0.665, 0, 1.636, 0]
# self.arm.set_joint_value_target(anchor_pose)
# self.arm.go()
# rospy.loginfo("已移动到停泊点")
# rospy.spin()
####################### 提前规划 ######################
if data.orientation_x: # ox != 0, trangle
middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
place_pose = [1.571, 1.297, -0.270, -0.003, 0.544, 0.003]
else: # ox == 0, circle
middle_pose = [2.285, 0.597, -0.665, 0, 1.636, 0]
place_pose = [2.285, 1.471, -0.687, -0.002, 0.788, 0.715]
# 停泊点 =》 下料点
# self.anchor_point_to_middle_point(True)
# middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(middle_pose)
self.arm.go()
rospy.loginfo("已移动到中间点")
# 测试，下料点定点使用
# rospy.spin()
# middle_point = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
# waypoints = []
# waypoints.append(middle_point)
# waypoints.append(self.place_pose)
# traj, fraction = self.cartesian_plan(self.arm, waypoints)
# self.arm.set_start_state_to_current_state()
# if fraction == 1.0:
# self.arm.execute(traj)
# rospy.loginfo("已到达吸取点")
# else:
# rospy.spin()
# def callback(data):
# rospy.loginfo(data)
# rospy.Subscriber('joint_states', JointState, callback)
# rospy.spin()
# self.middle_point_to_place_point(True)
# place_pose = [1.571, 1.297, -0.270, -0.003, 0.544, 0.003]
self.arm.set_joint_value_target(place_pose)
self.arm.go()
rospy.loginfo("已到达下料点")
# rospy.sleep(1)
# rospy.spin()
# 气阀操作，吸盘释放
on_off_signal = 0
self.pub.publish(on_off_signal)
rospy.loginfo("Close the gas valve.")
self.rate.sleep()
rospy.sleep(1)
# 下料点 =》 停泊点
# self.middle_point_to_place_point(False)
# middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(middle_pose)
self.arm.go()
rospy.loginfo("已返回中间点")
# self.anchor_point_to_middle_point(False)
anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(anchor_pose)
self.arm.go()
rospy.loginfo("已返回停泊点")
rospy.sleep(1)
# rospy.spin()
# 停泊点 =》 下料点
# self.anchor_point_to_middle_point(True)
# middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(middle_pose)
self.arm.go()
rospy.loginfo("已移动到中间点")
# self.middle_point_to_place_point(True)
# place_pose = [1.571, 1.297, -0.270, -0.003, 0.544, 0.003]
self.arm.set_joint_value_target(place_pose)
self.arm.go()
rospy.loginfo("已返回下料点")
rospy.sleep(1)
# 气阀操作,吸盘吸取
on_off_signal = 1
self.pub.publish(on_off_signal)
rospy.loginfo("Open the gas valve.")
self.rate.sleep()
rospy.sleep(1)
# 下料点 =》 停泊点
# self.middle_point_to_place_point(False)
# middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(middle_pose)
self.arm.go()
rospy.loginfo("已返回中间点")
# self.anchor_point_to_middle_point(False)
anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0]
self.arm.set_joint_value_target(anchor_pose)
self.arm.go()
rospy.loginfo("已返回停泊点")
# rospy.spin()
# 停泊点 =》 释放点
self.anchor_point_to_release_point(True)
rospy.loginfo("已到达释放点")
# 气阀操作，吸盘释放
on_off_signal = 0
self.pub.publish(on_off_signal)
rospy.loginfo("Close the gas valve.")
self.rate.sleep()
# 释放点 =》 停泊点
self.anchor_point_to_release_point(False)
rospy.loginfo("已返回停泊点")
####################### 提前规划结束 ######################
self.schedule_percent.percent_complete = self.num / float(
self.goal.grasp_id)
self.server.publish_feedback(self.schedule_percent)
self.schedule_result.total_grasped = self.num
rospy.loginfo("Schedule: %0.2f %%",
self.schedule_percent.percent_complete * 100)
rospy.loginfo("Total Grasped: %d ",
int(self.schedule_result.total_grasped))
rospy.loginfo("------------- End: Wait For Next Grasp -----------")
def cartesian_plan(self, arm_object, point_list):
fraction = 0.0 # 路径规划覆盖率
maxtries = 100 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机械臂当前的状态为运动初始状态
arm_object.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(traj, fraction) = arm_object.compute_cartesian_path(
point_list, # 路点列表
0.01, # 终端步进值
0.0, # 跳跃阈值
True # 避障规划
)
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
return traj, fraction
def anchor_point_to_middle_point(self, dirc):
# 提前规划运动：
# dirc = true: 从停泊点到下料点
# dirc = false: 从下料点到停泊点
waypoints = []
if dirc:
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
waypoints.append(start_pose)
# waypoints.append(self.anchor_pose)
wpose = deepcopy(self.anchor_pose)
# 平面圆弧转动
rospy.loginfo("进入多边形迫近圆形插补方法规划")
i = 1
while wpose.position.x > 0:
# cos_5 = 0.99619
# sin_5 = 0.08716
# rad_5 = 5.0 / 180.0 * math.pi
cos_1 = 0.99985
sin_1 = 0.01745
rad_1 = 1.0 / 180.0 * math.pi
wpose.position.x = wpose.position.x * cos_1 - np.sign(
self.place_pose.position.y) * wpose.position.y * sin_1
wpose.position.y = np.sign(
self.place_pose.position.y
) * wpose.position.x * sin_1 + wpose.position.y * cos_1
# wpose.position.z += delta_z # z轴取消插补
rad_x = -np.sign(self.place_pose.position.y) * rad_1 * i
rad_y = 1.5708
rad_z = 0
list_orientation = quaternion_from_euler(rad_x, rad_y, rad_z)
wpose.orientation.x = list_orientation[0]
wpose.orientation.y = list_orientation[1]
wpose.orientation.z = list_orientation[2]
wpose.orientation.w = list_orientation[3]
i += 1
if self.cartesian:
waypoints.append(deepcopy(wpose))
else:
self.arm.set_pose_target(wpose)
self.arm.go()
# waypoints.append(self.middle_pose)
if not dirc:
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
waypoints.append(start_pose)
waypoints.reverse()
traj, fraction = self.cartesian_plan(self.arm, waypoints)
# f = open("plan_of_cartisen_demo.txt", "w")
# f.write(str(traj))
# f.close()
if fraction == 1.0:
self.arm.set_start_state_to_current_state()
self.arm.execute(traj)
rospy.loginfo("停泊点和中间点间运动已执行")
else:
rospy.loginfo("Anchor_to_Place_Path Planning failed with only " +
str(fraction) + "success after " + str(100) +
" attempts.")
def middle_point_to_place_point(self, dirc):
# 提前规划运动：
# dirc = true: 从中间点到下料点
# dirc = false: 从下料点到中间点
waypoints = []
if dirc:
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
waypoints.append(start_pose)
waypoints.append(self.place_pose)
else:
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
waypoints.append(start_pose)
waypoints.append(self.middle_pose)
traj, fraction = self.cartesian_plan(self.arm, waypoints)
if fraction == 1.0:
self.arm.set_start_state_to_current_state()
self.arm.execute(traj)
# def callback(data):
# rospy.loginfo(data)
# rospy.Subscriber('joint_states', JointState, callback)
# rospy.spin()
rospy.loginfo("中间点和下料点间运动已执行")
else:
rospy.loginfo("Anchor_to_Place_Path Planning failed with only " +
str(fraction) + "success after " + str(100) +
" attempts.")
def anchor_point_to_release_point(self, dirc):
# 提前规划运动：
# dirc = true: 从停泊点到释放点
# dirc = false: 从释放点到停泊点
waypoints = []
waypoints.append(self.anchor_pose)
waypoints.append(self.release_pose)
if not dirc:
waypoints.reverse()
traj, fraction = self.cartesian_plan(self.arm, waypoints)
if fraction == 1.0:
self.arm.set_start_state_to_current_state()
self.arm.execute(traj)
rospy.loginfo("停泊点和释放点间运动已执行")
else:
rospy.loginfo("Anchor_to_Release_Path Planning failed with only " +
str(fraction) + "success after " + str(100) +
" attempts.")
def B_spline(self, degree, l, coeff, knot, dense, points):
'''
degree: 阶数
l: l = n - k = point_num - k + 1
coeff: 控制点坐标,list
knot: 节点组,list
dense: 分辨率，每段点数
points: 轨迹点坐标,list
'''
u = 0
point_num = 0
i = degree
while i <= l + degree:
if knot[i + 1] > knot[i]:
for p in range(dense):
u = knot[i] + p * (knot[i + 1] - knot[i]) / dense
points[point_num] = self.deboor(degree, coeff, knot, u, i,
l)
point_num += 1
i += 1
# return points # 可能不需要返回值
def deboor(self, degree, coeff, knot, u, i, l):
'''
degree: 阶数
coeff: 控制点坐标,list
knot: 节点组,list
u 和 i: 主要是传参
'''
coeffa = [None] * (degree + 1)
j = i - degree
while j <= i:
coeffa[j] = coeff[j]
j += 1
k = 1
while k <= degree:
j = i
while j >= i - degree + k:
t1 = (knot[j + degree - k + 1] -
u) / (knot[j + degree - l + 1] - knot[j])
t2 = 1.0 - t1
coeffa[j] = t1 * coeffa[j - 1] + t2 * coeffa[j]
j -= 1
k += 1
return coeffa[i]
def middle_ctrl_point(self, start_pose, target_pose):
middle_point_X = abs(start_pose.position.x - target_pose.linear.x) / 2
middle_point_Y = abs(start_pose.position.y - target_pose.linear.y) / 2
theta = math.atan(middle_point_Y / middle_point_X)
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
print(middle_point_X, middle_point_Y)
print(theta, cos_theta, sin_theta)
middle_ctrl_point_X = start_pose.position.x * cos_theta
middle_ctrl_point_Y = start_pose.position.x * sin_theta
return middle_ctrl_point_X, middle_ctrl_point_Y
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = True
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('left_manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('leftbase_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
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 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
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.")
# # 控制机械臂回到初始化位置
# arm.set_named_target('home')
# arm.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)
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)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_drawing', anonymous=True)
#订阅stop话题
rospy.Subscriber("/probot_controller_ctrl", ControllerCtrl, callback)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
star_pose = PoseStamped()
star_pose.header.frame_id = reference_frame
star_pose.header.stamp = rospy.Time.now()
star_pose.pose.position.x = 0.30
star_pose.pose.position.y = 0.0
star_pose.pose.position.z = 0.30
star_pose.pose.orientation.w = 1.0
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(star_pose, end_effector_link)
arm.go()
radius = 0.05
centerY = 0.0
centerX = 0.40 - radius
# 初始化路点列表
waypoints = []
starPoints = []
pose_temp = star_pose
for th in numpy.arange(0, 6.2831855, 1.2566371):
pose_temp.pose.position.y = -(centerY + radius * math.sin(th))
pose_temp.pose.position.x = centerX + radius * math.cos(th)
pose_temp.pose.position.z = 0.30
wpose = deepcopy(pose_temp.pose)
starPoints.append(deepcopy(wpose))
# 将圆弧上的路径点加入列表
waypoints.append(starPoints[0])
waypoints.append(starPoints[2])
waypoints.append(starPoints[4])
waypoints.append(starPoints[1])
waypoints.append(starPoints[3])
waypoints.append(starPoints[0])
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)
# 控制机械臂先回到初始化位置
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('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)
# 设置每次运动规划的时间限制：20s
arm.set_planning_time(20)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table1_id = 'table1'
table2_id = 'table2'
target_id = 'cube_marker'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table1_id)
scene.remove_world_object(table2_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的三维尺寸[长, 宽, 高]
table1_size = [0.8, 1.5, 0.03]
table2_size = [1.5, 0.8, 0.03]
# 将两张桌子加入场景当中
table1_pose = PoseStamped()
table1_pose.header.frame_id = 'base_link'
table1_pose.pose.position.x = 0
table1_pose.pose.position.y = 1.05
table1_pose.pose.position.z = -0.05
table1_pose.pose.orientation.w = 1.0
scene.add_box(table1_id, table1_pose, table1_size)
table2_pose = PoseStamped()
table2_pose.header.frame_id = 'base_link'
table2_pose.pose.position.x = -1.05
table2_pose.pose.position.y = -0.1
table2_pose.pose.position.z = -0.05
table2_pose.pose.orientation.w = 1.0
scene.add_box(table2_id, table2_pose, table2_size)
# 将桌子设置成红色
self.setColor(table1_id, 0.8, 0.4, 0, 1.0)
self.setColor(table2_id, 0.8, 0.4, 0, 1.0)
#监听目标到'base_link'的tf变换
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
(trans,
rot) = listener.lookupTransform('base_link', 'ar_marker_0',
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo(
"Waiting for transform between 'base_link' and 'ar_marker_0'"
)
rospy.sleep(1)
rospy.loginfo("Found transform between 'base_link' and 'ar_marker_0'")
# 设置目标物体的尺寸
target_size = [0.05, 0.05, 0.2]
# 设置目标物体的位置
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = trans[0]
target_pose.pose.position.y = trans[1]
target_pose.pose.position.z = 0.065
target_pose.pose.orientation.x = rot[0]
target_pose.pose.orientation.y = rot[1]
target_pose.pose.orientation.z = rot[2]
target_pose.pose.orientation.w = rot[3]
# 将抓取的目标物体加入场景中
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(table2_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = -0.5
place_pose.pose.position.y = 0
place_pose.pose.position.z = 0.065
while not rospy.is_shutdown():
try:
listener.waitForTransform('ar_marker_0', 'ee_link',
rospy.Time(0), rospy.Duration(4.0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo(
"Waiting for transform between 'ar_marker_0' and 'ee_link'"
)
rospy.sleep(1)
rospy.loginfo("Found transform between 'ar_marker_0' and 'ee_link'")
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = 'ar_marker_0'
grasp_pose.pose.position.x = 0
grasp_pose.pose.position.y = -0.15
grasp_pose.pose.position.z = -0.1
# 生成抓取姿态
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)
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " +
str(err))
raise err
self.gripper_opened = [
rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01
]
self.gripper_closed = [
rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01
]
self.gripper_neutral = [
rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0]) /
2.0)
]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.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('target_pose',
PoseStamped,
queue_size=10)
# 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.04)
arm.set_goal_orientation_tolerance(0.01)
# 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)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 3
# Set a limit on the number of place attempts
max_place_attempts = 3
rospy.loginfo("Scaling for MoveIt timeout=" + str(
rospy.get_param(
'/move_group/trajectory_execution/allowed_execution_duration_scaling'
)))
# Give each of the scene objects a unique name
table_id = 'table'
target_id = 'target'
# Remove leftover objects from a previous run
self.scene.remove_world_object(table_id)
self.scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
# Set the height of the table off the ground
table_ground = 0.0
# Set the dimensions of the scene objects [l, w, h]
# Set the target size [l, w, h]
# target_size = [0.02, 0.005, 0.12]
target_size = [0.02, 0.02, 0.05]
#target pose array
target_pose_array = PoseArray()
target_pose_array.header.frame_id = REFERENCE_FRAME
pose1 = Pose()
pose1.position.x = 0.20
pose1.position.y = -0.10
pose1.position.z = 0.025
pose1.orientation.w = 1.0
target_pose_array.poses.append(pose1)
pose2 = Pose()
pose2.position.x = 0.20
pose2.position.y = 0.10
pose2.position.z = 0.025
pose2.orientation.w = 1.0
target_pose_array.poses.append(pose2)
# 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.29
target_pose.pose.position.y = -0.10
target_pose.pose.position.z = 0.025
target_pose.pose.orientation.w = 1.0
'''
target_pose.pose.position.x = target_pose_array.poses[1].position.x
target_pose.pose.position.y = target_pose_array.poses[1].position.y
target_pose.pose.position.z = target_pose_array.poses[1].position.z
target_pose.pose.orientation.w = target_pose_array.poses[
1].orientation.w
# Add the target object to the scene
self.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)
# 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
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
# 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],
[target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
#arm.set_named_target('resting')
#arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
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)
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, queue_size=10 )
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# 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(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)
# 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('hey_there')
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.75
# 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.25
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.21
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.19
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.22
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.18
place_pose.pose.position.y = -0.18
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
# 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('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)
# 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)
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 left arm
left_arm = MoveGroupCommander('left_arm')
# Initialize the MoveIt! commander for the gripper
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
end_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_eef, '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.3
# 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_eef
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_eef, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base'
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
left_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
left_arm.set_named_target('left_ready')
left_arm.go()
rospy.sleep(2)
# Return the arm in the "left_ready" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_eef, 'tool')
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')
# 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):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface('world')
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
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)
arm.set_planning_time(5)
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
rospy.sleep(1)
reference_frame = 'root'
table_ground = 0.55
tabel_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.7
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + tabel_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, tabel_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.65
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + tabel_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.63
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + tabel_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
self.setColor(table_id, 0.8, 0.0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 1.0)
self.setColor(box2_id, 0.8, 0.4, 1.0)
self.sendColors()
gripper = MoveGroupCommander('gripper')
gripper.set_named_target('Close')
gripper.go()
rospy.sleep(1)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.64
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + tabel_size[
2] + 0.05
target_quaternion = quaternion_from_euler(-1.6745735920568166,
-1.2113336804234238,
-1.718476017088519)
target_pose.pose.orientation.x = target_quaternion[0]
target_pose.pose.orientation.y = target_quaternion[1]
target_pose.pose.orientation.z = target_quaternion[2]
target_pose.pose.orientation.w = target_quaternion[3]
# arm.set_goal_position_tolerance(0.02)
# arm.set_goal_orientation_tolerance(0.01)
arm.set_pose_target(target_pose, end_effector_link)
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)
# 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)
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)
class MoveItCartesianDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node(NODE_NAME, anonymous=True)
self.pub = rospy.Publisher(ON_OFF_SIGNAL_TOPIC, Bool, queue_size=2)
self.rate = rospy.Rate(2)
# 是否需要使用笛卡尔运动规划
self.cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的self.arm group
self.arm = MoveGroupCommander(ARM_GROUP)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
self.arm.set_pose_reference_frame(REFERENCE_FRAME)
# 获取终端link的名称
end_effector_link = 'link_6'
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置位置（单位：米）和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# 环境建模
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
self.colors = dict()
rospy.sleep(1)
# 场景物体设置
table_id = 'table'
scene.remove_world_object(table_id)
#
table_groud = 0.5 - HEIGHT_OF_END_EFFECTOR
table_size = [0.5, 1.5, 0.04]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.6
table_pose.pose.position.y = -0.5
table_pose.pose.position.z = table_groud - table_size[2] / 2.0 - 0.02
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
self.setColor(table_id, 0, 0.8, 0, 1.0)
self.sendColors()
# 第五轴的关节角度旋转，从水平到竖直
joint_positions = [0, 0, 0, 0, 1.571, 0]
self.arm.set_joint_value_target(joint_positions)
self.arm.go()
rospy.sleep(1)
# 全局变量，对比目标点位置
global forward_pose
forward_pose = Twist()
# 全局变量，计数
global item_num
item_num = 0
while not rospy.is_shutdown():
# 开启web端action通讯，等待讯号
self.server = actionlib.SimpleActionServer('abb_grasp',
AbbGraspAction,
self.abb_execute, False)
self.server.start()
rospy.spin()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def abb_execute(self, goal):
# 统计参数，与action有关
self.num = 0
self.schedule_percent = AbbGraspFeedback()
self.schedule_result = AbbGraspResult()
self.goal = goal
rate = rospy.Rate(25)
while self.num < self.goal.grasp_id:
self.num += 1
# 接收视觉处理后的坐标位置 - topic 方法
rospy.Subscriber(TOPIC_NAME, VISUAL_TOPIC_TYPE, self.callback)
# rospy.spin()
rospy.wait_for_message(TOPIC_NAME, VISUAL_TOPIC_TYPE, timeout=None)
rospy.loginfo(self.num)
rospy.loginfo(self.goal.grasp_id)
self.server.set_succeeded(self.schedule_result)
rospy.loginfo(self.schedule_result)
rospy.loginfo("------------finished------------")
# 获取视觉处理的坐标位置后的处理函数
def callback(self, data):
# 设置机械臂工作空间中的目标位姿，位置使用x,y,z坐标描述
# 姿态使用四元数描述，基于base_link坐标系
# 检测本次目标位置和上次是否一致
global forward_pose
if (data.linear.x == forward_pose.linear.x and \
data.linear.y == forward_pose.linear.y and \
data.linear.z == forward_pose.linear.z):
return 0
else:
rospy.loginfo("目标点位置：")
rospy.loginfo(data)
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
start_pose.orientation.x = 0
start_pose.orientation.y = 0.707
start_pose.orientation.z = 0
start_pose.orientation.w = 0.707
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if self.cartesian:
waypoints.append(start_pose)
# 初始化路点数据
wpose = deepcopy(start_pose)
wpose.orientation.x = 0
wpose.orientation.y = 0.707
wpose.orientation.z = 0
wpose.orientation.w = 0.707
# 较远距离路径，多边形迫近圆形插补方法
if data.linear.x <= 0:
# print(np.sign(data.linear.y))
# delta_z = (data.linear.z - start_pose.position.z) / 18 / 2
i = 1
while wpose.position.x > 0:
cos_5 = 0.99619
sin_5 = 0.08716
rad_5 = 5.0 / 180.0 * math.pi
wpose.position.x = wpose.position.x * cos_5 - np.sign(
data.linear.y) * wpose.position.y * sin_5
wpose.position.y = np.sign(
data.linear.y
) * wpose.position.x * sin_5 + wpose.position.y * cos_5
# wpose.position.z += delta_z # z轴取消插补
rad_x = -np.sign(data.linear.y) * rad_5 * i
rad_y = 1.5708
rad_z = 0
list_orientation = quaternion_from_euler(
rad_x, rad_y, rad_z)
wpose.orientation.x = list_orientation[0]
wpose.orientation.y = list_orientation[1]
wpose.orientation.z = list_orientation[2]
wpose.orientation.w = list_orientation[3]
i += 1
if self.cartesian:
waypoints.append(deepcopy(wpose))
else:
self.arm.set_pose_target(wpose)
self.arm.go()
self.arm.set_start_state_to_current_state()
plan_0, fraction = self.cartesian_plan(self.arm, waypoints)
# 如果路径规划成功，则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo(
"Path to start pose computed successfully. Moving the self.arm."
)
self.arm.execute(plan_0)
rospy.loginfo("Path to start pose execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + "success after " +
str(100) + " attempts.")
else:
rospy.loginfo("进入常规规划")
# 常规路径,先水平面移动
wpose.position.x = data.linear.x
wpose.position.y = data.linear.y
wpose.position.z = data.linear.z + GAP_TO_ITEM
wpose.orientation.x = 0
wpose.orientation.y = 0.707
wpose.orientation.z = 0
wpose.orientation.w = 0.707
if self.cartesian:
waypoints.append(wpose)
traj, fraction = self.cartesian_plan(self.arm, waypoints)
self.arm.set_start_state_to_current_state()
# rospy.loginfo(traj)
self.arm.execute(traj)
# 垂直笛卡尔轨迹规划
rospy.loginfo("进入垂直路径规划")
start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose
start_pose.orientation.x = 0
start_pose.orientation.y = 0.707
start_pose.orientation.z = 0
start_pose.orientation.w = 0.707
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if self.cartesian:
waypoints.append(start_pose)
# 初始化路点数据
wpose = deepcopy(start_pose)
wpose.orientation.x = 0
wpose.orientation.y = 0.707
wpose.orientation.z = 0
wpose.orientation.w = 0.707
# 设置目标点的路点数据，并加入路点列表
wpose = deepcopy(start_pose)
wpose.position.x = data.linear.x
wpose.position.y = data.linear.y
wpose.position.z = data.linear.z
# 目标点四元数计算
# wpose.orientation.x = - np.sign(data.linear.y) * 0.5
# wpose.orientation.y = 0.5
# wpose.orientation.z = np.sign(data.linear.y) * 0.5
# wpose.orientation.w = 0.5
if self.cartesian:
waypoints.append(deepcopy(wpose))
else:
self.arm.set_pose_target(wpose)
self.arm.go()
# 设置当前的状态为运动初始状态
self.arm.set_start_state_to_current_state()
# 气阀操作，吸盘抓取
on_off_signal = 1
self.pub.publish(on_off_signal)
rospy.loginfo("Open the gas valve.")
self.rate.sleep()
plan_1, fraction = self.cartesian_plan(self.arm, waypoints)
# 第一段路程规划和执行
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
now1 = rospy.get_time()
rospy.loginfo(now1)
self.arm.execute(plan_1)
# rospy.sleep(1.499)
now3 = rospy.get_time()
rospy.loginfo(now3 - now1)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + "success after " + str(100) +
" attempts.")
waypoints.reverse()
plan_2, fraction = self.cartesian_plan(self.arm, waypoints)
self.arm.set_start_state_to_current_state()
# 第二段路程规划和执行，并紧接着第三段和第四段路程
if fraction == 1.0:
rospy.loginfo(
"Backpath computed successfully. Moving the arm.")
now1 = rospy.get_time()
rospy.loginfo(now1)
self.arm.execute(plan_2)
now2 = rospy.get_time()
rospy.loginfo(now2)
# rospy.sleep(1.998 - now2 + now1)
now3 = rospy.get_time()
rospy.loginfo(now3 - now1)
rospy.loginfo("Backpath execution complete.")
# 放回板材
rospy.loginfo("Path 2: Moving the arm.")
self.arm.set_start_state_to_current_state()
self.arm.execute(plan_1)
rospy.loginfo("Path execution complete.")
# 气阀操作，吸盘释放
on_off_signal = 0
self.pub.publish(on_off_signal)
rospy.loginfo("Close the gas valve.")
self.rate.sleep()
# rospy.sleep(1)
rospy.loginfo("Backpath 2: Moving the arm.")
self.arm.set_start_state_to_current_state()
self.arm.execute(plan_2)
rospy.loginfo("Backpath execution complete.")
# global item_num
# item_num += 1
# rospy.loginfo("Finished " + str(item_num) + " grasp.")
else:
rospy.loginfo("Backpath planning failed with only " +
str(fraction) + "success after " + str(100) +
" attempts.")
# rospy.sleep(5)
# # 控制机械臂回到初始化位置
# self.arm.set_named_target(HOME_POSE)
# self.arm.go()
# rospy.sleep(1)
# 记录本次目标位置
global forward_pose
forward_pose.linear.x = data.linear.x
forward_pose.linear.y = data.linear.y
forward_pose.linear.z = data.linear.z
rospy.loginfo(forward_pose)
self.schedule_percent.percent_complete = self.num / float(
self.goal.grasp_id)
self.server.publish_feedback(self.schedule_percent)
self.schedule_result.total_grasped = self.num
rospy.loginfo("schedule: %0.2f %%",
self.schedule_percent.percent_complete * 100)
rospy.loginfo("total grasped: %d ",
int(self.schedule_result.total_grasped))
rospy.loginfo("-----------wait for next response-----------")
def cartesian_plan(self, arm_object, point_list):
fraction = 0.0 # 路径规划覆盖率
maxtries = 100 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机械臂当前的状态为运动初始状态
arm_object.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm_object.compute_cartesian_path(
point_list, # 路点列表
0.01, # 终端步进值
0.0, # 跳跃阈值
True # 避障规划
)
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
return plan, fraction
def B_spline(self, degree, l, coeff, knot, dense, points):
'''
degree: 阶数
l: l = n - k = point_num - k + 1
coeff: 控制点坐标,list
knot: 节点组,list
dense: 分辨率，每段点数
points: 轨迹点坐标,list
'''
u = 0
point_num = 0
i = degree
while i <= l + degree:
if knot[i + 1] > knot[i]:
for p in range(dense):
u = knot[i] + p * (knot[i + 1] - knot[i]) / dense
points[point_num] = self.deboor(degree, coeff, knot, u, i,
l)
point_num += 1
i += 1
# return points # 可能不需要返回值
def deboor(self, degree, coeff, knot, u, i, l):
'''
degree: 阶数
coeff: 控制点坐标,list
knot: 节点组,list
u 和 i: 主要是传参
'''
coeffa = [None] * (degree + 1)
j = i - degree
while j <= i:
coeffa[j] = coeff[j]
j += 1
k = 1
while k <= degree:
j = i
while j >= i - degree + k:
t1 = (knot[j + degree - k + 1] -
u) / (knot[j + degree - l + 1] - knot[j])
t2 = 1.0 - t1
coeffa[j] = t1 * coeffa[j - 1] + t2 * coeffa[j]
j -= 1
k += 1
return coeffa[i]
def middle_ctrl_point(self, start_pose, target_pose):
middle_point_X = abs(start_pose.position.x - target_pose.linear.x) / 2
middle_point_Y = abs(start_pose.position.y - target_pose.linear.y) / 2
theta = math.atan(middle_point_Y / middle_point_X)
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
print(middle_point_X, middle_point_Y)
print(theta, cos_theta, sin_theta)
middle_ctrl_point_X = start_pose.position.x * cos_theta
middle_ctrl_point_Y = start_pose.position.x * sin_theta
return middle_ctrl_point_X, middle_ctrl_point_Y
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
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)
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.72
# 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.1) #0.05
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.5) #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_shoulder_tilt',
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('right_start')
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 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.right_arm.stop()
# Move to the resting position
self.right_arm.set_named_target('right_start')
self.right_arm.go()
# Relax the servos
self.relax_all_servos()
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)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_cartesian', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
#set cartesian parameters
ur5_manipulator = MoveGroupCommander('ur5_manipulator')
ur5_manipulator.allow_replanning(True)
ur5_manipulator.set_pose_reference_frame('base_link')
ur5_manipulator.set_goal_position_tolerance(0.01)
ur5_manipulator.set_goal_orientation_tolerance(0.1)
end_effector_link = ur5_manipulator.get_end_effector_link()
#set the ur5 initial pose
ur5_manipulator.set_named_target('ready')
ur5_manipulator.go()
#get the end effort information
start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose
print("The first waypoint:")
print(start_pose)
#define waypoints
waypoints = []
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.z -= 0.3
print("The second waypoint:")
print(wpose)
waypoints.append(deepcopy(wpose))
print(" ")
print(waypoints)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = ur5_manipulator.compute_cartesian_path(
waypoints, 0.01, 0.0, True)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
ur5_manipulator.execute(plan)
rospy.sleep(2)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
#ur5_manipulator.set_named_target('home')
#ur5_manipulator.go()
rospy.sleep(3)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class PickAndPlace:
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def make_gripper_translation(self, min_dist, desired, vector):
g = GripperTranslation()
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
g.direction.header.frame_id = "panda_link0"
g.min_distance = min_dist
g.desired_distance = desired
return g
def open_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.04, 0.04]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def close_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.0, 0.0]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def clear_all_object(self):
for world_object in (self.scene.get_known_object_names()):
self.scene.remove_world_object(world_object)
for attached_object in (self.scene.get_attached_objects()):
self.scene.remove_attached_object(attached_object)
def add_object_box(self, object_name, pose, color, frame, size):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param size: size of object(mm)
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_box(name=object_name,
pose=object_pose,
size=(size[0] / 1000.0, size[1] / 1000.0,
size[2] / 1000.0))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
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
