def _add_table_to_map(self, table, table_name):
co = CollisionObject()
co.id = table_name
if not co.id:
co.id = 'current_table'
co.header = table.pose.header
#we do NOT want a padded table
co.padding = -1
if len(table.convex_hull.vertices) > 0:
if self.table_thickness > 0:
table_z = range(0, int(1000*self.table_thickness/2.0), int(1000*TABLE_RESOLUTION))
table_z.append(1000.0*self.table_thickness/2.0)
sgnrng = [1.0, -1.0]
else:
table_z = [0]
sgnrng = [1.0]
for z in table_z:
for sgn in sgnrng:
co.shapes.append(table.convex_hull)
ps = tl.transform_pose_stamped(self._wi.world_frame, table.pose)
ps.pose.position.z += sgn*z/1000.0
co.poses.append(tl.transform_pose(table.pose.header.frame_id, ps.header.frame_id, ps.pose))
rospy.logdebug('Adding table as convex hull')
else:
bbox = Shape()
bbox.type = bbox.BOX
bbox.dimensions = [abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), self.table_thickness]
co.shapes.append(bbox)
co.poses.append(table.pose.pose)
rospy.logdebug('Adding table as bounding box')
self._wi.add_object(co)
return co.id
def process_collision_geometry_for_cluster(self, cluster):
rospy.loginfo("adding cluster with %d points to collision map" %
len(cluster.points))
many_boxes = CollisionObject()
many_boxes.operation.operation = CollisionObjectOperation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points) / 100.0)
random_indices = range(len(cluster.points))
scipy.random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005] * 3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0, 0, 0, 1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
collision_name = self.get_next_object_name()
many_boxes.id = collision_name
self.object_in_map_pub.publish(many_boxes)
return collision_name
def process_collision_geometry_for_table(self,
firsttable,
additional_tables=[]):
table_object = CollisionObject()
table_object.operation.operation = CollisionObjectOperation.ADD
table_object.header.frame_id = firsttable.pose.header.frame_id
table_object.header.stamp = rospy.Time.now()
#create a box for each table
for table in [
firsttable,
] + additional_tables:
object = Shape()
object.type = Shape.BOX
object.dimensions.append(math.fabs(table.x_max - table.x_min))
object.dimensions.append(math.fabs(table.y_max - table.y_min))
object.dimensions.append(0.01)
table_object.shapes.append(object)
#set the origin of the table object in the middle of the firsttable
table_mat = self.pose_to_mat(firsttable.pose.pose)
table_offset = scipy.matrix([
(firsttable.x_min + firsttable.x_max) / 2.0,
(firsttable.y_min + firsttable.y_max) / 2.0, 0.0
]).T
table_offset_mat = scipy.matrix(scipy.identity(4))
table_offset_mat[0:3, 3] = table_offset
table_center = table_mat * table_offset_mat
origin_pose = self.mat_to_pose(table_center)
table_object.poses.append(origin_pose)
table_object.id = "table"
self.object_in_map_pub.publish(table_object)
def add_collision_cluster(self, cluster, object_id):
'''
Adds a point cloud to the collision map as a single collision object composed of many small boxes.
**Args:**
**cluster (sensor_msg.msg.PointCloud):** The point cloud to add to the map
**object_id (string):** The name the point cloud should have in the map
'''
many_boxes = CollisionObject()
many_boxes.operation.operation = many_boxes.operation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points) / 100.0)
random_indices = range(len(cluster.points))
random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005] * 3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0, 0, 0, 1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
many_boxes.id = object_id
self._publish(many_boxes, self._collision_object_pub)
def add_collision_cluster(self, cluster, object_id):
'''
Adds a point cloud to the collision map as a single collision object composed of many small boxes.
**Args:**
**cluster (sensor_msg.msg.PointCloud):** The point cloud to add to the map
**object_id (string):** The name the point cloud should have in the map
'''
many_boxes = CollisionObject()
many_boxes.operation.operation = many_boxes.operation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points)/100.0)
random_indices = range(len(cluster.points))
random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005]*3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0,0,0,1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
many_boxes.id = object_id
self._publish(many_boxes, self._collision_object_pub)
def process_collision_geometry_for_table(self, firsttable, additional_tables = []):
table_object = CollisionObject()
table_object.operation.operation = CollisionObjectOperation.ADD
table_object.header.frame_id = firsttable.pose.header.frame_id
table_object.header.stamp = rospy.Time.now()
#create a box for each table
for table in [firsttable,]+additional_tables:
object = Shape()
object.type = Shape.BOX;
object.dimensions.append(math.fabs(table.x_max-table.x_min))
object.dimensions.append(math.fabs(table.y_max-table.y_min))
object.dimensions.append(0.01)
table_object.shapes.append(object)
#set the origin of the table object in the middle of the firsttable
table_mat = self.pose_to_mat(firsttable.pose.pose)
table_offset = scipy.matrix([(firsttable.x_min + firsttable.x_max)/2.0, (firsttable.y_min + firsttable.y_max)/2.0, 0.0]).T
table_offset_mat = scipy.matrix(scipy.identity(4))
table_offset_mat[0:3,3] = table_offset
table_center = table_mat * table_offset_mat
origin_pose = self.mat_to_pose(table_center)
table_object.poses.append(origin_pose)
table_object.id = "table"
self.object_in_map_pub.publish(table_object)
def process_collision_geometry_for_cluster(self, cluster):
rospy.loginfo("adding cluster with %d points to collision map"%len(cluster.points))
many_boxes = CollisionObject()
many_boxes.operation.operation = CollisionObjectOperation.ADD
many_boxes.header = cluster.header
many_boxes.header.stamp = rospy.Time.now()
num_to_use = int(len(cluster.points)/100.0)
random_indices = range(len(cluster.points))
scipy.random.shuffle(random_indices)
random_indices = random_indices[0:num_to_use]
for i in range(num_to_use):
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = [.005]*3
pose = Pose()
pose.position.x = cluster.points[random_indices[i]].x
pose.position.y = cluster.points[random_indices[i]].y
pose.position.z = cluster.points[random_indices[i]].z
pose.orientation = Quaternion(*[0,0,0,1])
many_boxes.shapes.append(shape)
many_boxes.poses.append(pose)
collision_name = self.get_next_object_name()
many_boxes.id = collision_name
self.object_in_map_pub.publish(many_boxes)
return collision_name
def make_gripper_obs(self):
'''
Creates a box obstacle from the dimensions of the gripper.
'''
obs = Shape()
obs.type = 1
obs.dimensions = [0.10, 0.10, 0.10]
return obs
def _get_bounding_box_collision_object(self, box):
#we don't try to extend the box down to the table...
#so hopefully we detected all the way down
co = CollisionObject()
co.header = box.pose.header
shape = Shape()
shape.type = shape.BOX
shape.dimensions.append(box.box_dims.x)
shape.dimensions.append(box.box_dims.y)
shape.dimensions.append(box.box_dims.z)
co.shapes.append(shape)
co.poses.append(box.pose.pose)
return co
def add_map_tables(wi):
(serving_table, dirty_table) = [add_map_table('serving_table', wi), add_map_table('dirty_table', wi)]
#little_table = add_map_table('little_table', wi)
st_base = CollisionObject()
st_base.header = serving_table.pose.header
st_base.id = "serving_table_base"
st_base.poses.append(copy.deepcopy(serving_table.pose.pose))
st_base.poses[0].position.z = 0.1
st_shape = Shape()
st_shape.type = Shape.CYLINDER
st_shape.dimensions = [0.3, 0.1]
st_base.shapes.append(st_shape)
wi.add_object(st_base)
def add_collision_box(self, box_pose, box_dims, frame_id, collision_name):
rospy.loginfo("adding box to collision map")
box = CollisionObject()
box.operation.operation = CollisionObjectOperation.ADD
box.header.frame_id = frame_id
box.header.stamp = rospy.Time.now()
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = box_dims
box.shapes.append(shape)
box.poses.append(box_pose)
box.id = collision_name
self.object_in_map_pub.publish(box)
def add_collision_box(self, box_pose, box_dims, frame_id, collision_name):
rospy.loginfo("adding box to collision map")
box = CollisionObject()
box.operation.operation = CollisionObjectOperation.ADD
box.header.frame_id = frame_id
box.header.stamp = rospy.Time.now()
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = box_dims
box.shapes.append(shape)
box.poses.append(box_pose)
box.id = collision_name
self.object_in_map_pub.publish(box)
def add_map_table(name, wi):
table = rospy.get_param('wg-sushi-tables/'+name)
upper_left = Point(x = table['upper_left']['x'], y = table['upper_left']['y'],
z = table['upper_left']['z'])
lower_left = Point(x = table['lower_left']['x'], y = table['lower_left']['y'],
z = table['lower_left']['z'])
upper_right = Point(x = table['upper_right']['x'], y = table['upper_right']['y'],
z = table['upper_right']['z'])
lower_right = Point(x = table['lower_right']['x'], y = table['lower_right']['y'],
z = table['lower_right']['z'])
table_object = CollisionObject()
table_object.header.frame_id = wi.world_frame
table_object.id = name
table_pose = Pose()
table_pose.position.x = (upper_left.x+upper_right.x+lower_left.x+lower_right.x)/4.0
table_pose.position.y = (upper_left.y+upper_right.y+lower_left.y+lower_right.y)/4.0
table_pose.position.z = (upper_left.z+upper_right.z+lower_left.z+lower_right.z)/4.0
table_pose.orientation.w = 1.0
table_shape = Shape()
table_shape.type = Shape.MESH
table_shape.vertices = [gt.inverse_transform_point(lower_left, table_pose),
gt.inverse_transform_point(upper_left, table_pose),
gt.inverse_transform_point(upper_right, table_pose),
gt.inverse_transform_point(lower_right, table_pose)]
table_shape.triangles = [0, 1, 2, 2, 3, 0]
table_object.shapes.append(table_shape)
table_upper_pose = copy.deepcopy(table_pose)
table_upper_pose.position.z += 0.02
table_object.poses.append(table_pose)
table_object.poses.append(table_upper_pose)
table_object.shapes.append(table_shape)
table_lower_pose = copy.deepcopy(table_pose)
table_lower_pose.position.z -= 0.02
table_object.poses.append(table_lower_pose)
table_object.shapes.append(table_shape)
if name == 'dirty_table':
#kind of a hack =D
for i in range(5):
table_lower_pose = copy.deepcopy(table_lower_pose)
table_lower_pose.position.z -= 0.02
table_object.poses.append(table_lower_pose)
table_object.shapes.append(table_shape)
wi.add_object(table_object)
table = Table()
table.pose.header = table_object.header
table.pose.pose = table_object.poses[0]
table.convex_hull = table_object.shapes[0]
return table
def __build_object_from_box(self, box_msg, object_id):
msg = CollisionObject()
msg.header.frame_id = box_msg.pose.header.frame_id
msg.header.stamp = rospy.Time.now()
msg.id = object_id
msg.operation.operation = msg.operation.ADD
shape = Shape()
shape.type = shape.BOX
scale = 1.0
shape.dimensions = [scale*box_msg.box_dims.x,
scale*box_msg.box_dims.y,
scale*box_msg.box_dims.z]
msg.shapes.append(shape)
msg.poses.append(box_msg.pose.pose)
return msg
def add_barrier(self):
box = CollisionObject()
box.header.frame_id = self.wi.world_frame
box.operation.operation = box.operation.ADD
box.id = "barrier1"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 2, 1]
box.shapes.append(shape)
pose = Pose()
pose.position.x = 1
pose.position.y = -1.5
pose.position.z = 0
pose.orientation.w = 1.0
box.poses.append(pose)
self.wi.add_object(box)
def build_allowed_contact_specification(self, box_pose, box_dimensions):
msg = AllowedContactSpecification()
msg.name = "grasping_object_region"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = box_dimensions
msg.shape = shape
msg.pose_stamped = box_pose
msg.link_names = [
"r_gripper_palm_link", "r_gripper_l_finger_link",
"r_gripper_r_finger_link", "r_gripper_l_finger_tip_link",
"r_gripper_r_finger_tip_link", "l_gripper_palm_link",
"l_gripper_l_finger_link", "l_gripper_r_finger_link",
"l_gripper_l_finger_tip_link", "l_gripper_r_finger_tip_link"
]
return msg
def __build_object_from_box(self, box_msg, object_id):
msg = CollisionObject()
msg.header.frame_id = box_msg.pose.header.frame_id
msg.header.stamp = rospy.Time.now()
msg.id = object_id
msg.operation.operation = msg.operation.ADD
shape = Shape()
shape.type = shape.BOX
scale = 1.0
shape.dimensions = [
scale * box_msg.box_dims.x, scale * box_msg.box_dims.y,
scale * box_msg.box_dims.z
]
msg.shapes.append(shape)
msg.poses.append(box_msg.pose.pose)
return msg
def build_allowed_contact_specification(self, box_pose, box_dimensions):
msg = AllowedContactSpecification()
msg.name = "grasping_object_region"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = box_dimensions
msg.shape = shape
msg.pose_stamped = box_pose
msg.link_names = ["r_gripper_palm_link",
"r_gripper_l_finger_link",
"r_gripper_r_finger_link",
"r_gripper_l_finger_tip_link",
"r_gripper_r_finger_tip_link",
"l_gripper_palm_link",
"l_gripper_l_finger_link",
"l_gripper_r_finger_link",
"l_gripper_l_finger_tip_link",
"l_gripper_r_finger_tip_link"]
return msg
def test_add_convert_objects():
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_collision_objects')
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = [
'r_gripper_palm_link', 'r_gripper_r_finger_link',
'r_gripper_l_finger_link', 'r_gripper_r_finger_tip_link',
'r_gripper_l_finger_tip_link', 'r_wrist_roll_link',
'r_wrist_flex_link', 'r_forearm_link',
'r_gripper_motor_accelerometer_link'
]
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .2
obj2.shapes[0].dimensions[1] = .4
obj2.shapes[0].dimensions[2] = .2
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
att_obj.object = obj2
r = rospy.Rate(6)
sent_twice = 0
while (True):
sent_twice += 1
if sent_twice >= 4 and sent_twice % 2 == 0:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
elif sent_twice > 4 and sent_twice % 2 == 1:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
print 'sending'
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def add_box(self):
box = CollisionObject()
box.header.frame_id = self.wi.world_frame
box.operation.operation = box.operation.ADD
box.id = "box"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.05, 0.15]
pose = Pose()
pose.position.x = 3.1
pose.position.y = -3.2
pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
pose.orientation.w = 1.0
box.shapes.append(shape)
box.poses.append(pose)
self.wi.add_object(box)
return box
def add_collision_box(self, box_pose_stamped, box_dims, object_id):
'''
Adds a box to the map as a collision object.
**Args:**
**box_pose_stamped (geometry_msgs.msg.PoseStamped):** The pose of the box.
**box_dims (tuple of 3 doubles):** The dimensions of the box as (x_dimension, y_dimension, z_dimension)
**object_id (string):** The ID the box should have in the collision map
'''
box = CollisionObject()
box.operation.operation = box.operation.ADD
box.header = box_pose_stamped.header
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = box_dims
box.shapes.append(shape)
box.poses.append(box_pose_stamped.pose)
box.id = object_id
self._publish(box, self._collision_object_pub)
return box
def add_collision_box(self, box_pose_stamped, box_dims, object_id):
'''
Adds a box to the map as a collision object.
**Args:**
**box_pose_stamped (geometry_msgs.msg.PoseStamped):** The pose of the box.
**box_dims (tuple of 3 doubles):** The dimensions of the box as (x_dimension, y_dimension, z_dimension)
**object_id (string):** The ID the box should have in the collision map
'''
box = CollisionObject()
box.operation.operation = box.operation.ADD
box.header = box_pose_stamped.header
shape = Shape()
shape.type = Shape.BOX
shape.dimensions = box_dims
box.shapes.append(shape)
box.poses.append(box_pose_stamped.pose)
box.id = object_id
self._publish(box, self._collision_object_pub)
return box
def get_virtual_gloves():
r_glove = AttachedCollisionObject()
r_glove.object.header.stamp = rospy.get_rostime()
r_glove.object.header.frame_id = '/r_gripper_palm_link'
r_glove.link_name = 'r_gripper_palm_link'
r_glove.object.id = 'r_gripper_glove'
r_glove.object.operation.operation = CollisionObjectOperation.ADD
glove_shape = Shape()
glove_shape.type = Shape.BOX
glove_shape.dimensions = [ 0.25, 0.18, 0.1 ]
glove_pose = Pose()
glove_pose.orientation.w = 1
glove_pose.position.x = 0.1
# Pose will be zero
r_glove.touch_links = ['r_end_effector',
'r_wrist_roll_link',
'r_wrist_flex_link',
'r_forearm_link']
r_glove.object.shapes.append(glove_shape)
r_glove.object.poses.append(glove_pose)
l_glove = copy.deepcopy(r_glove)
l_glove.object.id = 'l_gripper_glove'
l_glove.object.header.frame_id = '/l_gripper_palm_link'
l_glove.link_name = 'l_gripper_palm_link'
l_glove.touch_links = ['l_end_effector',
'l_wrist_roll_link',
'l_wrist_flex_link',
'l_forearm_link']
return r_glove, l_glove
def add_block(self):
block = CollisionObject()
block.header.frame_id = self.wi.world_frame
block.operation.operation = block.operation.ADD
block.id = "block"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.1, 0.13]
pose = Pose()
pose.orientation.w = 1.0
block.shapes.append(shape)
block.poses.append(pose)
block_p = copy.deepcopy(block)
block_p.poses[0].position.x = 2.9
block_p.poses[0].position.y = -3.5
block_p.poses[
0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
self.wi.add_object(block_p)
return ObjectType(type="FixedObject",
collision_object=block,
parameters=['far_table'])
def add_little_object(self):
little_obj = CollisionObject()
little_obj.header.frame_id = self.wi.world_frame
little_obj.operation.operation = little_obj.operation.ADD
little_obj.id = "little_obj"
shape = Shape()
shape.type = shape.CYLINDER
shape.dimensions = [0.05, 0.02]
pose = Pose()
pose.orientation.w = 1.0
little_obj.shapes.append(shape)
little_obj.poses.append(pose)
little_obj_p = copy.deepcopy(little_obj)
little_obj_p.poses[0].position.x = 3.1
little_obj_p.poses[0].position.y = -3.2
little_obj_p.poses[
0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[1] / 2.0
self.wi.add_object(little_obj_p)
return ObjectType(type="RoundObject",
collision_object=little_obj,
parameters=['far_corner'])
def setUp(self):
rospy.init_node('test_get_base_state_validity')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
rospy.wait_for_service(default_prefix + '/get_robot_state')
self.get_robot_state_server = rospy.ServiceProxy(
default_prefix + '/get_robot_state', GetRobotState)
rospy.wait_for_service(default_prefix + '/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(
default_prefix + '/get_state_validity', GetStateValidity)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = 'odom_combined'
obj1.id = 'table'
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .1
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = 4.25
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(5.)
def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.0)
def add_plate(self):
plate = CollisionObject()
plate.header.frame_id = self.wi.world_frame
plate.operation.operation = plate.operation.ADD
plate.id = "plate"
shape = Shape()
shape.type = shape.CYLINDER
shape.dimensions = [0.15, 0.04]
if self.box_plate:
shape.type = shape.BOX
shape.dimensions = [0.3, 0.3, 0.04]
pose = Pose()
if self.world < 6 or self.world == 8 or self.world == 9 or self.world == 12:
pose.position.x = 3.1
pose.position.y = -3.2
pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[
1] / 2.0
elif self.world == 6:
pose.position.x = 0.6 #0.894692#0.5
pose.position.y = -0.3 #-0.468198#-0.2
pose.position.z = CENTER_TABLE_HEIGHT + shape.dimensions[1] / 2.0
# elif self.world == 12:
# pose.position.x = -1.3
# pose.position.y = 2
# pose.position.z = DOOR_TABLE_HEIGHT + 0.02 + shape.dimensions[1]/2.0
else:
pose.position.x = 0.5 #0.894692#0.5
pose.position.y = 0.1
pose.position.z = CENTER_TABLE_HEIGHT + 0.02 + shape.dimensions[
1] / 2.0
pose.orientation.w = 1.0
plate.shapes.append(shape)
plate.poses.append(pose)
self.wi.add_object(plate)
return plate
def get_virtual_table(height = 0.42):
table_msg = CollisionObject()
table_msg.id = "table"
table_msg.operation.operation = CollisionObjectOperation.ADD
table_msg.header.stamp = rospy.get_rostime()
table_msg.header.frame_id = "base_footprint"
side_box = Shape()
side_box.type = Shape.BOX
side_box.dimensions = [ 3.0, 1.0, height ]
front_box = Shape()
front_box.type = Shape.BOX
front_box.dimensions = [ 1.0, 3.0, height ]
pose = Pose()
pose.position.x = 0.0
pose.position.y = 0.0
pose.position.z = height / 2
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
l_side_pose = copy.deepcopy(pose)
l_side_pose.position.y = 0.85
r_side_pose = copy.deepcopy(pose)
r_side_pose.position.y = -0.85
front_pose = copy.deepcopy(pose)
front_pose.position.x = 0.85
table_msg.shapes = [ side_box, side_box, front_box ]
table_msg.poses = [ l_side_pose, r_side_pose, front_pose ]
return table_msg
def make_gripper_obs(self):
obs = Shape()
obs.type = 1
obs.dimensions = [0.10, 0.10, 0.10]
return obs
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
import roslib
roslib.load_manifest('usarsim_tools')
import rospy
from arm_navigation_msgs.msg import CollisionObjectOperation, CollisionObject, Shape
from geometry_msgs.msg import Pose
from visualization_msgs.msg import Marker
import copy
rospy.init_node("collision_add")
table1 = CollisionObject()
table2 = CollisionObject()
cShape = Shape()
cShape.type = Shape.BOX
cShape.dimensions = (3.968, 2.880, 1.088)
table1.poses.append(Pose())
table1.poses[0].position.x = 1.468
table1.poses[0].position.y = 1.108
table1.poses[0].position.z = .8
#cPose.position.x = -4.416
#cPose.position.y = 0.992
#cPose.position.z = .8
table1.poses[0].orientation.x = 0.0
table1.poses[0].orientation.y = 0.0
table1.poses[0].orientation.z = 0.0
table1.poses[0].orientation.w = 1.0
def pick(self,pickup_goal):
#prepare result
pickresult = PickupResult()
#get grasps for the object
# fill up a grasp planning request
grasp_planning_req = GraspPlanningRequest()
grasp_planning_req.arm_name = pickup_goal.arm_name
grasp_planning_req.target = pickup_goal.target
object_to_attach = pickup_goal.collision_object_name
# call grasp planning service
grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req)
#print grasp_planning_res
# process grasp planning result
if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS):
rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
else:
rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object")
# for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online)
#for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose
motion_plan_res=GetMotionPlanResponse()
grasp_to_execute_=Grasp()
for index, grasp in enumerate(grasp_planning_res.grasps):
# extract grasp_pose
grasp_pose_ = PoseStamped()
grasp_pose_.header.frame_id = "/world";
grasp_pose_.pose = grasp.grasp_pose
grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here
# copy the grasp_pose as a pre-grasp_pose
pre_grasp_pose_ = copy.deepcopy(grasp_pose_)
# add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
# currently add this to Z because approach vector needs to be computed somehow first (TODO)
pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05
# for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to pre-grasp pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp")
#print interpolated_motion_plan_res
# check and plan motion to this pre_grasp_pose
motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ )
#if this pre-grasp pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Grasp number "+str(index)+" is possible, executing it")
# copy the grasp to execute for the following steps
grasp_to_execute_ = copy.deepcopy(grasp)
break
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Grasp number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#put hand in pre-grasp posture
if self.pre_grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Pre-grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach pregrasp pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
#time.sleep(20) # TODO use actionlib here
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#grasp
if self.grasp_exec(grasp_to_execute_)<0:
#QMessageBox.warning(self, "Warning",
# "Grasp action failed: ")
pickresult.manipulation_result.value = ManipulationResult.FAILED
return pickresult
time.sleep(self.simdelay) # TODO use actionlib here
#attach the collision object to the hand (should be cleaned-up)
rospy.loginfo("Now we attach the object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"]
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attach object published")
else:
rospy.logerr("None of the grasps tested is possible")
pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return pickresult
pickresult.manipulation_result.value = ManipulationResult.SUCCESS
pickresult.grasp= grasp_to_execute_
return pickresult
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(4)]
obj1.poses = [Pose() for _ in range(4)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .2
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = -.25
obj1.poses[1].position.z = .92
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .2
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = .2
obj1.poses[2].position.x = .8
obj1.poses[2].position.y = -.5
obj1.poses[2].position.z = .78
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[3].type = Shape.BOX
obj1.shapes[3].dimensions = [float() for _ in range(3)]
obj1.shapes[3].dimensions[0] = .2
obj1.shapes[3].dimensions[1] = .1
obj1.shapes[3].dimensions[2] = .2
obj1.poses[3].position.x = .8
obj1.poses[3].position.y = .18
obj1.poses[3].position.z = .78
obj1.poses[3].orientation.x = 0
obj1.poses[3].orientation.y = 0
obj1.poses[3].orientation.z = 0
obj1.poses[3].orientation.w = 1
obj_pub.publish(obj1)
rospy.sleep(5.0)
def setUp(self):
rospy.init_node('test_attached_object_collisions')
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject,
latch=True)
rospy.wait_for_service(default_prefix + '/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(
default_prefix + '/get_state_validity', GetStateValidity)
self.state_req = GetStateValidityRequest()
self.state_req.robot_state.joint_state.name = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.state_req.robot_state.joint_state.position = [
float(0.0) for _ in range(7)
]
self.state_req.check_collisions = True
self.table = CollisionObject()
self.table.header.stamp = rospy.Time.now()
self.table.header.frame_id = "base_link"
self.table.id = "tabletop"
self.table.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
self.table.shapes = [Shape() for _ in range(1)]
self.table.shapes[0].type = Shape.BOX
self.table.shapes[0].dimensions = [float() for _ in range(3)]
self.table.shapes[0].dimensions[0] = 1.0
self.table.shapes[0].dimensions[1] = 1.0
self.table.shapes[0].dimensions[2] = .05
self.table.poses = [Pose() for _ in range(1)]
self.table.poses[0].position.x = 1.0
self.table.poses[0].position.y = 0
self.table.poses[0].position.z = .5
self.table.poses[0].orientation.x = 0
self.table.poses[0].orientation.y = 0
self.table.poses[0].orientation.z = 0
self.table.poses[0].orientation.w = 1
#not publishing table right away
self.att_object = AttachedCollisionObject()
self.att_object.object.header.stamp = rospy.Time.now()
self.att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
self.att_object.link_name = "r_gripper_r_finger_tip_link"
self.att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
self.att_object.object = CollisionObject()
self.att_object.object.header.stamp = rospy.Time.now()
self.att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
self.att_object.object.id = "pole"
self.att_object.object.shapes = [Shape() for _ in range(1)]
self.att_object.object.shapes[0].type = Shape.CYLINDER
self.att_object.object.shapes[0].dimensions = [
float() for _ in range(2)
]
self.att_object.object.shapes[0].dimensions[0] = .02
self.att_object.object.shapes[0].dimensions[1] = 1.2
self.att_object.object.poses = [Pose() for _ in range(1)]
self.att_object.object.poses[0].position.x = 0
self.att_object.object.poses[0].position.y = 0
self.att_object.object.poses[0].position.z = 0
self.att_object.object.poses[0].orientation.x = 0
self.att_object.object.poses[0].orientation.y = 0
self.att_object.object.poses[0].orientation.z = 0
self.att_object.object.poses[0].orientation.w = 1
self.att_pub.publish(self.att_object)
self.touch_links = [
'r_gripper_palm_link', 'r_gripper_r_finger_link',
'r_gripper_l_finger_link', 'r_gripper_r_finger_tip_link',
'r_gripper_l_finger_tip_link'
]
rospy.sleep(2.)
def place(self,place_goal):
placeresult = PlaceResult()
target_pose_to_execute_ = PoseStamped()
#for location, check path from approach pose to release pose first and then check motion to approach pose
motion_plan_res=GetMotionPlanResponse()
object_to_attach = place_goal.collision_object_name
# get current hand pose
self.listener.waitForTransform('/world', '/palm', rospy.Time(), rospy.Duration(1.0))
try:
(trans,rot) = self.listener.lookupTransform('/world', '/palm', rospy.Time())
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("Cannot get current palm pose")
placeresult.manipulation_result.value = ManipulationResult.ERROR
return placeresult
current_pose_= PoseStamped()
current_pose_.header.frame_id = "/world"
current_pose_.pose.position = Point(trans[0],trans[1],trans[2])
current_pose_.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3])
# for each place location
for index, target_pose_ in enumerate(place_goal.place_locations):
#compute straight trajectory to approach distance
target_approach_pose_= PoseStamped()
target_approach_pose_.header.frame_id = "/world"
target_approach_pose_.pose.position = Point(target_pose_.pose.position.x,target_pose_.pose.position.y,target_pose_.pose.position.z+place_goal.approach.desired_distance)
target_approach_pose_.pose.orientation = Quaternion(target_pose_.pose.orientation.x,target_pose_.pose.orientation.y,target_pose_.pose.orientation.z,target_pose_.pose.orientation.w) #keep same orientation for the first test
# for distance from 0 (release_pose) to desired_approach distance (approach_pose) test IK/Collision and save result
# decompose this in X steps depending on distance to do and max speed
interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(target_approach_pose_, target_pose_, False)
# check the result (depending on number of steps etc...)
if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS):
number_of_interpolated_steps=0
# check if one approach trajectory is feasible
for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
if traj_error_code.val!=1:
rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val))
break
else:
number_of_interpolated_steps=interpolation_index
# if trajectory is feasible then plan reach motion to approach pose
if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
rospy.loginfo("Place pose number "+str(index)+" approach is possible, checking motion plan to approach pose")
#print interpolated_motion_plan_res
# check and plan motion to this approach pose
motion_plan_res = self.plan.plan_arm_motion( place_goal.arm_name, "jointspace" ,target_approach_pose_)#,object_to_attach)
#if this approach pose is successful do not test others
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
rospy.loginfo("Place pose number "+str(index)+" is possible, executing it")
# copy the pose to execute for the following steps
target_pose_to_execute_ = copy.deepcopy(target_pose_)
break
else:
rospy.logerr("Place pose number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
else:
rospy.logerr("Place pose number "+str(index)+" approach is impossible")
#print interpolated_motion_plan_res
# execution part
if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS):
#go there
# filter the trajectory
filtered_traj = self.filter_traj_(motion_plan_res)
self.display_traj_( filtered_traj )
# reach approach pose
if self.send_traj_( filtered_traj )<0:
#QMessageBox.warning(self, "Warning",
# "Reach trajectory execution failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
# approach
if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0:
#QMessageBox.warning(self, "Warning",
# "Approach trajectory execution failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
#put hand in pre-grasp posture (to gently release)
if self.pre_grasp_exec(place_goal.grasp)<0:
#QMessageBox.warning(self, "Warning",
# "Release action failed: ")
placeresult.manipulation_result.value = ManipulationResult.FAILED
return placeresult
time.sleep(self.simdelay) # TODO use actionlib here
#detach the object from the hand
rospy.loginfo("Now we detach the attached object")
att_object = AttachedCollisionObject()
att_object.link_name = "palm"
att_object.object.id = object_to_attach
att_object.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
att_object.object.header.frame_id = "palm"
att_object.object.header.stamp = rospy.Time.now()
object = Shape()
object.type = Shape.CYLINDER
object.dimensions.append(.03)
object.dimensions.append(0.1)
pose = Pose()
pose.position.x = 0.0
pose.position.y = -0.06
pose.position.z = 0.06
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = 0
pose.orientation.w = 1
att_object.object.shapes.append(object)
att_object.object.poses.append(pose);
self.att_object_in_map_pub_.publish(att_object)
rospy.loginfo("Attached object to be detached published")
else:
rospy.logerr("None of the place pose tested is possible")
placeresult.manipulation_result.value = ManipulationResult.UNFEASIBLE
return placeresult
placeresult.manipulation_result.value = ManipulationResult.SUCCESS
placeresult.place_location= target_pose_to_execute_
return placeresult
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
coll)
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
posture_controller.wait_for_server()
rospy.loginfo('connected to gripper posture controller')
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
# define allowed contacts
table = segmentation_result.table
table_contact = AllowedContactSpecification()
table_contact.name = coll_map_res.collision_support_surface_name
table_contact.shape = Shape(type=Shape.BOX, dimensions=[abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), 1e-6])
table_contact.pose_stamped = table.pose
table_contact.link_names = ['L9_right_finger_link', 'L8_left_finger_link', 'L7_wrist_roll_link', 'L6_wrist_pitch_link']
table_contact.penetration_depth = 0.01
allowed_contacts = []#[table_contact,]
# define temporary link paddings
gripper_paddings = [LinkPadding(l,0.0) for l in ('L9_right_finger_link', 'L8_left_finger_link')]
if not move_arm_to_grasping_joint_pose(ik_solution.joint_state.name, ik_solution.joint_state.position, allowed_contacts, gripper_paddings):
exit(1)
rospy.sleep(1)
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
def test_add_convert_objects():
obj_pub = rospy.Publisher('collision_object',CollisionObject)
att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
rospy.init_node('test_collision_objects')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = .1
obj1.shapes[0].dimensions[2] = .75
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .375
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2";
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.CYLINDER
obj2.shapes[0].dimensions = [float() for _ in range(2)]
obj2.shapes[0].dimensions[0] = .025
obj2.shapes[0].dimensions[1] = .5
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
att_obj.object = obj2
r = rospy.Rate(.1)
while(True):
obj1.header.stamp = rospy.Time.now()
obj_pub.publish(obj1)
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def add_spatula(self, arm):
spatula = CollisionObject()
spatula.id = "spatula"
spatula.header.frame_id = self.wi.world_frame
spatula.operation.operation = spatula.operation.ADD
paddle = Shape()
handle = Shape()
paddle.type = paddle.BOX
paddle.dimensions = [0.11, 0.12, 0.005]
handle.type = handle.CYLINDER
handle.dimensions = [0.02, 0.24]
paddle_pose = Pose()
handle_pose = Pose()
paddle_pose.position.y = paddle.dimensions[1] / 2.0
paddle_pose.orientation.w = 1.0
angle = np.pi / 5.0
handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
np.pi / 2.0 - angle)
handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
np.pi / 2.0 - angle)
handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)
spatula.shapes = [paddle, handle]
spatula.poses = [paddle_pose, handle_pose]
#this is the grasp transformation
pos_on_handle = handle.dimensions[1] - 0.1
inv_grasp = Transform()
grasp = RigidGrasp()
#really should be calculating this...
inv_grasp.translation.y = GRIPPER_LENGTH
inv_grasp.translation.z = pos_on_handle / 2.0
#flip 90 degrees
inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
origin = Pose()
origin.orientation.w = 1.0
grasp.transform = pose_to_transform(
gt.inverse_transform_pose(origin, g))
grasp.touch_links = [arm[0] + '_end_effector']
grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
grasp.min_approach_distance = 0
grasp.desired_approach_distance = 0.15
grasp.min_distance_from_surface = -1
spat_p = copy.deepcopy(spatula)
wtrans = Pose()
wtrans.orientation.x = np.sin(angle / 2.0)
wtrans.orientation.w = np.cos(angle / 2.0)
if self.world == -1:
wtrans.position.x = 3
wtrans.position.y = -2.8
wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['far_corner']
elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
wtrans.position.x = -1.7
wtrans.position.y = 2
wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['door_table']
else:
wtrans.position.x = 0.6
wtrans.position.y = -0.3
wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
0]
ss = ['center_table']
if self.world == -4 or self.world == -5:
wtrans.position.y = 0
rot = Quaternion()
rot.z = np.sin(np.pi / 2.0)
rot.w = np.cos(np.pi / 2.0)
wtrans.orientation = gt.multiply_quaternions(
rot, wtrans.orientation)
if self.world == -5:
wtrans.position.x = 0
for i in range(len(spat_p.poses)):
spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)
self.wi.add_object(spat_p)
return ObjectType(type="SpatulaObject",
collision_object=spatula,
parameters=ss,
numeric_parameters=paddle.dimensions +
handle.dimensions + [angle]), [grasp]
def add_walls(self):
window_wall = CollisionObject()
window_wall.header.frame_id = self.wi.world_frame
window_wall.operation.operation = window_wall.operation.ADD
window_wall.id = "window_wall"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [8, 0.1, 2.5]
pose = Pose()
pose.position.x = 0
pose.position.y = -2.63
pose.position.z = shape.dimensions[2] / 2.0 - 0.1
angle = np.pi / 6.25
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.shapes.append(shape)
window_wall.poses.append(pose)
self.wi.add_object(window_wall)
shape.dimensions[0] = shape.dimensions[1]
shape.dimensions[1] = 5
pose.position.x = -2.45
pose.position.y = 1
angle = 0.05
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "door_wall"
self.wi.add_object(window_wall)
shape.dimensions[1] = shape.dimensions[0]
shape.dimensions[0] = 8
pose.position.x = 0
pose.position.y = 2.3
angle = 0.1
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "cabinet_wall"
self.wi.add_object(window_wall)
shape.dimensions[0] = shape.dimensions[1]
shape.dimensions[1] = 3
pose.position.x = 4
pose.position.y = -3
angle = 0.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "banner_wall"
self.wi.add_object(window_wall)
shape.dimensions[1] = 4
shape.dimensions[2] = 1
pose.position.x = 3.25
pose.position.y = 0.65
pose.position.z = shape.dimensions[2] / 2.0 - 0.05
angle = 0
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "computer_wall"
self.wi.add_object(window_wall)
shape.dimensions[1] = shape.dimensions[0]
shape.dimensions[0] = 1.8
pose.position.x = 3.9
pose.position.y = -1.6
angle = np.pi / 5.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "nook_wall"
self.wi.add_object(window_wall)
k = table.triangles[tri_i+2]
v_i = vertices[i]
v_j = vertices[j]
v_k = vertices[k]
if point_in_triangle(p, v_i, v_j, v_k):
return True
return False
if __name__ == '__main__':
import roslib; roslib.load_manifest('simple_utils')
from matplotlib import pyplot as plt
from geometry_msgs.msg import Point
from arm_navigation_msgs.msg import Shape
table = Shape()
table.vertices = [
Point(0.0, 0.0, 0.0),
Point(0.5, 1.0, 0.0),
Point(0.7, 1.2, 0.0),
Point(1.5, 0.2, 0.0)
]
table.triangles = [0, 1, 2, 0, 2, 3]
on_table = []
not_on_table = []
for ii in range(300):
p = np.random.random((2,))
point = Point(p[0], p[1], 0.0)
if point_on_table(point, table):
def add_tables(self):
if self.screenshot:
return ['', '', '']
table = CollisionObject()
table.header.frame_id = self.wi.world_frame
table.operation.operation = table.operation.ADD
shape = Shape()
shape.type = shape.MESH
#center table
shape.vertices = [
Point(x=-0.2, y=-0.4, z=CENTER_TABLE_HEIGHT),
Point(x=0.97, y=-0.6, z=CENTER_TABLE_HEIGHT),
Point(x=1.0, y=0.25, z=CENTER_TABLE_HEIGHT),
Point(x=-0.25, y=0.3, z=CENTER_TABLE_HEIGHT)
]
shape.triangles = [0, 1, 2, 2, 3, 0]
pose = Pose()
pose.orientation.w = 1.0
poseb = copy.deepcopy(pose)
poseb.position.z = -0.02
poset = copy.deepcopy(pose)
poset.position.z = 0.02
table.shapes.append(shape)
table.shapes.append(shape)
table.shapes.append(shape)
table.poses.append(poset)
table.poses.append(pose)
table.poses.append(poseb)
table.id = 'center_table'
self.wi.add_object(table)
#table near the door
table.id = 'door_table'
shape.vertices = [
Point(x=-2.4, y=1.84, z=DOOR_TABLE_HEIGHT),
Point(x=-1.15, y=1.75, z=DOOR_TABLE_HEIGHT),
Point(x=-1.15, y=2.5, z=DOOR_TABLE_HEIGHT),
Point(x=-2.4, y=2.5, z=DOOR_TABLE_HEIGHT)
]
self.wi.add_object(table)
#table in far corner
table.id = 'far_corner'
shape.vertices = [
Point(x=3, y=-2.7, z=FAR_TABLE_HEIGHT),
Point(x=2.4, y=-3.8, z=FAR_TABLE_HEIGHT),
Point(x=3.2, y=-4.3, z=FAR_TABLE_HEIGHT),
Point(x=3.8, y=-3.2, z=FAR_TABLE_HEIGHT)
]
self.wi.add_object(table)
if self.fake_walls:
#these are the table feet
foot = CollisionObject()
foot.header.frame_id = self.wi.world_frame
foot.operation.operation = foot.operation.ADD
foot.id = "far_corner_foot"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.5, FAR_TABLE_HEIGHT / 2.0]
pose = Pose()
pose.position.x = 3
pose.position.y = -3.4
pose.position.z = shape.dimensions[2] / 2.0
angle = 0.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
foot.shapes.append(shape)
foot.poses.append(pose)
self.wi.add_object(foot)
foot.id = "center_table_foot1"
shape.dimensions = [0.1, 0.75, 0.3]
pose.position.x = 0.9
pose.position.y = -0.1
pose.position.z = shape.dimensions[2] / 2.0
angle = 0
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
self.wi.add_object(foot)
foot.id = "center_table_foot2"
pose.position.x = -0.2
self.wi.add_object(foot)
foot.id = "door_table_foot"
pose.position.x = -1.25
pose.position.y = 2.1
self.wi.add_object(foot)
return ['center_table', 'door_table', 'far_corner']
