def create_collision_object(shape_type,pos,size,frame_id,op,object_id): col = CollisionObject() col.id = object_id col.operation = op col.header.frame_id=frame_id # create primitive primitive = SolidPrimitive() primitive.type = shape_type primitive.dimensions = size # create pose pose = Pose() pose.position.x = pos[0] pose.position.y = pos[1] pose.position.z = pos[2] pose.orientation.x = pose.orientation.y = pose.orientation.z = 0 pose.orientation.w = 1 col.primitives = [primitive] col.primitive_poses = [pose] return col
def add_box_obstacle(self, size, name, pose):
"""
Adds a rectangular prism obstacle to the planning scene
Inputs:
size: 3x' ndarray; (x, y, z) size of the box (in the box's body frame)
name: unique name of the obstacle (used for adding and removing)
pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame
"""
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = size
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [pose.pose]
# Publish the object
self._planning_scene_publisher.publish(co)
def add_padded_lab():
rospy.loginfo("Adding padded order bin")
poses = []
objects = []
o_b_pose = Pose()
o_b_pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
o_b_pose.position = Point(x=0.5, y=0.2, z=0.22)
objects.append(
SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.43, 0.68, 0.46]))
poses.append(o_b_pose)
wiring_pose = Pose()
wiring_pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
wiring_pose.position = Point(x=-.55, y=0, z=0.15)
objects.append(
SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.6, 0.7, 0.4]))
poses.append(wiring_pose)
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "padded_lab"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
def form_attach_collision_object_msg(self, link_name, object_name, size,
pose):
obj = AttachedCollisionObject()
# The CollisionObject will be attached with a fixed joint to this link
obj.link_name = link_name
# This contains the actual shapes and poses for the CollisionObject
# to be attached to the link
col_obj = CollisionObject()
col_obj.id = object_name
col_obj.header.frame_id = link_name
col_obj.header.stamp = rospy.Time.now()
sp = SolidPrimitive()
sp.type = sp.BOX
sp.dimensions = [0.0] * 3
sp.dimensions[0] = size.x
sp.dimensions[1] = size.y
sp.dimensions[2] = size.z
col_obj.primitives = [sp]
col_obj.primitive_poses = [pose]
# Adds the object to the planning scene. If the object previously existed, it is replaced.
col_obj.operation = col_obj.ADD
obj.object = col_obj
# The weight of the attached object, if known
obj.weight = 0.0
return obj
def __init__(self, speed):
self.speed = speed
# Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the robot
self.robot = moveit_commander.RobotCommander()
# Initialize the planning scene
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
# Instantiate a move group
self.group = moveit_commander.MoveGroupCommander(GROUP_NAME)
# Set the maximum time MoveIt will try to plan before giving up
self.group.set_planning_time(.25)
# Set maximum velocity scaling
self.group.set_max_velocity_scaling_factor(1.0)
self.group.set_max_acceleration_scaling_factor(1.0)
# For displaying plans to RVIZ
self.display_pub = rospy.Publisher('/move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
print(self.group.get_current_pose())
print(self.group.get_end_effector_link())
print(self.group.get_pose_reference_frame())
print(self.group.get_goal_tolerance())
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = 'table'
co.header = TABLE_CENTER.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = TABLE_SIZE
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [TABLE_CENTER.pose]
# Publish the object (don't know why but need to publish twice)
self.scene_publisher.publish(co)
rospy.sleep(0.5)
self.scene_publisher.publish(co)
rospy.sleep(0.5)
def __make_sphere(self, name, pose, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
return co
def __make_sphere(self, name, pose, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def create_sphere(name, pose, radius, frame_id='/world_frame'):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header.frame_id = frame_id
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose]
return co
def __make_cylinder(name, pose, height, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
return co
def create_box(name, pose, size, frame_id='/world_frame'):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header.frame_id = frame_id
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def __make_cylinder(name, pose, height, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
return co
def find_block(self):
"""
Find block and add it to the scene
"""
# Get necessary values
while not self.object_location:
print "Waiting for object location..."
rospy.sleep(1.0)
object_location = self.object_location
image_size = self.left_camera_size
while not self.current_pose:
print "Waiting for current pose values..."
rospy.sleep(1.0)
current_pose = self.current_pose.position
endeffector_height = current_pose.z - self.table_height
# Store values for calculation
Pp = [object_location.x, object_location.y, object_location.z]
Cp = [image_size[0] / 2, image_size[1] / 2, 0]
Bp = [current_pose.x, current_pose.y, current_pose.z]
Go = [-0.0230, 0.0110, 0.1100]
cc = [0.0021, 0.0021, 0.0000]
d = [endeffector_height, endeffector_height, 0.0000]
# Calculate block's position in workspace
# workspace = (Pp - Cp) * cc * d + Bp + Go
pixel_difference = map(operator.sub, Pp, Cp)
camera_constant = map(operator.mul, cc, d)
pixel_2_real = map(operator.mul, pixel_difference, camera_constant)
pixel_2_real[2] = endeffector_height * -1
workspace_without_gripper = map(operator.add, pixel_2_real, Bp)
workspace = map(operator.add, workspace_without_gripper, Go)
# Add block to scene
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = workspace_without_gripper[0]
p.pose.position.y = workspace_without_gripper[1]
p.pose.position.z = workspace_without_gripper[2] + 0.06
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "block"
co.header = p.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list((0.1, 0.1, 0.1))
co.primitives = [box]
co.primitive_poses = [p.pose]
pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True)
pub_co.publish(co)
return
def __make_cylinder(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cyl = SolidPrimitive()
cyl.type = SolidPrimitive.CYLINDER
cyl.dimensions = list(size)
co.primitives = [cyl]
co.primitive_poses = [pose.pose]
return co
def update_collision_box(id,
position,
orientation=(1, 0, 0, 0),
dimensions=(1, 1, 1),
operation=CollisionObject.ADD):
pose = make_pose(position, orientation, robot.get_planning_frame())
co = CollisionObject()
co.operation = operation
co.id = id
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = dimensions
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
def attach_object(self, group_name):
rospy.loginfo('attaching object to ' + group_name)
# select toolframe depending on group name
tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT
# pose for attached object in tool frame coordiantes
pose = Pose()
pose.position.z = 0.05
pose.orientation.w = 1.0
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.07, 0.07, 0.07]
o = CollisionObject()
o.header.frame_id = tool_frame
o.id = "handed_object"
o.primitives = [primitive]
o.primitive_poses = [pose]
o.operation = o.ADD
a = AttachedCollisionObject()
a.object = o
# allow collisions with hand links
a.touch_links = ALLOWED_TOUCH_LINKS
# attach object to tool frame
a.link_name = tool_frame
# don't delete old planning scene, if we didn't get one so far, create a new one
scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene(
)
# add attached object to scene
scene.robot_state.attached_collision_objects.append(a)
# mark scene as changed
scene.is_diff = True
self.pub_ps.publish(scene)
def attach_object(self, group_name):
rospy.loginfo('attaching object to ' + group_name)
# select toolframe depending on group name
tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT
# pose for attached object in tool frame coordiantes
pose = Pose()
pose.position.z = 0.05
pose.orientation.w = 1.0
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.07, 0.07, 0.07]
o = CollisionObject()
o.header.frame_id = tool_frame
o.id = "handed_object"
o.primitives = [primitive]
o.primitive_poses = [pose]
o.operation = o.ADD
a = AttachedCollisionObject()
a.object = o
# allow collisions with hand links
a.touch_links = ALLOWED_TOUCH_LINKS
# attach object to tool frame
a.link_name = tool_frame
# don't delete old planning scene, if we didn't get one so far, create a new one
scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene()
# add attached object to scene
scene.robot_state.attached_collision_objects.append(a)
# mark scene as changed
scene.is_diff = True
self.pub_ps.publish(scene)
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_obstacle(position, operation=CollisionObject.ADD):
planning_scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
rospy.sleep(2)
pose = make_pose(position, [1, 0, 0, 0], robot.get_planning_frame())
co = CollisionObject()
co.operation = operation
co.id = "obstacle"
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = (0.5, .7, 0.1)
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
def attach_cylinder(link, name, pose, height, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def attach_sphere(link, name, pose, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def add_box_obstacle(self, dimensions, name, com_position, com_orientation):
"""
Add a rectangular prism obstacle to the planning scene.
Arguments:
dimensions: An array containing the width, length, and height of
the box (in the box's body frame, corresponding to x, y, and z).
name: A unique name for identifying this obstacle.
com_position: The position of the center-of-mass (COM) of this box,
relative to the global frame `frame_id`.
com_orientation: The orientation of the COM.
"""
pose = create_pose(self.frame_id, com_position, com_orientation)
obj = CollisionObject()
obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header
box = SolidPrimitive()
box.type, box.dimensions = SolidPrimitive.BOX, dimensions
obj.primitives, obj.primitive_poses = [box], [pose.pose]
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Added box object "{}" to planning scene: '
'(x={}, y={}, z={}).'.format(name, *dimensions))
def add_arbitrary_obstacle(size, id, pose, planning_scene_publisher, scene,
robot, operation):
"""
Adds an arbitrary rectangular prism obstacle to the planning scene.
This is currently only for the ground plane
size: numpy array of size 3 (x,y,z dimensions)
id: string (object id/name)
pose: PoseStamped objecct (objects pose with respect to world frame)
planning_scene_publisher: ros Publisher('/collision_object', CollisionObject)
scene: PlanningSceneInterface
robot: RobotCommander
operation: currently just use CollisionObject.ADD
"""
co = CollisionObject()
co.operation = operation
co.id = id
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = size
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
def plane(): wall_object = CollisionObject() wall_object.operation = wall_object.ADD wall_object.id = "wall_box" wall_object.header.frame_id = "base_link" wall_pose = Pose() wall_pose.orientation.w = 1 wall_pose.position.z = 0.15 wall_pose.position.x = -0.18 wall_pose.position.y = 0 wall = SolidPrimitive() wall.type = SolidPrimitive.BOX wall.dimensions = [0.01, 0.27, 0.3] wall_object.primitives = [wall] wall_object.primitive_poses.append(wall_pose) # ---------------------------------- wall2_object = CollisionObject() wall2_object.operation = wall_object.ADD wall2_object.id = "wall2_box" wall2_object.header.frame_id = "base_link" wall2_pose = Pose() wall2_pose.orientation.w = 1 wall2_pose.position.z = 0.15 wall2_pose.position.x = -0.05 wall2_pose.position.y = -0.15 wall2 = SolidPrimitive() wall2.type = SolidPrimitive.BOX wall2.dimensions = [0.24, 0.01, 0.3] wall2_object.primitives = [wall2] wall2_object.primitive_poses.append(wall2_pose) # ---------------------------------- wall3_object = CollisionObject() wall3_object.operation = wall_object.ADD wall3_object.id = "wall3_box" wall3_object.header.frame_id = "base_link" wall3_pose = Pose() wall3_pose.orientation.w = 1 wall3_pose.position.z = 0.15 wall3_pose.position.x = -0.05 wall3_pose.position.y = 0.15 wall3 = SolidPrimitive() wall3.type = SolidPrimitive.BOX wall3.dimensions = [0.24, 0.01, 0.3] wall3_object.primitives = [wall3] wall3_object.primitive_poses.append(wall3_pose) # ---------------------------------- base1_object = CollisionObject() base1_object.operation = base1_object.ADD base1_object.id = "base1_box" base1_object.header.frame_id = "base_link" base1_pose = Pose() base1_pose.orientation.w = 1 base1_pose.position.y = 0.0 base1_pose.position.x = -0.07 base1_pose.position.z = 0.03 base1 = SolidPrimitive() base1.type = SolidPrimitive.BOX base1.dimensions = [0.22, 0.27, 0.01] base1_object.primitives = [base1] base1_object.primitive_poses.append(base1_pose) # ---------------------------------- base2_object = CollisionObject() base2_object.operation = base1_object.ADD base2_object.id = "base2_box" base2_object.header.frame_id = "base_link" base2_pose = Pose() base2_pose.orientation.w = 1 base2_pose.position.y = 0.0 base2_pose.position.x = -0.07 base2_pose.position.z = 0.35 base2 = SolidPrimitive() base2.type = SolidPrimitive.BOX base2.dimensions = [0.20, 0.27, 0.01] base2_object.primitives = [base2] base2_object.primitive_poses.append(base2_pose) planning_scene = PlanningScene() planning_scene.is_diff = True planning_scene.world.collision_objects.append(wall_object) planning_scene.world.collision_objects.append(wall2_object) planning_scene.world.collision_objects.append(base1_object) planning_scene.world.collision_objects.append(base2_object) client(planning_scene)
def add_order_bin(x, y):
# Necessary parameters
bin_width, bin_depth, bin_height = 0.65, 0.4, 0.43
interior_width, interior_depth, interior_height = 0.45, 0.29, 0.18
wall_width, wall_depth = (bin_width - interior_width) / 2, (
bin_depth - interior_depth) / 2
base_height = bin_height - interior_height
objects = []
poses = []
# Create Bottom
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[bin_depth, bin_width, base_height],
))
poses.append(
Pose(
position=Point(x=x, y=y, z=base_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create left side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[bin_depth, wall_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x,
y=y - interior_width / 2 - wall_width / 2,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create right side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[bin_depth, wall_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x,
y=y + interior_width / 2 + wall_width / 2,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create near side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[wall_depth, interior_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x - interior_depth / 2 - wall_depth / 2,
y=y,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create far side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[wall_depth, interior_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x + interior_depth / 2 + wall_depth / 2,
y=y,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "order_bin"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
def add_bin(bin, prefix="/shelf"):
# Necessary parameters
_, max_x, min_y, max_y, min_z, max_z = rospy.get_param(prefix + "/bins/" +
bin)
shelf_max_x = DEPTH / 2
shelf_min_y, shelf_max_y = -WIDTH / 2, WIDTH / 2
shelf_min_z, shelf_max_z = 0, HEIGHT
objects = []
poses = []
# Create left side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH, abs(shelf_min_y - min_y), HEIGHT],
))
poses.append(
Pose(
position=Point(x=0, y=(shelf_min_y + min_y) / 2, z=HEIGHT / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create right side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH, abs(shelf_max_y - max_y), HEIGHT],
))
poses.append(
Pose(
position=Point(x=0, y=(shelf_max_y + max_y) / 2, z=HEIGHT / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create top
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH,
abs(max_y - min_y),
abs(shelf_max_z - max_z)],
))
poses.append(
Pose(
position=Point(x=0,
y=(max_y + min_y) / 2,
z=(shelf_max_z + max_z) / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create bottom
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH,
abs(max_y - min_y),
abs(shelf_min_z - min_z)],
))
poses.append(
Pose(
position=Point(x=0,
y=(max_y + min_y) / 2,
z=(shelf_min_z + min_z) / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create back
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[DEPTH / 2,
abs(max_y - min_y),
abs(max_z - min_z)],
))
poses.append(
Pose(
position=Point(x=shelf_max_x / 2,
y=(max_y + min_y) / 2,
z=(min_z + max_z) / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "shelf"
co.header.frame_id = "/shelf"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
def hand_over():
rospy.init_node('moveit_node')
listener = tf.TransformListener()
#listener.waitForTransform('left_hand_camera', 'base')
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
#rospy.init_node('moveit_node')
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
#right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
#right_arm.set_planner_id('RRTConnectkConfigDefault')
#right_arm.set_planning_time(10)
# Add a collison object
# Build the attached object
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
#shape.dimensions.resize(3)
# print(shape.dimensions)
shape.dimensions.append(0.6)
shape.dimensions.append(1.5)
shape.dimensions.append(0.6)
# print(shape)
# Create pose
obj_pose = Pose()
obj_pose.position.x = 0.665
obj_pose.position.y = 0.062
obj_pose.position.z = -0.7
obj_pose.orientation.x = 0
obj_pose.orientation.y = 0
obj_pose.orientation.z = 0
obj_pose.orientation.w = 1
# create collision object
cobj = CollisionObject()
cobj.primitives = shape
cobj.primitive_poses=obj_pose
cobj.header.frame_id='base'
cobj.id = 'Table'
# create collision object message
# ATTACHED_COLLISION_OBJECT.link_name = TCP_LINK_NAME
# ATTACHED_COLLISION_OBJECT.object = cobj
# ATTACHED_COLLISION_OBJECT.object.header.frame_id = TCP_LINK_NAME
# ATTACHED_COLLISION_OBJECT.touch_links.push_back("gripper_body")
# Attache it to the scene
cobj.operation = CollisionObject.ADD
attach_object_publisher = rospy.Publisher('collision_object',CollisionObject,queue_size = 10)
attach_object_publisher.publish(cobj)
rospy.sleep(1)
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
input=1
pile_1=[]
pile_2=[]
pile_3=[]
while input:
#Original pose ------------------------------------------------------
original = PoseStamped()
original.header.frame_id = "base"
#the pose the baxter will first turn to
original.pose.position.x = 0.505
original.pose.position.y = 0.487
original.pose.position.z = 0.515
original.pose.orientation.x = 0.505
original.pose.orientation.y = 0.487
original.pose.orientation.z = 0.515
original.pose.orientation.w = -0.493
left_arm.set_pose_target(original)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
left_plan = left_arm.plan()
raw_input('Press <Enter> to move the left arm to the original pose: ')
left_arm.execute(left_plan)
rospy.sleep(2)
# First goal pose to pick up ----------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
while not rospy.is_shutdown():
try:
marker = rospy.wait_for_message('ar_pose_marker',AlvarMarkers)
#rospy.sleep(1)
#print(marker)
ar_name = marker.markers[0].id
arTagName = 'ar_marker_' + str(ar_name)
print(arTagName)
break
except:
continue
#arTagName = 'ar_marker_' + str(ar_name)
# Set up your subscriber and define its callback
image_topic = "/cameras/left_hand_camera/image"
my_image_msg = rospy.wait_for_message(image_topic, Image)
print("Received an image!")
# Convert your ROS Image message to OpenCV2
cv2_img = bridge.imgmsg_to_cv2(my_image_msg, "bgr8")
# Save your OpenCV2 image as a jpeg
cv2.imwrite('camera_image'+str(ar_name)+'.png', cv2_img)
# Carry out the digit recognition alg
# zipcode = digitRecog('camera_image'+str(ar_name)+'.png')
# Display the original image on Baxter's screen
display_pub = rospy.Publisher('/robot/xdisplay',Image, latch=True)
display_pub.publish(my_image_msg)
rospy.sleep(1)
while not rospy.is_shutdown():
try:
(trans_marker_cam, rot_marker_cam) = listener.lookupTransform(arTagName,'left_hand_camera', rospy.Time())
(trans_cam_hand, rot_cam_hand) = listener.lookupTransform('left_hand_camera', 'left_hand', rospy.Time())
(trans_hand_base, rot_hand_base) = listener.lookupTransform('left_hand', 'base', rospy.Time())
break
except:
continue
# Find the transform between the AR tag and the base
g_marker_cam = quat_to_rbt(trans_marker_cam, rot_marker_cam)
g_cam_hand = quat_to_rbt(trans_cam_hand, rot_cam_hand)
g_hand_base = quat_to_rbt(trans_hand_base, rot_hand_base)
g_final = linalg.inv(np.dot(np.dot(g_marker_cam, g_cam_hand), g_hand_base))
# print('final')
# print(g_final)
#x, y, and z position
x_pos = "%.3f" % (g_final[0,3])
y_pos = "%.3f" % (g_final[1,3])
z_pos = "%.3f" % (g_final[2,3])
# print(x_pos)
# print(y_pos)
# print(z_pos)
goal_1.pose.position.x = float(x_pos)
goal_1.pose.position.y = float(y_pos) - 0.2
goal_1.pose.position.z = float(z_pos)
#Orientation as a quaternion
goal_1.pose.orientation.x = -0.689
goal_1.pose.orientation.y = -0.013
goal_1.pose.orientation.z = -0.035
goal_1.pose.orientation.w = 0.723
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback)
rospy.sleep(1)
# print(mail_range)
#Check using IR to see of the mail is close enough
incre = 0.01
while mail_range > 0.06 and incre < 0.05:
print('Too far from the mail. Trying again...')
rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback)
rospy.sleep(1)
print(mail_range)
goal_1.pose.position.y = float(y_pos) - 0.1 + incre
incre += 0.01
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to get the mail: ')
left_arm.execute(left_plan)
# print(mail_range)
#Close the Gripper
raw_input('Press <Enter> to close the gripper')
left_gripper.close(timeout=60)
print('Gripper closed')
#TODO- generate the recognized image and publish it
# Check http://sdk.rethinkrobotics.com/wiki/Display_Image_-_Code_Walkthrough
# display_pub.sleep()
img = cv2.imread('withRecog'+str(ar_name)+'.png')
msg = CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
rec_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
rec_pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(1)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
if ar_name in (1, 3, 12):
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0.6
goal_2.pose.position.z = -0.18
pile_1.insert(0,ar_name)
elif ar_name in (0,9,15,16):
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0.3
goal_2.pose.position.z = -0.18
pile_2.insert(0,ar_name)
else:
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0
goal_2.pose.position.z = -0.18
pile_3.insert(0,ar_name)
print('pile_1')
print(pile_1)
print('pile_2')
print(pile_2)
print('pile_3')
print(pile_3)
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Execute the plan
#raw_input('Press <Enter> to move the left arm to the zipcode region: ')
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to the zipcode region: ')
left_arm.execute(left_plan)
#Release the Gripper
raw_input('Press <Enter> to open the gripper')
left_gripper.open(block=True)
print('Gripper opened')
#reg_pub.sleep()
#Continue or stop
input=int(raw_input('Press 1 to start/continue moving, or 0 to stop moving '))
return (pile_1,pile_2,pile_3)
handle_pose.position.z = 1.22
handle_pose.orientation.x = 0
handle_pose.orientation.y = 0
handle_pose.orientation.z = 0
handle_pose.orientation.w = 1
surface_pose = deepcopy(handle_pose)
surface_pose.position.x += 0.25
surface_pose.position.z += 0.11
handle_size = [0.02, 0.155, 0.16]
surface_size = [0.48, 0.23, 0.08]
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "Scoop"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
handle = SolidPrimitive()
handle.type = SolidPrimitive.BOX
handle.dimensions = handle_size
surface = SolidPrimitive()
surface.type = SolidPrimitive.BOX
surface.dimensions = surface_size
co.primitives = [handle, surface]
co.primitive_poses = [handle_pose, surface_pose]
scene._pub_co.publish(co)
print "Published scoop"
# rospy.spin()
moveit_commander.roscpp_shutdown()
def __init__(self):
"""
Initialize class
"""
# Overall MoveIt initialization
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
# MoveIt scene initialization
self.scene = moveit_commander.PlanningSceneInterface()
self.table_height = -0.320
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.8
p.pose.position.y = 0.025
p.pose.position.z = -0.6
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "table"
co.header = p.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list((0.75, 1.25, 0.55))
co.primitives = [box]
co.primitive_poses = [p.pose]
pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True)
pub_co.publish(co)
# Moveit group initialization
self.group = moveit_commander.MoveGroupCommander("left_arm")
self.group.set_goal_position_tolerance(0.12)
self.group.set_goal_orientation_tolerance(0.10)
# Gripper initialization
self.left_gripper = baxter_interface.Gripper("left", CHECK_VERSION)
self.left_gripper.reboot()
self.left_gripper.calibrate()
# Camera initialization
self.left_camera = baxter_interface.CameraController("left_hand_camera")
self.right_camera = baxter_interface.CameraController("right_hand_camera")
self.head_camera = baxter_interface.CameraController("head_camera")
self.right_camera.close()
self.head_camera.close()
self.left_camera.open()
self.left_camera.resolution = [1280, 800]
self.left_camera_size = [800, 1280]
_camera_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self._on_camera)
# Open CV initialization
cv2.namedWindow("Original", cv2.WINDOW_NORMAL)
cv2.moveWindow("Original", 1990, 30)
cv2.namedWindow("Thresholded", cv2.WINDOW_NORMAL)
cv2.moveWindow("Thresholded", 3270, 30)
self.low_h = 165
self.high_h = 179
self.low_s = 70
self.high_s = 255
self.low_v = 60
self.high_v = 255
self.bridge = CvBridge()
# Object location initialization
self.object_location = None
# Subscribe to pose at all times
self.current_pose = None
_pose_sub = rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, self.set_current_pose)
return
TABLE_CENTER.pose = Pose(position=Point(0, -1.37, .76))
TABLE_CENTER.pose.orientation.w = 1
TABLE_SIZE = [1.525, 2.74, 0.0201]
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = 'table'
co.header = TABLE_CENTER.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = TABLE_SIZE
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [TABLE_CENTER.pose]
print(co)
count = 0
while not rospy.is_shutdown():
pub.publish(co)
co.primitive_poses[0].position.x += 1
count += 1
print(count)
rospy.sleep(0.5)
