def __init__(self, arms = ['L','R'], calcPosePostAdjustment=None, adjustmentInterpolation=True, errorCorrectionMode='NONE'):
     self.arms = arms
     self.errorCorrectionMode = errorCorrectionMode
     self.truthPose = {}
     self.calcPose = {}
     self.calcPoseAtTruth = {}
     self.sys_error = {}
     self.sys_error= {}
     self.est_pose_pub = {}
     self.pose_error_pub = {}
     self.estimateFromImage = dict()
     self.estimatedPose = defaultdict(lambda: (None,None))
     if self.errorCorrectionMode == 'SYS':
         trained_data_R = pickle.load(open(os.path.join(roslib.packages.get_pkg_subdir('raven_2_params','data/right_arm'),'right_train_data.pkl')))
         trained_data_L =  pickle.load(open(os.path.join(roslib.packages.get_pkg_subdir('raven_2_params','data/left_arm'),'left_train_data.pkl')))
         self.sys_error['R'] = tfx.transform(trained_data_R["sys_robot_tf"]['R'])
         self.sys_error['L'] = tfx.transform(trained_data_L["sys_robot_tf"]['L'])
         print self.sys_error['R']
         print self.sys_error['L']
     elif self.errorCorrectionMode == 'PRE_POST':    
         self.pre_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms)
         self.post_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms)
         self.adjustment_side = dict((arm,'post') for arm in self.arms)
         self.switch_adjustment_update = True
         self.adjustmentInterpolation = {'pre': 1, 'post': 1}
         if adjustmentInterpolation is True:
             self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = 0.5
         elif isinstance(adjustmentInterpolation,dict):
             self.adjustmentInterpolation['pre'] = adjustmentInterpolation.get('pre',0.5)
             self.adjustmentInterpolation['post'] = adjustmentInterpolation.get('post',0.5)
         elif isinstance(adjustmentInterpolation,(list,tuple)):
             self.adjustmentInterpolation['pre'] = adjustmentInterpolation[0]
             self.adjustmentInterpolation['post'] = adjustmentInterpolation[1]
         else:
             self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = adjustmentInterpolation
     
         self.calcPosePostAdjustment = dict((arm,tfx.identity_tf()) for arm in self.arms)
         if calcPosePostAdjustment:
             for k,v in calcPosePostAdjustment:
                 self.calcPosePostAdjustment[k] = tfx.transform(v,copy=True)
     
         self.pre_adj_pub = {}
         self.post_adj_pub = {}
         for arm in self.arms:
             rospy.Subscriber(raven_constants.GripperTape.Topic+'_'+arm, PoseStamped, partial(self._truthCallback,arm))
             self.pre_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_pre_adjument_%s'%arm, TransformStamped)
             self.post_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_post_adjument_%s'%arm, TransformStamped)
             
     for arm in self.arms:
         self.est_pose_pub[arm] = rospy.Publisher('estimated_gripper_pose_%s'%arm, PoseStamped)
         self.pose_error_pub[arm] = rospy.Publisher('estimated_gripper_pose_error_%s'%arm, PoseStamped)    
     rospy.Subscriber(raven_constants.RavenTopics.RavenState, RavenState, self._ravenStateCallback)
Exemple #2
0
    def __init__(self, grasp_center, grasp_axis, grasp_width, jaw_width = 0, grasp_angle = 0,
                 tf = stf.SimilarityTransform3D(tfx.identity_tf(from_frame = 'world'), 1.0)):
        """
        Create a point grasp for parallel jaws with given center and width (relative to object)
        Params: (Note: all in meters!)
            grasp_center - numpy 3-array for center of grasp
            grasp_axis - numpy 3-array for grasp direction (should be normalized)
            grasp_width - how wide the jaws open in meters
            jaw_width - the width of the jaws tangent to the axis (0 means classical point grasp)
            grasp_angle - the angle of approach for parallel-jaw grippers (the 6th DOF for point grasps)
                wrt the orthogonal dir to the axis in the XY plane
            tf - the similarity tf of the grasp wrt the world
        """
        if not isinstance(grasp_center, np.ndarray) or grasp_center.shape[0] != 3:
            raise ValueError('Center must be numpy ndarray of size 3')
        if not isinstance(grasp_axis, np.ndarray)  or grasp_axis.shape[0] != 3:
            raise ValueError('Axis must be numpy ndarray of size 3')
        if jaw_width != 0:
            raise ValueError('Nonzero jaw width not yet supported')

        self.center_ = grasp_center
        self.axis_ = grasp_axis / np.linalg.norm(grasp_axis)
        self.grasp_width_ = grasp_width
        self.jaw_width_ = jaw_width
        self.approach_angle_ = grasp_angle
        self.tf_ = tf

        self.quality_ = None
Exemple #3
0
    def __init__(self,
                 vertices,
                 triangles,
                 normals=None,
                 metadata=None,
                 pose=tfx.identity_tf(),
                 scale=1.0,
                 density=1.0,
                 category='',
                 component=0):
        self.vertices_ = vertices
        self.triangles_ = triangles
        self.normals_ = normals
        self.metadata_ = metadata
        self.pose_ = pose
        self.scale_ = scale
        self.density_ = density
        self.category_ = category
        self.component_ = component

        # compute mesh properties
        self._compute_bb_center()
        self._compute_centroid()
        self._compute_com_uniform()
        self._compute_mass()
    def __init__(self,
                 sdf,
                 mesh=None,
                 features=None,
                 tf=stf.SimilarityTransform3D(tfx.identity_tf(), 1.0),
                 key='',
                 category='',
                 model_name='',
                 mass=1.0):
        """ 2D objects are initialized with sdfs only"""
        if not isinstance(sdf, s.Sdf3D):
            raise ValueError('Must initialize graspable object 3D with 3D sdf')
        if mesh is not None and not (isinstance(mesh, m.Mesh3D)
                                     or isinstance(mesh, mx.Mesh)):
            raise ValueError('Must initialize graspable object 3D with 3D sdf')

        self.center_of_mass_ = sdf.center_world()  # use SDF bb center for now
        GraspableObject.__init__(self,
                                 sdf,
                                 mesh=mesh,
                                 features=features,
                                 tf=tf,
                                 key=key,
                                 category=category,
                                 model_name=model_name,
                                 mass=mass)
Exemple #5
0
    def __init__(self, points, sdf_meas, dims, meas_variance = 0.1, kernel_name = 'rbf', kernel_hyps = [1., 3.],
                 origin = np.array([0,0]), resolution = 1.0, pose = tfx.identity_tf(frame="world")):
        if len(dims) != 3:
            raise ValueError('Dimensions must be 3D!')
        
        # store params
        self.pts_ = points
        self.data_ = sdf_meas
        self.var_ = meas_variance
        self.dims_ = dims

        self.origin_ = origin
        self.resolution_ = resolution
        self.pose_ = pose

        self._compute_flat_indices()

        # set up gp
        self.set_kernel(kernel_name, kernel_hyps)
        self.gp_ = gpy.models.GPRegression(points, sdf_meas, self.kernel_, Y_variance=meas_variance)
        self.gp_.optimize()
        print(self.gp_)

        # predict mean and variance of grid
        (self.mean_pred_, self.var_pred_) = self.predict_locations(self.grid_pts_, full_cov=False)

        self.feature_vector_ = None #Kmeans feature representation
Exemple #6
0
def grasp_model_figure():
    np.random.seed(100)

    h = plt.figure()
    ax = h.add_subplot(111, projection = '3d')

    sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.sdf'
    sf = sdf_file.SdfFile(sdf_3d_file_name)
    sdf_3d = sf.read()

    mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.obj'
    of = obj_file.ObjFile(mesh_name)
    m = of.read()

    clean_mesh_name = 'data/test/meshes/flashlight.obj'
    mc = mesh_cleaner.MeshCleaner(m)
    mc.rescale_vertices(0.07)
    oof = obj_file.ObjFile(clean_mesh_name)
    oof.write(mc.mesh())

    graspable = graspable_object.GraspableObject3D(sdf_3d, mesh=m, model_name=clean_mesh_name,
                                                   tf=stf.SimilarityTransform3D(tfx.identity_tf(), 0.75))

    rave.raveSetDebugLevel(rave.DebugLevel.Error)
    grasp_checker = OpenRaveGraspChecker()

    center = np.array([0, 0.01, 0])
    axis = np.array([1, 0, 0]) 
    axis = axis / np.linalg.norm(axis)
    width = 0.1
    grasp = g.ParallelJawPtGrasp3D(center, axis, width)

    rotated_grasps = grasp.transform(graspable.tf, 2.0 * np.pi / 20.0)
    print len(rotated_grasps)
    grasp_checker.prune_grasps_in_collision(graspable, rotated_grasps, auto_step=False, close_fingers=True, delay=1)
Exemple #7
0
        def __init__(self, height, width, fx, fy=None, cx=None, cy=None, pose=tfx.identity_tf()):
                '''
                Init camera parameters
                
                Params:
                   height: (int or float) height of image
                   width: (int of flaot) width of image
                   fx: (float) x focal length of camera in pixels
                   fy: (float) y focal length of camera in pixels
                   cx: (float) optical center of camera in pixels along x axis
                   cy: (float) optical center of camera in pixels along y axis
                '''
                self.height_ = height
                self.width_ = width
                self.fx_ = fx
                self.pose_ = pose

                # set focal, camera center automatically if under specified
                if fy is None:
                        self.fy_ = fx
                else:
                        self.fy_= fy
                if cx is None:
                        self.cx_ = float(width) / 2
                else:
                        self.cx_ = cx
                if cy is None:
                        self.cy_ = float(height) / 2
                else:
                        self.cy_ = cy
                # set camera projection matrix
                self.K_ = np.array([[self.fx_,        0, self.cx_],
                                    [       0, self.fy_, self.cy_],
                                    [       0,        0,        1]])
Exemple #8
0
    def __init__(self, sdf_data, origin, resolution, tf = stf.SimilarityTransform3D(tfx.identity_tf(), scale = 1.0), frame = None, use_abs = True):
        self.data_ = sdf_data
        self.origin_ = origin
        self.resolution_ = resolution
        self.dims_ = self.data_.shape

        # set up surface params
        self.surface_thresh_ = self.resolution_ * np.sqrt(2) / 2 # resolution is max dist from surface when surf is orthogonal to diagonal grid cells
        self.center_ = np.array([(self.dims_[0]-1) / 2, (self.dims_[1]-1) / 2, (self.dims_[2]-1) / 2])
        self.points_buf_ = np.zeros([Sdf3D.num_interpolants, 3], dtype=np.int)
        self.coords_buf_ = np.zeros([3,])

        # set up tf
        self.tf_ = tf

        # tranform sdf basis to grid (X and Z axes are flipped!)
        R_sdf_mesh = np.eye(3)
        self.tf_grid_sdf_ = stf.SimilarityTransform3D(tfx.canonical.CanonicalTransform(R_sdf_mesh, -R_sdf_mesh.T.dot(self.center_)), 1.0 / self.resolution_)
        self.tf_sdf_grid_ = self.tf_grid_sdf_.inverse()

        # optionally use only the absolute values (useful for non-closed meshes in 3D)
        if use_abs:
            self.data_ = np.abs(self.data_)

        self._compute_flat_indices()
        self._compute_gradients()

        self.feature_vector_ = None #Kmeans feature representation
 def __init__(self,
              sdf,
              tf=stf.SimilarityTransform3D(tfx.identity_tf(), 1.0)):
     """ 2D objects are initialized with sdfs only"""
     if not isinstance(sdf, s.Sdf2D):
         raise ValueError('Must initialize graspable object 2D with 2D sdf')
     GraspableObject.__init__(self, sdf, tf=tf)
    def __init__(self,
                 sdf,
                 mesh=None,
                 features=None,
                 tf=stf.SimilarityTransform3D(tfx.identity_tf(), 1.0),
                 key='',
                 model_name='',
                 category='',
                 mass=1.0):
        if not isinstance(tf, stf.SimilarityTransform3D):
            raise ValueError(
                'Must initialize graspable objects with 3D similarity transform'
            )
        self.sdf_ = sdf
        self.mesh_ = mesh
        self.tf_ = tf

        self.key_ = key
        self.model_name_ = model_name  # for OpenRave usage, gross!
        self.category_ = category
        self.mass_ = mass

        self.features_ = features  # shot features

        # make consistent poses, scales
        self.sdf_.tf = self.tf_
        if self.mesh_ is not None:
            self.mesh_.tf = self.tf_
 def __init__(self, arms = ['L','R'], calcPosePostAdjustment=None, adjustmentInterpolation=True):
     self.arms = arms
     
     self.truthPose = {}
     self.calcPose = {}
     self.calcPoseAtTruth = {}
     self.estimatedPose = defaultdict(lambda: (None,None))
     
     self.pre_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms)
     self.post_adjustment = dict((arm,tfx.identity_tf()) for arm in self.arms)
     self.adjustment_side = dict((arm,'post') for arm in self.arms)
     self.switch_adjustment_update = True
     self.adjustmentInterpolation = {'pre': 1, 'post': 1}
     if adjustmentInterpolation is True:
         self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = 0.5
     elif isinstance(adjustmentInterpolation,dict):
         self.adjustmentInterpolation['pre'] = adjustmentInterpolation.get('pre',0.5)
         self.adjustmentInterpolation['post'] = adjustmentInterpolation.get('post',0.5)
     elif isinstance(adjustmentInterpolation,(list,tuple)):
         self.adjustmentInterpolation['pre'] = adjustmentInterpolation[0]
         self.adjustmentInterpolation['post'] = adjustmentInterpolation[1]
     else:
         self.adjustmentInterpolation['pre'] = self.adjustmentInterpolation['post'] = adjustmentInterpolation
     
     self.estimateFromImage = dict()
     
     self.calcPosePostAdjustment = dict((arm,tfx.identity_tf()) for arm in self.arms)
     if calcPosePostAdjustment:
         for k,v in calcPosePostAdjustment:
             self.calcPosePostAdjustment[k] = tfx.transform(v,copy=True)
     
     self.est_pose_pub = {}
     self.pose_error_pub = {}
     self.pre_adj_pub = {}
     self.post_adj_pub = {}
     for arm in self.arms:
         rospy.Subscriber(Constants.GripperTape.Topic+'_'+arm, PoseStamped, partial(self._truthCallback,arm))
         self.est_pose_pub[arm] = rospy.Publisher('estimated_gripper_pose_%s'%arm, PoseStamped)
         self.pose_error_pub[arm] = rospy.Publisher('estimated_gripper_pose_error_%s'%arm, PoseStamped)
         self.pre_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_pre_adjument_%s'%arm, TransformStamped)
         self.post_adj_pub[arm] = rospy.Publisher('estimated_gripper_pose_post_adjument_%s'%arm, TransformStamped)
         
     rospy.Subscriber(Constants.RavenTopics.RavenState, RavenState, self._ravenStateCallback)
Exemple #12
0
    def __init__(self, sdf_data, origin = np.array([0,0]), resolution = 1.0, pose = tfx.identity_tf(from_frame="world"), scale = 1.0):
        self.data_ = sdf_data
        self.origin_ = origin
        self.resolution_ = resolution
        self.dims_ = self.data_.shape
        self.pose_ = pose
        self.scale_ = scale

        self.surface_thresh_ = self.resolution_ * np.sqrt(2) / 2 # resolution is max dist from surface when surf is orthogonal to diagonal grid cells
        self.center_ = np.array([self.dims_[0] / 2, self.dims_[1] / 2])

        self._compute_flat_indices()
        self._compute_gradients()

        self.feature_vector_ = None #Kmeans feature representation
Exemple #13
0
    def __init__(self, vertices, triangles, normals=None, metadata=None, pose=tfx.identity_tf(), scale = 1.0, density=1.0, category='', component=0):
        self.vertices_ = vertices
        self.triangles_ = triangles
        self.normals_ = normals
        self.metadata_ = metadata
        self.pose_ = pose
        self.scale_ = scale
        self.density_ = density
        self.category_ = category
        self.component_ = component

        # compute mesh properties
        self._compute_bb_center()
        self._compute_centroid()
        self._compute_com_uniform()
        self._compute_mass()
Exemple #14
0
    def __init__(self, sdf, mesh=None, features=None, tf = stf.SimilarityTransform3D(tfx.identity_tf(), 1.0), key='', model_name='', category=''):
        if not isinstance(tf, stf.SimilarityTransform3D):
            raise ValueError('Must initialize graspable objects with 3D similarity transform')
        self.sdf_ = sdf
        self.mesh_ = mesh
        self.tf_ = tf

        self.key_ = key
        self.model_name_ = model_name # for OpenRave usage, gross!
        self.category_ = category

        self.features_ = features # shot features

        # make consistent poses, scales
        self.sdf_.tf = self.tf_
        if self.mesh_ is not None:
            self.mesh_.tf = self.tf_
Exemple #15
0
    def __init__(self,
                 sdf_data,
                 origin,
                 resolution,
                 tf=stf.SimilarityTransform3D(tfx.identity_tf(), scale=1.0),
                 frame=None,
                 use_abs=True):
        self.data_ = sdf_data
        self.origin_ = origin
        self.resolution_ = resolution
        self.dims_ = self.data_.shape

        # set up surface params
        self.surface_thresh_ = self.resolution_ * np.sqrt(
            2
        ) / 2  # resolution is max dist from surface when surf is orthogonal to diagonal grid cells
        self.center_ = np.array([(self.dims_[0] - 1) / 2,
                                 (self.dims_[1] - 1) / 2,
                                 (self.dims_[2] - 1) / 2])
        self.points_buf_ = np.zeros([Sdf3D.num_interpolants, 3], dtype=np.int)
        self.coords_buf_ = np.zeros([
            3,
        ])

        # set up tf
        self.tf_ = tf

        # tranform sdf basis to grid (X and Z axes are flipped!)
        R_sdf_mesh = np.eye(3)
        self.tf_grid_sdf_ = stf.SimilarityTransform3D(
            tfx.canonical.CanonicalTransform(R_sdf_mesh,
                                             -R_sdf_mesh.T.dot(self.center_)),
            1.0 / self.resolution_)
        self.tf_sdf_grid_ = self.tf_grid_sdf_.inverse()

        # optionally use only the absolute values (useful for non-closed meshes in 3D)
        if use_abs:
            self.data_ = np.abs(self.data_)

        self._compute_flat_indices()
        self._compute_gradients()

        self.feature_vector_ = None  #Kmeans feature representation
Exemple #16
0
def grasp_model_figure():
    np.random.seed(100)

    h = plt.figure()
    ax = h.add_subplot(111, projection='3d')

    sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.sdf'
    sf = sdf_file.SdfFile(sdf_3d_file_name)
    sdf_3d = sf.read()

    mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.obj'
    of = obj_file.ObjFile(mesh_name)
    m = of.read()

    clean_mesh_name = 'data/test/meshes/flashlight.obj'
    mc = mesh_cleaner.MeshCleaner(m)
    mc.rescale_vertices(0.07)
    oof = obj_file.ObjFile(clean_mesh_name)
    oof.write(mc.mesh())

    graspable = graspable_object.GraspableObject3D(
        sdf_3d,
        mesh=m,
        model_name=clean_mesh_name,
        tf=stf.SimilarityTransform3D(tfx.identity_tf(), 0.75))

    rave.raveSetDebugLevel(rave.DebugLevel.Error)
    grasp_checker = OpenRaveGraspChecker()

    center = np.array([0, 0.01, 0])
    axis = np.array([1, 0, 0])
    axis = axis / np.linalg.norm(axis)
    width = 0.1
    grasp = g.ParallelJawPtGrasp3D(center, axis, width)

    rotated_grasps = grasp.transform(graspable.tf, 2.0 * np.pi / 20.0)
    print len(rotated_grasps)
    grasp_checker.prune_grasps_in_collision(graspable,
                                            rotated_grasps,
                                            auto_step=False,
                                            close_fingers=True,
                                            delay=1)
Exemple #17
0
    def sdf_3d(data):
        """ Converts HDF5 data provided in dictionary |data| to an SDF object """
        sdf_data = np.array(data[SDF_DATA_KEY])
        origin = np.array(data.attrs[SDF_ORIGIN_KEY])
        resolution = data.attrs[SDF_RES_KEY]

        # should never be used, really
        pose = tfx.identity_tf()
        scale = 1.0
        if SDF_POSE_KEY in data.attrs.keys():
            pose = data.attrs[SDF_POSE_KEY]
        if SDF_SCALE_KEY in data.attrs.keys():
            scale = data.attrs[SDF_SCALE_KEY]
        tf = stf.SimilarityTransform3D(pose, scale)

        frame = None
        if SDF_FRAME_KEY in data.attrs.keys():
            frame = data.attrs[SDF_FRAME_KEY]

        return sdf.Sdf3D(sdf_data, origin, resolution, tf, frame)
Exemple #18
0
    def __init__(self,
                 sdf_data,
                 origin=np.array([0, 0]),
                 resolution=1.0,
                 pose=tfx.identity_tf(from_frame="world"),
                 scale=1.0):
        self.data_ = sdf_data
        self.origin_ = origin
        self.resolution_ = resolution
        self.dims_ = self.data_.shape
        self.pose_ = pose
        self.scale_ = scale

        self.surface_thresh_ = self.resolution_ * np.sqrt(
            2
        ) / 2  # resolution is max dist from surface when surf is orthogonal to diagonal grid cells
        self.center_ = np.array([self.dims_[0] / 2, self.dims_[1] / 2])

        self._compute_flat_indices()
        self._compute_gradients()

        self.feature_vector_ = None  #Kmeans feature representation
 def _publish_state(self):
     with self.lock:
         ma = MarkerArray()
         
         all_centers_marker = Marker()
         all_centers_marker.ns = 'all_centers'
         all_centers_marker.id = 0
         all_centers_marker.type = Marker.POINTS
         all_centers_marker.action = Marker.ADD
         all_centers_marker.pose = tfx.identity_tf().msg.Pose()
         all_centers_marker.scale.x = 0.002
         all_centers_marker.scale.y = 0.002
         all_centers_marker.lifetime = rospy.Duration(1)
         
         all_centers_marker.color.r = 1
         all_centers_marker.color.g = 1
         all_centers_marker.color.b = 0
         all_centers_marker.color.a = 0.5
         for center in self._getAllCenters():
             all_centers_marker.header = center.msg.Header()
             all_centers_marker.points.append(center.msg.Point())
             
         ma.markers.append(all_centers_marker)
         
         curr_centers_marker = Marker()
         curr_centers_marker.ns = 'current_centers'
         curr_centers_marker.id = 0
         curr_centers_marker.type = Marker.POINTS
         curr_centers_marker.action = Marker.ADD
         curr_centers_marker.pose = tfx.identity_tf().msg.Pose()
         curr_centers_marker.scale.x = 0.001
         curr_centers_marker.scale.y = 0.001
         curr_centers_marker.lifetime = rospy.Duration(1)
         
         curr_centers_marker.color.r = 0
         curr_centers_marker.color.g = 0
         curr_centers_marker.color.b = 1
         curr_centers_marker.color.a = 1
         for center in self.currentCenters:
             curr_centers_marker.header = center.msg.Header()
             curr_centers_marker.points.append(center.msg.Point())
         
         ma.markers.append(curr_centers_marker)
             
         for id, arm in enumerate('LR'):
             if self.allocations.get(arm) is None:
                 continue
             alloc = self.allocations[arm]
             alloc_marker = Marker()
             alloc_marker.ns = 'alloc'
             alloc_marker.id = id
             alloc_marker.type = Marker.CYLINDER
             alloc_marker.action = Marker.ADD
             alloc_marker.header = alloc.msg.Header()
             alloc_marker.pose = tfx.pose(alloc).msg.Pose()
             alloc_marker.scale.x = self.allocationRadius
             alloc_marker.scale.y = self.allocationRadius
             alloc_marker.scale.z = 0.01
             alloc_marker.lifetime = rospy.Duration(1)
             
             alloc_marker.color.r = 0
             alloc_marker.color.g = 1
             alloc_marker.color.b = 1
             alloc_marker.color.a = 0.25
             
             ma.markers.append(alloc_marker)
             
         
         self.marker_pub.publish(ma)
Exemple #20
0
    def __init__(self, sdf, mesh = None, features=None, tf = stf.SimilarityTransform3D(tfx.identity_tf(), 1.0), key='', category='',
                 model_name=''):
        """ 2D objects are initialized with sdfs only"""
        if not isinstance(sdf, s.Sdf3D):
            raise ValueError('Must initialize graspable object 3D with 3D sdf')
        if mesh is not None and not (isinstance(mesh, m.Mesh3D) or isinstance(mesh,mx.Mesh)):
            raise ValueError('Must initialize graspable object 3D with 3D sdf')

        self.center_of_mass_ = sdf.center_world() # use SDF bb center for now
        GraspableObject.__init__(self, sdf, mesh=mesh, features=features, tf=tf, key=key, category=category, model_name=model_name)
Exemple #21
0
 def __init__(self, sdf, tf = stf.SimilarityTransform3D(tfx.identity_tf(), 1.0)):
     """ 2D objects are initialized with sdfs only"""
     if not isinstance(sdf, s.Sdf2D):
         raise ValueError('Must initialize graspable object 2D with 2D sdf')
     GraspableObject.__init__(self, sdf, tf=tf)