def create_scene(self):
scene = PlanningSceneInterface()
rospy.sleep(1)
# clean the scene
scene.remove_world_object('box')
scene.remove_world_object('object')
#return
obj_pose = PoseStamped()
#p.header.frame_id = robot.get_planning_frame()
obj_pose.header.frame_id = 'world'
obj_pose.pose.position = self.obj_pos
obj_pose.pose.orientation = self.obj_ort
scene.add_mesh(
'grasping_object', obj_pose,
'/home/kai/Workspace/urlg_robot_ws/src/urlg_robots_gazebo/worlds/objects/pringle/optimized_poisson_texture_mapped_mesh.dae'
)
box_pose = PoseStamped()
#p.header.frame_id = robot.get_planning_frame()
box_pose.header.frame_id = 'world'
box_pose.pose.position = self.box_pos
box_pose.pose.orientation = self.box_ort
scene.add_box('box', box_pose, (0.5, 0.5, 0.5))
rospy.sleep(1)
def setup_environment(lower_arm, upper_arm, botharms):
# add object to Planning Scene
rospy.loginfo( "Planning Scene Settings")
scene = PlanningSceneInterface()
rospy.sleep(2) # Waiting for PlanningSceneInterface
scene.remove_world_object()
# upper tool
box_pose = PoseStamped()
box_pose.header.frame_id = upper_arm.get_planning_frame()
pos = upper_arm.get_current_pose()
rot_o = 90.0/180.0*math.pi
rot_a = 0.0/180.0*math.pi
rot_t = -90.0/180.0*math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
# setting origin of upper tool
box_pose.pose.position.x = pos.pose.position.x
box_pose.pose.position.y = pos.pose.position.y
box_pose.pose.position.z = pos.pose.position.z
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
rospack = rospkg.RosPack()
resourcepath = rospack.get_path('khi_robot_sample')+"/config/work/"
meshpath = resourcepath + "upper_tool_001.stl"
scene.attach_mesh('upper_link_j4', 'upper_tool', box_pose, meshpath, (1,1,1),['upper_link_j3','upper_link_j4'])
rospy.sleep(1)
rospy.loginfo( "Planning Scene Settings Finish")
class CollisionSceneExample(object):
def __init__(self):
self._scene = PlanningSceneInterface()
# clear the scene
self._scene.remove_world_object()
self.robot = RobotCommander()
# pause to wait for rviz to load
print(
"============ Waiting while RVIZ displays the scene with obstacles..."
)
# TODO: need to replace this sleep by explicitly waiting for the scene to be updated.
rospy.sleep(2)
def add_one_box(self):
box1_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1]
box1_dimensions = [0.25, 0.25, 0.75]
self.add_box_object("box1", box1_dimensions, box1_pose)
print("============ Added one obstacle to RViz!!")
def add_four_boxes(self):
box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1]
box1_dimensions = [0.2, 0.2, 0.5]
box2_pose = [-0.55, -0.55, 0, 0, 0, 0, 1]
box2_dimensions = [0.25, 0.25, 1.75]
box3_pose = [0.5, -0.55, 0.14, 0, 0, 0, 1]
box3_dimensions = [0.28, 0.28, 0.22]
box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1]
box4_dimensions = [0.25, 0.25, 1.1]
self.add_box_object("box1", box1_dimensions, box1_pose)
self.add_box_object("box2", box2_dimensions, box2_pose)
self.add_box_object("box3", box3_dimensions, box3_pose)
self.add_box_object("box4", box4_dimensions, box4_pose)
print("========== Added 4 obstacles to the scene!!")
def add_box_object(self, name, dimensions, pose):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.position.z = pose[2]
p.pose.orientation.x = pose[3]
p.pose.orientation.y = pose[4]
p.pose.orientation.z = pose[5]
p.pose.orientation.w = pose[6]
self._scene.add_box(name, p,
(dimensions[0], dimensions[1], dimensions[2]))
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化需要使用move group控制的机械臂中的arm group
rospy.Subscriber("obj_pose", Pose, obj_pose_get)
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
rospy.sleep(1)
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.3, 0.01]
box1_size = [0.03, 0.001, 0.08]
box2_size = [0.03, 0.03, 0.05]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = table_size[0]/2.0+obj_pose.position.x - 0.05
table_pose.pose.position.y = obj_pose.position.y
table_pose.pose.position.z = obj_pose.position.z - table_size[2] / 2.0 - box1_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 = obj_pose.position.x
box1_pose.pose.position.y = obj_pose.position.y
box1_pose.pose.position.z = obj_pose.position.z
box1_pose.pose.orientation.w = 1.0
# scene.add_box(box1_id, box1_pose, box1_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0, 0, 0, 1)
self.setColor(box1_id, 0.8, 0, 0, 1.0)
self.setColor(box2_id, 0.8, 0, 0, 1.0)
# self.setColor(table_id, 0.8, 0, 0, 1.0) red
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0) yellow
# self.setColor(box1_id, 1, 1, 1, 1.0) white
# self.setColor(sphere_id, 0, 0, 0, 1) black
# 将场景中的颜色设置发布
self.sendColors()
def handle_clean_moveit_scene(self, req):
scene = PlanningSceneInterface()
rospy.sleep(1)
# clean the scene
#scene.remove_world_object('table_box')
scene.remove_world_object('obj_mesh')
response = ManageMoveitSceneResponse()
response.success = True
return response
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化需要使用move group控制的机械臂中的arm group
rospy.Subscriber("obj_pose", Pose, obj_pose_get)
# 设置场景物体的颜色
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
rospy.sleep(1)
reference_frame = 'ｂase_link'
# 设置桌面的高度
table_ground = 0.37
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.3, 0.01]
box1_size = [0.01, 0.01, 0.19]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = -table_size[0] / 2.0
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.header.frame_id = reference_frame
box1_pose.pose.position.x = -0.09
box1_pose.pose.position.y = table_size[0] / 2.0
box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
class CreateScene1(object):
def __init__(self):
self._scene = PlanningSceneInterface()
# clear the scene
self._scene.remove_world_object()
self.robot = RobotCommander()
# pause to wait for rviz to load
rospy.sleep(4)
floor_pose = [0, 0, -0.12, 0, 0, 0, 1]
floor_dimensions = [4, 4, 0.02]
box1_pose = [0.70, 0.70, 0.25, 0, 0, 0, 1]
box1_dimensions = [0.5, 0.5, 0.5]
box2_pose = [0.5, -0.5, 0.60, 0, 0, 0, 1]
box2_dimensions = [0.25, 0.25, 0.25]
box3_pose = [0.5, -0.5, 0.24, 0, 0, 0, 1]
box3_dimensions = [0.48, 0.48, 0.48]
box4_pose = [-0.8, 0.7, 0.5, 0, 0, 0, 1]
box4_dimensions = [0.4, 0.4, 0.4]
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("box1", box1_dimensions, box1_pose)
self.add_box_object("box2", box2_dimensions, box2_pose)
self.add_box_object("box3", box3_dimensions, box3_pose)
self.add_box_object("box4", box4_dimensions, box4_pose)
def add_box_object(self, name, dimensions, pose):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.position.z = pose[2]
p.pose.orientation.x = pose[3]
p.pose.orientation.y = pose[4]
p.pose.orientation.z = pose[5]
p.pose.orientation.w = pose[6]
self._scene.add_box(name, p,
(dimensions[0], dimensions[1], dimensions[2]))
rospy.sleep(0.2)
class ObjectServer(object):
def __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.p = PoseStamped()
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
def make_object(self,object_name,x,y,z,sx,sy,sz):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object(object_name)
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Making "+object_name+"...")
self.p.header.frame_id = self.robot.get_planning_frame()
self.p.pose.position.x = x
self.p.pose.position.y = y
self.p.pose.position.z = z
self.scene.add_box(object_name, self.p, (sx,sy,sz))
self.wait_for_planning_scene_object(object_name)
def init_mp(cls):
movegroup_ns = ManipulatorActions.get_ns()
robot_description = ManipulatorActions.get_robdesc_ns()
sc = PlanningSceneInterface(ns=movegroup_ns)
mg = MoveGroupCommander('arm',
ns=movegroup_ns,
robot_description=robot_description)
mg.set_max_acceleration_scaling_factor(0.5)
mg.set_max_velocity_scaling_factor(0.6)
mg.set_end_effector_link('tool_frame')
rospy.sleep(1.0)
sc.remove_world_object()
ManipulatorActions.rc = RobotCommander(ns=movegroup_ns)
ManipulatorActions.mg = mg
ManipulatorActions.sc = sc
ManipulatorActions.add_table_plane()
ManipulatorActions.measure_weight(calibrate=True)
ManipulatorActions.move_up(home=True)
ManipulatorActions.measure_weight(calibrate=True)
return mg, sc
def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('paintingrobot_planningscene')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('/renov_up_level/planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 设置目标位置所使用的参考坐标系
reference_frame = 'map'
stl_id = 'stl'
scene.remove_world_object(stl_id)
stl_size = [1.0, 1.0, 1.0]
stl_pose = PoseStamped()
stl_pose.header.frame_id = reference_frame
stl_pose.pose.position.x = 0.0
stl_pose.pose.position.y = 0.0
stl_pose.pose.position.z = -2.4701
stl_pose.pose.orientation.w = 1.0
scene.add_mesh(
stl_id, stl_pose,
'/home/zy/catkin_ws/src/paintingrobot/paintingrobot_underusing/painting_robot_demo/matlab/bim_document/second_scan_2.stl'
)
self.setColor(stl_id, 0.8, 0.4, 0, 1.0)
# 将场景中的颜色设置发布
while not rospy.is_shutdown():
self.sendColors()
# 关闭并退出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()
# 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)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_demo')
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
#cartesian = rospy.get_param('~cartesian', True)
print "===== It is OK ===="
rospy.sleep(3)
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=1)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Set the left arm reference frame
left_arm.set_pose_reference_frame('base')
# Allow 5 seconds per planning attempt
left_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 = 10
# Give the scene a chance to catch up
rospy.sleep(2)
#object1_id = 'object1'
table_id = 'table'
target_id = 'target'
#tool_id = 'tool'
#obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
#scene.remove_world_object(object1_id)
scene.remove_world_object(table_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('base', 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
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.0
#object1_size = [0.088, 0.04, 0.02]
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
# 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
#obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = left_reference_frame
#obstacle1_pose.pose.position.x = 0.96
#obstacle1_pose.pose.position.y = 0.24
#obstacle1_pose.pose.position.z = 0.04
#obstacle1_pose.pose.orientation.w = 1.0
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
#self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
#object1_pose = PoseStamped()
#object1_pose.header.frame_id = left_reference_frame
#object1_pose.pose.position.x = 0.80
#object1_pose.pose.position.y = 0.04
#object1_pose.pose.position.z = table_ground + table_size[2] + object1_size[2] / 2.0
#object1_pose.pose.orientation.w = 1.0
#scene.add_box(object1_id, object1_pose, object1_size)
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = left_reference_frame
table_pose.pose.position.x = 1
table_pose.pose.position.y = 0.7
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 = left_reference_frame
target_pose.pose.position.x = 1
target_pose.pose.position.y = 0.7
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1
# 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(object1_id, 0.8, 0, 0, 1.0)
self.setColor(table_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
left_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 = left_reference_frame
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = -0.18
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
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 = left_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = left_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
left_arm.set_named_target('left_arm_zero')
left_arm.go()
# Open the gripper to the neutral position
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__ == "__main__":
roscpp_initialize(sys.argv)
rospy.init_node("moveit_py_demo", anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
rospy.logwarn("cleaning world")
# right_arm.set_named_target("r_start")
# right_arm.go()
# right_gripper.set_named_target("open")
# right_gripper.go()
rospy.sleep(3)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_link'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.4
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.6
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.60
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.60
box2_pose.pose.position.y = 0.25
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
rospy.sleep(2)
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.40
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.1
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
#glovebox pose
p2.pose.position.x = 0
p2.pose.position.y = 0
p2.pose.position.z = 0.56
b2 = tf.transformations.quaternion_from_euler(1.57,0,0)
p2.pose.orientation.x = b2[0]
p2.pose.orientation.y = b2[1]
p2.pose.orientation.z = b2[2]
p2.pose.orientation.w = b2[3]
#detach/remove current scene objects
print 'removing objects...'
robot.detach_object("bowl")
rospy.sleep(1)
scene.remove_world_object("bowl")
scene.remove_world_object("punch")
scene.remove_world_object("glovebox")
rospy.sleep(2)
#reset the gripper and arm position to home
robot.set_start_state_to_current_state()
robot.set_named_target("start_glove")
robot.go()
gripper.set_start_state_to_current_state()
gripper.go()
#add scene objects
print 'adding scene objects'
scene.add_mesh("bowl", p, "8inhemi.STL")
class scene_generator:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('scene_generator')
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('custom_1')
table_id = 'table'
self.tid = self.pwh.name.index('table')
obstacle1_id = 'obstacle1'
self.o1id = self.pwh.name.index('wood_cube_5cm')
obstacle2_id = 'obstacle2'
self.o2id = self.pwh.name.index('custom_2')
# Set the sizes [l, w, h]
table_size = [1.5, 0.8, 0.03]
target_size = [0.05, 0.05, 0.15]
obstacle1_size = [0.05, 0.05, 0.05]
obstacle2_size = [0.05, 0.05, 0.10]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
if self.obj_att is None:
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
obstacle1_pose = PoseStamped()
obstacle1_pose.header.frame_id = REFERENCE_FRAME
obstacle1_pose.pose = self.pwh.pose[self.o1id]
obstacle1_pose.pose.position.z += 0.025
# Add the target object to the scene
self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
obstacle2_pose = PoseStamped()
obstacle2_pose.header.frame_id = REFERENCE_FRAME
obstacle2_pose.pose = self.pwh.pose[self.o2id]
# Add the target object to the scene
self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
#threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.orientation.w = 1.0
p.pose.position.x = 0.15
p.pose.position.y = -0.3
p.pose.position.z = 0.55
id_part = 1
while not rospy.is_shutdown():
# clean the scene
scene.remove_world_object("part" + str(id_part - 1))
# publish a demo scene
new_p = copy.deepcopy(p)
new_p.pose.position.x += random.random() * 0.07 * random.choice([1, -1])
new_p.pose.position.y += random.random() * 0.07 * random.choice([1, -1])
new_p.pose.position.z += random.random() * 0.07 * random.choice([1, -1])
scene.add_box("part" + str(id_part), new_p, (0.05, 0.05, 0.05))
print "Picking " + "part" + str(id_part) + " at: " + str(new_p.pose.position)
rospy.sleep(0.5)
# pick an object
robot.right_arm.pick("part" + str(id_part))
id_part += 1
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.3
box1_pose.pose.position.y = 0
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.3
box2_pose.pose.position.y = 0.25
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
rospy.wait_for_service('grasp_hand')
scene = PlanningSceneInterface()
robot = RobotCommander()
pose = PoseStamped()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("grasp_point")
scene.remove_world_object("right_shoe")
# move_group
group_right = MoveGroupCommander("r_arm")
group_left = MoveGroupCommander("l_arm")
#Get Current state
state = robot.get_current_variable_values()
# Link off contact
links_off=[]
links_off.append("r_thumb1")
links_off.append("r_thumb2")
links_off.append("r_thumb3")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
eef = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
# right_arm.set_goal_position_tolerance(0.05)
# right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
#grasp_pose.header.frame_id = 'gazebo_wolrd'
# Shift the grasp pose by half the width of the target to center it
# grasp_pose.pose.position.y -= target_size[1] / 2.0
# grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0
# grasp_pose.pose.position.x = target_pose.pose.position.x -0.07
# grasp_pose.pose.position.z += 0.18
#Allowed touch object list
# ato = [target_id, 'r_forearm_link']
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id]
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
# print grasp.grasp_pose
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
#Allowed touch link list
atl = ['r_forearm_link']
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_pick_attempts:
success = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(0.2)
if success:
self.ac.send_goal(g_close)
rospy.sleep(3)
## If the pick was successful, attempt the place operation
#if success:
#success = False
#n_attempts = 0
## Generate valid place poses
#places = self.make_places(place_pose)
## Repeat until we succeed or run out of attempts
#while success == False and n_attempts < max_place_attempts:
#for place in places:
#success = right_arm.place(target_id, place)
#if success:
#break
#n_attempts += 1
#rospy.loginfo("Place attempt: " + str(n_attempts))
#rospy.sleep(0.2)
#if not success:
#rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
#else:
#rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
## Return the arm to the "resting" pose stored in the SRDF file
##right_arm.set_named_target('resting')
##right_arm.go()
## Open the gripper to the neutral position
#right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
#right_gripper.go()
#rospy.sleep(1)
#rospy.spin()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.left_arm = MoveGroupCommander('left_arm')
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.left_gripper = MoveGroupCommander('left_gripper')
# eel = len(self.right_arm.get_end_effector_link())
# print eel
# Allow 5 seconds per planning attempt
# self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
### Attach / Remove Object Flag ###
self.aro = None
# Run and keep in the BG the scene generator with ctrl^c kill ###
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
## Give some time to ensure the thread starts!! ##
rospy.sleep(5)
### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ###
self.idx_list = self.bl()
### GIVE SCENE TIME TO CATCH UP ###
rospy.sleep(5)
################################## GRASP EXECUTION #####################################
print "==================== Executing ==========================="
start_time = time.time()
### PERSONAL REMINDER!!! WHAT IS WHAT!!! ###
# print obj_id[obj_id.index('target')]
# print obj_id.index('target')
### MOVE LEFT ARM OUT OF THE WAY ###
self.lasp()
success = False
while success == False and len(self.idx_list)>0:
success, pgr_target = self.grasp_attempt()
print ("GA Returns:", success)
if success is not False:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_target, obj_id.index('target'),'true')
self.place_object(obj_id.index('target'))
break
else:
idx = self.idx_list[0]
ds, pgr_col_obj = self.declutter_scene(idx)
print ("DS Returns:", ds)
if ds == True:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true')
self.place_object(obj_id.index(obj_id[idx]))
self.idx_list.pop(0)
print "==================== THE END! ======================"
print("--- %s seconds ---" % (time.time() - start_time))
rospy.sleep(5)
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def grasp_attempt(self):
# start_time = time.time()
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
# while obj_id[obj_id.index('target')] is not 'target':
# print '!!!!!'
# rospy.sleep(0.05)
pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg')
gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i = 1
for pg, gr in izip(pre_grasps, grasps):
self.gripper_pose_pub.publish(gr)
print ("G Attempt: ", i)
plp = self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go(wait=True)
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
success = True
retreat = gr
print "Grasping"
break
# print("--- %s seconds ---" % (time.time() - start_time))
return success , retreat
def declutter_scene(self,index):
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
pg = self.grasp_pose(obj_pose[index], axis, 'pg')
gp = self.grasp_pose(obj_pose[index], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i= 1
for pg, gr in izip(pre_grasps, grasps):
plp = self.right_arm.plan(pg.pose)
print (" DS Attempt: ", i)
self.gripper_pose_pub.publish(gr)
self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go()
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
print "Grasping"
success = True
retreat = gr
break
return success, retreat
def place_object(self, obj_idx):
self.aro = obj_idx
### GENERATE PLACE POSES ###
places = self.place_generator()
### TRY THESE POSES ###
i = 1
for place in places:
print (" Place Attempt: ", i)
plpl = self.right_arm.plan(place.pose)
print len(plpl.joint_trajectory.points)
if len(plpl.joint_trajectory.points) == 0:
i+=1
continue
self.right_arm.plan(plpl)
self.right_arm.go(wait=True)
### INFORM SCENE ###
# self.open_gripper()
# self.aro = None
### RETURN HAND TO STARTING POSITION ###
self.post_grasp(place,obj_idx, 'false')
self.rasp()
break
def post_grasp(self,new_pose, obj_idx, fl):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
if fl == 'true':
self.close_gripper()
self.aro = obj_idx
else:
self.open_gripper()
self.aro = None
rospy.sleep(2)
### POST GRASP RETREAT ###
M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w])
M1[0,3] = new_pose.pose.position.x
M1[1,3] = new_pose.pose.position.y
M1[2,3] = new_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T1 = np.dot(M2, M1)
npo = deepcopy(new_pose)
npo.pose.position.x = T1[0,3]
npo.pose.position.y = T1[1,3]
npo.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
npo.pose.orientation.x = quat[0]
npo.pose.orientation.y = quat[1]
npo.pose.orientation.z = quat[2]
npo.pose.orientation.w = quat[3]
npo.header.frame_id = REFERENCE_FRAME
self.right_arm.plan(npo.pose)
self.right_arm.go(wait=True)
def place_generator(self):
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.57
place_pose.pose.position.y = 0.16
place_pose.pose.position.z = 0.56
place_pose.pose.orientation.w = 1.0
P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w])
P[0,3] = place_pose.pose.position.x
P[1,3] = place_pose.pose.position.y
P[2,3] = place_pose.pose.position.z
places =[]
yaw_angles = [0, 1,57, -1,57 , 3,14]
x_vals = [0, 0.05 ,0.1 , 0.15]
z_vals = [0.05 ,0.1 , 0.15]
for y in yaw_angles:
G = transformations.euler_matrix(0, 0, y)
G[0,3] = 0.0 # offset about x
G[1,3] = 0.0 # about y
G[2,3] = 0.0 # about z
for z in z_vals:
for x in x_vals:
TM = np.dot(P, G)
pl = deepcopy(place_pose)
pl.pose.position.x = TM[0,3] +x
pl.pose.position.y = TM[1,3]
pl.pose.position.z = TM[2,3] +z
quat = transformations.quaternion_from_matrix(TM)
pl.pose.orientation.x = quat[0]
pl.pose.orientation.y = quat[1]
pl.pose.orientation.z = quat[2]
pl.pose.orientation.w = quat[3]
pl.header.frame_id = REFERENCE_FRAME
places.append(deepcopy(pl))
return places
def grasp_pose(self, target_pose, axis, stage):
############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE #########
if axis == 0:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
if stage == 'pg':
M2[0,3] = -0.25 # offset about x
elif stage == 'gp':
M2[0,3] = -0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 1:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = -0.25 # about y
elif stage == 'gp':
M2[1,3] = -0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 2:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, -1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = 0.25 # about y
elif stage == 'gp':
M2[1,3] = 0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 3:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 3.14)
if stage == 'pg':
M2[0,3] = 0.25 # offset about x
elif stage == 'gp':
M2[0,3] = 0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 4:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
else:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(1.57, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
T1 = np.dot(M1, M2)
grasp_pose = deepcopy(target_pose)
grasp_pose.pose.position.x = T1[0,3]
grasp_pose.pose.position.y = T1[1,3]
grasp_pose.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
grasp_pose.pose.orientation.x = quat[0]
grasp_pose.pose.orientation.y = quat[1]
grasp_pose.pose.orientation.z = quat[2]
grasp_pose.pose.orientation.w = quat[3]
grasp_pose.header.frame_id = REFERENCE_FRAME
return grasp_pose
def grasp_generator(self, initial_poses):
# A list to hold the grasps
grasps = []
o = [] # Original Pose of the object (o)
O=[]
i= 0
while i < len(initial_poses):
o.append(initial_poses[i])
i+=1
G = transformations.euler_matrix(0, 0, 0)
# Generate a grasps for along z axis (x and y directions)
k = 0
while k <= 5:
O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w]))
O[k][0,3] = o[k].pose.position.x
O[k][1,3] = o[k].pose.position.y
O[k][2,3] = o[k].pose.position.z
if k in range(0,4):
for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ###
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3] +z
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
elif k == 4:
for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3] +x
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
else:
for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3] +y
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
k+=1
print len(grasps)
# Return the list
return grasps
def scene_generator(self):
while True:
# print "happening"
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
# obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
self.setColor(obj_id[i], 1, 0.623, 0, 1.0)
### Make the target purple and table green ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0)
self.scene.remove_attached_object(GRIPPER_FRAME)
# Send the colors to the planning scene
self.sendColors()
else:
if self.flag == 0:
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
self.flag +=1
time.sleep(0.5)
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def bl(self):
blist = ['target','custom_2','custom_3', 'custom_table']
self.blist = []
for name in obj_id:
if name not in blist:
self.blist.append(obj_id.index(name))
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)])
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
return self.blist
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0899, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def lasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.3665
sp.pose.position.y = 0.74094
sp.pose.position.z = 1.1449
sp.pose.orientation.x = 0.80503
sp.pose.orientation.y = -0.18319
sp.pose.orientation.z = 0.31988
sp.pose.orientation.w = 0.46481
self.left_arm.plan(sp)
self.left_arm.go(wait=True)
def rasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.39571
sp.pose.position.y = -0.40201
sp.pose.position.z = 1.1128
sp.pose.orientation.x =0.00044829
sp.pose.orientation.y = 0.57956
sp.pose.orientation.z = 9.4878e-05
sp.pose.orientation.w = 0.81493
self.right_arm.plan(sp)
self.right_arm.go(wait=True)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('MotionSequence')
# 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 left wam finger poses
self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped)
self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped)
self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Define move group comander for each moveit group
left_wam = moveit_commander.MoveGroupCommander('left_wam')
right_wam = moveit_commander.MoveGroupCommander('right_wam')
left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1')
left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2')
left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3')
right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')
left_wam.set_planner_id("PRMstarkConfigDefault")
right_wam.set_planner_id("PRMstarkConfigDefault")
#left_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
# Get the name of the end-effector link
left_end_effector_link = left_wam.get_end_effector_link()
right_end_effector_link = right_wam.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link))
rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link))
# Allow some leeway in position (meters) and orientation (radians)
right_wam.set_goal_position_tolerance(0.01)
right_wam.set_goal_orientation_tolerance(0.05)
left_wam.set_goal_position_tolerance(0.01)
left_wam.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_wam.set_planning_time(15)
left_wam.set_planning_time(25)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Set the reference frame for wam arms
left_wam.set_pose_reference_frame(REFERENCE_FRAME)
right_wam.set_pose_reference_frame(REFERENCE_FRAME)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
spoon_id = 'spoon'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
scene.remove_world_object(spoon_id)
# Remove leftover objects from a previous run
scene.remove_attached_object(right_end_effector_link, 'spoon')
#right_wam.set_named_target('right_wam_start')
#right_wam.go()
#rospy.sleep(2)
# Closing the hand first
# Closing the hand
#right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
#right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
#right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
#right_wam_finger_1.execute(right_wam_finger_1.plan())
#rospy.sleep(5)
#right_wam_finger_2.execute(right_wam_finger_2.plan())
#rospy.sleep(5)
#right_wam_finger_3.execute(right_wam_finger_3.plan())
#rospy.sleep(5)
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = right_end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.02
# Align the object with the gripper (straight out)
p.pose.orientation.x = -0.5
p.pose.orientation.y = 0.5
p.pose.orientation.z = -0.5
p.pose.orientation.w = 0.5
# Attach the tool to the end-effector
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [0.90128, 0.381, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
scene.add_box(table_id, table_pose, table_size)
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = REFERENCE_FRAME
bowl_pose.pose.position.x = 0.015
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
# Set the height of the pitcher
#pitcher_ground = 0.57816875
#pitcher_pose = PoseStamped()
#pitcher_pose.header.frame_id = REFERENCE_FRAME
#pitcher_pose.pose.position.x = 0.25
#pitcher_pose.pose.position.y = 0.847725
#pitcher_pose.pose.position.z = pitcher_ground
#pitcher_pose.pose.orientation.w = -0.5
#pitcher_pose.pose.orientation.z = 0.707
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
#self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0)
self.sendColors()
rospy.sleep(2)
start = input("Start left_wam planning ? ")
# Set the support surface name to the table object
#left_wam.set_support_surface_name(table_id)
#right_wam.set_support_surface_name(table_id)
# Set the target pose.
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.40363476287
target_pose.pose.position.y = 0.847725
target_pose.pose.position.z = 0.721472317843
target_pose.pose.orientation.x = 0.707
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = -0.707
target_pose.pose.orientation.w = 0
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(target_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
left_wam.shift_pose_target(5, 0, left_end_effector_link)
start = input("Left wam starting position ")
# Pause for a second
# Closing the hand
left_wam_finger_1.set_named_target("left_wam_finger_1_grasp")
left_wam_finger_2.set_named_target("left_wam_finger_2_grasp")
left_wam_finger_3.set_named_target("left_wam_finger_3_grasp")
left_wam_finger_1.execute(left_wam_finger_1.plan())
#rospy.sleep(3)
left_wam_finger_2.execute(left_wam_finger_2.plan())
#rospy.sleep(3)
left_wam_finger_3.execute(left_wam_finger_3.plan())
#rospy.sleep(3)
start = input("Left wam hand closing ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose)
intermidiate_pose = deepcopy(end_pose)
intermidiate_pose.position.z = intermidiate_pose.position.z + 0.05
plan = self.StraightPath(end_pose, intermidiate_pose, left_wam)
left_wam.set_start_state_to_current_state()
left_wam.execute(plan)
start = input("Hold up the Pitcher ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose)
intermidiate_pose = deepcopy(end_pose)
intermidiate_pose.position.x = intermidiate_pose.position.x - 0.1
plan = self.StraightPath(end_pose, intermidiate_pose, left_wam)
left_wam.set_start_state_to_current_state()
left_wam.execute(plan)
start = input("left_wam into pouring position ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link))
back_pose = deepcopy(end_pose)
end_pose.pose.orientation.x = 0.97773401145
end_pose.pose.orientation.y = 0
end_pose.pose.orientation.z = -0.209726592658
end_pose.pose.orientation.w = 0
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(end_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
start = input("Pour the water ")
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(back_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link))
end_pose.pose.position.x = end_pose.pose.position.x + 0.1
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(end_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
start = input("Left_wam back to prep position")
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = -0.600350195463908
target_pose.pose.position.y = 0.80576308041
target_pose.pose.position.z = 0.794775212132
target_pose.pose.orientation.x = 3.01203251908e-05
target_pose.pose.orientation.y = 0.705562870053
target_pose.pose.orientation.z = 4.55236739937e-05
target_pose.pose.orientation.w = 0.708647326547
start = input("Start right_wam planning ? ")
right_wam.set_start_state_to_current_state()
right_wam.set_pose_target(target_pose, right_end_effector_link)
right_wam.execute(right_wam.plan())
start = input("right_wam into position. ")
start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6
plan = self.StraightPath(start_pose, intermidiate_pose, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
rospy.sleep(3)
# Closing the hand
right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
right_wam_finger_1.execute(right_wam_finger_1.plan())
#rospy.sleep(3)
right_wam_finger_2.execute(right_wam_finger_2.plan())
#rospy.sleep(3)
right_wam_finger_3.execute(right_wam_finger_3.plan())
rospy.sleep(1)
scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl')
#create a circle path
circles = input("How many circles you want the wam to mix ? ")
start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
plan = self.CircularPath(start_pose, circles, right_wam)
#execute the circle path
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("Mix the oatmeal ")
#put the right_wam back to preparation pose
end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose1 = deepcopy(end_pose)
intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1
plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("wait for the execution of straight path ")
end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose2 = deepcopy(end_pose)
intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25
plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("right_wam back into prep position ")
#left_wam.shift_pose_target(5, 0, left_end_effector_link)
#left_wam.go()
#rospy.sleep(2)
#left_wam.shift_pose_target(0, -0.05, left_end_effector_link)
#left_wam.go()
#rospy.sleep(2)
# Initialize the grasp pose to the target pose
#grasp_pose = target_pose
# Generate a list of grasps
#grasps = self.make_grasps(grasp_pose, [pitcher_id])
# Publish the grasp poses so they can be viewed in RViz
#for grasp in grasps:
# self.left_wam_finger_1_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
# self.left_wam_finger_2_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
# self.left_wam_finger_3_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
grasp.post_grasp_retreat.direction.header.frame_id = frame
grasp.post_place_retreat.direction.header.frame_id = frame
return grasp
if __name__ == "__main__":
roscpp_initialize(sys.argv)
rospy.init_node('moveit_regrasping_app', anonymous=True)
rospy.loginfo("Starting grasp app")
# Access the planning scene
scene = PlanningSceneInterface()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("ground")
scene.remove_world_object("cup")
scene.remove_world_object("pen")
rospy.sleep(1)
# create a pose
p = PoseStamped()
p.header.frame_id = "world" #robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.05
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
print "Esperando al servicio ... grasp_service ... from hermes_grasp_service"
rospy.wait_for_service('grasp_service')
scene = PlanningSceneInterface()
robot = RobotCommander()
pose = PoseStamped()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("right_shoe")
# move_group
group_right = MoveGroupCommander("r_arm")
group_left = MoveGroupCommander("l_arm")
#Get Current state
state = robot.get_current_variable_values()
# Link off contact
links_off=[]
links_off.append("r_thumb1")
links_off.append("r_thumb2")
links_off.append("r_thumb3")
links_off.append("r_index1")
class PigeonPickAndPlace:
#Class constructor (parecido com java, inicializa o que foi instanciado)
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
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)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.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
# Pick 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')
def __del__(self):
# Clean the scene
self._scene.remove_world_object(self._grasp_object_name)
def _add_object_grasp(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame() #pose
p.header.stamp = rospy.Time.now()
rospy.loginfo(self._robot.get_planning_frame())
p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
#p.pose.orientation.x = 0.0028925
#p.pose.orientation.y = -0.0019311
#p.pose.orientation.z = -0.71058
#p.pose.orientation.w = 0.70361
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/,
# using the measure tape tool from meshlab.
# The box is the bounding box of the lego cylinder.
# The values are taken from the cylinder base diameter and height.
# the size is length (x),thickness(y),height(z)
# I don't know if the detector return this values of object
self._scene.add_box(name, p, (0.032, 0.016, 0.020))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
#rospy.logerr('Generated: %f grasps:' % grasps.size)
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
#goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_object_grasp, 0.016)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
self.obj_att = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = target_pose
initial_pose.header.frame_id = 'gazebo_world'
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
# M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
# M1[0,3] = target_pose.pose.position.x
# M1[1,3] = target_pose.pose.position.y
# M1[2,3] = target_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 1.57, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.25 # about z
# T = np.dot(M1, M2)
# pre_grasping = deepcopy(target_pose)
# pre_grasping.pose.position.x = T[0,3]
# pre_grasping.pose.position.y = T[1,3]
# pre_grasping.pose.position.z = T[2,3]
# quat = transformations.quaternion_from_matrix(T)
# pre_grasping.pose.orientation.x = quat[0]
# pre_grasping.pose.orientation.y = quat[1]
# pre_grasping.pose.orientation.z = quat[2]
# pre_grasping.pose.orientation.w = quat[3]
# pre_grasping.header.frame_id = 'gazebo_world'
# self.plan_exec(pre_grasping)
################# GENERATE GRASPS ###################
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
grasps = self.grasp_generator(initial_pose)
possible_grasps = []
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
possible_grasps.append(grasp.pose)
rospy.sleep(0.2)
#print possible_grasps
self.right_arm.pick(target_id, grasps)
# target_name = target_id
# group_name = GROUP_NAME_ARM
# end_effector = GROUP_NAME_GRIPPER
# attached_object_touch_links = ['r_forearm_link']
# #print (target_name, group_name, end_effector)
# PickupGoal(target_name, group_name ,end_effector, possible_grasps )
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def grasp_generator(self, initial_pose):
# Pitch angles to try
pitch_vals = [1, 1.57,0, 1,2 ]
# Yaw angles to try
yaw_vals = [0]#, 1.57, -1.57]
# A list to hold the grasps
grasps = []
g = PoseStamped()
g.header.frame_id = REFERENCE_FRAME
g.pose = initial_pose.pose
#g.pose.position.z += 0.18
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = transformations.quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.pose.orientation.x = q[0]
g.pose.orientation.y = q[1]
g.pose.orientation.z = q[2]
g.pose.orientation.w = q[3]
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
# obstacle1_id = 'obstacle1'
# self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
# obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
if self.obj_att is None:
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
# obstacle1_pose = PoseStamped()
# obstacle1_pose.header.frame_id = REFERENCE_FRAME
# obstacle1_pose.pose = self.pwh.pose[self.o1id]
# # Add the target object to the scene
# scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
#threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
class BTMotion:
def __init__(self, name):
# create messages that are used to publish feedback/result
self._feedback = amazon_challenge_bt_actions.msg.BTFeedback()
self._result = amazon_challenge_bt_actions.msg.BTResult()
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, amazon_challenge_bt_actions.msg.BTAction, execute_cb=self.execute_cb, auto_start = False)
self.pub_posed = rospy.Publisher('arm_posed', String, queue_size=10)
self.pub_rate = rospy.Rate(30)
self._planning_scene = PlanningSceneInterface()
# get ROS parameters
rospy.loginfo('Getting parameters...')
while not rospy.is_shutdown():
try:
self._base_move_params = rospy.get_param('/base_move')
self._timeout = rospy.get_param(name + '/timeout')
self._sim = rospy.get_param(name + '/sim')
self._base_pos_dict = rospy.get_param('/base_pos_dict')
self._left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
self._right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
break
except:
rospy.sleep(random.uniform(0,2))
continue
self._exit = False
while not rospy.is_shutdown():
try:
self._robot = moveit_commander.RobotCommander()
self._left_arm = self._robot.get_group('left_arm')
self._right_arm = self._robot.get_group('right_arm')
self._arms = self._robot.get_group('arms')
self._torso = self._robot.get_group('torso')
self._head = self._robot.get_group('head')
self._arms_dict = {'left_arm': self._left_arm, 'right_arm': self._right_arm}
break
except:
rospy.sleep(random.uniform(0,2))
continue
self._tf_listener = tf.TransformListener()
self._next_task_sub = rospy.Subscriber("/amazon_next_task", String, self.get_task)
self._shelf_pose_sub = rospy.Subscriber("/pubShelfSep", PoseStamped, self.get_shelf_pose)
self._got_shelf_pose = False
self._l_gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)
while not rospy.is_shutdown():
try:
self._tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04])
self._contest = rospy.get_param('/contest', True)
break
except:
rospy.sleep(random.uniform(0,1))
continue
if self._contest:
self._length_tool = 0.18 + self._tool_size[0]
else:
self._length_tool = 0.216 + self._tool_size[0]
self._as.start()
rospy.loginfo('['+rospy.get_name()+']: ready!')
def get_shelf_pose(self, msg):
self._shelf_pose = msg
self._got_shelf_pose = True
def get_bm_srv(self):
while not rospy.is_shutdown():
try:
rospy.wait_for_service('/base_move_server/move', 5.0)
rospy.wait_for_service('/base_move_server/preempt', 5.0)
break
except:
rospy.loginfo('[' + rospy.get_name() + ']: waiting for base move server')
continue
self._bm_move_srv = rospy.ServiceProxy('/base_move_server/move', BaseMove)
self._bm_preempt_srv = rospy.ServiceProxy('/base_move_server/preempt', Empty)
def timer_callback(self, event):
self._timer_started = True
rospy.logerr('[' + rospy.get_name() + ']: TIMED OUT!')
self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object')
self._planning_scene.remove_world_object('grasped_object')
# pull the base back 60 cm
self._left_arm.stop()
self._right_arm.stop()
r = rospy.Rate(1.0)
while not self._got_shelf_pose:
rospy.loginfo('[' + rospy.get_name() + ']: waiting for shelf pose')
r.sleep()
base_pos_goal = [-1.42, -self._shelf_pose.pose.position.y, 0.0, 0.0, 0.0, 0.0]
self.get_bm_srv()
self._bm_preempt_srv.call(EmptyRequest())
while not self.go_base_pos_async(base_pos_goal):
rospy.sleep(1.0)
left_arm_joint_pos_goal = copy.deepcopy(self._left_arm_joint_pos_dict['start'])
right_arm_joint_pos_goal = copy.deepcopy(self._right_arm_joint_pos_dict['start'])
joint_pos_goal = left_arm_joint_pos_goal + right_arm_joint_pos_goal
self._arms.set_joint_value_target(joint_pos_goal)
self._arms.go()
self._exit = True
def execute_exit(self):
if self._exit:
self._success = False
self.set_status('FAILURE')
self._timer.shutdown()
return True
return False
def execute_cb(self, goal):
print 'bt motion execute callback'
def shutdown_simtrack(self):
# get simtrack switch objects service
while not rospy.is_shutdown():
try:
rospy.wait_for_service('/simtrack/switch_objects', 10.0)
break
except:
rospy.loginfo('[' + rospy.get_name() + ']: waiting for simtrack switch object service')
continue
simtrack_switch_objects_srv = rospy.ServiceProxy('/simtrack/switch_objects', SwitchObjects)
simtrack_switch_objects_srv.call()
def init_as(self):
self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object')
self._planning_scene.remove_world_object('grasped_object')
self._timer_started = False
self._exit=False
self._timer = rospy.Timer(rospy.Duration(self._timeout), self.timer_callback, oneshot=True)
self.shutdown_simtrack()
rospy.sleep(2.0)
def get_task(self, msg):
text = msg.data
text = text.replace('[','')
text = text.replace(']','')
words = text.split(',')
self._bin = words[0]
self._item = words[1]
def get_row(self):
'''
For setting the torso height and arm pose
'''
while not rospy.is_shutdown():
try:
if self._bin=='bin_A' or self._bin=='bin_B' or self._bin=='bin_C':
return 'row_1'
elif self._bin=='bin_D' or self._bin=='bin_E' or self._bin=='bin_F':
return 'row_2'
elif self._bin=='bin_G' or self._bin=='bin_H' or self._bin=='bin_I':
return 'row_3'
elif self._bin=='bin_J' or self._bin=='bin_K' or self._bin=='bin_L':
return 'row_4'
except:
pass
def get_column(self):
'''
For setting the base pose
'''
while not rospy.is_shutdown():
try:
if self._bin=='bin_A' or self._bin=='bin_D' or self._bin=='bin_G' or self._bin=='bin_J':
return 'column_1'
elif self._bin=='bin_B' or self._bin=='bin_E' or self._bin=='bin_H' or self._bin=='bin_K':
return 'column_2'
elif self._bin=='bin_C' or self._bin=='bin_F' or self._bin=='bin_I' or self._bin=='bin_L':
return 'column_3'
except:
pass
def go_joint_goal_async(self, group, joint_pos_goal, normalize_angles=False):
q_goal = np.array(joint_pos_goal)
if normalize_angles:
q_goal = self.normalize_angles(joint_pos_goal)
group.set_joint_value_target(joint_pos_goal)
if not group.go(wait=False):
return False
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
q_tol = group.get_goal_joint_tolerance()
if group.get_name()=='left_arm' or group.get_name()=='right_arm' or group.get_name()=='arms' or group.get_name()=='head':
q_tol = 0.04
elif group.get_name()=='torso':
q_tol = 0.003
t_print = rospy.Time.now()
r = rospy.Rate(1.0)
# check for preemption while the arm hasn't reach goal configuration
while np.max(np.abs(q_goal-q_now)) > q_tol and not rospy.is_shutdown():
if self.execute_exit():
return False
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
group.stop()
rospy.loginfo('action halted')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
if (rospy.Time.now()-t_print).to_sec()>3.0:
t_print = rospy.Time.now()
rospy.loginfo('[' + rospy.get_name() + ']: executing action')
#HERE THE CODE TO EXECUTE AS LONG AS THE BEHAVIOR TREE DOES NOT HALT THE ACTION
r.sleep()
if rospy.is_shutdown():
return False
return True
def normalize_angles(self, q):
'''
normalize angles to -pi, pi
'''
q_normalized = np.mod(q, 2*np.pi)
for i in xrange(np.size(q)):
if q_normalized[i] > np.pi:
q_normalized[i] = -(2*np.pi - q_normalized[i])
return q_normalized
def go_base_pos_async(self, base_pos_goal):
angle = base_pos_goal[5]
pos = base_pos_goal[0:2]
r = rospy.Rate(20.0)
req = BaseMoveRequest()
req.x = pos[0]
req.y = pos[1]
req.theta = angle
self.get_bm_srv()
res = self._bm_move_srv.call(req)
if self.execute_exit():
return False
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while moving base')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
return res.result
def go_base_moveit_group_pos_async(self, base_pos_goal, group, joint_pos_goal, normalize_angles=False):
angle = base_pos_goal[5]
pos = base_pos_goal[0:2]
r = rospy.Rate(20.0)
q_goal = np.array(joint_pos_goal)
if normalize_angles:
q_goal = self.normalize_angles(joint_pos_goal)
group.set_joint_value_target(joint_pos_goal)
group.go(wait=False)
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
q_tol = group.get_goal_joint_tolerance()
if group.get_name()=='left_arm' or group.get_name()=='right_arm' or group.get_name()=='arms' or group.get_name()=='head':
q_tol = 0.04
elif group.get_name()=='torso':
q_tol = 0.003
t_print = rospy.Time.now()
req = BaseMoveRequest()
req.x = pos[0]
req.y = pos[1]
req.theta = angle
self.get_bm_srv()
res = self._bm_move_srv.call(req)
if self.execute_exit():
return False
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while moving base')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
# check for preemption while the arm hasn't reach goal configuration
while np.max(np.abs(q_goal-q_now)) > q_tol and not rospy.is_shutdown():
if self.execute_exit():
return False
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
group.stop()
rospy.loginfo('action halted')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
if (rospy.Time.now()-t_print).to_sec()>3.0:
t_print = rospy.Time.now()
rospy.loginfo('[' + rospy.get_name() + ']: executing action')
#HERE THE CODE TO EXECUTE AS LONG AS THE BEHAVIOR TREE DOES NOT HALT THE ACTION
r.sleep()
if rospy.is_shutdown():
return False
return res.result
def request_detection(self):
client = actionlib.SimpleActionClient('amazon_detector', amazon_challenge_bt_actions.msg.DetectorAction)
# Waits until the action server has started up and started
# listening for goals.
rospy.loginfo('Start Waiting')
client.wait_for_server()
# Creates a goal to send to the action server.
goal = amazon_challenge_bt_actions.msg.DetectorGoal(parameter=1)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.loginfo('Goal Sent')
# Waits for the server to finish performing the action.
client.wait_for_result()
def set_status(self,status):
if status == 'SUCCESS':
self.pub_posed.publish("SUCCESS")
rospy.sleep(1)
self._feedback.status = 1
self._result.status = self._feedback.status
rospy.loginfo('Action %s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
elif status == 'FAILURE':
self._feedback.status = 2
self._result.status = self._feedback.status
rospy.loginfo('Action %s: Failed' % self._action_name)
self._as.set_succeeded(self._result)
else:
rospy.logerr('Action %s: has a wrong return status' % self._action_name)
def open_l_gripper(self):
gripper_command_msg = Pr2GripperCommand()
gripper_command_msg.max_effort = 40.0
gripper_command_msg.position = 10.0
r = rospy.Rate(10.0)
t_init = rospy.Time.now()
while (rospy.Time.now()-t_init).to_sec()<3.0 and not rospy.is_shutdown():
if self.execute_exit():
return False
self._l_gripper_pub.publish(gripper_command_msg)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while opening gripper')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
r.sleep()
return True
def move_l_arm_z(self, z_desired):
'''
computes straight line cartesian path in z direction
:param z_desired: of tool tip w.r.t. odom_combined
:return:
'''
waypoints = []
# waypoints.append(self._left_arm.get_current_pose().pose)
wpose = copy.deepcopy(self._left_arm.get_current_pose().pose)
wpose.position.z = z_desired + self._length_tool
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self._left_arm.compute_cartesian_path(waypoints, 0.05, 0.0)
# TODO make this asynchronous
self._left_arm.execute(plan)
return True
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
#right_arm.set_planner_id("KPIECEkConfigDefault");
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("right_gripper_open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.7
p.pose.position.y = -0.2
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.47636
start_pose.pose.position.y = -0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
## Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
print "==================== Generating Transformations ==========================="
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
# # Initialize the grasp object
# g = Grasp()
# grasps = []
# # Set the first grasp pose to the input pose
# g.grasp_pose = pre_grasping
# g.allowed_touch_objects = [target_id]
# grasps.append(deepcopy(g))
# right_arm.pick(target_id, grasps)
#Change the frame_id for the planning to take place!
#target_pose.header.frame_id = 'gazebo_world'
self.p_pub.publish(pre_grasping)
right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
plan = right_arm.plan()
rospy.sleep(2)
right_arm.go(wait=True)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
pr_place.position.x = 0.315
pr_place.position.y = -0.855
pr_place.position.z = -0.1
pr_place.orientation.x = 0.505
pr_place.orientation.y = 0.485
pr_place.orientation.z = -0.497
pr_place.orientation.w = 0.512
right = MoveGroupCommander("right_arm")
left = MoveGroupCommander("left_arm")
#group.set_random_target()
#move(right, pr)
move(left, pl)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("bowl")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.4
p.pose.position.y = -0.9
p.pose.position.z = -0.4
scene.add_box("table", p, (1.6, 0.8, 0.4))
# p.pose.position.x = 0.8
# p.pose.position.y = 0.0
# p.pose.position.z = -0.02
class MoveItDemo:
def __init__(self):
global obj_att
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# 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.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # Initialize the grasp object
## g = Grasp()
## grasps = []
## # Set the first grasp pose to the input pose
## g.grasp_pose = pre_grasping
## g.allowed_touch_objects = [target_id]
## grasps.append(deepcopy(g))
## right_arm.pick(target_id, grasps)
# #Change the frame_id for the planning to take place!
# #target_pose.header.frame_id = 'gazebo_world'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault");
left_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#left_arm.set_named_target('left_start')
#left_arm.go()
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.56
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.51
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.49
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class GraspObjectServer:
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_objects = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
goal.set_rejected() # "No objects to grasp were received on the objects topic."
return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run grasping state machine
self.grasping_sm()
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def grasping_sm(self):
if self.current_goal:
self.update_feedback("running clustering")
(object_points, obj_bbox_dims,
object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
#TODO visualize bbox
#TODO publish filtered pointcloud?
print obj_bbox_dims
########
self.update_feedback("check reachability")
########
self.update_feedback("generate grasps")
#transform pose to base_link
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2]/2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("no grasps received")
return
self.publish_grasps_as_poses(grasp_list)
self.feedback.grasps = grasp_list
self.current_goal.publish_feedback(self.feedback)
self.result.grasps = grasp_list
########
self.update_feedback("setup planning scene")
#remove old objects
self.scene.remove_world_object("object_to_grasp")
# add object to grasp to planning scene
self.scene.add_box("object_to_grasp", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.result.object_scene_name = "object_to_grasp"
########
if self.current_goal.get_goal().execute_grasp:
self.update_feedback("execute grasps")
pug = self.createPickupGoal("object_to_grasp", grasp_list)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Result is:")
print result
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.result.object_pose = object_pose
#bounding box in a point message
self.result.bounding_box = Point()
self.result.bounding_box.x = obj_bbox_dims[0]
self.result.bounding_box.y = obj_bbox_dims[1]
self.result.bounding_box.z = obj_bbox_dims[2]
self.current_goal.set_succeeded(result = self.result)
#self.current_goal.set_aborted()
def update_feedback(self, text):
self.feedback.last_state = text
self.current_goal.publish_feedback(self.feedback)
def update_aborted(self, text = ""):
self.update_feedback("aborted." + text)
self.current_goal.set_aborted()
def generate_grasps(self, pose, width):
#send request to block grasp generator service
if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)):
return []
rospy.loginfo("Successfully connected.")
goal = GenerateBlockGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
t_start = rospy.Time.now()
self.grasps_ac.wait_for_result()
t_end = rospy.Time.now()
t_total = t_end - t_start
rospy.loginfo("Result received in " + str(t_total.to_sec()))
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def publish_grasps_as_poses(self, grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
self.grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"]
pug.allowed_touch_objects.append(target)
#pug.attached_object_touch_links.append('all')
return pug
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self, traj, time, positions, velocities=None):
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if velocities is not None:
point.velocities = copy.deepcopy(velocities)
point.time_from_start = rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self, q_traj, t_traj):
assert (len(q_traj) == len(t_traj))
#Insert current position to beginning.
if t_traj[0] > 1.0e-2:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)
def QTrajToDQTraj(self, q_traj, t_traj):
dof = len(q_traj[0])
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj = []
for t in t_traj:
dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self, q, t, dq=None):
jp = trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions = q
jp.time_from_start = rospy.Duration(t)
if dq is not None: jp.velocities = dq
return jp
def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
assert (len(q_traj) == len(t_traj))
if dq_traj is not None: (len(dq_traj) == len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names = joint_names
if dq_traj is not None:
self.traj.points = [
self.ROSGetJTP(q, t, dq)
for q, t, dq in zip(q_traj, t_traj, dq_traj)
]
else:
self.traj.points = [
self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
]
self.traj.header.stamp = rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self, q_traj):
if len(q_traj) == 0: return
q_prev = np.array(q_traj[0])
q_offset = np.array([0] * len(q_prev))
for q in q_traj:
q_diff = np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d] < -math.pi: q_offset[d] += 1
elif q_diff[d] > math.pi: q_offset[d] -= 1
q_prev = copy.deepcopy(q)
q[:] = q + q_offset * 2.0 * math.pi
def add_target(self, target_pose, target_size, frame, x, y, o1, o2, o3,
o4):
target_pose.header.frame_id = frame
target_pose.pose.position.x = x
target_pose.pose.position.y = y
target_pose.pose.position.z = self.table_ground + self.table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = o1
target_pose.pose.orientation.y = o2
target_pose.pose.orientation.z = o3
target_pose.pose.orientation.w = o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self, start_pose, mid_pose, end_pose, maxtries, exe_signal=False):
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
if mid_pose != 0:
waypoints.append(mid_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
for i in range(2, len(plan.joint_trajectory.points)):
q_traj.append(
plan.joint_trajectory.points[i].positions)
t_traj.append(t_traj[-1] + 0.07)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
#move and rotate
def cts_rotate(self,
start_pose,
end_pose,
angle_r,
maxtries,
exe_signal=False):
angle = angle_r * 3.14 / 180.0
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0.0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
per_angle = angle / (len(plan.joint_trajectory.points) - 2)
for i in range(2, len(plan.joint_trajectory.points)):
joint_inc_list = [
j
for j in plan.joint_trajectory.points[i].positions
]
#plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
joint_inc_list[6] -= per_angle * (i - 1)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1] + 0.05)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self, initial_state, end_joint_state, freq, times):
q_traj = [initial_state]
t_traj = [0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(6)
#shaking_a: vertical bottle axis
def shake_a(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x += amp * math.sin(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y -= amp * math.sin(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.cos(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def shake_b(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x -= amp * math.cos(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y += amp * math.cos(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.sin(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.0
table_pose.pose.position.y = 0.65
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
#Params: pose of bottle, grasp_radius, grasp_height, grasp_theta
def get_grasp_pose(self, pose, r, theta):
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = pose.pose.position.x + r * math.sin(theta)
g_p.pose.position.y = pose.pose.position.y - r * math.cos(theta)
g_p.pose.position.z = pose.pose.position.z
g_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
g_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return g_p
def get_pour_pose(self, pose, h, r, theta):
p_p = PoseStamped()
p_p.header.frame_id = self.arm_end_effector_link
p_p.pose.position.x = pose.pose.position.x - r * math.cos(theta)
p_p.pose.position.y = pose.pose.position.y + r * math.sin(theta)
p_p.pose.position.z = pose.pose.position.z + h
theta *= -1
p_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
p_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return p_p
def pour_rotate(self, angle_pre, angle_r, r, maxtries):
angle_pre = (angle_pre / 180.0) * 3.14
angle_r_1 = (angle_r / 180.0) * 3.14
initial_state = self.arm.get_current_joint_values()
initial_pose = self.arm.get_current_pose(self.arm_end_effector_link)
final_pose = deepcopy(initial_pose)
final_pose.pose.position.x += r * (
1 - math.cos(angle_r_1)) * math.cos(angle_pre)
final_pose.pose.position.y += r * (
1 - math.cos(angle_r_1)) * math.sin(angle_pre)
final_pose.pose.position.z += r * math.sin(angle_r_1)
signal, e_j_s = self.cts_rotate(initial_pose, final_pose, angle_r,
maxtries, True)
return signal
def pg_g_pp(self, pose, r, pre_r):
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
p_i_radian = np.arctan(abs(pose.pose.position.x /
pose.pose.position.y))
p_i_angle = p_i_radian * 180.0 / 3.14
pick_list = [
0.0, p_i_angle, 5.0, 25.0, 45.0, 65.0, 15.0, 35.0, 55.0, 75.0,
10.0, 20.0, 30.0, 40.0, 50.0, 60.0
]
for i in range(len(pick_list)):
theta = (pick_list[i] / 180.0) * 3.14
if pose.pose.position.x > 0:
theta *= -1.0
pre_grasp_pose = self.get_grasp_pose(pose, r + pre_r, theta)
#pick up
grasp_pose = pre_grasp_pose
grasp_pose.pose.position.x -= pre_r * math.sin(theta)
grasp_pose.pose.position.y += pre_r * math.cos(theta)
signal, e_j_s = self.cts(start_pose, pre_grasp_pose, grasp_pose,
300, True)
if signal == 0: continue
break
rospy.sleep(1)
self.gripperCtrl(0)
rospy.sleep(2)
#move to ori_pose
current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e_j_s = self.cts(current_pose, pre_grasp_pose, start_pose, 300,
True)
rospy.sleep(2)
rospy.loginfo("Grasping done.")
return start_pose, pre_grasp_pose, grasp_pose
def pp_ps(self, target_pose, pour_angle, r_pour, h_pour):
for i in range(len(pour_angle)):
maxtries = 300
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
theta = (pour_angle[i] / 180.0) * 3.14
pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
#move to pose
signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
maxtries, True)
if signal_1 == 0: continue
pre_pour_angle = pour_angle[i]
rospy.loginfo("Ready for pouring.")
return pre_pour_angle
def go_back(self, ori_pose, pre_grasp_pose, grasp_pose):
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True)
rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, grasp_pose, 300,
True)
rospy.loginfo("back to grasp pose, open gripper..")
self.gripperCtrl(255)
rospy.sleep(1)
signal_2, e3 = self.cts(grasp_pose, pre_grasp_pose, ori_pose, 300,
True)
rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")
def rotate_back(self, angle):
angle = angle * 3.14 / 180.0
current_j_state = self.arm.get_current_joint_values()
current_j_state[6] += angle + 1.57
self.arm.go()
def last_step_go_back(self, ori_pose):
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True)
rospy.loginfo("back to last step...")
rospy.sleep(1)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.colors = dict()
rospy.sleep(1)
self.robot = moveit_commander.robot.RobotCommander()
self.arm = MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target1_id = 'target1_object'
self.target2_id = 'target2_object'
self.target3_id = 'target3_object'
self.target4_id = 'target4_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
self.scene.remove_world_object(self.target3_id)
self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
target1_size = [0.035, 0.035, 0.19]
target2_size = target1_size
target3_size = target1_size
target4_size = target1_size
self.f_target_size = [0.2, 0.2, 0.04]
f_target_pose = PoseStamped()
pre_pour_pose = PoseStamped()
target1_pose = PoseStamped()
target2_pose = PoseStamped()
target3_pose = PoseStamped()
target4_pose = PoseStamped()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
#final target
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
self.add_target(f_target_pose, self.f_target_size,
self.reference_frame, 0.2, 0.6, 0, 0, 0, 1)
self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
#msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
self.add_target(target1_pose, target1_size, self.reference_frame,
-0.25, 0.8, 0, 0, 0, 1)
self.scene.add_box(self.target1_id, target1_pose, target1_size)
self.add_target(target2_pose, target2_size, self.reference_frame,
-0.12, 0.87, 0, 0, 0, 1)
self.scene.add_box(self.target2_id, target2_pose, target2_size)
self.add_target(target3_pose, target3_size, self.reference_frame, 0.02,
0.88, 0, 0, 0, 1)
self.scene.add_box(self.target3_id, target3_pose, target3_size)
self.add_target(target4_pose, target4_size, self.reference_frame, 0.15,
0.80, 0, 0, 0, 1)
self.scene.add_box(self.target4_id, target4_pose, target4_size)
#attach_pose
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
self.setColor(self.target3_id, 0.8, 0, 0, 1.0)
self.setColor(self.target4_id, 0.8, 0, 0, 1.0)
self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state = [
-1.2628753185272217, -0.442996621131897, -0.1326361745595932,
2.333048105239868, -0.15598002076148987, -1.2167049646377563,
3.1414425373077393
]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#parameter setup
tar_num = 4
maxtries = 300
r_grasp = 0.15
r_pre_g = 0.1
r_bottle = 0.1
for i in range(0, tar_num):
#grasp target
rospy.loginfo("Choosing source...")
if i == 0:
target_pose = target1_pose
target_id = self.target1_id
target_size = target1_size
amp = 0.1
freq = 2.75
r_angle = 45.0
elif i == 1:
target_pose = target2_pose
target_id = self.target2_id
target_size = target2_size
amp = 0.15
freq = 2.0
r_angle = 30.0
elif i == 2:
target_pose = target3_pose
target_id = self.target3_id
target_size = target3_size
amp = 0.1
freq = 2.75
r_angle = 45.0
elif i == 3:
target_pose = target4_pose
target_id = self.target4_id
target_size = target4_size
amp = 0.1
freq = 2.75
r_angle = 45.0
r_pour = 0.1
h_pour = 0.1
#pour_angle=[15.0,30.0,45.0,60.0,75.0]
pour_angle = [
0.0, -15.0, -10.0, -5.0, 0.0, 10.0, 15.0, 30.0, 45.0, 60.0,
75.0
]
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: " + str(i + 1) + " ")
#grasp and back
ori_pose, mid_pose, g_pose = self.pg_g_pp(target_pose, r_grasp,
r_pre_g)
#move to target position for pouring
pre_p_angle = self.pp_ps(f_target_pose, pour_angle, r_pour, h_pour)
rospy.loginfo("pre_pour_angle : " + str(pre_p_angle))
#rotation and shaking
ps_signal = self.pour_rotate(pre_p_angle, r_angle, r_bottle,
maxtries)
if ps_signal != 1:
rospy.loginfo(
"Pre_shaking_rotation failed, back to ori_pose...")
self.last_step_go_back(ori_pose)
rospy.sleep(3)
continue
else:
rospy.loginfo("Shaking planning...")
shake_per_times = 3
shake_times = 0
shake_times_tgt = 1
signal = True
rospy.loginfo("Parameter loaded")
rospy.sleep(1)
signal, end_joint_state = self.shake_a(pre_p_angle, r_angle,
amp)
while signal == 1:
#area_ratio= rospy.wait_for_message('/color_area_ratio',Float64)
#if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
if (shake_times < shake_times_tgt):
self.shaking(start_joint_state, end_joint_state, freq,
shake_per_times)
shake_times += 1
rospy.loginfo("shaking times : " + str(shake_times) +
" .")
else:
signal = 0
self.rotate_back(r_angle)
self.go_back(ori_pose, mid_pose, g_pose)
rospy.sleep(2)
continue
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# specify the planner
group.set_planner_id("RRTkConfigDefault")
rospy.sleep(3)
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
# display_trajectory_publisher = rospy.Publisher(
# '/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory)
print ">>>>> remove scenes"
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("ball")
scene.remove_world_object("table")
scene.remove_world_object("ground_plane")
rospy.sleep(5)
print ">>>>> add scenes"
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = -1
p.pose.position.z = 0.2
p.pose.orientation.w = 1.0
scene.add_box("pole", p, (0.4, 0.1, 0.4))
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
# 设置桌面的高度
table_ground = 0.7
# 设置table和tool的三维尺寸
table_size = [0.1, 0.3, 0.02]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0
p.pose.position.z = -0.015
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.4
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2]
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
-0.5110620856285095, 0.32814791798591614, 0.5454912781715393,
1.0146701335906982, 0.8498637080192566, -0.45695754885673523
]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class ObjectManipulationAS:
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_clusters = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
# TODO: Check if pose is not empty, if it is, reject
# elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id:
# goal.set_rejected() # "No objects to grasp were received on the objects topic."
# return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run the corresponding operation
if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK:
rospy.loginfo("ObjectManipulation: PICK!")
self.pick_operation()
elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE:
rospy.loginfo("ObjectManipulation: PLACE!")
self.place_operation()
else:
rospy.logerr("ObjectManipulation: Erroneous operation!")
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def pick_operation(self):
"""Execute pick operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking hand to use")
# Check which arm group was requested and if it's currently free of objects
if 'right' in goal_message_field.group:
if self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand already contains an object.")
return # necessary?
self.current_side = 'right'
elif 'left' in goal_message_field.group:
if self.left_hand_object:
self.update_aborted("Left hand already contains an object.")
return
self.current_side = 'left'
# Publish pose of goal position
if DEBUG_MODE:
self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose)
self.update_feedback("Detecting clusters")
if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published
self.update_aborted("Failed detecting clusters")
# Search closer cluster
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
object_to_grasp_pose = goal_message_field.target_pose.pose
self.update_feedback("Searching closer cluster while clustering")
(closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose)
rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
#TODO visualize bbox
#TODO publish filtered pointcloud?
rospy.logdebug("BBOX: " + str(obj_bbox_dims))
########
self.update_feedback("Check reachability")
########
self.update_feedback("Generating grasps")
rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
#transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2] / 2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("No grasps received")
return
if DEBUG_MODE: # TODO: change to dynamic param
publish_grasps_as_poses(grasp_list)
self.current_goal.publish_feedback(self.as_feedback)
########
self.update_feedback("Setup planning scene")
# Add object to grasp to planning scene
self.scene.add_box(self.current_side + "_hand_object", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.as_result.object_scene_name = self.current_side + "_hand_object"
########
self.update_feedback("Execute grasps")
pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
rospy.loginfo("Sending pickup goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result...")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.as_result.object_pose = object_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if self.current_side == 'right':
self.right_hand_object = self.current_side + "_hand_object"
elif self.current_side == 'left':
self.left_hand_object = self.current_side + "_hand_object"
self.current_goal.set_succeeded(result=self.as_result)
else:
# Remove object as it has not been picked
self.scene.remove_world_object(self.current_side + "_hand_object")
self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def place_operation(self):
"""Execute the place operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking arm to use")
# Check which arm group was requested and if it currently has an object
if 'right' in goal_message_field.group:
if not self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand does not contain an object.")
return # necessary?
self.current_side = 'right'
current_target = self.right_hand_object
elif 'left' in goal_message_field.group:
if not self.left_hand_object:
self.update_aborted("Left hand does not contain an object.")
return
self.current_side = 'left'
current_target = self.left_hand_object
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
placing_pose = goal_message_field.target_pose.pose
#### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE ####
self.update_feedback("Sending place order to MoveIt!")
placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose)
goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target)
rospy.loginfo("Sending place goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result...")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
self.as_result.object_pose = placing_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
# Emptying hand
self.update_feedback("Emptying hand")
if self.current_side == 'right':
self.as_result.object_scene_name = current_target
self.right_hand_object = None
elif self.current_side == 'left':
self.as_result.object_scene_name = current_target
self.left_hand_object = None
# Removing object from the collision world
self.scene.remove_world_object(current_target)
self.current_goal.set_succeeded(result=self.as_result)
else:
self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def message_fields_ok(self):
"""Check if the minimal fields for the message are fulfilled"""
if self.current_goal:
goal_message_field = self.current_goal.get_goal()
if len(goal_message_field.group) == 0:
rospy.logwarn("Group field empty.")
return False
if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE:
rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation))
return False
if len(goal_message_field.target_pose.header.frame_id) == 0:
rospy.logwarn("Target pose frame_id is empty")
return False
return True
def update_feedback(self, text):
"""Publish feedback with given text"""
self.as_feedback.last_state = text
self.current_goal.publish_feedback(self.as_feedback)
def update_aborted(self, text="", manipulation_result=None):
"""Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled"""
self.update_feedback("aborted." + text)
if manipulation_result == None:
aborted_result = ObjectManipulationResult()
aborted_result.error_message = text
self.current_goal.set_aborted(result=aborted_result)
else:
self.current_goal.set_aborted(result=manipulation_result)
def generate_grasps(self, pose, width):
"""Send request to block grasp generator service"""
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
grasp_options = GraspGeneratorOptions()
grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y
grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN
grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF
goal.options.append(grasp_options)
self.grasps_ac.send_goal(goal)
if DEBUG_MODE:
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
self.grasps_ac.wait_for_result()
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def get_id_of_closest_cluster_to_pose(self, input_pose):
"""Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped)
and all the associated clustering information"""
current_id = 0
closest_pose = None
closest_object_points = None
closest_id = 0
closest_bbox = None
closest_bbox_dims = None
closest_distance = 99999.9
for myobject in self.last_clusters.objects:
(object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0])
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose))
if closest_pose == None: # first loop iteration
closest_object_points = object_points
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_distance = dist_between_poses(closest_pose, input_pose)
else:
if dist_between_poses(object_pose, input_pose) < closest_distance:
closest_object_points = object_points
closest_distance = dist_between_poses(object_pose, input_pose)
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_id = current_id
current_id += 1
rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose))
return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose)
def wait_for_recognized_array(self, wait_time=6, timeout_time=10):
"""Ask for depth images until we get a recognized array
wait for wait_time between depth throtle calls
stop if timeout_time is achieved
If we dont find it in the correspondent time return false, true otherwise"""
initial_time = rospy.Time.now()
self.last_clusters = None
count = 0
num_calls = 1
self.depth_service.call(EmptyRequest())
rospy.loginfo("Depth throtle server call #" + str(num_calls))
rospy.loginfo("Waiting for a recognized array...")
while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None:
rospy.sleep(0.1)
count += 1
if count >= wait_time / 10:
self.depth_service.call(EmptyRequest())
num_calls += 1
rospy.loginfo("Depth throtle server call #" + str(num_calls))
if self.last_clusters == None:
return False
else:
return True
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()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(60)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('right_arm_up')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(5)
# Set the height of the table off the ground
table_ground = 0.04
# 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]
target_x = 0.135
#target_y = -0.32
target_y = -0.285290879999
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# 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 = target_x
target_pose.pose.position.y = target_y
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 blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
p = PoseStamped()
p.header.frame_id = "up1_footprint"
p.pose.position.x = 0.12792118579
p.pose.position.y = -0.285290879999
p.pose.position.z = 0.120301181892
p.pose.orientation.x = 0.0
p.pose.orientation.y = 0.0
p.pose.orientation.z = -0.706825181105
p.pose.orientation.w = 0.707388269167
right_gripper.set_pose_target(p.pose)
# pick an object
right_arm.allow_replanning(True)
right_arm.allow_looking(True)
right_arm.set_goal_tolerance(0.05)
right_arm.set_planning_time(60)
print "arm grasp"
success = 0
attempt = 0
while not success:
p_plan = right_arm.plan()
attempt = attempt + 1
print "Planning attempt: " + str(attempt)
if p_plan.joint_trajectory.points != []:
success = 1
print "arm grasp"
right_arm.execute(p_plan)
rospy.sleep(5)
right_gripper.set_joint_value_target(GRIPPER_GRASP)
right_gripper.go()
print "gripper closed"
rospy.sleep(5)
scene.attach_box(GRIPPER_FRAME, target_id)
print "object attached"
right_arm.set_named_target('right_arm_up')
right_arm.go()
print "arm up"
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class ModelManager:
def __init__(self):
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model",
SpawnModel)
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model",
DeleteModel)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.object_list = {}
def pose2msg(self, x, y, z, roll, pitch, yaw):
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def spawn_model(self, model_name, model_pose):
with open(
os.path.join(
rospkg.RosPack().get_path('irb120_robotiq85_gazebo'),
'models', model_name, 'model.sdf'), "r") as f:
model_xml = f.read()
self.spawn_model_srv(model_name, model_xml, "", model_pose, "world")
def delete_model(self, model_name):
self.delete_model_srv(model_name)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def respawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
this_object = self.object_list[object_name]
print("Respawning {}".format(object_name))
# remove old objects in Gazebo
self.delete_model(object_name)
# respawn new objects in Gazebo
roll, pitch, yaw, x, y, z = self.msg2pose(this_object.abs_pose)
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
# respawn objects in Rviz
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(object_name)
p.pose = copy.copy(this_object.relative_pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
rospy.loginfo("All objects are respawned")
def spawn_all_model(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
robot_roll = objects_info["robot"]["pose"]["roll"]
robot_pitch = objects_info["robot"]["pose"]["pitch"]
robot_yaw = objects_info["robot"]["pose"]["yaw"]
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
# add object in Gazebo
# self.delete_model(object_name)
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
## add object in planning scene(Rviz)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = x - robot_x
p.pose.position.y = y - robot_y
p.pose.position.z = z - robot_z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
size = (x, y, z)
self.scene.add_box(name, p, size)
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, object_pose,
height, width, length,
shape, color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.scene.add_cylinder(name, p, height, radius)
self.object_list[name] = Object(p.pose, object_pose,
height, radius * 2,
radius * 2, shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.scene.add_sphere(name, p, radius)
self.object_list[name] = Object(p.pose, object_pose,
radius * 2, radius * 2,
radius * 2, shape, color)
rospy.sleep(0.5)
rospy.loginfo("Spawning Obstacles in planning scene")
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for obstacle_name in obstacles_name:
name = obstacle_name
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = obstacles[name]["pose"]["x"] - robot_x
p.pose.position.y = obstacles[name]["pose"]["y"] - robot_y
p.pose.position.z = obstacles[name]["pose"]["z"] - robot_z
q = quaternion_from_euler(robot_roll, robot_pitch, robot_yaw)
p.pose.orientation = Quaternion(*q)
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
size = (x, y, z)
self.scene.add_box(name, p, size)
rospy.sleep(0.5)
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
pointing_height = 0.06
grasp_height = 0.05
drop_height = 0.07
cruising_height = 0.1
START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving
GOAL_TOLERANCE = 0.005
moving = False
paused = False
move_group_state = "IDLE"
last_joint_trajectory_goal = ""
last_joint_trajectory_result = ""
def __init__(self, num_planning_attempts=100, safe_zone=None):
rospy.init_node("daarm_server", anonymous=True)
self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}]
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def raise_table(self):
#raises the table obstacle to protect blocks on the table during transport
self.scene.remove_world_object("blockProtector")
self.scene.add_box("blockProtector", self.blockProtectorPose,
self.blockProtectorDimension)
def lower_table(self):
#lowers the table to allow grasping into it
self.scene.remove_world_object("blockProtector")
def init_params(self):
try:
self.grasp_height = get_ros_param(
"GRASP_HEIGHT", "Grasp height defaulting to 0.01")
self.drop_height = get_ros_param("DROP_HEIGHT",
"Drop height defaulting to 0.07")
self.cruising_height = get_ros_param(
"CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
self.pointing_height = get_ros_param(
"POINT_HEIGHT", "Pointing height defaulting to 0.06")
except ValueError as e:
rospy.loginfo(e)
def handle_param_update(self, message):
self.init_params()
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results",
CalibrationParams,
queue_size=1)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs",
String,
queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles,
self.update_joints)
self.joint_trajectory_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/status', GoalStatusArray,
self.update_joint_trajectory_state)
self.joint_trajectory_goal_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal)
self.joint_trajectory_result_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self.update_joint_trajectory_result)
self.finger_position_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/finger_position', FingerPosition,
self.update_finger_position)
self.param_update_subscriber = rospy.Subscriber(
"/param_update", String, self.handle_param_update)
self.moveit_status_subscriber = rospy.Subscriber(
'/move_group/status', GoalStatusArray,
self.update_move_group_status)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback,
self.update_move_group_state)
#Topic for getting joint torques
rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles,
self.monitorJointTorques)
#Topic for getting cartesian force on end effector
rospy.Subscriber('/j2s7s300_driver/out/tool_wrench',
geometry_msgs.msg.WrenchStamped,
self.monitorToolWrench)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer(
"calibrate_arm", CalibrateAction, self.calibrate, auto_start=False)
self.calibration_server.start()
self.move_block_server = actionlib.SimpleActionServer(
"move_block",
MoveBlockAction,
self.handle_move_block,
auto_start=False)
self.move_block_server.start()
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
self.move_pose_server = actionlib.SimpleActionServer(
"move_pose",
MovePoseAction,
self.handle_move_pose,
auto_start=False)
self.move_pose_server.start()
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand,
self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand,
self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand,
self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand,
self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
#
def init_service_clients(self):
self.is_simulation = False
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
#self.home_arm_client()
self.home_arm_kinova()
return "done"
def custom_home_arm(self):
angles_set = [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 539.743164062
]
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angles_set[0]
goal.angles.joint2 = angles_set[1]
goal.angles.joint3 = angles_set[2]
goal.angles.joint4 = angles_set[3]
goal.angles.joint5 = angles_set[4]
goal.angles.joint6 = angles_set[5]
goal.angles.joint7 = angles_set[6]
self.joint_action_client.send_goal(goal)
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
# self.arm.set_named_target("Home")
angles_set = map(np.deg2rad, [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 269.743164062
])
self.arm.clear_pose_targets()
try:
self.arm.set_joint_value_target(angles_set)
except MoveItCommanderException as e:
pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
# This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value
def monitorJointTorques(self, torques):
if abs(torques.joint1) > 1:
return
#self.emergency_stop() #Stop arm driver
#rospy.sleep(1.0)
#self.group.stop() #Stop moveit execution
# This callback function monitors the Joint Wrench and stops the current
# execution if the Joint Wrench exceeds certain value
def monitorToolWrench(self, wrenchStamped):
return
#toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2)
##print toolwrench
#if toolwrench > 100:
# self.emergency_stop() # Stop arm driver
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn)
goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn)
goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
print(message)
pick_x = message.source.x
pick_y = message.source.y
pick_x_threshold = message.source_x_tolerance
pick_y_threshold = message.source_y_tolerance
block_id = message.id
place_x = message.target.x
place_y = message.target.y
place_x_threshold = message.target_x_tolerance
place_y_threshold = message.target_y_tolerance
self.move_block(block_id, pick_x, pick_y, pick_x_threshold,
pick_y_threshold, place_x, place_y, place_x_threshold,
place_y_threshold, message.block_size)
def handle_pick_failure(self, exception):
rospy.loginfo("Pick failed, going home.")
self.open_gripper()
self.home_arm()
raise exception
def handle_place_failure(self, safe_zone, block_size, exception):
#should probably figure out if I'm holding the block first so it doesn't look weird
#figure out how to drop the block somewhere safe
#pass none and none if you are certain you haven't picked up a block yet
if safe_zone is None and block_size is None:
self.home_arm()
raise exception
rospy.loginfo("HANDLING PLACE FAILURE")
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(
safe_zone[0]['x'],
safe_zone[0]['y'],
safe_zone[1]['x_tolerance'],
safe_zone[1]['y_tolerance'],
current_block_state,
block_size,
num_attempts=1000)
try:
arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("panic arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
except Exception as e:
rospy.loginfo(
"ERROR: Cannot panic place the block! Get ready to catch it!")
self.open_gripper()
self.home_arm()
raise exception
def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
return candidate_blocks
def move_block(self,
block_id,
pick_x,
pick_y,
pick_x_tolerance,
pick_y_tolerance,
place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
block_size=None,
safe_zone=None,
pick_tries=2,
place_tries=1,
point_at_block=True):
if block_size is None:
block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
# check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
# moved while we are checking for a drop location
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
None, None,
Exception("no room in the target zone to drop the block"))
# here we are actually building a set of candidate blocks to pick
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
# select best block to pick and pick up
pick_location = None
if len(candidate_blocks) < 1:
raise Exception("no block of id " + str(block_id) +
" found within the source zone")
elif len(candidate_blocks) == 1:
pick_location = Point(candidate_blocks[0]['x'],
candidate_blocks[0]['y'], 0)
else:
pick_location = Point(*self.find_most_isolated_block(
candidate_blocks, current_block_state))
if point_at_block == True:
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.close_gripper()
self.point_at_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0))
self.point_at_block(location=Point(arm_drop_location[0],
arm_drop_location[1], 0))
self.home_arm()
except Exception as e:
rospy.loginfo(str(e))
rospy.loginfo("failed trying to point at block. giving up.")
self.home_arm()
self.move_block_server.set_succeeded(
MoveBlockResult(drop_location))
return
for i in range(pick_tries):
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.pick_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0),
check_grasp=True)
break
except Exception as e:
if i < pick_tries - 1:
rospy.loginfo("pick failed and trying again..." + str(e))
else:
rospy.loginfo("pick failed and out of attempts..." +
str(e))
self.handle_pick_failure(e)
if safe_zone == None:
if self.safe_zone == None:
safe_zone = [{
'x': pick_x,
'y': pick_y
}, {
'x_tolerance': pick_x_tolerance,
'y_tolerance': pick_y_tolerance
}]
else:
safe_zone = self.safe_zone
# calculate drop location
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
safe_zone, block_size,
Exception("no room in the target zone to drop the block"))
rospy.loginfo("tuio drop" + str(drop_location))
for i in range(place_tries):
try:
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
break
except Exception as e:
if i < place_tries - 1:
rospy.loginfo("place failed and trying again..." + str(e))
else:
rospy.loginfo("place failed and out of attempts..." +
str(e))
# check to see if we've defined a safe zone to drop the blocks
self.handle_place_failure(safe_zone, block_size, e)
# assume success if we made it this far
self.move_block_server.set_succeeded(MoveBlockResult(drop_location))
# check if a certain x, y position is within the bounds of another x,y position
@staticmethod
def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold):
if x <= x_origin + x_threshold and x >= x_origin - x_threshold \
and y <= y_origin + y_threshold and y >= y_origin - y_threshold:
return True
return False
# calculate the best location to drop the block
def calculate_drop_location(self,
x,
y,
x_threshold,
y_threshold,
blocks,
block_size,
num_attempts=1000):
attempt = 0
x_original, y_original = x, y
while (attempt < num_attempts):
valid = True
for block in blocks:
if self.check_block_bounds(block['x'], block['y'], x, y,
0.8 * block_size, block_size):
valid = False
break
if valid:
return Point(x, y, 0)
else:
x = random.uniform(x_original - x_threshold,
x_original + x_threshold)
y = random.uniform(y_original - y_threshold,
y_original + y_threshold)
attempt += 1
#if none found in num_attempts, return none
return None
# candidates should have more than one element in it
@staticmethod
def find_most_isolated_block(candidates, all_blocks):
print(candidates)
min_distances = [] # tuples of candidate, distance to closest block
for candidate in candidates:
min_dist = -1
for block in all_blocks:
if block['x'] == candidate['x'] and block['y'] == candidate[
'y']:
continue
else:
dist = DaArmServer.block_dist(candidate, block)
if min_dist == -1 or dist < min_dist:
min_dist = dist
min_distances.append([candidate, min_dist])
most_isolated, _ = max(min_distances, key=lambda x: x[
1]) # get most isolated candidate, and min_distance
return most_isolated['x'], most_isolated['y'], 0
@staticmethod
def block_dist(block_1, block_2):
return np.sqrt((block_2['x'] - block_1['x'])**2 +
(block_2['y'] - block_1['y'])**2)
def update_finger_position(self, message):
self.finger_positions = [
message.finger1, message.finger2, message.finger3
]
def check_grasp(self):
closed_pos = 0.95 * 6800
distance_from_closed = 0
for fp in self.finger_positions:
distance_from_closed += (closed_pos - fp)**2
if np.sqrt(distance_from_closed
) > 130: #this is just some magic number for now
return True #if the fingers aren't fully closed, then grasp is good
else:
return False
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
try:
rospy.loginfo("Opening Gripper!!")
self.move_fingers(50, 50, 50)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
# self.gripper.set_named_target("Close")
# self.gripper.go()
try:
rospy.loginfo("Closing Gripper!!")
self.move_fingers(95, 95, 95)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def handle_move_pose(self, message):
# takes a geometry_msgs/Pose message
self.move_arm_to_pose(message.target.position,
message.target.orientation,
action_server=self.move_pose_server)
self.move_pose_server.set_succeeded()
def check_plan_result(self, target_pose, cur_pose):
#we'll do a very lenient check, this is to find failures, not error
#also only checking position, not orientation
rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose))
if np.abs(target_pose.pose.position.x -
cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2:
print("x error too far")
return False
if np.abs(target_pose.pose.position.y -
cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2:
print("y error too far")
return False
if np.abs(target_pose.pose.position.z -
cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2:
print("z error too far")
return False
print("tolerable error")
return True
# expects cooridinates for position to be in arm space
def move_arm_to_pose(self,
position,
orientation,
delay=0.5,
waypoints=[],
corrections=4,
action_server=None):
for i in range(corrections + 1):
rospy.loginfo("TRY NUMBER " + str(i))
if len(waypoints) > 0 and i < 1:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(
waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
rospy.loginfo("PLANNING TO " + str(p))
self.arm.set_pose_target(p)
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
plan = self.arm.go(wait=False)
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 90 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 90 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
if (self.check_plan_result(p, self.arm.get_current_pose())):
break #we're there, no need to retry
#rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE")
if False:
# # get the last pose to correct if desired
# ptPos = plan.joint_trajectory.points[-1].positions
# # print "=================================="
# # print "Last point of the current trajectory: "
# angle_set = list()
# for i in range(len(ptPos)):
# tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
# angle_set.append(tempPos)
if action_server:
pass
#action_server.publish_feedback(CalibrateFeedback("Plan Found"))
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
self.arm.execute(plan, wait=False)
# this is a bit naive, but I'm going to loop until a new trajectory goal gets published
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 15 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 15 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
#action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
pass
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def update_move_group_status(self, message):
if message.status_list:
#rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status))
self.move_group_status = message.status_list[0].status
def update_joint_trajectory_state(self, message):
# print(message.status_list)
if len(message.status_list) > 0:
self.joint_trajectory_state = message.status_list[0].status
else:
self.joint_trajectory_state = 0
def update_joint_trajectory_goal(self, message):
#print("goalisasis", message.goal_id.id)
self.last_joint_trajectory_goal = message.goal_id.id
def update_joint_trajectory_result(self, message):
#print("resultisasis", message.status.goal_id.id)
self.last_joint_trajectory_result = message.status.goal_id.id
def get_grasp_orientation(self, position):
#return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2))
return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633)
def update_joints(self, joints):
self.joint_angles = [
joints.joint1, joints.joint2, joints.joint3, joints.joint4,
joints.joint5, joints.joint6, joints.joint7
]
def move_z_relative(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def move_z_absolute(self, height):
p = self.arm.get_current_pose()
p.pose.position.z = height
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self,
location,
check_grasp=False,
retry_attempts=0,
delay=0,
action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
# print('Position: ', position)
self.open_gripper()
position = Point(location.x, location.y, self.cruising_height)
orientation = self.get_grasp_orientation(position)
try:
self.raise_table()
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
position = Point(location.x, location.y, self.grasp_height)
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.crusing_height)
self.raise_table()
raise (
e
) #problem because sometimes we get exception e.g. if we're already there
# and it will skip the close if so.
if action_server:
action_server.publish_feedback()
self.close_gripper()
self.move_z_absolute(self.cruising_height)
#try to wait until we're up to check grasp
rospy.sleep(0.5)
if check_grasp is True:
if (self.check_grasp() is False):
print("Grasp failed!")
self.raise_table()
raise (Exception("grasp failed!"))
# for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.raise_table()
rospy.sleep(delay)
def place_block(self,
location,
check_grasp=False,
delay=0,
action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.drop_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z_absolute(self.cruising_height)
self.raise_table()
self.close_gripper()
def point_at_block(self, location, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.pointing_height)
self.move_z_absolute(self.cruising_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.raise_table()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
print("moving on...")
self.calibration_server.publish_feedback(
CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(
CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(
CalibrateFeedback("moving to drop"))
try:
self.move_arm_to_pose(Point(0.4, -0.4, 0.1),
Quaternion(1, 0, 0, 0),
corrections=0)
except Exception as e:
rospy.loginfo("THIS IS TH PRoblem" + str(e))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(
CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0}
calibration_action_belief = {
"action": "add",
"block": calibration_block
}
self.action_belief_publisher.publish(
String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise (ValueError("Failed getting block state to calibrate: " +
str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise (ValueError(
"Failed calibration, either couldn't find block or > 1 block on TUI!"
))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y,
tuio_bounds)
arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
class MoveIt(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.arm = MoveGroupCommander("arm")
# self.arm.set_goal_joint_tolerance(0.1)
self.gripper = MoveGroupCommander("gripper")
# already default
self.arm.set_planner_id("RRTConnectkConfigDefault")
self.end_effector_link = self.arm.get_end_effector_link()
self.arm.allow_replanning(True)
self.arm.set_planning_time(5)
self.transformer = tf.TransformListener()
rospy.sleep(2) # allow some time for initialization of moveit
def __del__(self):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def _open_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [0, 0]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
def _close_gripper(self):
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.get_rostime()
joint_trajectory.joint_names = [
"m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
]
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = [1.2, 1.2]
joint_trajectory_point.time_from_start = rospy.Duration(5.0)
joint_trajectory.points.append(joint_trajectory_point)
return joint_trajectory
# Template function for creating the Grasps
def _create_grasps(self, x, y, z, z_max, rotation):
grasps = []
# You can create multiple grasps and add them to the grasps list
grasp = Grasp() # create a new grasp
# Set the pre grasp posture (the fingers)
grasp.pre_grasp_posture = self._open_gripper()
# Set the grasp posture (the fingers)
grasp.grasp_posture = self._close_gripper()
# Set the position of where to grasp
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
# Set the orientation of the end effector
q = quaternion_from_euler(math.pi, 0.0, rotation)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# Set the pre_grasp_approach
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# # Set the post_grasp_approach
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm)
grasps.append(
grasp
) # add all your grasps in the grasps list, MoveIT will pick the best one
for z_offset in np.arange(z + 0.02, z_max, 0.01):
new_grasp = copy.deepcopy(grasp)
new_grasp.grasp_pose.pose.position.z = z_offset
grasps.append(new_grasp)
return grasps
# Template function, you can add parameters if needed!
def grasp(self, x, y, z, z_max, rotation, size):
print '******************* grasp'
# Object distance:
obj_dist = np.linalg.norm(np.asarray((x, y, z)))
if obj_dist > 0.5:
rospy.loginfo(
"Object too far appart ({} m), skipping pick".format(obj_dist))
return False
# Add collision object, easiest to name the object, "object"
object_pose = PoseStamped()
object_pose.header.frame_id = "m1n6s200_link_base"
object_pose.pose.position.x = x
object_pose.pose.position.y = y
object_pose.pose.position.z = z
q = quaternion_from_euler(math.pi, 0.0, rotation)
object_pose.pose.orientation.x = q[0]
object_pose.pose.orientation.y = q[1]
object_pose.pose.orientation.z = q[2]
object_pose.pose.orientation.w = q[3]
self.scene.add_box("object", object_pose, size)
rospy.sleep(0.5)
self.clear_octomap()
rospy.sleep(1.0)
# Create and return grasps
# z += size[2]/2 # Focus on the top of the object only
# z += size[2]/2 + 0.02 # Focus on the top of the object only
grasps = self._create_grasps(x, y, z, z_max, rotation)
print '******************************************************************************'
result = self.arm.pick(
'object', grasps) # Perform pick on "object", returns result
print '******************************************************************************'
# self.move_to(x, y, z + 0.15, rotation)
if result == MoveItErrorCodes.SUCCESS:
print 'Success grasp'
return True
else:
print 'Failed grasp'
return False
def clear_object(self, x, y, z, z_max, rotation, size):
print '******************* clear_object'
self.move_to_waypoint()
success = self.grasp(x, y, z, z_max, rotation, size)
if success:
self.move_to_waypoint()
success = self.move_to_drop_zone()
if success:
print 'success move to drop zone'
else:
print 'failed move to drop zone'
self.open_fingers()
self.remove_object()
rospy.sleep(1.0)
self.close_fingers()
return success
def open_fingers(self):
print '******************* open_fingers'
self.gripper.set_joint_value_target([0.0, 0.0])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def close_fingers(self):
print '******************* close_fingers'
self.gripper.set_joint_value_target([1.3, 1.3])
self.gripper.go(wait=True)
rospy.sleep(2.0)
def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"):
print '******************* move_to'
q = quaternion_from_euler(math.pi, 0.0, rotation)
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def move_to_waypoint(self):
print '******************* move_to_waypoint'
return self.move_to(0.35, 0, 0.25, 1.57)
def rtb(self):
print '******************* rtb'
self.move_to_drop_zone()
# pose = PoseStamped()
# pose.header.frame_id = 'base_footprint'
# pose.pose.position.x = -0.191258927921
# pose.pose.position.y = 0.1849306168113
# pose.pose.position.z = 0.813729734732
# pose.pose.orientation.x = -0.934842026356
# pose.pose.orientation.y = 0.350652799078
# pose.pose.orientation.z = -0.00168532388516
# pose.pose.orientation.w = 0.0557688079539
#
# self.arm.set_pose_target(pose, self.end_effector_link)
# plan = self.arm.plan()
# self.arm.go(wait=True)
# self.arm.stop()
# self.arm.clear_pose_targets()
def move_to_drop_zone(self):
print '******************* move_to_drop_zone'
pose = PoseStamped()
pose.header.frame_id = "m1n6s200_link_base"
pose.pose.position.x = 0.2175546259709541
pose.pose.position.y = 0.18347985269448372
pose.pose.position.z = 0.16757751444136426
pose.pose.orientation.x = 0.6934210704552356
pose.pose.orientation.y = 0.6589390059796749
pose.pose.orientation.z = -0.23223137602833943
pose.pose.orientation.w = -0.17616808290725341
self.arm.set_pose_target(pose, self.end_effector_link)
plan = self.arm.plan()
success = self.arm.go(wait=True)
self.arm.stop()
self.arm.clear_pose_targets()
return success
def print_position(self):
pose = self.arm.get_current_pose()
self.transformer.waitForTransform("m1n6s200_link_base",
"base_footprint", rospy.Time.now(),
rospy.Duration(10))
eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose)
orientation = eef_pose.pose.orientation
orientation = [
orientation.x, orientation.y, orientation.z, orientation.w
]
euler = euler_from_quaternion(orientation)
print "z:", eef_pose.pose.position.x
print "y:", eef_pose.pose.position.y
print "z:", eef_pose.pose.position.z
print "yaw (degrees):", math.degrees(euler[2])
def remove_object(self):
self.scene.remove_attached_object(self.end_effector_link, "object")
self.scene.remove_world_object("object")
rospy.loginfo("Object removed")
def __init__(self):
"""
Setup ROS communication between the Kinect and Baxter
:return:
"""
# Initialise variables
self.rgb_img = None
self.depth_img = None
self.marker_box = None
self.marker_center_x = None
self.marker_center_y = None
self.marker_depth = None
self.bridge = CvBridge()
# First initialize moveit_commander and rospy.
print "============ Initialising Baxter"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('can_node', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander("left_arm")
self.right_arm = moveit_commander.MoveGroupCommander("right_arm")
self.right_arm_navigator = Navigator('right')
# Setup grippers
self.right_gripper = baxter_interface.Gripper('right')
# Setup the table in the scene
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene)
table_id = 'table'
scene.remove_world_object(table_id)
rospy.sleep(1)
table_ground = -0.3
table_size = [4.0, 4.6, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = self.robot.get_planning_frame()
table_pose.pose.position.x = 0.7
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)
# Create a dictionary to hold object colors
self.colors = dict()
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.sendColors()
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
# We can get a list of all the groups in the robot
print "============ Right Pose:"
print self.right_arm.get_current_pose()
print "============ Left Pose:"
print self.left_arm.get_current_pose()
# Setup the subscribers and publishers
self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb)
self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.callback_points)
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=5)
self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10)
self.right_hand_range_pub = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1)
self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1)
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
#gripper=MoveGroupCommander('gripper')
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)
#gripper.set_goal_position_tolerance(0.005)
#gripper.set_goal_orientation_tolerance(0.025)
#gripper.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'
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.59
table_size=[0.5,1,0.01]
#box1_size=[0.1,0.05,0.03]
box2_size=[0.15,0.15,0.02]
r_tool_size=[0.05,0.04,0.22]
l_tool_size=[0.05,0.04,0.22]
target_size=[0.05,0.05,0.1]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.7
table_pose.pose.position.y=0.0
table_pose.pose.position.z=table_ground+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.55
box2_pose.pose.position.y=-0.12
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.58
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.06
l_p.pose.position.z=0.11
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.06
r_p.pose.position.z=0.11
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
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_arm")
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)
print arm.get_current_joint_values()
'''
pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415]
arm.set_joint_value_target(pre_joint_state)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
arm.shift_pose_target(0,0.09,end_effector_link)
arm.go()
rospy.sleep(2)
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is pouring
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
joint_state_1=arm.get_current_joint_values()
joint_state_1[0]-=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
joint_state_2=arm.get_current_joint_values()
joint_state_2[6]-=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(1)
#print arm.get_current_joint_values()
#pouring test
for i in range(1,5):
joint_state_2[6]+=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
joint_state_2[6]-=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
print i
joint_state_2[6]+=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(2)
joint_state_1[0]+=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link,'l_tool')
rospy.sleep(1)
scene.remove_attached_object(end_effector_link,'r_tool')
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('ur_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)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
# 设置桌面的高度
table_ground = 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_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.21
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.19
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.2
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.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
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' We also remove the table object in order to run a test
#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.018, 0.018, 0.018
] #[0.02, 0.005, 0.12] original object dimensions in meters
# 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] -0.08 / 2.0 #0.4+0.01/2 aka table_ground + table_size[2] + target_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 = -0.03 #table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] / 2.0 table_ground + table_size[2] + target_size[2] -0.08 / 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 = -0.03 #table_pose.pose.position.x - 0.03
place_pose.pose.position.y = -0.15
place_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] -0.08 / 2.0 0.4+0.01+0.018/2 aka 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 #from here we are forcing the success cases by stting and making it always true it makes an error appear and the arm stays to the middle position
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts: #while not has been removed added = after operator <
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)
break
if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " +
str(n_attempts) + " attempts.")
else: #end of forcing
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)
from control_msgs.msg import GripperCommandAction, FollowJointTrajectoryAction
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
eef = MoveGroupCommander("gripper")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
client = actionlib.SimpleActionClient('gripper_controller/gripper_action',
GripperCommandAction)
if not client.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
# publish a demo scene
k = PoseStamped()
k.header.frame_id = robot.get_planning_frame()
k.pose.position.x = 0
k.pose.position.y = 0
k.pose.position.z = -0.05 - 0.025
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
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()
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.02
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 __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
class Pick_Place:
def __init__(self):
self.object_list = {}
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.05)
self.gripper.set_goal_tolerance(0.02)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.add_objects()
self.add_table()
#self.add_ground()
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
rospy.sleep(1.0)
# pick up object in pose
def pickup(self, object_name, pose):
grasps = self.generate_grasps(object_name, pose)
self.arm.pick(object_name, grasps)
#self.gripper.stop()
rospy.loginfo('Pick up successfully')
self.arm.detach_object(object_name)
self.clean_scene(object_name)
#rospy.sleep(1)
# place object to goal pose
def place(self, pose):
self.move_pose_arm(pose)
rospy.sleep(1)
# pose.position.z -= 0.1
# self.move_pose_arm(pose)
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z -= 0.15
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.move_joint_hand(0)
rospy.sleep(1)
# pose.position.z += 0.1
# self.move_pose_arm(pose)
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
rospy.loginfo('Place successfully')
def get_object_pose(self, object_name):
pose = self.object_list[object_name].pose
return pose
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def add_ground(self):
self.clean_scene("ground")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
size = (5, 5, 0.02)
#self.scene.add_box("ground",p, size)
rospy.sleep(2)
def add_table(self):
self.clean_scene("table")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.8
p.pose.position.y = 0.0
p.pose.position.z = 0.1
size = (0.8, 1.5, 0.028)
self.scene.add_box("table", p, size)
rospy.sleep(2)
def add_objects(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add box
name = "box"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.025 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.05, 0.05, 0.05)
self.scene.add_box(name, p, size)
height = 0.05
width = 0.05
shape = "cube"
color = "red"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add box")
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add cylinder
name = "cylinder"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = 0.2
p.pose.position.z = 0.05 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
height = 0.1
radius = 0.03
self.scene.add_cylinder(name, p, height, radius)
height = 0.1
width = 0.03
shape = "cylinder"
color = "green"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add cylinder")
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add sphere
name = "ball"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = -0.2
p.pose.position.z = 0.03 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
radius = 0.03
self.scene.add_sphere(name, p, radius)
height = 0.03
width = 0.03
shape = "sphere"
color = "red"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add ball")
rospy.sleep(1)
#print(self.object_list)
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def back_to_home(self):
self.move_joint_arm(0, 0, 0, 0, 0, 0)
self.move_joint_hand(0)
rospy.sleep(1)
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
# Inverse Kinematics: Move the robot arm to desired pose
def move_pose_arm(self, pose_goal):
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.5
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.5
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
grasp.max_contact_force = 1000
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.0)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.4)
elif name == "ball":
traj.positions.append(0.3)
elif name == "cylinder":
traj.positions.append(0.3)
#traj.velocities.append(0.2)
#traj.effort.append(100)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
# Implement the main algorithm here
def MyAlgorithm(self):
self.back_to_home()
# pick cylinder
object_name = "cylinder"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.16
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = 0
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
# pick box
object_name = "box"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.15
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = 0.1
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
# pick ball
object_name = "ball"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.14
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = -0.1
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
class Pick_Place:
def __init__(self):
self.objects_name = ["cylinder", "box", "sphere"]
self.object_width = 0.03
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.2)
self.gripper.set_goal_tolerance(0.05)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.add_ground()
rospy.sleep(1.0)
for i in range(3):
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 1.57, 0, 0.4, 0, 0.6)
rospy.sleep(1)
self.object_name = self.objects_name[i]
self.scene.remove_world_object(self.object_name)
self.pose_object, dx, dy, dz = self.add_object(self.object_name)
rospy.sleep(1.0)
self.pose_object.position.x += dx
self.pose_object.position.y += dy
self.pose_object.position.z += dz
print(self.objects_name[i], self.pose_object.position)
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
print("start pick and place")
# Pick
grasps = self.generate_grasps(self.object_name, self.pose_object)
self.arm.pick(self.object_name, grasps)
self.gripper.stop()
rospy.loginfo('Pick up successfully')
self.arm.detach_object(self.object_name)
self.clean_scene(self.object_name)
rospy.sleep(1)
curr_pose = self.arm.get_current_pose().pose
x = curr_pose.position.x
y = curr_pose.position.y
z = curr_pose.position.z
# quaternion = (curr_pose.orientation.x,
# curr_pose.orientation.y,
# curr_pose.orientation.z,
# curr_pose.orientation.w)
# euler = euler_from_quaternion(quaternion)
# roll = euler[0]
# pitch = euler[1]
# yaw = euler[2]
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
z += 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
x = -curr_pose.position.y
y = 0.6
z -= 0.1
#z = self.pose_object.position.z+0.05
# if self.object_name == "cylinder":
# yaw = numpy.deg2rad(90.0)
#z+= 0.001
self.move_pose_arm(roll, pitch, yaw, x, y, z)
rospy.sleep(0.5)
z -= 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
self.move_joint_hand(0)
#if self.object_name == "cylinder":
z += 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
# target_pose = curr_pose
# target_pose.position.x = x
# target_pose.position.y = y
# target_pose.position.z = z
# q = quaternion_from_euler(roll, pitch, yaw)
# target_pose.orientation = Quaternion(*q)
# place = self.generate_places(target_pose)
# self.arm.place(self.object_name, place)
# self.arm.detach_object(self.object_name)
# self.clean_scene(self.object_name)
rospy.loginfo('Place successfully')
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6)
rospy.sleep(1)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def add_ground(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
size = (5, 5, 0.02)
self.scene.add_box("ground", p, size)
def add_object(self, name):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
dx = 0
dy = 0
dz = 0
if name == "box":
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.015 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.03, 0.03, 0.03)
self.scene.add_box(name, p, size)
dz = 0.16
elif name == "cylinder":
p.pose.position.x = 0.5
p.pose.position.y = 0.2
p.pose.position.z = 0.05 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
height = 0.1
radius = 0.03
self.scene.add_cylinder(name, p, height, radius)
dz = 0.16
#dx = -0.135
elif name == "sphere":
p.pose.position.x = 0.5
p.pose.position.y = -0.2
p.pose.position.z = 0.03 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
radius = 0.03
self.scene.add_sphere(name, p, radius)
dz = 0.15
return p.pose, dx, dy, dz
def move_pose_arm(self, roll, pitch, yaw, x, y, z):
pose_goal = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose_goal.orientation.x = quat[0]
pose_goal.orientation.y = quat[1]
pose_goal.orientation.z = quat[2]
pose_goal.orientation.w = quat[3]
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
#for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)):
# Create place location:
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.2
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.2
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
# if name != "cylinder":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# else:
# #grasp.pre_grasp_approach.direction.vector.z = -0.05
# grasp.pre_grasp_approach.direction.vector.x = 0.1
# #grasp.post_grasp_retreat.direction.vector.z = 0.05
# grasp.post_grasp_retreat.direction.vector.x = -0.1
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.03)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.57)
elif name == "sphere":
traj.positions.append(0.3)
else:
traj.positions.append(0.3)
#traj.velocities.append(0.2)
traj.effort.append(800)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
def generate_places(self, target):
#places = []
now = rospy.Time.now()
# for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self.robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
# q = quaternion_from_euler(0.0, 0, 0.0)
# place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self.approach_retreat_desired_dist
place.pre_place_approach.min_distance = self.approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist
place.post_place_retreat.min_distance = self.approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
place.allowed_touch_objects.append(self.object_name)
place.post_place_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0)
#traj.effort.append(0)
traj.time_from_start = rospy.Duration.from_sec(1.0)
place.post_place_posture.points.append(traj)
# Add place:
#places.append(place)
return place
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.2
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.31
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.29
box2_pose.pose.position.y = -0.4
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.04, 0.04, 0.05]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化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)
rospy.Subscriber("chatter", Float64MultiArray, callback)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
gripper = MoveGroupCommander('gripper')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
gripper.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.2)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
# cy_id = 'cy'
box1_id = 'box1'
box2_id = 'box2'
box3_id = 'box3'
sphere_id = 'sphere'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(box3_id)
scene.remove_world_object(sphere_id)
rospy.sleep(1)
#控制机械臂先回到初始化位置
arm.set_named_target('init')
arm.go()
rospy.sleep(1)
# arm.set_named_target('start')
# arm.go()
# rospy.sleep(1)
gripper.set_joint_value_target([0.05])
gripper.go()
rospy.sleep(0)
# 设置桌面的高度
table_ground = 0.37
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.3, 0.01]
box1_size = [0.01, 0.01, 0.19]
box2_size = [0.01, 0.01, 0.19]
box3_size = [0.005, 0.01, 0.3]
sphere_R = 0.01
error = 0.03
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = -table_size[0] / 2.0
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)
#scene.add_cylinder
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = -0.09
box1_pose.pose.position.y = table_size[0] / 2.0
box1_pose.pose.position.z = 0.18 + 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.09
box2_pose.pose.position.y = -table_size[0] / 2.0
box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# box3_pose = PoseStamped()
# box3_pose.header.frame_id = reference_frame
# box3_pose.pose.position.x = pos_aim[0]
# box3_pose.pose.position.y = pos_aim[1]
# box3_pose.pose.position.z = box3_size[2]/2.0+0.1
# box3_pose.pose.orientation.w = 1.0
# scene.add_box(box3_id, box3_pose, box3_size)
sphere_pose = PoseStamped()
sphere_pose.header.frame_id = reference_frame
sphere_pose.pose.position.x = pos_aim[0] + 0
sphere_pose.pose.position.y = pos_aim[1]
sphere_pose.pose.position.z = pos_aim[2]
sphere_pose.pose.orientation.w = 1.0
scene.add_sphere(sphere_id, sphere_pose, sphere_R)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0, 0, 0, 1)
# self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box1_id, 1, 1, 1, 1.0)
self.setColor(box2_id, 1, 1, 1, 1.0)
self.setColor(box3_id, 1, 1, 1, 1.0)
self.setColor(sphere_id, 0.8, 0, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# rospy.INFO("waiting...")
# if(Recive_FLAG==1):
# rospy.INFO("OK！")
# 设置机械臂的运动目标位置
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = pos_aim[0] - error
target_pose.pose.position.y = pos_aim[1]
target_pose.pose.position.z = pos_aim[2]
# target_pose.pose.orientation.w = 1.0
#####0.3
# 0.2 0.2 0.2 0.15 0.15
# 0.12 0.11 0.12 0.15 0.15
# 0.30 0.245 0.35 0.35 0.25
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
gripper.set_joint_value_target([0.03])
gripper.go()
rospy.sleep(1)
# # 控制机械臂终端向x移动5cm
arm.shift_pose_target(0, 0.01, end_effector_link)
arm.go()
rospy.sleep(1)
gripper.set_joint_value_target([0.05])
gripper.go()
rospy.sleep(1)
# # 设置机械臂的运动目标位置，进行避障规划
# target_pose2 = PoseStamped()
# target_pose2.header.frame_id = reference_frame
# target_pose2.pose.position.x = 0.15
# target_pose2.pose.position.y = 0 #-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('init')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
rospy.spin()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('SceneSetup')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Set the reference frame for pose targets
reference_frame = 'rack1'
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [1.0128, 0.481, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = reference_frame
bowl_pose.pose.position.x = 0
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
# Set the height of the pitcher
pitcher_ground = 0.57816875
pitcher_pose = PoseStamped()
pitcher_pose.header.frame_id = reference_frame
pitcher_pose.pose.position.x = 0.4
pitcher_pose.pose.position.y = 0.847725
pitcher_pose.pose.position.z = pitcher_ground
pitcher_pose.pose.orientation.w = -0.5
pitcher_pose.pose.orientation.z = 0.707
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
self.sendColors()
scene.add_box(table_id, table_pose, table_size)
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class PlanningSceneHelper:
def __init__(self, package=None, mesh_folder_path=None):
# interface to the planning and collision scenes
self.psi = PlanningSceneInterface()
# self.scene_pub = Publisher("/collision_object", CollisionObject,
# queue_size=1, latch=True)
self.vis_pub = Publisher("/collision_scene",
MarkerArray,
queue_size=10)
self.marker_array = MarkerArray()
sleep(1.0)
if package is not None and mesh_folder_path is not None:
# Build folder path for use in load
rospack = RosPack()
self.package = package
self.mesh_folder_path = mesh_folder_path
self.package_path = rospack.get_path(package)
self.folder_path = os.path.join(self.package_path, mesh_folder_path)\
+ os.sep
else:
loginfo(
"Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added"
)
# Create dict of objects, for removing markers later
self.objects = {}
self.next_id = 1
# Loads the selected mesh file into collision scene at the desired location.
def add_mesh(self,
object_id,
frame,
filename,
visual=None,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
if self.package is None:
logwarn("No package was provided; no meshes can be loaded.")
if visual is None:
visual = filename
filename = self.folder_path + filename
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
try:
self.psi.add_mesh(object_id, stamped_pose, filename)
loginfo("Loaded " + object_id + " from" + filename +
" as collision object.")
marker = self.make_marker_stub(stamped_pose, color=color)
marker.type = Marker.MESH_RESOURCE
# marker.mesh_resource = "file:/" + self.folder_path + visual
marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
loginfo("Loaded " + object_id + " from " + marker.mesh_resource +
" as marker.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
except AssimpError as ae:
loginfo("Problem loading " + object_id + " collision object: " +
str(ae))
def add_cylinder(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
# Cylinders are special - they are not supported directly by
# moveit_commander. It must be published manually to collision scene
cyl = CollisionObject()
cyl.operation = CollisionObject.ADD
cyl.id = object_id
cyl.header = stamped_pose.header
prim = SolidPrimitive()
prim.type = SolidPrimitive.CYLINDER
prim.dimensions = [size[0], size[1]]
cyl.primitives = [prim]
cyl.primitive_poses = [stamped_pose.pose]
# self.scene_pub.publish(cyl)
marker = self.make_marker_stub(stamped_pose,
[size[1] * 2, size[2] * 2, size[0]],
color=color)
marker.type = Marker.CYLINDER
self.publish_marker(object_id, marker)
sleep(0.5)
logdebug("Loaded " + object_id +
" as cylindrical collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_sphere(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_sphere(object_id, stamped_pose, size[0])
loginfo("got past adding collision scene object")
marker = self.make_marker_stub(
stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2],
color=color)
marker.type = Marker.SPHERE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_box(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_box(object_id, stamped_pose, size)
marker = self.make_marker_stub(stamped_pose, size, color=color)
marker.type = Marker.CUBE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def attach_box(self,
link,
object_id,
frame,
size,
attach_to_link,
pose=None,
position=None,
orientation=None,
rpy=None):
"""TODO: color is not yet supported, since it's not internally
supported by psi.attach_box. Basically duplicate this method
but with color support."""
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy),
self.psi.attach_box(link, object_id, stamped_pose, size,
attach_to_link)
sleep(0.5)
loginfo("Attached " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem attaching " + object_id + " collision object: " +
str(e))
@staticmethod
def make_stamped_pose(frame,
pose=None,
position=None,
orientation=None,
rpy=None):
if orientation is not None and rpy is not None:
logwarn("Collision object has both orientation (quaternion) and "
"Rotation (rpy) defined! Defaulting to quaternion "
"representation")
stamped_pose = PoseStamped()
stamped_pose.header.frame_id = frame
stamped_pose.header.stamp = Time.now()
# use a pose if one is provided, otherwise, make your own from the
# position and orientation.
if pose is not None:
stamped_pose.pose = pose
else:
stamped_pose.pose.position.x = position[0]
stamped_pose.pose.position.y = position[1]
stamped_pose.pose.position.z = position[2]
# for orientation, allow either quaternion or RPY specification
if orientation is not None:
stamped_pose.pose.orientation.x = orientation[0]
stamped_pose.pose.orientation.y = orientation[1]
stamped_pose.pose.orientation.z = orientation[2]
stamped_pose.pose.orientation.w = orientation[3]
elif rpy is not None:
quaternion = transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
stamped_pose.pose.orientation.x = quaternion[0]
stamped_pose.pose.orientation.y = quaternion[1]
stamped_pose.pose.orientation.z = quaternion[2]
stamped_pose.pose.orientation.w = quaternion[3]
else:
stamped_pose.pose.orientation.w = 1 # make basic quaternion
return stamped_pose
# Fill a ROS Marker message with the provided data
def make_marker_stub(self, stamped_pose, size=None, color=None):
if color is None:
color = (0.5, 0.5, 0.5, 1.0)
if size is None:
size = (1, 1, 1)
marker = Marker()
marker.header = stamped_pose.header
# marker.ns = "collision_scene"
marker.id = self.next_id
marker.lifetime = Time(0) # forever
marker.action = Marker.ADD
marker.pose = stamped_pose.pose
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
if len(color) == 4:
alpha = color[3]
else:
alpha = 1.0
marker.color.a = alpha
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
self.next_id += 1
return marker
def make_new_marker_array_msg(self):
ma = MarkerArray()
return ma
# publish a single marker
def publish_marker(self, object_id, marker):
loginfo("Publishing marker for object {}".format(object_id))
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
# Remove the provided object from the world.
def remove(self, object_id):
# if object_id not in self.objects:
# logwarn("PlanningSceneHelper was not used to create object with id "
# + object_id + ". Attempting to remove it anyway.")
try:
# first remove the actual collision object
self.psi.remove_world_object(object_id)
if object_id in self.objects:
# then remove the marker associated with it
marker = self.objects[object_id]
marker.action = Marker.DELETE
self.publish_marker(object_id, marker)
self.objects.pop(object_id)
logdebug("Marker for collision object " + object_id + " removed.")
except ROSException as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
except KeyError as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
loginfo("Model " + object_id + " removed from collision scene.")
# Remove the provided attached collision object.
def remove_attached(self, link, object_id):
try:
self.psi.remove_attached_object(link=link, name=object_id)
loginfo("Attached object '" + object_id +
"' removed from collision scene.")
except:
loginfo("Problem attached object '" + object_id +
"' removing from collision scene.")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Robot Links for arm:"
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print right_arm.get_end_effector_link()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
#scene.remove_world_object("part")
scene.remove_world_object("grasp_marker")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
print p.header.frame_id
p.pose.position.x = 0.4
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.0
#scene.add_box("pole", p, (0.3, 0.1, 1.0))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
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 arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the 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)
# 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 "grasp" pose stored in the SRDF file
arm.set_named_target('left_arm_up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.21
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.19
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.24
target_pose.pose.position.y = 0.275
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 blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 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 = 0.18
place_pose.pose.position.y = 0
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
grasp_pose.pose.position.x = 0.12792118579 + .1
grasp_pose.pose.position.y = 0.285290879999 + 0.05
grasp_pose.pose.position.z = 0.120301181892
#grasp_pose.pose.orientation =
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id, table_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)
break
# 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 = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = 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:
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('left_arm_rest')
arm.go()
# Open the gripper to the open position
gripper.set_joint_value_target(GRIPPER_OPEN)
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):
# 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)
class Pick_Place:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1)
# 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()
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
#
# self._pose_table = self._scene.get_object_poses(self._table_object_name)
# self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name)
# self.setup()
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
rospy.sleep(1.0)
self._scene.remove_world_object(self._grasp_object_name)
rospy.sleep(1.0)
# # Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
rospy.sleep(2.0)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(2.0)
# 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.06
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))
# 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 setup():
# 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_grasp_block_(self._grasp_object_name)
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_attached_object()
def _add_table(self, name):
rospy.loginfo("entered table")
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.2
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (9 , 9, 0.02))
return p.pose
def _add_grasp_block_(self, name):
rospy.loginfo("entered box grabber")
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.05
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.1, 0.1, 0.1))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle )
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def _add_objects(self):
"""
Here add all the objects that you want to add to the _scene
"""
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
rospy.sleep(1)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
# remove part
scene.remove_world_object(PICK_OBJECT)
scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT)
rospy.sleep(2)
i += 1
# end
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
