class SubTaskGrasp:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('subtask_grasp')
rospy.on_shutdown(self.shutdown)
self.grasp_srv = rospy.Service('subtask_grasp', rp_grasp,
self.graspSrvCallback)
def graspSrvCallback(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, e:
print "Service call failed: %s" % e
self.shutdown()
print(target.name)
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# 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)
target_id = 'chipstar_red'
scene.remove_world_object(target_id)
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
arm.set_named_target('l_arm_init')
arm.go()
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# target_size = [(target.offset_x*2), (target.offset_y*2), (target.offset_z*2)]
target_size = [0.03, 0.03, 0.12]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target.x
target_pose.pose.position.y = target.y
target_pose.pose.position.z = target.z + target.offset_z
# q = quaternion_from_euler(target.rr, target.rp, target.ry)
q = quaternion_from_euler(0, 0, 0)
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]
scene.add_box(target_id, target_pose, target_size)
rospy.sleep(2)
print('test2-1')
# 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.x -= target_size[0] / 2.0 - 0.01
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
print('test3')
# 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
print('test4')
# 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 = arm.pick(target_id, grasps)
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
scene.remove_attached_object(GRIPPER_FRAME, target_id)
ret = rp_graspResponse()
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success the pick operation")
ret.result = True
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
ret.result = False
return ret
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.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#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.68
table_size = [0.5, 1, 0.01]
#box1_size=[0.1,0.05,0.03]
box2_size = [0.05, 0.05, 0.1]
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.1]
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.15
#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)
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.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
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.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
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, pickPos, placePos):
# 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
pick_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the momitive group for the right gripper
pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = pick_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
pick_arm.set_goal_position_tolerance(0.05)
pick_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
pick_arm.allow_replanning(True)
# Set the right arm reference frame
pick_arm.set_pose_reference_frame(REFERENCE_FRAME)
#pick_arm.set_planner_id("RRTConnectkConfigDefault")
#pick_arm.set_planner_id("RRTstarkConfigDefault")
pick_arm.set_planner_id("RRTstarkConfigDefault")
# Allow 5 seconds per planning attempt
pick_arm.set_planning_time(3)
# 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
base_table_id = 'base_table'
target_id = 'target'
limit_table_id = 'limit_table'
#tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(base_table_id)
#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)
scene.remove_world_object(limit_table_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
# table_ground = 0.6
table_ground = 0.0
# Set the dimensions of the scene objects [l, w, h]
base_table_size = [2, 2, 0.04]
#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.055, 0.055, 0.12]
target_radius = 0.03305
target_height = 0.12310
limit_table_size = [0.6, 1.8, 0.04]
# Add a base table to the scene
base_table_pose = PoseStamped()
base_table_pose.header.frame_id = REFERENCE_FRAME
base_table_pose.pose.position.x = 0.0
base_table_pose.pose.position.y = 0.0
base_table_pose.pose.position.z = -0.3
base_table_pose.pose.orientation.w = 1.0
scene.add_box(base_table_id, base_table_pose, base_table_size)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
rospy.sleep(1)
# 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.5
#table_pose.pose.position.y = -0.4
#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)
# 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 = pickPos.pose.position.x
target_pose.pose.position.y = pickPos.pose.position.y
target_pose.pose.position.z = pickPos.pose.position.z
target_pose.pose.orientation.w = pickPos.pose.orientation.w
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
#scene.add_cylinder(target_id,target_pose,target_height,target_radius)
#Add the limit_table object to the scene
limit_table_pose = PoseStamped()
limit_table_pose.header.frame_id = REFERENCE_FRAME
limit_table_pose.pose.position.x = 0.58
limit_table_pose.pose.position.y = -0.4
limit_table_pose.pose.position.z = 0.18
limit_table_pose.pose.orientation.w = 1.0
#scene.add_box(limit_table_id, limit_table_pose, limit_table_size)
self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0)
# 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
#pick_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 = placePos.pose.position.x
place_pose.pose.position.y = placePos.pose.position.y
place_pose.pose.position.z = placePos.pose.position.z
place_pose.pose.orientation.w = placePos.pose.orientation.w
place_pose.pose.orientation.x = placePos.pose.orientation.x
place_pose.pose.orientation.y = placePos.pose.orientation.y
place_pose.pose.orientation.z = placePos.pose.orientation.z
#设置机器人运行最大速度位百分之～
pick_arm.set_max_velocity_scaling_factor(0.5)
# 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
# this way is just used by PR2 robot
#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 RV
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:
while result != 1 and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
#moveit中的pick接口，target_id为moveit添加场景的id，此处为目标物体，grasps为可尝试抓取的点位序列
result = pick_arm.pick(target_id, grasps)
#打印信息
rospy.logerr("pick_arm.pick： " + str(result))
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
#如果抓取成功，尝试放置操作
result_g = None
n_attempts = 2
#if result == MoveItErrorCodes.SUCCESS:
while result_g != True and n_attempts < max_pick_attempts: #and result == 1:
# Generate valid place poses
#places = self.make_places(place_pose)
n_attempts += 1
print("-------------------")
print(place_pose)
#更新当前的机械臂状态
pick_arm.set_start_state_to_current_state()
#设置moveit运动的目标点位
pick_arm.set_pose_target(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 = pick_arm.place(target_id, place)
# if result == MoveItErrorCodes.SUCCESS:
# break
# rospy.sleep(0.2)
#moveit的运动接口，wait=True表示等到执行完成才返回
result_g = pick_arm.go(wait=True)
rospy.logerr("pick_arm.go： " + str(result_g))
#打开夹爪
open_client(500)
rospy.sleep(0.2)
#if result == "False"
# rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
#else:
# rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "home" pose stored in the SRDF file
#将机械臂返回到SRDF中的“home”姿态
#scene.remove_world_object(target_id)
#scene.remove_attached_object(GRIPPER_FRAME, target_id)
#scene.remove_world_object(target_id)
rospy.sleep(1)
open_client(500)
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
rospy.logerr("pick_server over ")
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=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
print(end_effector_link)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
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 = 2
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, "part")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('default')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(1)
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.240
place_pose.pose.position.y = 0.01
place_pose.pose.position.z = 0.3
scene.add_box("part", place_pose, (0.07, 0.01, 0.2))
# Specify a pose to place the target after being picked up
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
# start the gripper in a neutral pose part way to the target
target_pose.pose.position.x = 0.0733923763037
target_pose.pose.position.y = 0.0129100652412
target_pose.pose.position.z = 0.3097191751
target_pose.pose.orientation.x = -0.524236500263
target_pose.pose.orientation.y = 0.440069645643
target_pose.pose.orientation.z = -0.468739062548
target_pose.pose.orientation.w = 0.558389186859
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
print("going to start pose")
right_arm.set_pose_target(target_pose)
right_arm.go()
rospy.sleep(2)
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
#grasp_pose.pose.position.x = 0.12792118579
#grasp_pose.pose.position.y = -0.285290879999
#grasp_pose.pose.position.z = 0.120301181892
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, 'part')
# Publish the grasp poses so they can be viewed in RViz
print "Publishing grasps"
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('part', 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('part', 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:
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_rest')
right_arm.go()
# Open the gripper to the open position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self, handeye_client, mode):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
if mode == "sim":
arm.set_max_velocity_scaling_factor(1.0)
elif mode == "real":
arm.set_max_velocity_scaling_factor(0.5)
arm.set_planning_time(0.05) # 规划时间限制
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
# print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('calib1')
# arm.go()
# self.go_to_joint_state([2.5228, 0.8964, 0.9746, 0.8614, 2.7515, -1.8821, 0.1712])
self.mode = mode
self.listener = tf.TransformListener()
self.handeye_client = handeye_client
self.im_converter = image_converter()
self.aruco_pix_listener = aruco_pix_listener()
self.data_num = 0
if mode == "sim":
self.camera_link = "camera_link_optical"
self.states = [
[2.5230, 0.8961, 0.9750, 0.8609, 2.7510, -1.8819, 0.1710],
[2.4861, 0.9171, 0.9988, 0.8576, 2.8072, -1.8743, 0.1717],
[2.5231, 0.8961, 0.9738, 0.8604, 2.7517, -1.8818, 0.1700],
[2.4003, 0.7911, 0.9820, 1.0503, 2.8352, -1.6524, 0.3760],
[2.5049, 0.9006, 0.9405, 0.8542, 2.8436, -1.8724, 0.4127],
[2.6273, 0.8107, 0.9139, 0.9618, 2.7511, -1.7930, 0.5062],
[2.7162, 0.8069, 0.8263, 0.9200, 2.7147, -1.8470, 0.7433],
[2.7065, 0.8050, 0.8584, 0.9477, 2.4444, -1.8806, 0.1071],
[2.2901, 0.7806, 1.0289, 1.2211, 2.7996, -1.7092, 0.1444],
[2.1136, 0.7398, 1.0706, 1.2892, 2.7842, -1.5650, -0.0094],
[2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462],
[2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462],
[2.6515, 0.6861, 1.0594, 1.4848, 2.5607, -1.5177, 0.8066],
[2.7958, 0.8070, 0.9759, 1.2940, 2.4786, -1.6845, 0.7778],
[2.8772, 0.7243, 0.9153, 1.4943, 2.3478, -1.4724, 1.0759],
[2.3152, 0.8412, 1.1005, 1.1974, 2.5480, -1.6883, 0.2808],
[2.0790, 0.8522, 1.0670, 1.6003, 2.6245, -1.2628, 0.5669],
[2.1475, 1.0192, 1.0940, 1.3640, 2.6444, -1.3627, 0.3612],
[0.2200, -0.5008, -2.8969, 1.7277, 2.6600, -1.1654, 1.5059],
[0.4877, -0.7181, -0.2344, -1.3239, -0.1559, -1.4678, 0.5835],
[0.3773, -0.6314, -0.2302, -1.4444, -0.1600, -1.3351, 0.4067],
[-2.2371, 1.3616, 1.5669, -1.2129, 0.9923, -1.8344, -0.1351],
[-2.3273, 1.3885, 1.5713, -0.9358, 1.0748, -1.9300, 0.5635],
[-2.2465, 1.2267, 1.8483, -1.0450, 0.7416, -1.6672, 0.3275],
[-1.7879, 1.2150, 1.6576, -1.5896, 0.7221, -1.4276, 0.6940],
[-2.2530, 1.0939, 1.8129, -1.3149, 0.7196, -1.6466, 0.4342],
[-2.1126, 1.1457, 1.7960, -1.2610, 0.6560, -1.6326, 0.4547],
[-2.1270, 1.2311, 1.8561, -1.0124, 0.5739, -1.8181, 0.4290],
]
elif mode == "real":
self.camera_link = "camera_color_optical_frame"
self.states = [
[-0.1629, -0.1209, -0.0899, -0.6650, 0.1250, -1.9180, -1.7391],
[0.1768, -0.2480, -0.0637, -0.6867, -0.2229, -1.8912, -1.2967],
[0.1064, -0.0347, -0.0530, -1.1423, -0.0809, -1.5052, -1.6230],
[0.4235, -0.0861, -0.1177, -1.1818, -0.2675, -1.4303, -1.5148],
[0.4016, -0.0026, -0.0992, -1.2851, -0.3538, -1.4122, -1.5428],
[0.2777, -0.0294, -0.1911, -1.2395, -0.0141, -1.3563, -2.0564],
[2.9142, 0.1833, -2.7987, -1.0661, -0.0965, -1.4191, -1.8634],
[2.8310, -0.1007, -2.8406, -1.3918, 0.0263, -1.2798, -1.8718],
[2.6899, -0.0448, -2.9686, -1.3379, 0.2609, -1.3600, -2.1981],
[2.7554, -0.2997, -2.5511, -1.5645, -0.2672, -1.2493, -1.5705],
[2.7252, 0.2366, -2.6045, -0.9946, -0.2552, -1.5880, -1.2153],
[2.3309, 0.2733, -2.6384, -1.1497, 0.2391, -1.3979, -1.6642],
[2.1696, 0.5283, -2.6540, -0.8638, 0.3913, -1.5963, -1.8724],
[2.2318, 0.5134, -2.7120, -0.8413, 0.3381, -1.6116, -1.6351],
[2.3469, 0.5802, -2.7714, -0.6768, 0.2250, -1.7614, -1.4197],
[2.3452, 0.4840, -2.7232, -0.8959, 0.2437, -1.5798, -1.5464],
[2.3888, 0.3196, -2.7301, -1.1076, 0.1213, -1.4436, -1.5656],
[2.5365, 0.7079, -2.6854, -0.5944, 0.1202, -1.7089, -1.5256],
[2.3243, 0.5048, -2.5923, -1.0267, 0.1985, -1.3861, -1.6988],
[2.6772, 0.3100, -2.6433, -1.0222, -0.1752, -1.4295, -1.1358],
[2.5951, 0.3157, -2.5408, -1.0630, -0.2109, -1.3997, -1.1196],
[2.5800, 0.3364, -2.4705, -1.0478, -0.2944, -1.4289, -1.6568],
[2.7948, 0.3616, -2.5705, -1.0294, -0.4474, -1.5064, -0.9428],
[2.7595, 0.2951, -2.5461, -1.1454, -0.4809, -1.3790, -1.1017],
]
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.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的“up”姿态
arm.set_named_target('up')
arm.go()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
print "start pos", start_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 = 10000 #最大尝试规划次数
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
print "fraction rate", fraction
print "\n"
# 打印运动规划进程
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('up')
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()
# 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):
rospy.init_node('commander_example', anonymous=True)
self.group_l = MoveGroupCommander("left_arm")
self.group_r = MoveGroupCommander("right_arm")
self.group_l.set_planner_id('RRTConnectkConfigDefault')
self.group_r.set_planner_id('RRTConnectkConfigDefault')
def main():
# moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('get_position_client')
scene = PlanningSceneInterface()
objects = scene.get_known_object_names()
# print("objects", objects)
# print("scene", scene)
left_arm = MoveGroupCommander("left_arm")
# left_gripper_group = moveit_commander.MoveGroupCommander("left_gripper")
right_arm = MoveGroupCommander("right_arm")
# right_gripper = moveit_commander.MoveGroupCommander("right_gripper")
dual_arm_group = MoveGroupCommander("dual_arm")
grippers = MoveGroupCommander("grippers")
get_position = rospy.ServiceProxy('get_position', target_position)
rate = rospy.Rate(2.0)
done_l = False
done_r = False
# dual_joints = dual_arm_group.get_joints()
# print("dual_joints" , dual_joints)
# dual_joint_values = dual_arm_group.get_current_joint_values()
# print("dual_joint_values", dual_joint_values)
# left_arm_group.set_named_target("left_home")
# plan1 = left_arm_group.plan()
# rospy.sleep(2)
# left_arm_group.go()
# print "...Left Done..."
# rospy.sleep(1)
dual_arm_group.set_named_target("both_home")
plan1 = dual_arm_group.plan()
rospy.sleep(2)
# print("dual_arm plan1", plan1)
dual_arm_group.go()
print "...Both Done..."
rospy.sleep(1)
# print("scene", scene)
while not rospy.is_shutdown():
try:
rospy.wait_for_service('get_position', timeout=5)
res = get_position()
left_handle = res.points[0]
right_handle = res.points[1]
# scene.remove_world_object()
# print "right_handle" , right_handle
# pose_target_l = []
# pose_target_l.append(( left_handle.x - 0.03))
# pose_target_l.append(left_handle.y)
# pose_target_l.append(left_handle.z)
# pose_target_r = []
# pose_target_r.append(right_handle.x)
# pose_target_r.append(right_handle.y)
# pose_target_r.append(right_handle.z)
# print(pose_target_l)
print('left_handle', left_handle, 'right_handle', right_handle)
# left_arm_group.set_position_target(pose_target_l)
pose_target_l = Pose()
pose_target_r = Pose()
# pose_target_l.position = left_handle;
# pose_target_l.position.x -= 0.06
# pose_target_l.position.y -= 0.005
# pose_target_l.position.z -= 0.005
# q = quaternion_from_euler(-1.57, 0, -1.57) # Horizental Gripper : paraller to x-axis
# # q = quaternion_from_euler(-2.432, -1.57, -0.709) # Vertical Gripper: Vertical to x-axis
# pose_target_l.orientation.x = q[0]
# pose_target_l.orientation.y = q[1]
# pose_target_l.orientation.z = q[2]
# pose_target_l.orientation.w = q[3]
# print ("pose_target_l",pose_target_l )
# left_arm_group.set_pose_target(pose_target_l)
# left_arm_group.set_planning_time(10)
# plan1 = left_arm_group.plan()
# rospy.sleep(2)
# left_arm_group.go()
# done_l = True
# pose_target_r.position.x = 0.5232
# pose_target_r.position.y = -0.2743
# pose_target_r.position.z = 0.6846
# right_gripper.set_named_target("right_open")
# right_gripper.plan()
# right_gripper.go()
pose_target_r.position = right_handle
translation = get_translations(right_handle)
# pose_target_r.position.x -= 0.065
pose_target_r.position.x -= translation.x
print("translation.x", translation.x)
pose_target_r.position.y -= translation.y
pose_target_r.position.z -= translation.z
# pose_target_r.position.z = 0.7235
# q = quaternion_from_euler(-1.57, 0, -1.57) # This works from 2.35 : Gripper paraller to x-axis
q = quaternion_from_euler(
-1.57, 1.57, -1.57
) # This work from 2.35 : Gripper Vertical to x-axis (-2.36, 1.5708, -2.36)
pose_target_r.orientation.x = q[0]
pose_target_r.orientation.y = q[1]
pose_target_r.orientation.z = q[2]
pose_target_r.orientation.w = q[3]
# print ("pose_target_r",pose_target_r )
right_arm.set_pose_target(pose_target_r)
right_arm.set_planning_time(10)
plan1 = right_arm.plan()
# rospy.sleep(1)
# right_arm.go()
if done_r == False:
# print ("target pose",pose_target_r)
if (plan1.joint_trajectory.points
): # True if trajectory contains points
# last = (len(plan1.joint_trajectory.points) - 1)
# print("joint_trajectory", (plan1.joint_trajectory.points[-1].positions) ) # getting the last position of trajectory
r_joints = plan1.joint_trajectory.points[-1].positions
d_joints = get_dual_trajectory(r_joints)
# print("l_joints", l_joints)
# d_joints = l_joints + list(r_joints)
# print ("d_joints", d_joints)
dual_arm_group.set_joint_value_target(d_joints)
plan2 = dual_arm_group.plan()
if (plan2.joint_trajectory.points):
move_success = dual_arm_group.execute(plan2, wait=True)
# eef_pose = right_arm.get_current_pose()
# print("eef_pose", eef_pose)
# move_success = right_arm.execute(plan1, wait = True)
if move_success == True:
rospy.sleep(2)
right_arm.set_start_state_to_current_state()
left_arm.set_start_state_to_current_state()
waypoints_l = []
waypoints = []
wpose = right_arm.get_current_pose().pose
wpose_l = left_arm.get_current_pose().pose
print("wpose", wpose)
# Open the gripper to the full position
grippers.set_named_target("both_open")
grippers.plan()
grippers.go()
# Create Cartesian Path to move forward mainting the end-effector pose
# waypoints = []
# rospy.sleep(5)
# wpose = right_arm.get_current_pose().pose
if (translation.x >= 0.0505):
wpose.position.x += (translation.x + 0.00
) # move forward in (x)
wpose_l.position.x += (translation.x + 0.002)
wpose_l.position.z += 0.001
else:
wpose.position.x += 0.051 # move forward in (x)
wpose_l.position.x += 0.053
wpose_l.position.z += 0.001
waypoints.append(deepcopy(wpose))
(plan1,
fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0)
waypoints_l.append(deepcopy(wpose_l))
(plan_l,
fraction) = left_arm.compute_cartesian_path(
waypoints_l, # waypoints to follow
0.01, # eef_step
0.0)
# print("plan1 len", len(plan1.joint_trajectory.points))
# waypoints.append(copy.deepcopy(wpose))
# (plan2, fraction) = dual_arm_group.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.005, # eef_step
# 0.0)
rospy.sleep(1)
if plan1.joint_trajectory.points:
move_success = right_arm.execute(plan1,
wait=True)
if move_success == True:
rospy.loginfo(
"Right Move forward successful")
# done_r = True
# break;
if plan_l.joint_trajectory.points:
move_success_l = left_arm.execute(plan_l,
wait=True)
if move_success_l == True:
rospy.loginfo(
"Left Move forward successful")
# done_r = True
if move_success == True and move_success_l == True:
rospy.sleep(1)
grippers.set_named_target("both_close")
grippers.plan()
grippers.go()
# rospy.sleep(1)
waypoints = []
rospy.sleep(1)
wpose = right_arm.get_current_pose().pose
# q = quaternion_from_euler(-1.57, 1.57, -1.57) # wrist up 5 degrees = 1.66 10deg = 1.75
# wpose.orientation.x = q[0]
# wpose.orientation.y = q[1]
# wpose.orientation.z = q[2]
# wpose.orientation.w = q[3]
wpose.position.z += 0.07 # move up in (z)
waypoints.append(deepcopy(wpose))
(plan1,
fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0)
if plan1.joint_trajectory.points:
# move_success = right_arm.execute(plan1, wait=True) # for testing right arm trajectory
# done_r = True
r_joints = plan1.joint_trajectory.points[
-1].positions
# print ("r_joints", r_joints)
d_joints = get_dual_trajectory(r_joints)
# print ("d_joints", d_joints)
dual_arm_group.set_joint_value_target(
d_joints)
# plan2 = 0
plan2 = dual_arm_group.plan()
# print("planed plan len", len(plan2.joint_trajectory.points))
# print("dual arm trajectory", (plan2.joint_trajectory.points))
# done_r = True
if (plan2.joint_trajectory.points):
move_success = dual_arm_group.go()
print("move_success", move_success)
done_r = True
# traj_points = plan2.joint_trajectory.points[0:-3]
# plan2.joint_trajectory.points = traj_points
# print("eddited plan len", len(plan2.joint_trajectory.points))
# move_success = dual_arm_group.execute(plan2, wait = True)
# # done_r = True
# move_success = right_arm.execute(plan1, wait=True)
# if move_success == True:
# rospy.loginfo ("Lift Successful")
# done_r = True
# r_joints = plan1.joint_trajectory.points[-1].positions
# d_joints = get_dual_trajectory(r_joints)
# # print("l_joints", l_joints)
# # d_joints = l_joints + list(r_joints)
# # print ("d_joints", d_joints)
# dual_arm_group.set_joint_value_target(d_joints)
# plan2 = dual_arm_group.plan()
# if (plan2.joint_trajectory.points) :
# move_success = dual_arm_group.execute(plan2, wait = True)
# if move_success == True:
# rospy.loginfo("Move forward successful")
# rospy.sleep(1)
# grippers.set_named_target("both_close")
# grippers.plan()
# grippers.go()
# rospy.sleep(1)
# waypoints = []
# rospy.sleep(2)
# wpose = right_arm.get_current_pose().pose
# wpose.position.z += 0.1 # move up in (z)
# waypoints.append(copy.deepcopy(wpose))
# (plan1, fraction) = right_arm.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.01, # eef_step
# 0.0)
# if plan1.joint_trajectory.points:
# r_joints = plan1.joint_trajectory.points[-1].positions
# d_joints = get_dual_trajectory(r_joints)
# # print ("d_joints", d_joints)
# dual_arm_group.set_joint_value_target(d_joints)
# plan2 = dual_arm_group.plan()
# if (plan2.joint_trajectory.points) :
# move_success = dual_arm_group.execute(plan2, wait = True)
# done_r = True
# move_success = right_arm.execute(plan1, wait=True)
# if move_success == True:
# rospy.loginfo ("Lift Successful")
# done_r = True
# else:
# rospy.logwarn("Cartesian Paht Planning Failed for forward movement")
else:
print("Execution Completed")
except rospy.ROSException:
rospy.logerr('get_position_server did not respond in 5 sec')
return
rate.sleep()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# arm.set_max_velocity_scaling_factor(0.01)
arm.set_named_target("home")
arm.go()
def traj_pack(plan, joint_num=6, save_name=None):
traj_len = len(plan.joint_trajectory.points)
time_ls, pos_ls, vel_ls, acc_ls = [], [], [], []
# print plan.joint_trajectory.points
for point in plan.joint_trajectory.points:
time_ls.append(point.time_from_start.secs + point.time_from_start.nsecs / 1e9)
pos_ls.append(point.positions)
vel_ls.append(point.velocities)
acc_ls.append(point.accelerations)
pos, vel, acc = [], [], []
for i in range(traj_len):
pos.append(pos_ls[i][joint_num])
vel.append(vel_ls[i][joint_num])
acc.append(acc_ls[i][joint_num])
# 保存为CSV
if save_name is not None:
np.savetxt("%s.csv" % save_name, np.array([time_ls, pos, vel, acc]).transpose(), delimiter=',', header="time,pos,vel,acc", comments="")
return time_ls, pos, vel, acc
# ************************ 测试普通轨迹规划 *******************************
# arm.set_start_state_to_current_state()
# arm.set_named_target("test_1")
# plan = arm.plan()
# ************************ 测试连续路径轨迹规划（零点->中间点->零点） *******************************
# 重新计算轨迹时间
def retime_trajectory(plan, scale):
ref_state = robot.get_current_state()
retimed_plan = arm.retime_trajectory(ref_state, plan, velocity_scaling_factor=scale)
return retimed_plan
# 轨迹点拼接
def stitch_trajectory(plan_list):
new_traj = RobotTrajectory()
new_traj.joint_trajectory.joint_names = plan_list[0].joint_trajectory.joint_names
# 轨迹点拼接
new_points = []
for plan in plan_list:
new_points += list(plan.joint_trajectory.points)
new_traj.joint_trajectory.points = new_points
# 重新计算轨迹时间
new_traj = retime_trajectory(new_traj, scale=1.0)
return new_traj
# 轨迹列表
plan_list = []
# 设置初始状态
state = robot.get_current_state()
arm.set_start_state(state)
# 设置目标状态
aim_position1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.57]
arm.set_joint_value_target(aim_position1)
plan1 = arm.plan()
plan_list.append(plan1)
# 保存轨迹
traj_pack(plan1, save_name = "trajectory1")
# 设置初始状态
state.joint_state.position = aim_position1
arm.set_start_state(state)
# 设置目标状态
aim_position2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
arm.set_joint_value_target(aim_position2)
plan2 = arm.plan()
plan_list.append(plan2)
plan2 = stitch_trajectory(plan_list) # 拼接轨迹1和2
traj_pack(plan2, save_name = "trajectory2")
# 设置初始状态
state.joint_state.position = aim_position2
arm.set_start_state(state)
# 设置目标状态
aim_position3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.57]
arm.set_joint_value_target(aim_position3)
plan3 = arm.plan()
plan_list.append(plan3)
plan3 = stitch_trajectory(plan_list) # 拼接轨迹1、2和3
traj_pack(plan3, save_name = "trajectory3")
# 执行轨迹
arm.execute(plan3)
# exit(0)
####################################轨迹可视化#############################################
plan = plan3 # 用于可视化的轨迹
traj_len = len(plan.joint_trajectory.points)
print "traj_len:", traj_len
time_ls, pos, vel, acc = traj_pack(plan)
# 显示原始轨迹
print "\npos", pos
print "\nvel", vel
print "\nacc", acc
# plt.plot(time_ls, pos, 'ro') # 轨迹点
# plt.plot(time_ls, pos, 'k') # 连接轨迹点
# plt.show()
plt.figure(figsize=(6, 10))
plt.subplot(3,1,1)
plt.plot(time_ls, pos, 'ro')
# plt.plot(time_ls, pos, 'k')
plt.grid('on')
plt.title('Origin trajectory')
plt.xlabel('time (s)')
plt.ylabel('position (rad)')
plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5)
plt.ylim(min(pos) - 0.1, max(pos) + 0.1)
plt.subplot(3,1,2)
plt.plot(time_ls, vel, 'ro')
# plt.plot(time_ls, vel, 'k')
plt.grid('on')
plt.xlabel('time (s)')
plt.ylabel('velocity (rad / s)')
plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5)
plt.subplot(3,1,3)
plt.plot(time_ls, acc, 'ro')
# plt.plot(time_ls, acc, 'k')
plt.grid('on')
plt.xlabel('time (s)')
plt.ylabel('acceleration (rad / s$^{2}$)')
plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5)
plt.savefig('Origin trajectory.svg', dpi=600, bbox_inches='tight')
# plt.savefig('Origin trajectory.pdf', dpi=600, bbox_inches='tight')
plt.show()
# 插值测试
# Interpolation(time_ls, pos, vel, acc)
# 关闭并退出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')
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 = 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 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' These boxes are commented as we do not need them
#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) These boxes are commented as we do not need them
#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.4
# 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] commented for the same reasons as previously
#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() These two blocks of code are commented as they assign postion to unwanted boxes
#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)
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!")
# 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:
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 execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO,
"\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if self._gdict.has_key(clist[1]):
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.FAIL,
"Initializing " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.FAIL,
"Unable to initialize " + clist[1])
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (MoveGroupInfoLevel.INFO,
"trace is on" if self._trace else "trace is off")
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, 'r')
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print("%s:%d: %s" %
(filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (MoveGroupInfoLevel.WARN,
"Error at %s:%d: %s\n%s" %
(filename, line_num, line_content, str(e)))
else:
return (MoveGroupInfoLevel.WARN,
"Processing " + filename + ": " + str(e))
except:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" %
(filename, line_num, line_content))
else:
return (MoveGroupInfoLevel.WARN,
"Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, 'w')
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" +
" ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " +
str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
ptu_action = FollowTrajectoryClient(
"ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"])
ptu_action.move_to([
0.0,
-0.75,
])
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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()
print("End Effector Link", end_effector_link)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.05)
# 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 15 seconds per planning attempt
right_arm.set_planning_time(15)
# 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 = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
find_topic = "basic_grasping_perception/find_objects"
sub_topic = "handles_position"
rospy.Subscriber(sub_topic, target_pose, getHandlesCb)
rospy.loginfo(
"Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient(
find_topic, 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('right_down')
# 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(10.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))
rospy.loginfo("Found %d Support Surfaces" %
len(find_result.support_surfaces))
# print("perception_results_primitives",find_result.objects.primitives[0])
# print("perception_results_primitives_poses",find_result.objects.primitives_poses)
# 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
# target_position = None
the_object = None
the_object_dist = 1.0
count = -1
# target_size
# 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
print("target_size %d ", count, target_size)
# target_position = obj.object.primitive_poses[0].position
# print("target_size %d ", %count, target_size )
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
print("target_pose_before ", target_pose)
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.519617092464
target_pose.pose.orientation.y = -0.510439243162
target_pose.pose.orientation.z = 0.479912175114
target_pose.pose.orientation.w = -0.489013456294
print("target_pose_after ", target_pose)
# 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] ]
# obj.primitive_poses[0].position.z += -height/1.5
# 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
print("table_size", table_size)
# 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]
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)
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize
import moveit_commander
import copy
joint_state_topic = ['joint_states:=/robot/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.init_node('foo', anonymous=False)
roscpp_initialize(sys.argv)
both_arms = MoveGroupCommander('both_arms')
right_arm = MoveGroupCommander('right_arm')
pose1 = right_arm.get_current_pose().pose
pose2 = copy.deepcopy(pose1)
pose2.position.z -= 0.1
print pose1
print pose2
waypoints = [pose1, pose2]
(plan, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoints to follow
0.01, # eef_step
0.0)
print plan
print fraction
right_arm.execute(plan, wait=True)
rospy.sleep(0.5)
raw_input("please input")
# [00000000]
print right_arm.get_joint_value_target()
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('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置（单位：米）和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
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)
# 控制机械臂先回到初始化位置
arm.set_named_target("home")
arm.go()
rospy.sleep(2)
# 设置桌面的高度
table_groud = 0.25
# 设置table，box1，box2的三维尺寸
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.26
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_groud + 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_groud + 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_groud + table_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, 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.2
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_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
gripper = robot_gripper.Gripper('left')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
#Construct the request
request1 = GetPositionIKRequest()
request1.ik_request.group_name = "left_arm"
request1.ik_request.ik_link_name = "left_gripper"
request1.ik_request.attempts = 20
request1.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request1.ik_request.pose_stamped.pose.position.x = .663
request1.ik_request.pose_stamped.pose.position.y = .341
request1.ik_request.pose_stamped.pose.position.z = 0
request1.ik_request.pose_stamped.pose.orientation.x = 0.0
request1.ik_request.pose_stamped.pose.orientation.y = 1.0
request1.ik_request.pose_stamped.pose.orientation.z = 0.0
request1.ik_request.pose_stamped.pose.orientation.w = 0.0
#Request 2
request2 = GetPositionIKRequest()
request2.ik_request.group_name = "left_arm"
request2.ik_request.ik_link_name = "left_gripper"
request2.ik_request.attempts = 50
request2.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request2.ik_request.pose_stamped.pose.position.x = .802
request2.ik_request.pose_stamped.pose.position.y = .048
request2.ik_request.pose_stamped.pose.position.z = 0
request2.ik_request.pose_stamped.pose.orientation.x = 0.0
request2.ik_request.pose_stamped.pose.orientation.y = 1.0
request2.ik_request.pose_stamped.pose.orientation.z = 0.0
request2.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request1)
#Print the response HERE
print(response)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request1.ik_request.pose_stamped)
# Plan IK and execute
group.go()
#Calibration
print('Calibrating...')
gripper.calibrate()
rospy.sleep(2.0)
#Close the gripper
print('Closing...')
gripper.close()
rospy.sleep(1.0)
response2 = compute_ik(request2)
#Print the response HERE
print(response2)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request2.ik_request.pose_stamped)
# Plan IK and execute
group.go()
#Open the gripper
print('Opening...')
gripper.open()
rospy.sleep(1.0)
print('Done!')
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
# arm.set_goal_position_tolerance(0.01)
# arm.set_goal_orientation_tolerance(0.1)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('cali_2')
# arm.set_named_target('test_1')
arm.set_joint_value_target(
[0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575])
arm.go()
# exit()
pose = cal_end_pose_by_quat(arm.get_current_pose().pose, -0.2, 2)
arm.set_pose_target(pose)
arm.go()
# raw_input()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose().pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(start_pose)
# 按末端坐标系方向向量平移, 计算终点位姿
wpose = cal_end_pose_by_quat(start_pose, 0.35, 2)
waypoints.append(deepcopy(wpose))
def scale_trajectory_speed(traj, scale):
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.positions = traj.joint_trajectory.points[i].positions
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / scale
point.velocities = list(
traj.joint_trajectory.points[i].velocities)
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[
j] = point.accelerations[j] * scale * scale
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def move_cartesian(waypoints, scale):
group = arm
# 开始笛卡尔空间轨迹规划
fraction = 0.0 # 路径规划覆盖率
maxtries = 10 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
group.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = group.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.")
plan = scale_trajectory_speed(plan, scale)
group.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
move_cartesian(waypoints, 0.5)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
#!/usr/bin/env python
from moveit_commander import MoveGroupCommander
import rospy
if __name__ == '__main__':
group = MoveGroupCommander("manipulator")
rospy.init_node("vs060_demo_wy")
temp_pose = group.get_current_pose()
temp_pose.pose.position.z = temp_pose.pose.position.z - 0.1
group.set_pose_target(temp_pose)
group.go()
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'coke_can')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.06)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~endeffector', 'endeffector')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
self._manipulator = MoveGroupCommander("manipulator")
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.5
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def graspobject():
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
right_arm.set_planner_id('RRTConnectkConfigDefault')
# 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_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
#right_arm.set_end_effector_link(end_effector_link)
#start_pose = Pose()
#start_pose.position.x = 0.587
#start_pose.position.y = 0.034
#start_pose.position.z = -0.010
#start_pose.orientation.x =0.508
#start_pose.orientation.y = 0.508
#start_pose.orientation.z = -0.491
#start_pose.orientation.w = 0.491
#right_arm.set_pose_target(start_pose)
#traj = right_arm.plan()
#rospy.sleep(2)
#right_arm.execute(traj)
#rospy.sleep(2)
global getkey
print '回到初始位置结束'
startclient = rospy.ServiceProxy('/detect_grasps/actionstart', npy)
startclient(0)
print 'call start over'
sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspcallback)
rate = rospy.Rate(200)
while not rospy.is_shutdown():
if getkey==1:
break
rate.sleep()
getkey=0
global grasps
print '检测到抓取位置，开始执行'
global ser
ser.write(('0\n').encode())
rospy.sleep(2)
print "-start movingA------------------------------------"
pose = []
currentcamera=current_cameralinkpose()
grasppose1,grasppose2= transformgrasp(currentcamera,grasps,0.24,0.15)
start_pose = current_pose()
print 'start_pose',start_pose
pose.append(start_pose)
pose.append(grasppose1)
pose.append(grasppose2)
#end_pose = copy.deepcopy(start_pose)
# end_pose.position.x = grasps.approach.x
# end_pose.position.y = grasps.approach.y
# end_pose.position.z = grasps.approach.z
catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
print "-start movingB------------------------------------"
print '抓'
ser.write(('1\n').encode())
rospy.sleep(2)
##############################################################
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(1)
right_arm.execute(plan2)#先移到观测姿态
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =2.6649599075317383#机器人的观测姿态定义在这里
group_variable_values[1] = -1.493981663380758
group_variable_values[2] = -1.7679532209979456
group_variable_values[3] =-1.3932693640338343
group_variable_values[4] = 1.5719022750854492
group_variable_values[5] = 1.5657243728637695
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(2)
right_arm.execute(plan2)#先移到观测姿态
print "-start movingB------------------------------------"
print '笛卡尔插值到放置位置完成'
ser.write(('0\n').encode())
rospy.sleep(2)
# pose = []
#posecurrent=current_pose()#获取现有的机器人末端姿态
# pose.append(posecurrent)
#start_pose = Pose()#似乎这里不能接受相对的移动
#start_pose= objectpose#获取目标末端姿态
# posecurrent.position.z =posecurrent.position.z -0.1
# pose.append(posecurrent)
# print start_pose
print "-------------------------------------"
# print pose
print "-------------------------------------"
# catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
#回到初始位置
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(1)
right_arm.execute(plan2)#先移到观测姿态
rospy.sleep(3)
print '回到初始位置结束'
startclient2 = rospy.ServiceProxy('/detect_grasps/actionover', npy)
startclient2(0)
print 'call start over'
def __init__(self, manip_name="manipulator", eef_name="endeffector"):
self.robot = RobotCommander()
self.man = MoveGroupCommander(manip_name)
self.eef = MoveGroupCommander(eef_name)
self.target_poses = []
self.picked = []
def __init__(self):
# Initialize the move_group API
print 'starting'
moveit_commander.roscpp_initialize(sys.argv)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
right_arm.set_planner_id('RRTConnectkConfigDefault')
# 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_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
#right_arm.set_end_effector_link(end_effector_link)
# Get the current pose so we can add it as a waypoint
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(2)
right_arm.execute(plan2)#先移到观测姿态
try:
rate = rospy.Rate(10)
while not rospy.is_shutdown():
print '.'
graspobject( )
rate.sleep()
except rospy.ROSInterruptException:
pass
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)
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'test'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.5
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# # 将三个物体加入场景当中
# table_pose = PoseStamped()
# table_pose.header.frame_id = REFERENCE_FRAME
# table_pose.pose.position.x = 0.80
# 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.76
# 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.74
# 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)
#
# # 将桌子设置成红色，两个box设置成橙色
# self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
# self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.02, 0.01, 0.12]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.72
target_pose.pose.position.y = 0.22
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.62
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 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)
# 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)
#!/usr/bin/env python
# coding: utf-8
import sys
import rospy
import copy
import geometry_msgs.msg
from moveit_commander import MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
from math import sin, copysign
if __name__ == '__main__':
print "--- Straight line gesture ---"
rospy.init_node('straight_line', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
"""
start_pose = geometry_msgs.msg.Pose()
start_pose.position.x = -0.0206330384032
start_pose.position.y = 0.077582282778
start_pose.position.z = 1.39283677496
start_pose.orientation.x = 0.5
start_pose.orientation.y = 0.5
start_pose.orientation.z = 0.5
start_pose.orientation.w = 0.5
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "Plan start"
rospy.sleep(5)
right_arm.execute(plan_start)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = position[0]
request.ik_request.pose_stamped.pose.position.y = position[1]
request.ik_request.pose_stamped.pose.position.z = position[2]
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
# response = compute_ik(request)
# #Print the response HERE
# print(response)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
# group.close()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
position_index -= 1
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
roscpp_initialize(sys.argv)
rospy.init_node('control_Husky_UR3', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
##모바일 파트 관련 변수 선언
x = 0.0
y = 0.0
theta = 0.0
## 매니퓰레이터 변수 선언
group_name = "ur3_manipulator"
move_group = MoveGroupCommander(group_name)
FIXED_FRAME = 'world'
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=20)
def newOdom(msg):
global x
global y
global theta
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
#position_list = [[0.861, 0.139, -0.152]]
#position_list = [[0.5, 0.5,0.0]]
position_list = [[0.618, 0.424, -0.169]]
position_index = 0
while not rospy.is_shutdown():
# raw_input('Press [ Enter ]: ')
if position_index >= len(position_list):
break
position = position_list[position_index]
position_index += 1
# while True:
# position = raw_input('Press enter to compute an IK solution:(x,y,z)\n')
# position = position.split()
# if len(position) != 3:
# print "error: Please enter 3 floats"
# else:
# position = [float(i) for i in position]
# break
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = position[0]
request.ik_request.pose_stamped.pose.position.y = position[1]
request.ik_request.pose_stamped.pose.position.z = position[2]
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
# response = compute_ik(request)
# #Print the response HERE
# print(response)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
# group.close()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
position_index -= 1
#!/usr/bin/env python
# coding: utf-8
import sys
import rospy
import copy
import geometry_msgs.msg
import moveit_msgs
from std_msgs.msg import Float32MultiArray
from moveit_commander import MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
from math import sin, copysign
if __name__ == '__main__':
rospy.init_node('mimic', anonymous=True)
right_shoulder_to_wrist = MoveGroupCommander("right_arm")
start_state = right_shoulder_to_wrist.getCurrentState()
print(start_state)
print "---------------------------------------End"
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
