def link_to_collision_object(link, full_linkname): supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) root_collision_model = get_root_collision_model(link) link_pose_in_root_frame = pysdf.homogeneous2pose_msg( concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world))) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple( float(val) for val in linkpart.geometry_data['scale'].split()) mesh_path = linkpart.geometry_data['uri'].replace( 'model://', pysdf.models_path) link_pose_stamped = PoseStamped() link_pose_stamped.pose = link_pose_in_root_frame make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale) elif linkpart.geometry_type == 'box': scale = tuple( float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius']) collision_object.primitives.append(sphere) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple( (2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append(link_pose_in_root_frame) #print('CollisionObject for %s:\n%s' % (full_linkname, collision_object)) return collision_object
def link_to_collision_object(link, full_linkname): supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) root_collision_model = get_root_collision_model(link) link_pose_in_root_frame = pysdf.homogeneous2pose_msg(concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world))) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple(float(val) for val in linkpart.geometry_data['scale'].split()) for models_path in pysdf.models_paths: test_mesh_path = linkpart.geometry_data['uri'].replace('model://', models_path) if os.path.isfile(test_mesh_path): mesh_path = test_mesh_path break if mesh_path: link_pose_stamped = PoseStamped() link_pose_stamped.pose = link_pose_in_root_frame make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale) else: print("ERROR: No mesh found for '%s' in element '%s'." % (linkpart.geometry_data['uri'], full_linkname)) elif linkpart.geometry_type == 'box': scale = tuple(float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius']) collision_object.primitives.append(sphere) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple((2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append(link_pose_in_root_frame) #print('CollisionObject for %s:\n%s' % (full_linkname, collision_object)) return collision_object
def update_scene(self, points): """ for debugging purposes, sends object, table and octomap clearing in same message""" ps = PlanningSceneWorld() # clear the octomap ps.octomap.header.frame_id = 'odom_combined' ps.octomap.octomap.id = 'OcTree' # add object bounding box (box_pose, box_dims) = self.get_bounding_box(points) bbox_primitive = SolidPrimitive() bbox_primitive.type = bbox_primitive.BOX bbox_primitive.dimensions = [box_dims.x, box_dims.y, box_dims.z] bbox = CollisionObject() bbox.id = 'obj1' bbox.header.frame_id = box_pose.header.frame_id bbox.operation = bbox.ADD bbox.primitives.append(bbox_primitive) bbox.primitive_poses.append(box_pose.pose) ps.collision_objects.append(bbox) # and, table table_pose = PoseStamped() table_pose.header.frame_id = "odom_combined" table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = 0.75 table_pose.pose.orientation.w = 1.0 table_dims = Point() table_dims.x = 0.7 table_dims.y = 1.0 table_dims.z = 0.05 table_primitive = SolidPrimitive() table_primitive.type = table_primitive.BOX # TODO: this is a hack to make the buffer bigger. # By default, it appears to be 10cm x 10cm x 0cm, but I'm not sure which dimension is which table_primitive.dimensions = [table_dims.x, table_dims.y, table_dims.z] table = CollisionObject() table.id = 'table' table.header.frame_id = table_pose.header.frame_id table.operation = table.ADD table.primitives.append(table_primitive) table.primitive_poses.append(table_pose.pose) ps.collision_objects.append(table) self.scene_diff_pub.publish(ps)
def __add_free_space(name, x, y, operation=CollisionObject.ADD): sp = SolidPrimitive() sp.type = SolidPrimitive.BOX sp.dimensions = [0.5, 0.5, 0.1] spp = PoseStamped() spp.header.frame_id = 'map' spp.pose.position.x = x spp.pose.position.y = y spp.pose.position.z = 0.05 spp.pose.orientation.w = 1 co = CollisionObject() co.operation = operation co.id = name co.type.db = json.dumps({ 'table': 'NONE', 'type': 'free_space', 'name': co.id }) co.header.frame_id = 'map' co.header.stamp = rospy.get_rostime() co.primitives.append(sp) co.primitive_poses.append(TF2().transform_pose(spp, spp.header.frame_id, co.header.frame_id).pose) psw = PlanningSceneWorld() psw.collision_objects.append(co) obstacle_pub.publish(psw) rospy.loginfo("Added a fake obstacle named '%s'", name)
def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name, touch_links=None, detach_posture=None, weight=0.0, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX p = Pose() p.position.x = x p.position.y = y p.position.z = z p.orientation.w = 1.0 o = self.makeSolidPrimitive(name, s, p) o.header.frame_id = link_name a = self.makeAttached(link_name, o, touch_links, detach_posture, weight) self._attached_objects[name] = a self._attached_pub.publish(a) if wait: self.waitForSync()
def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name, touch_links=None, detach_posture=None, weight=0.0, use_service=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX p = Pose() p.position.x = x p.position.y = y p.position.z = z p.orientation.w = 1.0 o = self.makeSolidPrimitive(name, s, p) o.header.frame_id = link_name a = self.makeAttached(link_name, o, touch_links, detach_posture, weight) self._attached_objects[name] = a self.sendUpdate(None, a, use_service)
def cucumber(cucumber, pose): ''' Create a cylinder that represents the gripped cucumber. @return An AttachedCollisionObject representing the roof ''' aco = AttachedCollisionObject() aco.link_name = "ee_link" aco.object.header.frame_id = "world" aco.object.id = "cucumber" acoPose = pose acoPose.position.y += cucumber.width / 2 + 0.15 acoPose.position.z -= cucumber.height / 2 + 0.071 acoCyl = SolidPrimitive() acoCyl.type = acoCyl.CYLINDER acoCyl.dimensions = [cucumber.height, cucumber.width / 2] aco.object.primitives.append(acoCyl) aco.object.primitive_poses.append(acoPose) aco.object.operation = aco.object.ADD return aco
def add_board_object(): # Some publisher scene_diff_publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=1) rospy.sleep(10.0) # Create board object board = CollisionObject() board.header.frame_id = 'base' board.id = 'board' board_box = SolidPrimitive() board_box.type = 1 # board_box.dimensions = [3.0, 4.0, 0.185] board_box.dimensions = [DEPTH_BIAS*2, 4.0, 3.0] board.primitives.append(board_box) board_pose = Pose() board_pose.position.x = transformed_message.pose.position.x board_pose.position.y = transformed_message.pose.position.y board_pose.position.z = transformed_message.pose.position.z # board_pose.orientation.x = transformed_message.pose.orientation.x board_pose.orientation.x = 0 # board_pose.orientation.y = transformed_message.pose.orientation.y board_pose.orientation.y = 0 # board_pose.orientation.z = transformed_message.pose.orientation.z board_pose.orientation.z = 0 # board_pose.orientation.w = transformed_message.pose.orientation.w board_pose.orientation.w = 0 board.primitive_poses.append(board_pose) scene = PlanningScene() scene.world.collision_objects.append(board) scene.is_diff = True scene_diff_publisher.publish(scene)
def virtualObjectLeftHand(): print "Creating a virtual object attached to gripper..." # for more details refer to ROS docs for moveit_msgs/AttachedCollisionObject object1 = AttachedCollisionObject() object1.link_name = "left_HandGripLink" object1.object.header.frame_id = "left_HandGripLink" object1.object.id = "object1" object1_prim = SolidPrimitive() object1_prim.type = SolidPrimitive.CYLINDER object1_prim.dimensions=[None, None] # set initial size of the list to 2 object1_prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = 0.23 object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.06 object1_pose = pm.toMsg(PyKDL.Frame(PyKDL.Rotation.RotY(math.pi/2))) object1.object.primitives.append(object1_prim) object1.object.primitive_poses.append(object1_pose) object1.object.operation = CollisionObject.ADD object1.touch_links = ['left_HandPalmLink', 'left_HandFingerOneKnuckleOneLink', 'left_HandFingerOneKnuckleTwoLink', 'left_HandFingerOneKnuckleThreeLink', 'left_HandFingerTwoKnuckleOneLink', 'left_HandFingerTwoKnuckleTwoLink', 'left_HandFingerTwoKnuckleThreeLink', 'left_HandFingerThreeKnuckleTwoLink', 'left_HandFingerThreeKnuckleThreeLink'] return object1
def pub_obj(self, obj_type, pos, dim): m = Marker() m.id = self.obj_types.index(obj_type) m.ns = "simple_tabletop" m.type = Marker.CUBE m.pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1)) m.header = Header(0, rospy.Time(0), "base_link") m.scale = Vector3(*dim) m.color = self.colors[obj_type] mtext = deepcopy(m) mtext.type = Marker.TEXT_VIEW_FACING mtext.id += 100 mtext.text = obj_type M = MarkerArray([m, mtext]) self.pub.publish(M) #moveit collision objects co = CollisionObject() co.header = deepcopy(m.header) co.id = obj_type obj_shape = SolidPrimitive() obj_shape.type = SolidPrimitive.BOX obj_shape.dimensions = list(dim) co.primitives.append(obj_shape) obj_pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1)) co.primitive_poses = [obj_pose] self.pub_co.publish(co)
def _add_box(self, name, px, py, pz, sl, sw, sh): """ Performs the simple adding of a single box with given dimensions. """ obj = CollisionObject() obj.id = name obj.operation = obj.ADD obj.header.frame_id = "odom_combined" obstacle = SolidPrimitive() obstacle.type = obstacle.BOX obstacle.dimensions = (sl, sw, sh) pos = Pose() pos.position.x = px pos.position.y = py pos.position.z = pz pos.orientation.x = 0 pos.orientation.y = 0 pos.orientation.z = 0 pos.orientation.w = 1 obj.primitives.append(obstacle) obj.primitive_poses.append(pos) self.diff.world.collision_objects.append(obj)
def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name, touch_links=None, detach_posture=None, weight=0.0, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX p = Pose() p.position.x = x p.position.y = y p.position.z = z p.orientation.w = 1.0 r = self.makeSolidPrimitive(name, s, p) # first remove from environment r.operation = r.REMOVE o = self.makeSolidPrimitive(name, s, p) o.header.frame_id = link_name a = self.makeAttached(link_name, o, touch_links, detach_posture, weight) self._attached_objects[name] = a self.sentUpdate(r, a, wait)
def __addBoxWithOrientation(self, name, size_x, size_y, size_z, x, y, z, roll, pitch, yaw, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX ps = PoseStamped() ps.header.frame_id = self._scene._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) ps.pose.orientation.x = q[0] ps.pose.orientation.y = q[1] ps.pose.orientation.z = q[2] ps.pose.orientation.w = q[3] self._scene.addSolidPrimitive(name, s, ps.pose, wait)
def add_box_obstacle(self, size, name, pose): """ Adds a rectangular prism obstacle to the planning scene Inputs: size: 3x' ndarray; (x, y, z) size of the box (in the box's body frame) name: unique name of the obstacle (used for adding and removing) pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame """ # Create a CollisionObject, which will be added to the planning scene co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header # Create a box primitive, which will be inside the CollisionObject box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = size # Fill the collision object with primitive(s) co.primitives = [box] co.primitive_poses = [pose.pose] # Publish the object self._planning_scene_publisher.publish(co)
def create_collision_object(shape_type,pos,size,frame_id,op,object_id): col = CollisionObject() col.id = object_id col.operation = op col.header.frame_id=frame_id # create primitive primitive = SolidPrimitive() primitive.type = shape_type primitive.dimensions = size # create pose pose = Pose() pose.position.x = pos[0] pose.position.y = pos[1] pose.position.z = pos[2] pose.orientation.x = pose.orientation.y = pose.orientation.z = 0 pose.orientation.w = 1 col.primitives = [primitive] col.primitive_poses = [pose] return col
def make_box(self, name, header, pose, size, scale, actualize = '1'): box_names = ['9', '10', '11', '12', '0', '1', '2', '3'] triangle_names = ['13', '14', '15', '6', '7', '8'] exagon_names = ['4', '5'] if (actualize == '1'): self.co.operation = CollisionObject.APPEND elif (actualize == '2'): self.co.operation = CollisionObject.ADD elif (actualize == '3'): self.co.operation = CollisionObject.REMOVE self.co.id = name print('co.id: ', self.co.id) self.co.header = header box = SolidPrimitive() if name in box_names: box.type = SolidPrimitive.BOX elif name in triangle_names: box.type = SolidPrimitive.CONE elif name in exagon_names: box.type = SolidPrimitive.CYLINDER size = [(x*scale) for x in size] box.dimensions = list(size) self.co.primitives = [box] self.co.primitive_poses = [pose.pose] return self.co
def form_attach_collision_object_msg(self, link_name, object_name, size, pose): obj = AttachedCollisionObject() # The CollisionObject will be attached with a fixed joint to this link obj.link_name = link_name # This contains the actual shapes and poses for the CollisionObject # to be attached to the link col_obj = CollisionObject() col_obj.id = object_name col_obj.header.frame_id = link_name col_obj.header.stamp = rospy.Time.now() sp = SolidPrimitive() sp.type = sp.BOX sp.dimensions = [0.0] * 3 sp.dimensions[0] = size.x sp.dimensions[1] = size.y sp.dimensions[2] = size.z col_obj.primitives = [sp] col_obj.primitive_poses = [pose] # Adds the object to the planning scene. If the object previously existed, it is replaced. col_obj.operation = col_obj.ADD obj.object = col_obj # The weight of the attached object, if known obj.weight = 0.0 return obj
def _make_box(self, name, size, pos, quat=None, frame_id=None): sp = SolidPrimitive() sp.type = SolidPrimitive.BOX sp.dimensions = size return self._add_command(name, self._make_pose(pos, quat), solid=sp, frame_id=frame_id)
def addCylinder(self, name, height, radius, ps, use_service=True): ''' Insert new cylinder into planning scene ''' s = SolidPrimitive() s.dimensions = [height, radius] s.type = s.CYLINDER self.addSolidPrimitive(name, s, ps, use_service)
def publish_shape(self): # send vehicle shape (only publish once) shape = SolidPrimitive() shape.type = SolidPrimitive.BOX shape.dimensions = [self.carla_actor.bounding_box.extent.x * 2, self.carla_actor.bounding_box.extent.y * 2, self.carla_actor.bounding_box.extent.z * 2] self.publish_message(self.get_topic_prefix() + "/shape", shape, True)
def _getCollisionObject(name, urdf, pose, operation): ''' This function takes an urdf and a TF pose and updates the collision geometry associated with the current planning scene. ''' co = CollisionObject() co.id = name co.operation = operation if len(urdf.links) > 1: rospy.logwarn( 'collison object parser does not currently support kinematic chains' ) for l in urdf.links: # Compute the link pose based on the origin if l.origin is None: link_pose = pose else: link_pose = pose * kdl.Frame(kdl.Rotation.RPY(*l.origin.rpy), kdl.Vector(*l.origin.xyz)) for c in l.collisions: # Only update the geometry if we actually need to add the # object to the collision scene. # check type of each collision tag. if isinstance(c.geometry, urdf_parser_py.urdf.Box): primitive = True if co.operation == CollisionObject.ADD: size = c.geometry.size element = SolidPrimitive() element.type = SolidPrimitive.BOX element.dimensions = list(c.geometry.size) co.primitives.append(element) elif isinstance(c.geometry, urdf_parser_py.urdf.Mesh): primitive = False if co.operation == CollisionObject.ADD: scale = (1, 1, 1) if c.geometry.scale is not None: scale = c.scale element = _loadMesh(c.geometry, scale) co.meshes.append(element) else: raise NotImplementedError( "we do not currently support geometry of type %s" % (str(type(c.geometry)))) pose = kdl.Frame(kdl.Rotation.RPY(*c.origin.rpy), kdl.Vector(*c.origin.xyz)) pose = link_pose * pose if primitive: co.primitive_poses.append(pm.toMsg(pose)) else: # was a mesh co.mesh_poses.append(pm.toMsg(pose)) return co
def __init__(self, speed): self.speed = speed # Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Initialize the robot self.robot = moveit_commander.RobotCommander() # Initialize the planning scene self.scene = moveit_commander.PlanningSceneInterface() self.scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) # Instantiate a move group self.group = moveit_commander.MoveGroupCommander(GROUP_NAME) # Set the maximum time MoveIt will try to plan before giving up self.group.set_planning_time(.25) # Set maximum velocity scaling self.group.set_max_velocity_scaling_factor(1.0) self.group.set_max_acceleration_scaling_factor(1.0) # For displaying plans to RVIZ self.display_pub = rospy.Publisher('/move_group/display_planned_path', DisplayTrajectory, queue_size=10) print(self.group.get_current_pose()) print(self.group.get_end_effector_link()) print(self.group.get_pose_reference_frame()) print(self.group.get_goal_tolerance()) # Create a CollisionObject, which will be added to the planning scene co = CollisionObject() co.operation = CollisionObject.ADD co.id = 'table' co.header = TABLE_CENTER.header # Create a box primitive, which will be inside the CollisionObject box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = TABLE_SIZE # Fill the collision object with primitive(s) co.primitives = [box] co.primitive_poses = [TABLE_CENTER.pose] # Publish the object (don't know why but need to publish twice) self.scene_publisher.publish(co) rospy.sleep(0.5) self.scene_publisher.publish(co) rospy.sleep(0.5)
def create_sphere(name, pose, radius, frame_id='/world_frame'): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header.frame_id = frame_id sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [radius] co.primitives = [sphere] co.primitive_poses = [pose] return co
def add_box(co, pose, size = (0, 0, 1), offset=(0,0,0)): box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives.append(box) p = deepcopy(pose) p.position.x += offset[0] p.position.y += offset[1] p.position.z += offset[2] co.primitive_poses.append(p) return co
def _create_collision_object_from_primitive(self, model_instance_name, model_pose, model_type, size): collision_object = self._create_collision_object_base( model_instance_name) primitive = SolidPrimitive() if ModelStates.BOX == model_type: primitive.type = SolidPrimitive.BOX primitive.dimensions = [i * 2 for i in size] elif ModelStates.CYLINDER == model_type: primitive.type = SolidPrimitive.CYLINDER primitive.dimensions = [size[1] * 2, size[0]] elif ModelStates.SPHERE == model_type: primitive.type = SolidPrimitive.SPHERE primitive.dimensions = [size[0]] else: raise TypeError( "Primitive type {} not supported".format(model_type)) collision_object.primitives.append(primitive) collision_object.primitive_poses.append(model_pose) return collision_object
def create_box(name, pose, size, frame_id='/world_frame'): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header.frame_id = frame_id box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose] return co
def __make_box(self, name, pose, size): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose.pose] return co
def __make_sphere(self, name, pose, radius): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [radius] co.primitives = [sphere] co.primitive_poses = [pose.pose] return co
def __make_cylinder(name, pose, height, radius): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = [height, radius] co.primitives = [cylinder] co.primitive_poses = [pose.pose] return co
def handle_sphere(node): if node.tag != 'sphere': raise ValueError v = node.attrib c = node.getchildren() p = SolidPrimitive(type=SolidPrimitive.SPHERE) p.dimensions = [0] p.dimensions[SolidPrimitive.SPHERE_RADIUS] = float(v['sphere_radius']) return p,XMLSceneParser.handle_pose(c[0])
def __make_cylinder(self, name, pose, size): co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cyl = SolidPrimitive() cyl.type = SolidPrimitive.CYLINDER cyl.dimensions = list(size) co.primitives = [cyl] co.primitive_poses = [pose.pose] return co
def find_block(self): """ Find block and add it to the scene """ # Get necessary values while not self.object_location: print "Waiting for object location..." rospy.sleep(1.0) object_location = self.object_location image_size = self.left_camera_size while not self.current_pose: print "Waiting for current pose values..." rospy.sleep(1.0) current_pose = self.current_pose.position endeffector_height = current_pose.z - self.table_height # Store values for calculation Pp = [object_location.x, object_location.y, object_location.z] Cp = [image_size[0] / 2, image_size[1] / 2, 0] Bp = [current_pose.x, current_pose.y, current_pose.z] Go = [-0.0230, 0.0110, 0.1100] cc = [0.0021, 0.0021, 0.0000] d = [endeffector_height, endeffector_height, 0.0000] # Calculate block's position in workspace # workspace = (Pp - Cp) * cc * d + Bp + Go pixel_difference = map(operator.sub, Pp, Cp) camera_constant = map(operator.mul, cc, d) pixel_2_real = map(operator.mul, pixel_difference, camera_constant) pixel_2_real[2] = endeffector_height * -1 workspace_without_gripper = map(operator.add, pixel_2_real, Bp) workspace = map(operator.add, workspace_without_gripper, Go) # Add block to scene p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = workspace_without_gripper[0] p.pose.position.y = workspace_without_gripper[1] p.pose.position.z = workspace_without_gripper[2] + 0.06 co = CollisionObject() co.operation = CollisionObject.ADD co.id = "block" co.header = p.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list((0.1, 0.1, 0.1)) co.primitives = [box] co.primitive_poses = [p.pose] pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True) pub_co.publish(co) return
def handle_cone(node): if node.tag != 'cone': raise ValueError v = node.attrib c = node.getchildren() p = SolidPrimitive(type=SolidPrimitive.CONE) p.dimensions = [0,0] p.dimensions[SolidPrimitive.CONE_RADIUS] = float(v['cone_radius']) p.dimensions[SolidPrimitive.CONE_HEIGHT] = float(v['cone_height']) return p,XMLSceneParser.handle_pose(c[0])
def addBox(self, name, size_x, size_y, size_z, x, y, z, use_service=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX ps = PoseStamped() ps.header.frame_id = self._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.pose.orientation.w = 1.0 self.addSolidPrimitive(name, s, ps.pose, use_service)
def addCylinder(self, name, height, radius, x, y, z, wait=True): s = SolidPrimitive() s.dimensions = [height, radius] s.type = s.CYLINDER ps = PoseStamped() ps.header.frame_id = self._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.pose.orientation.w = 1.0 self.addSolidPrimitive(name, s, ps.pose, wait)
def addCylinder(self, name, height, radius, x, y, z, use_service=True): s = SolidPrimitive() s.dimensions = [height, radius] s.type = s.CYLINDER ps = PoseStamped() ps.header.frame_id = self._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.pose.orientation.w = 1.0 self.addSolidPrimitive(name, s, ps.pose, use_service)
def addSphere(self, name, radius, x, y, z, use_service=True): s = SolidPrimitive() s.dimensions = [radius] s.type = s.SPHERE ps = PoseStamped() ps.header.frame_id = self._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.pose.orientation.w = 1.0 self.addSolidPrimitive(name, s, ps.pose, use_service)
def handle_box(node): if node.tag != 'box': raise ValueError v = node.attrib c = node.getchildren() p = SolidPrimitive(type=SolidPrimitive.BOX) p.dimensions = [0,0,0] p.dimensions[SolidPrimitive.BOX_X] = float(v['box_x']) p.dimensions[SolidPrimitive.BOX_Y] = float(v['box_y']) p.dimensions[SolidPrimitive.BOX_Z] = float(v['box_z']) return p,XMLSceneParser.handle_pose(c[0])
def addBox(self, name, size_x, size_y, size_z, x, y, z, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX ps = PoseStamped() ps.header.frame_id = self._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.pose.orientation.w = 1.0 self.addSolidPrimitive(name, s, ps.pose, wait)
def insert_object(self, pose, dims, name): """ Expects a PoseStamped to be input, along with a Point and string """ primitive = SolidPrimitive() primitive.type = primitive.BOX # TODO: this is a hack to make the buffer bigger. # By default, it appears to be 10cm x 10cm x 0cm, but I'm not sure which dimension is which primitive.dimensions = [dims.x, dims.y, dims.z] add_object = CollisionObject() add_object.id = name add_object.header.frame_id = pose.header.frame_id add_object.operation = add_object.ADD add_object.primitives.append(primitive) add_object.primitive_poses.append(pose.pose) self.collision_pub.publish(add_object)
def attach_object(self, group_name): rospy.loginfo('attaching object to ' + group_name) # select toolframe depending on group name tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT # pose for attached object in tool frame coordiantes pose = Pose() pose.position.z = 0.05 pose.orientation.w = 1.0 primitive = SolidPrimitive() primitive.type = primitive.BOX primitive.dimensions = [0.07, 0.07, 0.07] o = CollisionObject() o.header.frame_id = tool_frame o.id = "handed_object" o.primitives = [primitive] o.primitive_poses = [pose] o.operation = o.ADD a = AttachedCollisionObject() a.object = o # allow collisions with hand links a.touch_links = ALLOWED_TOUCH_LINKS # attach object to tool frame a.link_name = tool_frame # don't delete old planning scene, if we didn't get one so far, create a new one scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene() # add attached object to scene scene.robot_state.attached_collision_objects.append(a) # mark scene as changed scene.is_diff = True self.pub_ps.publish(scene)
def set_target_object(self, primitive_type, target_pose, dimensions): primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4} self.sought_object = Object() primitive = SolidPrimitive() primitive.type = primitive_types[primitive_type.upper()] primitive.dimensions = dimensions self.sought_object.primitives.append(primitive) p = Pose() p.position.x = target_pose[0] p.position.y = target_pose[1] p.position.z = target_pose[2] quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5]) p.orientation.x = quaternion[0] p.orientation.y = quaternion[1] p.orientation.z = quaternion[2] p.orientation.w = quaternion[3] self.sought_object.primitive_poses.append(p)
def __init__(self): """ Initialize class """ # Overall MoveIt initialization moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() # MoveIt scene initialization self.scene = moveit_commander.PlanningSceneInterface() self.table_height = -0.320 p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0.8 p.pose.position.y = 0.025 p.pose.position.z = -0.6 co = CollisionObject() co.operation = CollisionObject.ADD co.id = "table" co.header = p.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list((0.75, 1.25, 0.55)) co.primitives = [box] co.primitive_poses = [p.pose] pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True) pub_co.publish(co) # Moveit group initialization self.group = moveit_commander.MoveGroupCommander("left_arm") self.group.set_goal_position_tolerance(0.12) self.group.set_goal_orientation_tolerance(0.10) # Gripper initialization self.left_gripper = baxter_interface.Gripper("left", CHECK_VERSION) self.left_gripper.reboot() self.left_gripper.calibrate() # Camera initialization self.left_camera = baxter_interface.CameraController("left_hand_camera") self.right_camera = baxter_interface.CameraController("right_hand_camera") self.head_camera = baxter_interface.CameraController("head_camera") self.right_camera.close() self.head_camera.close() self.left_camera.open() self.left_camera.resolution = [1280, 800] self.left_camera_size = [800, 1280] _camera_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self._on_camera) # Open CV initialization cv2.namedWindow("Original", cv2.WINDOW_NORMAL) cv2.moveWindow("Original", 1990, 30) cv2.namedWindow("Thresholded", cv2.WINDOW_NORMAL) cv2.moveWindow("Thresholded", 3270, 30) self.low_h = 165 self.high_h = 179 self.low_s = 70 self.high_s = 255 self.low_v = 60 self.high_v = 255 self.bridge = CvBridge() # Object location initialization self.object_location = None # Subscribe to pose at all times self.current_pose = None _pose_sub = rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, self.set_current_pose) return
def moveToPose(self, pose_stamped, gripper_frame, tolerance=0.01, wait=True, **kwargs): # Check arguments supported_args = ("max_velocity_scaling_factor", "planner_id", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToJointPosition: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped) # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance * tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose_transformed.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in request group name g.request.group_name = self._group # 8. fill in request number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 10. fill in planning options diff g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() return self._action.get_result() else: return None