コード例 #1
0
 def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
     aco = AttachedCollisionObject()
     if pose!=None and not filename.empty():
         aco.object = self.__make_mesh(name, pose, filename, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self._pub_aco.publish(aco)
コード例 #2
0
 def attach_mesh_custom(self, link, name, mesh, pose=None, touch_links=[]):
     aco = AttachedCollisionObject()
     if pose != None and mesh:
         aco.object = self.__make_mesh_custom(name, pose, mesh, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self._pub_aco.publish(aco)
コード例 #3
0
 def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
     aco = AttachedCollisionObject()
     if (pose is not None) and filename:
         aco.object = self.__make_mesh(name, pose, filename, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self.__submit(aco, attach=True)
コード例 #4
0
 def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
     aco = AttachedCollisionObject()
     if pose!=None and not filename.empty():
         aco.object = self.__make_mesh(name, pose, filename, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self._pub_aco.publish(aco)
コード例 #5
0
 def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
     aco = AttachedCollisionObject()
     if (pose is not None) and filename:
         aco.object = self.__make_mesh(name, pose, filename, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self.__submit(aco, attach=True)
コード例 #6
0
 def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = []):
     aco = AttachedCollisionObject()
     if pose!=None:
         aco.object = self.__make_box(name, pose, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     else:
         aco.touch_links = [link]
     self._pub_aco.publish(aco)
コード例 #7
0
 def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]):
     aco = AttachedCollisionObject()
     if pose is not None:
         aco.object = self.__make_box(name, pose, size)
     else:
         aco.object = self.__make_existing(name)
     aco.link_name = link
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     else:
         aco.touch_links = [link]
     self.__submit(aco, attach=True)
コード例 #8
0
def publish_parsed_urdf(parsed_urdf, scene, ap, attach=False):
    """ Publish link geometry for every joint's child. """
    for name, joint in parsed_urdf["joints"].items():
        # get the child link data
        link = parsed_urdf["links"][joint["child"]]

        # publish the child links collision geometry
        if link["type"] == "box":
            if attach:
                scene.attach_box("table", joint["child"], joint["pose"],
                                 link["size"], ["table"])
            else:
                scene.add_box(joint["child"], joint["pose"], link["size"])
        elif link["type"] == "cylinder":
            if attach:
                aco = AttachedCollisionObject()
                aco.touch_links = ["table"]
                aco.object = make_cylinder(joint["child"], joint["pose"],
                                           link["length"], link["radius"])
                aco.link_name = "table"
                print(aco.object)
                ap.publish(aco)
            else:
                scene.add_cylinder(joint["child"], joint["pose"],
                                   link["length"], link["radius"])

        else:
            scene.add_mesh(joint["child"], joint["pose"], link["filename"],
                           link["scale"])
コード例 #9
0
ファイル: moveit_test_pick.py プロジェクト: impet14/yaskawa
    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
コード例 #10
0
ファイル: stackit.py プロジェクト: Aharobot/baxter_demos
 def publish_attached(self, attached_obj, side):
     attached_obj.operation = CollisionObject.ADD
     msg = AttachedCollisionObject()
     msg.object = attached_obj
     msg.link_name = side+"_gripper" 
     touch_links = [side+"_gripper", side+"_gripper_base", side+"_hand_camera", side+"_hand_range", "octomap"]
     self.attached_pub.publish(msg)
コード例 #11
0
 def attach_box(self, link, name, pose, size=(1, 1, 1), touch_links=[]):
     aco = AttachedCollisionObject()
     aco.object = self.make_box(name, pose, size)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self._pub_aco.publish(aco)
コード例 #12
0
 def attach_mesh(self, link, name, pose, filename, touch_links=[]):
     aco = AttachedCollisionObject()
     aco.object = self.__make_mesh(name, pose, filename)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self._pub_aco.publish(aco)
コード例 #13
0
 def attach_mesh(self, link, name, pose, filename, touch_links = []):
     aco = AttachedCollisionObject()
     aco.object = self.__make_mesh(name, pose, filename)
     aco.link_name = link
     aco.touch_links = [link]
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     self._pub_aco.publish(aco)
 def _add_payload_to_planning_scene_msg(self, payload):
     
     msg=payload.payload_msg
     
     co = CollisionObject()
     co.id = payload.payload_msg.name
     co.header.frame_id = msg.header.frame_id
     
     payload.attached_link=msg.header.frame_id
     
     urdf_root=self.urdf.get_root()
     urdf_chain=self.urdf.get_chain(urdf_root, payload.attached_link, joints=True, links=False)
     urdf_chain.reverse()
     touch_links=[]
     touch_root = None        
     for j_name in urdf_chain:
         j=self.urdf.joint_map[j_name]
         if j.type != "fixed":
             break
         touch_root=j.parent
     
     def _touch_recurse(touch):
         ret=[touch]
         if touch in self.urdf.child_map:                
             for c_j,c in self.urdf.child_map[touch]:
                 if self.urdf.joint_map[c_j].type == "fixed":
                     ret.extend(_touch_recurse(c))
         return ret
     
     if touch_root is not None:
         touch_links.extend(_touch_recurse(touch_root))        
     
     for i in xrange(len(msg.collision_geometry.mesh_resources)):
         
         mesh_filename=urlparse.urlparse(resource_retriever.get_filename(msg.collision_geometry.mesh_resources[i])).path
         mesh_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.mesh_poses[i]))            
         mesh_scale=msg.collision_geometry.mesh_scales[i]
         mesh_scale = (mesh_scale.x, mesh_scale.y, mesh_scale.z)
         
         mesh_msg = load_mesh_file_to_mesh_msg(mesh_filename, mesh_scale)
         co.meshes.extend(mesh_msg)
         co.mesh_poses.extend([mesh_pose]*len(mesh_msg))
     
     for i in xrange(len(msg.collision_geometry.primitives)):
         co.primitives.append(msg.collision_geometry.primitives[i])
         primitive_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.primitive_poses[i]))
         co.primitive_poses.append(primitive_pose)
     
     aco = AttachedCollisionObject()    
     aco.link_name = payload.attached_link
     aco.object = co
     aco.touch_links = touch_links
     aco.weight = msg.inertia.m 
     
     #self._pub_aco.publish(aco)
     return aco
コード例 #15
0
 def publish_attached(self, attached_obj, side):
     attached_obj.operation = CollisionObject.ADD
     msg = AttachedCollisionObject()
     msg.object = attached_obj
     msg.link_name = side + "_gripper"
     touch_links = [
         side + "_gripper", side + "_gripper_base", side + "_hand_camera",
         side + "_hand_range", "octomap"
     ]
     self.attached_pub.publish(msg)
コード例 #16
0
 def makeAttached(self, link_name, obj, touch_links, detach_posture,
                  weight):
     o = AttachedCollisionObject()
     o.link_name = link_name
     o.object = obj
     if touch_links:
         o.touch_links = touch_links
     if detach_posture:
         o.detach_posture = detach_posture
     o.weight = weight
     return o
コード例 #17
0
 def makeAttached(self, link_name, obj, touch_links, detach_posture,
                  weight):
     o = AttachedCollisionObject()
     o.link_name = link_name
     o.object = obj
     if touch_links:
         o.touch_links = touch_links
     if detach_posture:
         o.detach_posture = detach_posture
     o.weight = weight
     return o
コード例 #18
0
	def attach(self, selected_id_index = 0):
		grasping_group = 'end_effector'
		touch_links = self.robot.get_link_names(group=grasping_group)
		aco = AttachedCollisionObject()
		aco.object = self.co_list[selected_id_index]
		aco.link_name = 'ee_link'
		if len(touch_links) > 0:
		    aco.touch_links = touch_links
		else:
		    aco.touch_links = ['ee_link']
		self.__submit(aco, attach=True)

		return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=4)
コード例 #19
0
ファイル: moveit_test_pick.py プロジェクト: impet14/yaskawa
    def form_remove_all_attached_msg(self, link_name):
        obj = AttachedCollisionObject()
        # The CollisionObject will be attached with a fixed joint to this link
        obj.link_name = link_name

        col_obj = CollisionObject()

        # If action is remove and no object.id is set, all objects
        # attached to the link indicated by link_name will be removed
        col_obj.operation = col_obj.REMOVE
        obj.object = col_obj

        return obj
コード例 #20
0
ファイル: hand_over_node.py プロジェクト: mvieth/hand_over
    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)
コード例 #21
0
    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)
コード例 #22
0
ファイル: collision.py プロジェクト: yazici/motoman_control
def attach_sphere(link, name, pose, radius, touch_links=[]):
    aco = AttachedCollisionObject()

    co = CollisionObject()
    co.operation = CollisionObject.ADD
    co.id = name
    co.header = pose.header
    sphere = SolidPrimitive()
    sphere.type = SolidPrimitive.SPHERE
    sphere.dimensions = [radius]
    co.primitives = [sphere]
    co.primitive_poses = [pose.pose]
    aco.object = co

    aco.link_name = link
    if len(touch_links) > 0:
        aco.touch_links = touch_links
    else:
        aco.touch_links = [link]
    scene._pub_aco.publish(aco)
コード例 #23
0
ファイル: collision.py プロジェクト: yazici/motoman_control
def attach_cylinder(link, name, pose, height, radius, touch_links=[]):
    aco = AttachedCollisionObject()

    co = CollisionObject()
    co.operation = CollisionObject.ADD
    co.id = name
    co.header = pose.header
    cylinder = SolidPrimitive()
    cylinder.type = SolidPrimitive.CYLINDER
    cylinder.dimensions = [height, radius]
    co.primitives = [cylinder]
    co.primitive_poses = [pose.pose]
    aco.object = co

    aco.link_name = link
    if len(touch_links) > 0:
        aco.touch_links = touch_links
    else:
        aco.touch_links = [link]
    scene._pub_aco.publish(aco)
コード例 #24
0
ファイル: baxter_scene.py プロジェクト: ysl208/Baxter_PbD
 def attachCylinder(self,link,name,pose,size,touch_links=[]):
     """
         Attaches a cylinder to a gripper
         
         :param name: Name of the new cylinder object
         :type name: str
         :param pose: Pose of the new object
         :type pose: geometry_msgs.msg.PoseStamped
         :param size: Dimensions of the new object (height[m],radius[m])
         :type size: (float, float)
         :param touch_links: links that are already in collision with the attached object (added links do not trigger a collision)
         :type touch_links: list(str)
     """
     #print "attach object with name", name
     #print "touch links",touch_links
     aco = AttachedCollisionObject()
     aco.object = self.__make_cylinder(name, pose, size)
     aco.link_name = link
     if len(touch_links) > 0:
         aco.touch_links = touch_links
     else:
         aco.touch_links = [link]
     self.scene._pub_aco.publish(aco)
  def init_cluster_grasper(self, cluster, probabilities=[], use_probability=True):

    self.cluster_frame = cluster.header.frame_id

    #use PCA to find the object frame and bounding box dims, and to get the cluster points in the object frame (z=0 at bottom of cluster)
    if use_probability:
      if len(probabilities) == 0:
        (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
          self.object_to_base_frame, self.object_to_cluster_frame) = \
          self.cbbf.find_object_frame_and_bounding_box(cluster, []) #empty probabilities
        if self.draw_box:
          self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_object_box', color=[0.2,0.2,1])
          print 'probabilistic_object_box'
      else:
        (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
            self.object_to_base_frame, self.object_to_cluster_frame) = \
            self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities)
        #draw the bounding box
        if self.draw_box:
          self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_weighted_object_box', color=[0,0.9,0.9])
          print 'probabilistic_weighted_object_box'

    else:
      (self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
              self.object_to_base_frame, self.object_to_cluster_frame) = \
              self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities)
      if self.draw_box:
        self.draw_bounding_box(self.object_bounding_box_dims, name='deterministic_object_box', color=[1,0,0])
        print 'deterministic_object_box'

    # MoveIt Stuff: CollisionObject
    '''
    print "self.object_bounding_box_dims=", self.object_bounding_box_dims
    print "self.object_bounding_box=", self.object_bounding_box
    print "self.object_to_base_frame.shape=", self.object_to_base_frame.shape
    print "self.object_to_base_frame=", self.object_to_base_frame
    print "self.object_to_cluster_frame=", self.object_to_cluster_frame
    '''
    # Add the bounding box as a CollisionObject
    co = CollisionObject()
    co.header.stamp = rospy.get_rostime()
    #co.header.frame_id = "base_footprint"
    co.header.frame_id = "base_link"

    co.primitives.append(SolidPrimitive())
    co.primitives[0].type = SolidPrimitive.BOX

    # Clear the previous CollisionObject
    co.id = "part"
    co.operation = co.REMOVE
    self.pub_co.publish(co)

    # Clear the previously attached object
    aco = AttachedCollisionObject()
    aco.object = co
    self.pub_aco.publish(aco)

    # Add the new CollisionObject
    box_height = self.object_bounding_box_dims[2]

    co.operation = co.ADD
    co.primitives[0].dimensions.append(self.object_bounding_box_dims[0])
    co.primitives[0].dimensions.append(self.object_bounding_box_dims[1])
    co.primitives[0].dimensions.append(self.object_bounding_box_dims[2])
    co.primitive_poses.append(Pose())
    co.primitive_poses[0].position.x = self.object_to_base_frame[0,3]
    co.primitive_poses[0].position.y = self.object_to_base_frame[1,3]
    co.primitive_poses[0].position.z = self.object_to_base_frame[2,3] + box_height/2

    quat = tf.transformations.quaternion_about_axis(math.atan2(self.object_to_base_frame[1,0], self.object_to_base_frame[0,0]), (0,0,1))
    #quat = tf.transformations.quaternion_from_matrix(self.object_to_base_frame)
    co.primitive_poses[0].orientation.x = quat[0]
    co.primitive_poses[0].orientation.y = quat[1]
    co.primitive_poses[0].orientation.z = quat[2]
    co.primitive_poses[0].orientation.w = quat[3]

    self.pub_co.publish(co)
    # END MoveIt! stuff

    #for which directions does the bounding box fit within the hand?
    gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)]
    self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)]

    #only half benefit for the longer dimension, if one is significantly longer than the other
    if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]:
      if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8:
        self._box_fits_in_hand[1] *= .5
      elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8:
        self._box_fits_in_hand[0] *= .5
    #rospy.loginfo("bounding box dims: "+pplist(self.object_bounding_box_dims))
    #rospy.loginfo("self._box_fits_in_hand: "+str(self._box_fits_in_hand))

    #compute the average number of points per sq cm of bounding box surface (half the surface only)
    bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \
        (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \
        (self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100)
    self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf