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_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 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']