def _getAllCenters(self):
     with self.lock:
         self._filterCenterHistory()
         allCenters = []
         for centers in self.centerHistory.itervalues():
             for center in centers:
                 for c in allCenters:
                     center, c = tfx.point(center), tfx.point(c)
                     if (center-c).norm < self.centerCombiningThreshold:
                         break
                 else:
                     allCenters.append(center)
         return [tfx.point(center) for center in allCenters]
    def __init__(self, normal=None):

        self.objectPoint = None

        # gripper pose. Must both have frame_id of respective tool frame
        self.leftGripperPose = None
        self.rightGripperPose = None
        self.leftGripperPoseTf = None
        self.rightGripperPoseTf = None

        self.newLeftGripperPose = False
        self.newRightGripperPose = False
        
        self.gripperPoseIsEstimate = False

        # home position: likely in front of the camera, close
        #receptacle point: must have frame_id of link_0
        #  is the exact place to drop off (i.e. don't need to do extra calcs to move away)
        self.homePoint = {}
        self.receptaclePoint = {}
        self.homePoint[Constants.Arm.Left] = tfx.point([-.010,.016,-.095],frame=Constants.Frames.Link0).msg.PointStamped()
        self.receptaclePoint[Constants.Arm.Left] = tfx.point([.039, .045, -.173],frame=Constants.Frames.Link0).msg.PointStamped()
        
        self.homePoint[Constants.Arm.Right] = tfx.point([-.11,.016,-.095],frame=Constants.Frames.Link0).msg.PointStamped()
        self.receptaclePoint[Constants.Arm.Right] = tfx.point([-.13, .025, -.173],frame=Constants.Frames.Link0).msg.PointStamped()
        
        #table normal. Must be according to global (or main camera) frame
        if normal != None:
            self.normal = normal
        else:
            # for tape side
            self.normal = tfx.tb_angles(-90,90,0).msg
            # for non-tape side
            #self.normal = tfx.tb_angles(90,90,0).msg

        self.listener = tf.TransformListener()
        self.tf_br = tf.TransformBroadcaster()
        self.registerObjectPublisher()
        
        #rospy.Subscriber(Constants.Foam.Topic, PointStamped, self.foamCallback)
        
        self.tapeMsg = None
        
        # tape callback
        rospy.Subscriber(Constants.GripperTape.Topic+'_L', PoseStamped, self.tapeCallbackLeft)
        rospy.Subscriber(Constants.GripperTape.Topic+'_R', PoseStamped, self.tapeCallbackRight)

        rospy.sleep(2)
Esempio n. 3
0
    def left_image_callback(self, msg):
        if rospy.is_shutdown():
            return
        self.left_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        self.left_points = self.process_image(self.left_image)

        poses = []
        if self.right_points is not None and self.left_points is not None:
            self.left_points.sort(key=lambda x: x[0])
            self.right_points.sort(key=lambda x: x[0])
            disparities = self.assign_disparities(self.left_points,
                                                  self.right_points)
            for i in range(len(self.left_points)):
                x = self.left_points[i][0]
                y = self.left_points[i][1]
                disparity = disparities[i]
                print x, y, disparity
                pt = Util.convertStereo(x, y, disparity, self.info)
                pt = tfx.point(pt)
                pt = tfx.convertToFrame(pt, '/two_remote_center_link')
                pose = tfx.pose(pt)
                pose = pose.as_tf() * tfx.pose(tfx.tb_angles(
                    -90, 0, 180)).as_tf() * tfx.pose(tfx.tb_angles(90, 0, 0))
                poses.append(pose)

        print poses

        pose_array = PoseArray()
        pose_array.header = poses[0].msg.PoseStamped().header
        for pose in poses:
            pose_array.poses.append(pose)
        self.grasp_poses_pub.publish(pose_array)
Esempio n. 4
0
 def get_pixel_from_point(self, x, subsampled=True, is_round=True):
     #x_mat = np.array(tfx.point(x).as_pose().matrix)
     #x_mat[:3,:3] = np.zeros((3,3))
     
     x_mat = np.eye(4)
     x_mat[0:3,3] = tfx.point(x).array
     
     x_mat_tilde = np.dot(np.linalg.inv(self.sensor.GetTransform()), x_mat)
     
     #print x_mat
     #print ''
     #print x_mat_tilde
     
     if subsampled:
         y = np.dot(Camera.KK_sub, x_mat_tilde[0:3,3])
     else:
         y = np.dot(Camera.KK, x_mat_tilde[0:3,3])
     
     y_pixel = y[1]/y[2]
     x_pixel = y[0]/y[2]
     
     if is_round:
         y_pixel, x_pixel = np.round(y_pixel), np.round(x_pixel)
         
     return (y_pixel, x_pixel)
Esempio n. 5
0
    def get_pixel_from_point(self, x, subsampled=True, is_round=True):
        #x_mat = np.array(tfx.point(x).as_pose().matrix)
        #x_mat[:3,:3] = np.zeros((3,3))

        x_mat = np.eye(4)
        x_mat[0:3, 3] = tfx.point(x).array

        x_mat_tilde = np.dot(np.linalg.inv(self.sensor.GetTransform()), x_mat)

        #print x_mat
        #print ''
        #print x_mat_tilde

        if subsampled:
            y = np.dot(Camera.KK_sub, x_mat_tilde[0:3, 3])
        else:
            y = np.dot(Camera.KK, x_mat_tilde[0:3, 3])

        y_pixel = y[1] / y[2]
        x_pixel = y[0] / y[2]

        if is_round:
            y_pixel, x_pixel = np.round(y_pixel), np.round(x_pixel)

        return (y_pixel, x_pixel)
Esempio n. 6
0
 def project_triangles(self, triangles3d):
     """
     Projects 3d triangles onto image plane and returns 2d triangles
     
     :param triangles3d: list of geometry3d.Triangle with points in frame 'base_link'
     :return list of geometry2d.Triangle with points in frame 'base_link'
     """
     triangles2d = list()
     for triangle3d in triangles3d:
         a_proj = self.pixel_from_point(tfx.point(triangle3d.a, frame='base_link'))
         b_proj = self.pixel_from_point(tfx.point(triangle3d.b, frame='base_link'))
         c_proj = self.pixel_from_point(tfx.point(triangle3d.c, frame='base_link'))
         
         triangles2d.append(geometry2d.Triangle(a_proj, b_proj, c_proj))
         
     return triangles2d
Esempio n. 7
0
def test_voxel_grid():
    env = rave.Environment()
    env.Load('../envs/pr2-empty.env.xml')
    env.SetViewer('qtcoin')
    #env.GetViewer().SendCommand('SetFiguresInCamera 1') # also shows the figures in the image
    time.sleep(1)
    robot = env.GetRobots()[0]
    
    sensor = robot.GetAttachedSensor('head_cam').GetSensor()

    cam = Camera(robot, sensor)

    table = env.GetKinBody('table')
    table_pos = tfx.point(table.GetTransform()).array
    
    vgrid = VoxelGrid(cam, table_pos + np.array([0,0,0]), 1.5, 2, 1, resolution = 50)
    #vgrid.plot_centers()
    
    object = table_pos + np.array([.15,0,-.2])
    vgrid.object = object # TEMP
    h = utils.plot_point(env, object, size=.05, color=[1,0,0])
    
    vgrid.update_TSDF()
    #vgrid.update_ODF(object)
    #vgrid.signed_distance_complete()
    
    #vgrid.plot_TSDF()
    #vgrid.plot_ODF()
    #vgrid.plot_FOV()
    
    IPython.embed()
def test_down():
    down = tfx.tb_angles(-0.0,90.0,-0.0)
    print(down.quaternion)
    point = tfx.point(1,2,3)
    pose = tfx.pose(point)
    pose.orientation = down
    print(pose)
    code.interact(local=locals())
Esempio n. 9
0
    def project_triangles(self, triangles3d):
        """
        Projects 3d triangles onto image plane and returns 2d triangles
        
        :param triangles3d: list of geometry3d.Triangle with points in frame 'base_link'
        :return list of geometry2d.Triangle with points in frame 'base_link'
        """
        triangles2d = list()
        for triangle3d in triangles3d:
            a_proj = self.pixel_from_point(
                tfx.point(triangle3d.a, frame='base_link'))
            b_proj = self.pixel_from_point(
                tfx.point(triangle3d.b, frame='base_link'))
            c_proj = self.pixel_from_point(
                tfx.point(triangle3d.c, frame='base_link'))

            triangles2d.append(geometry2d.Triangle(a_proj, b_proj, c_proj))

        return triangles2d
Esempio n. 10
0
 def signed_distance(self, point, truncated_frustum):
     """
     Calculates signed-distance of point to truncated view frustum
     
     :param point: tfx.point/np.array
     :param truncated_frustum: list of geometry3d.Pyramid
     :return float signed-distance
     """
     point = tfx.point(point).array
     return min([pyramid3d.signed_distance(point) for pyramid3d in truncated_frustum])
Esempio n. 11
0
 def is_in_fov(self, point, zbuffer, subsampled=True):
     point = tfx.point(point)
     if subsampled:
         h, w = Camera.H_SUB, Camera.W_SUB
     else:
         h, w = Camera.HEIGHT, Camera.WIDTH
         
     y, x = self.get_pixel_from_point(point, subsampled)
     
     if (y < 0) or (y >= h) or (x < 0) or (x >= w):
         return False
     
     if zbuffer[y,x] < (tfx.point(self.sensor.GetTransform()) - point).norm:
         return False
     
     if np.dot(np.linalg.inv(self.sensor.GetTransform()),point.as_pose().matrix)[2,3] < 0:
         return False
     
     return True
Esempio n. 12
0
 def get_points(self):
     data = self.get_data()
     point_mat = data.ranges
     in_range = np.array(data.intensity, dtype=int)
     
     sensor_pose = tfx.pose(self.sensor.GetTransform())
     points = list()
     for i in xrange(point_mat.shape[0]):
         if in_range[i] == 1:
             points.append(tfx.point(sensor_pose.position + point_mat[i,:]))
             
     return points
 def _foam_points_cb(self,msg):
     if self.ignore: return
     if rospy.is_shutdown():
         return
     with self.lock:
         self._filterCenterHistory()
         
         t = tfx.stamp(msg.header.stamp)
         all_pts = tuple([tfx.convertToFrame(tfx.point(pt,frame=msg.header.frame_id),raven_constants.Frames.Link0) for pt in msg.points])#,header=msg.header
         self.centerHistory[t] = all_pts
         pts = []
         for pt in all_pts:
             for _, estPose in self.estPose.iteritems():
                 if estPose is not None:
                     tfxPoint = tfx.convertToFrame(tfx.point(pt,frame=msg.header.frame_id),estPose.frame)
                     if estPose.position.distance(tfxPoint) < self.estPoseExclusionRadius:
                         break
             else:
                 pts.append(pt)
         self.currentCenters = pts
         self.newCenters = True
Esempio n. 14
0
    def is_in_fov(self, point, zbuffer, subsampled=True):
        point = tfx.point(point)
        if subsampled:
            h, w = Camera.H_SUB, Camera.W_SUB
        else:
            h, w = Camera.HEIGHT, Camera.WIDTH

        y, x = self.get_pixel_from_point(point, subsampled)

        if (y < 0) or (y >= h) or (x < 0) or (x >= w):
            return False

        if zbuffer[y,
                   x] < (tfx.point(self.sensor.GetTransform()) - point).norm:
            return False

        if np.dot(np.linalg.inv(self.sensor.GetTransform()),
                  point.as_pose().matrix)[2, 3] < 0:
            return False

        return True
Esempio n. 15
0
 def signed_distance(self, point, truncated_frustum):
     """
     Calculates signed-distance of point to truncated view frustum
     
     :param point: tfx.point/np.array
     :param truncated_frustum: list of geometry3d.Pyramid
     :return float signed-distance
     """
     point = tfx.point(point).array
     return min([
         pyramid3d.signed_distance(point) for pyramid3d in truncated_frustum
     ])
Esempio n. 16
0
    def get_points(self):
        data = self.get_data()
        point_mat = data.ranges
        in_range = np.array(data.intensity, dtype=int)

        sensor_pose = tfx.pose(self.sensor.GetTransform())
        points = list()
        for i in xrange(point_mat.shape[0]):
            if in_range[i] == 1:
                points.append(tfx.point(sensor_pose.position +
                                        point_mat[i, :]))

        return points
Esempio n. 17
0
def setup_environment(obs_type, M=1000, lr='r', zero_seed=True):
    if zero_seed:
        random.seed(0)
        
    brett = pr2_sim.PR2('../envs/pr2-test.env.xml')
    env = brett.env
    
    larm = brett.larm
    rarm = brett.rarm
    l_kinect = brett.l_kinect
    r_kinect = brett.r_kinect
    
    larm.set_posture('mantis')
    rarm.set_posture('mantis')
    
    table = env.GetKinBody('table')
    base = table.GetLink('base')
    extents = base.Geometry.GetBoxExtents(base.GetGeometries()[0])
    
    table_pose = tfx.pose(table.GetTransform())
    # assume table has orientation np.eye(3)
    x_min, x_max = table_pose.position.x - extents[0], table_pose.position.x + extents[0]
    y_min, y_max = table_pose.position.y - extents[1], table_pose.position.y + extents[1]
    z_min, z_max = table_pose.position.z + extents[2], table_pose.position.z + extents[2] + .2
    
    mug = env.GetKinBody('mug')
    mug_pose = tfx.pose(mug.GetTransform())
    
    #x_min, x_max = mug_pose.position.x - .03, mug_pose.position.x + .03
    #y_min, y_max = mug_pose.position.y + .03, mug_pose.position.y + .03
    #z_min, z_max = mug_pose.position.z + extents[2], mug_pose.position.z + extents[2] + .2
    
    particles = list()
    for i in xrange(M):
        x = random_within(x_min, x_max)
        y = random_within(y_min, y_max)
        z = random_within(z_min, z_max)
        particle = tfx.point([x,y,z])
        particles.append(particle)
        
    particles = sorted(particles, key=lambda x: (x-mug_pose.position).norm)
        
    arm = larm if lr == 'l' else rarm
    kinect = l_kinect if lr == 'l' else r_kinect
        
    eih_sys = EihSystem(env, arm, kinect, obs_type)
    kinect.render_on()
    #arm.set_pose(tfx.pose([2.901, -1.712,  0.868],tfx.tb_angles(-143.0, 77.9, 172.1))) # FOR rarm ONLY
    time.sleep(1)
    
    return eih_sys, particles
Esempio n. 18
0
    def _table_callback(self, msg):
        assert (msg.header.frame_id.count('base_link') > 0)
        assert (len(msg.vectors) == 3)

        # calculate rotation matrix and extent lengths
        extents = [tfx.point(e) for e in msg.vectors]
        R = np.zeros((3, 3))
        for i, e in enumerate(extents):
            R[:, i] = e.array / e.norm

        extent_lengths = np.array([e.norm for e in extents])
        extent_lengths[-1] *= 1

        self.table_pose = tfx.pose(msg.center, R).matrix
        self.table_extents = extent_lengths
Esempio n. 19
0
 def _table_callback(self, msg):
     assert(msg.header.frame_id.count('base_link') > 0)
     assert(len(msg.vectors) == 3)
     
     # calculate rotation matrix and extent lengths
     extents = [tfx.point(e) for e in msg.vectors]
     R = np.zeros((3,3))
     for i, e in enumerate(extents):
         R[:,i] = e.array/e.norm
         
     extent_lengths = np.array([e.norm for e in extents])
     extent_lengths[-1] *= 1
         
     self.table_pose = tfx.pose(msg.center, R).matrix
     self.table_extents = extent_lengths
Esempio n. 20
0
    def construct_pyramid(self, cam):
        cam_pose = np.array(cam.get_pose().matrix)
        cam_rot, cam_trans = cam_pose[:3,:3], cam_pose[:3,3]
        
        abs_points3d = list()
        for pt, frame in zip(self.points, self.point_frames):
            assert(frame.count('base_link') > 0 or frame.count('camera') > 0)
            
            if frame.count('camera') > 0:
                pt = cam_rot.dot(pt.T) + cam_trans
                
            abs_seg3d = cam.segment_through_pixel(cam.pixel_from_point(tfx.point(pt,frame='base_link')))
            abs_points3d += [abs_seg3d.p0, abs_seg3d.p1]

        pyramid3d = geometry3d.TruncatedPyramid(*abs_points3d)
        return pyramid3d
    def getObjectPose(self):
        """
        Returns object point plus the table normal as the orientation
        
        Returns None if no object found
        """
        if not self.hasFoundObject():
            return None

        objectPoint = tfx.point(self.objectPoint, stamp=rospy.Time.now())
        tf_point_to_normal = tfx.lookupTransform(Constants.Frames.Link0, objectPoint.frame, wait=10)
        objectPoint = tf_point_to_normal * objectPoint


        objectPose = tfx.pose(self.objectPoint, self.normal)

        return objectPose.msg.PoseStamped()
Esempio n. 22
0
    def construct_pyramid(self, cam):
        cam_pose = np.array(cam.get_pose().matrix)
        cam_rot, cam_trans = cam_pose[:3, :3], cam_pose[:3, 3]

        abs_points3d = list()
        for pt, frame in zip(self.points, self.point_frames):
            assert (frame.count('base_link') > 0 or frame.count('camera') > 0)

            if frame.count('camera') > 0:
                pt = cam_rot.dot(pt.T) + cam_trans

            abs_seg3d = cam.segment_through_pixel(
                cam.pixel_from_point(tfx.point(pt, frame='base_link')))
            abs_points3d += [abs_seg3d.p0, abs_seg3d.p1]

        pyramid3d = geometry3d.TruncatedPyramid(*abs_points3d)
        return pyramid3d
Esempio n. 23
0
    def add_circle(self,
                   plane,
                   name,
                   center,
                   radius,
                   orientation,
                   num_circles=1,
                   arm=None,
                   duration_per_circle=True,
                   duration=None,
                   speed=None):
        if duration and duration_per_circle:
            duration = duration * num_circles
        if duration is None:
            if speed is None:
                speed = self.default_speed
            total_dist = 2 * pi * radius * num_circles
            duration = total_dist / speed
        center = tfx.point(center)
        orientation = tfx.quaternion(orientation)

        def fn(cmd, t):
            if plane == 'xy':
                pt = center + [
                    radius * cos(num_circles * 2. * pi * t),
                    num_circles * radius * sin(num_circles * 2. * pi * t), 0
                ]
            elif plane == 'xz':
                pt = center + [
                    radius * cos(num_circles * 2. * pi * t), 0,
                    num_circles * radius * sin(num_circles * 2. * pi * t)
                ]
            elif plane == 'yz':
                pt = center + [
                    0, radius * cos(num_circles * 2. * pi * t),
                    num_circles * radius * sin(num_circles * 2. * pi * t)
                ]

            tool_pose = Pose()
            tool_pose.position = pt.msg.Point()
            tool_pose.orientation = orientation.msg.Quaternion()

            TrajectoryPlayer.add_arm_pose_cmd(cmd, self._check(arm), tool_pose)

        self.add_stage(name, duration, fn)
Esempio n. 24
0
    def process_image(self):
        rospy.loginfo("Processing image...")
        if self.left_image is None or self.right_image is None:
            return # because we need both images

        left_points = self.get_circle_edge_points(self.left_image.copy())
        right_points = self.get_circle_edge_points(self.right_image.copy(), seed = left_points[0])

        left_points = left_points[1:]
        right_points = right_points[1:]
        # left_points = right_points

        # get the 3d points from the left and right points
        points_3d = self.get_points_3d(left_points, right_points)

        # fit a plane to the 3d points amd get the normal to the plane
        normal = self.least_squares_plane_normal(points_3d)

        # convert the points to the frame of the normal and project them onto the yz plane (x values all zero)
        projected_points = self.project_points_onto_plane(normal, points_3d)

        # create an array of 2d points and fit an ellipse to them
        points_2d = np.array([[pt.y, pt.z] for pt in projected_points], dtype=np.float32)
        ellipse = cv2.fitEllipse(points_2d)
        ellipse_points = self.get_ellipse_points(ellipse)

        # project the ellipse points back to 3d and transform back to global frame
        ellipse_points_3d = [tfx.point([0, a[0], a[1]]) for a in ellipse_points]
        # for a in ellipse_points_3d:
        #     a.frame = 'left_BC'
        # print ellipse_points_3d
        ellipse_points_3d = [normal.as_transform()*a for a in ellipse_points_3d] # convert back to global frame

        if DEBUG:
            markers = MarkerArray()
            for i in range(len(ellipse_points_3d)):
                point = ellipse_points_3d[i]
                marker = Util.createMarker(tfx.pose(point).msg.PoseStamped(), id=i+200, lifetime=2, r=0, g=255, b=0, scale = 0.004)
                markers.markers.append(marker)

            self.ellipse_points_pub.publish(markers)
	def add_circle(self,plane,name,center,radius,orientation,num_circles=1,arm=None,duration_per_circle=True,duration=None,speed=None):
		if duration and duration_per_circle:
			duration = duration * num_circles
		if duration is None:
			if speed is None:
				speed = self.default_speed
			total_dist = 2 * pi * radius * num_circles
			duration = total_dist / speed
		center = tfx.point(center)
		orientation = tfx.quaternion(orientation)
		def fn(cmd,t):
			if plane == 'xy':
				pt = center + [radius*cos(num_circles*2.*pi*t),num_circles*radius*sin(num_circles*2.*pi*t),0]
			elif plane == 'xz':
				pt = center + [radius*cos(num_circles*2.*pi*t),0,num_circles*radius*sin(num_circles*2.*pi*t)]
			elif plane == 'yz':
				pt = center + [0,radius*cos(num_circles*2.*pi*t),num_circles*radius*sin(num_circles*2.*pi*t)]
			
			tool_pose = Pose()
			tool_pose.position = pt.msg.Point()
			tool_pose.orientation = orientation.msg.Quaternion()
		
			TrajectoryPlayer.add_arm_pose_cmd(cmd,self._check(arm),tool_pose)
		self.add_stage(name,duration,fn)
Esempio n. 26
0
def test_voxel_grid():
    env = rave.Environment()
    env.Load('../envs/pr2-empty.env.xml')
    env.SetViewer('qtcoin')
    #env.GetViewer().SendCommand('SetFiguresInCamera 1') # also shows the figures in the image
    time.sleep(1)
    robot = env.GetRobots()[0]

    sensor = robot.GetAttachedSensor('head_cam').GetSensor()

    cam = Camera(robot, sensor)

    table = env.GetKinBody('table')
    table_pos = tfx.point(table.GetTransform()).array

    vgrid = VoxelGrid(cam,
                      table_pos + np.array([0, 0, 0]),
                      1.5,
                      2,
                      1,
                      resolution=50)
    #vgrid.plot_centers()

    object = table_pos + np.array([.15, 0, -.2])
    vgrid.object = object  # TEMP
    h = utils.plot_point(env, object, size=.05, color=[1, 0, 0])

    vgrid.update_TSDF()
    #vgrid.update_ODF(object)
    #vgrid.signed_distance_complete()

    #vgrid.plot_TSDF()
    #vgrid.plot_ODF()
    #vgrid.plot_FOV()

    IPython.embed()
def main():
	rospy.init_node(os.path.basename(sys.argv[0]),anonymous=True)
	
	parser = argparse.ArgumentParser(add_help=False)
	
	parser.add_argument('grid_size',type=int,nargs=2)
	
	#parser.add_argument('-s','--spacing',type=float,default=(0.00635*2))
	parser.add_argument('-l','--lift',type=float,default=0.01)
	
	args = parser.parse_args(rospy.myargv()[1:])
	
#	sub = rospy.Subscriber('tool_pose/R',PoseStamped,InitPoseHolder.callback)
#	rate = rospy.Rate(10)
#	while InitPoseHolder.init_pose is None and not rospy.is_shutdown():
#		rate.sleep()
#	init_pose = InitPoseHolder.init_pose

	#orientation = tfx.tb_angles(0,90,0)
	orientation = tfx.tb_angles(0,66,0)

	grid_height = -0.155
	
	grid_11 = tfx.point(-0.060, -0.039, grid_height)
	
	#grid_11 = tfx.point(-0.055, -0.032, grid_height)
	#grid_1N = tfx.point(-0.105, -0.034, grid_height)
	grid_1N = grid_11 - [0.00635*2 * (args.grid_size[0]-1),0,0]
	#grid_M1 = tfx.point(-0.057, -0.085, grid_height)
	grid_M1 = grid_11 - [0,0.00635*2 * (args.grid_size[1]-1),0]
	grid_MN = None
	
	row_vector = grid_1N-grid_11
	col_vector = grid_M1-grid_11
	
	row_spacing = norm(row_vector) / (args.grid_size[0]-1)
	col_spacing = norm(row_vector) / (args.grid_size[0]-1)
	
	print 'spacing: %f, %f' % (row_spacing * 100 / 2.54, col_spacing * 100 / 2.54)
	
	
	def get_pt(row,col):
		row_interp = (float(row-1)/(args.grid_size[0]-1))
		col_interp = (float(col-1)/(args.grid_size[1]-1))
		point = grid_11 + row_interp * row_vector + col_interp * col_vector 
		return point
	
	player = TrajectoryPlayer(arms='L')
	
	d = 1.75
	
	
	player.add_set_gripper(0)
	player.add_goto_first_pose(tfx.pose(grid_11 + [0,0,2*args.lift],orientation),speed=0.02, name='Move to 1,1')
	
	for row in xrange(1,args.grid_size[0]+1):
		if row != 1:
			player.add_point_to_point_with_lift('Move to %d,%d' % (row,1),get_pt(row-1,args.grid_size[1]),get_pt(row,1),orientation,args.lift,duration=d)
		for col in xrange(2,args.grid_size[1]+1):
			player.add_point_to_point_with_lift('Move to %d,%d' % (row,col),get_pt(row,col-1),get_pt(row,col),orientation,args.lift,duration=d)
	player.add_point_to_point_with_lift('Move back to 1,1',get_pt(args.grid_size[0],args.grid_size[1]),get_pt(1,1),orientation,args.lift,duration=3)
	player.add_point_to_point('Lift at 1,1',get_pt(1,1),get_pt(1,1)+[0,0,args.lift],orientation,duration=3)

	success = player.play(dry_run=False)
    def handleBoth(self, image_time=None):
        """ 
        Returns the quaternion and position of the pieces of tape (which should correspond to the 
        orientation and position of the gripper 
        """
        image_time = image_time or rospy.Time.now()
        
        try:
            found = self.color1.left.found and self.color2.left.found and self.color1.right.found and self.color2.right.found
            if self.print_messages:
                print "found"
                print "left_%s:\t\t%s" % (self.color1.name,self.color1.left.found)
                print "right_%s:\t\t%s" % (self.color1.name,self.color1.right.found)
                print "left_%s:\t\t%s" % (self.color2.name,self.color2.left.found)
                print "right_%s:\t\t%s" % (self.color2.name,self.color2.right.found)
            if found:
                    
                
                color2_lower_pt = self.convertStereo(self.color2.left.lower[0], self.color2.left.lower[1], math.fabs(self.color2.left.lower[0] - self.color2.right.lower[0]))
                color2_upper_pt = self.convertStereo(self.color2.left.upper[0], self.color2.left.upper[1], math.fabs(self.color2.left.upper[0] - self.color2.right.upper[0]))
                color1_lower_pt = self.convertStereo(self.color1.left.lower[0], self.color1.left.lower[1], math.fabs(self.color1.left.lower[0] - self.color1.right.lower[0]))
                color1_upper_pt = self.convertStereo(self.color1.left.upper[0], self.color1.left.upper[1], math.fabs(self.color1.left.upper[0] - self.color1.right.upper[0]))
                
                
                polygon_points = [color2_lower_pt, color2_upper_pt, color1_upper_pt, color1_lower_pt]
                polygon_points = [Point32(*x.tolist()) for x in polygon_points]
                polygon = PolygonStamped()
                polygon.polygon.points = polygon_points
                polygon.header.frame_id = "left_BC"
                polygon.header.stamp = rospy.Time.now()
                self.polygon_pub_base.publish(polygon)
                
                
                points = self.get_corrected_points_from_plane(color1_lower_pt, color1_upper_pt, color2_lower_pt, color2_upper_pt)
               
                color1_lower_pt = points[0]
                color1_upper_pt = points[1]
                color2_lower_pt = points[2]
                color2_upper_pt = points[3] 
                
                
                color2_vector = []
                color1_vector = []
                for i in range(0, 3):
                    color2_vector.append(color2_upper_pt[i] - color2_lower_pt[i])
                    color1_vector.append(color1_upper_pt[i] - color1_lower_pt[i])
                
                origin = (color1_lower_pt + color2_lower_pt)/2.
                p = tfx.point(self.get_position_from_lines(origin, color1_vector, color2_vector)).msg.Point()
                
                
                #p0 = Point()
                #position = (color2_lower_pt+color2_upper_pt+color1_lower_pt+color1_upper_pt)/4
                #p0.x = position[0]
                #p0.y = position[1]
                #p0.z = position[2]
                #print 'p0: {0}\n'.format(p0)

                polygon_points = [color2_lower_pt, color2_upper_pt, color1_upper_pt, color1_lower_pt]
                polygon_points = [Point32(*x.tolist()) for x in polygon_points]
                polygon = PolygonStamped()
                polygon.polygon.points = polygon_points
                polygon.header.frame_id = "left_BC"
                polygon.header.stamp = rospy.Time.now()
                self.polygon_pub.publish(polygon)
                
                quat = self.get_orientation_from_lines(color2_vector, color1_vector)
                tb = tfx.tb_angles(quat)
                q = Quaternion()
                q.x = quat[0]
                q.y = quat[1]
                q.z = quat[2]
                q.w = quat[3]

                if self.prevQuaternion == None:
                    self.prevQuaternion = q
                else:
                    prevQuat = tfx.tb_angles(self.prevQuaternion).msg
                    currQuat = tfx.tb_angles(q).msg
                    if SLERP:
                        try:
                            """if DYNAMIC_SLERP:
                                angle = angleBetweenQuaternions(prevQuat, currQuat)
                                print "ANGLE: "+angle
                                slerp_constant = calculateSLERPConstant(angle)
                            else:
                                slerp_constant = SLERP_CONSTANT
                            print "SLERP: "+slerp_constant"""
                            q = slerp(prevQuat, currQuat, SLERP_CONSTANT)
                        except ZeroDivisionError:
                            q = currQuat
                        self.prevQuaternion = q
                    else:
                        q = currQuat
                        self.prevQuaternion = q
                self.detected_gripper_pose = tfx.pose(p,q,frame=self.camera_frame,stamp=image_time)
                self.quat_pub.publish(q)
                self.pose_pub.publish(self.detected_gripper_pose.msg.PoseStamped())
                t_left = time.clock() - self.t0['left']
                t_right = time.clock() - self.t0['right']
                if self.show_time:
                    print "RUNNING TIME FROM LEFT IMAGE: "+str(t_left)
                    print "RUNNING TIME FROM RIGHT IMAGE: "+str(t_right)
                if self.print_messages:                
                    print "WORKING FINE"
            else:
                #self.pose_pub.publish(self.prevQuaternion) #if the points aren't detected, continue publishing the last known location
                if self.print_messages:                
                     print "POINTS NOT DETECTED"
        except (TypeError, ValueError) as e:
            if self.print_messages:
                print "CAUGHT ERROR"    
                print e
            #self.pose_pub.publish(self.prevQuaternion) #if some kind of error comes up, continue publishing the last known location #FIXME we may want to change this behavior
            pass
Esempio n. 29
0
    def handleBoth(self, image_time=None):
        """ 
        Returns the quaternion and position of the pieces of tape (which should correspond to the 
        orientation and position of the gripper 
        """
        image_time = image_time or rospy.Time.now()

        try:
            found = self.color1.left.found and self.color2.left.found and self.color1.right.found and self.color2.right.found
            if self.print_messages:
                print "found"
                print "left_%s:\t\t%s" % (self.color1.name,
                                          self.color1.left.found)
                print "right_%s:\t\t%s" % (self.color1.name,
                                           self.color1.right.found)
                print "left_%s:\t\t%s" % (self.color2.name,
                                          self.color2.left.found)
                print "right_%s:\t\t%s" % (self.color2.name,
                                           self.color2.right.found)
            if found:

                color2_lower_pt = self.convertStereo(
                    self.color2.left.lower[0], self.color2.left.lower[1],
                    math.fabs(self.color2.left.lower[0] -
                              self.color2.right.lower[0]))
                color2_upper_pt = self.convertStereo(
                    self.color2.left.upper[0], self.color2.left.upper[1],
                    math.fabs(self.color2.left.upper[0] -
                              self.color2.right.upper[0]))
                color1_lower_pt = self.convertStereo(
                    self.color1.left.lower[0], self.color1.left.lower[1],
                    math.fabs(self.color1.left.lower[0] -
                              self.color1.right.lower[0]))
                color1_upper_pt = self.convertStereo(
                    self.color1.left.upper[0], self.color1.left.upper[1],
                    math.fabs(self.color1.left.upper[0] -
                              self.color1.right.upper[0]))

                polygon_points = [
                    color2_lower_pt, color2_upper_pt, color1_upper_pt,
                    color1_lower_pt
                ]
                polygon_points = [Point32(*x.tolist()) for x in polygon_points]
                polygon = PolygonStamped()
                polygon.polygon.points = polygon_points
                polygon.header.frame_id = "left_BC"
                polygon.header.stamp = rospy.Time.now()
                self.polygon_pub_base.publish(polygon)

                points = self.get_corrected_points_from_plane(
                    color1_lower_pt, color1_upper_pt, color2_lower_pt,
                    color2_upper_pt)

                color1_lower_pt = points[0]
                color1_upper_pt = points[1]
                color2_lower_pt = points[2]
                color2_upper_pt = points[3]

                color2_vector = []
                color1_vector = []
                for i in range(0, 3):
                    color2_vector.append(color2_upper_pt[i] -
                                         color2_lower_pt[i])
                    color1_vector.append(color1_upper_pt[i] -
                                         color1_lower_pt[i])

                origin = (color1_lower_pt + color2_lower_pt) / 2.
                p = tfx.point(
                    self.get_position_from_lines(origin, color1_vector,
                                                 color2_vector)).msg.Point()

                #p0 = Point()
                #position = (color2_lower_pt+color2_upper_pt+color1_lower_pt+color1_upper_pt)/4
                #p0.x = position[0]
                #p0.y = position[1]
                #p0.z = position[2]
                #print 'p0: {0}\n'.format(p0)

                polygon_points = [
                    color2_lower_pt, color2_upper_pt, color1_upper_pt,
                    color1_lower_pt
                ]
                polygon_points = [Point32(*x.tolist()) for x in polygon_points]
                polygon = PolygonStamped()
                polygon.polygon.points = polygon_points
                polygon.header.frame_id = "left_BC"
                polygon.header.stamp = rospy.Time.now()
                self.polygon_pub.publish(polygon)

                quat = self.get_orientation_from_lines(color2_vector,
                                                       color1_vector)
                tb = tfx.tb_angles(quat)
                q = Quaternion()
                q.x = quat[0]
                q.y = quat[1]
                q.z = quat[2]
                q.w = quat[3]

                if self.prevQuaternion == None:
                    self.prevQuaternion = q
                else:
                    prevQuat = tfx.tb_angles(self.prevQuaternion).msg
                    currQuat = tfx.tb_angles(q).msg
                    if SLERP:
                        try:
                            """if DYNAMIC_SLERP:
                                angle = angleBetweenQuaternions(prevQuat, currQuat)
                                print "ANGLE: "+angle
                                slerp_constant = calculateSLERPConstant(angle)
                            else:
                                slerp_constant = SLERP_CONSTANT
                            print "SLERP: "+slerp_constant"""
                            q = slerp(prevQuat, currQuat, SLERP_CONSTANT)
                        except ZeroDivisionError:
                            q = currQuat
                        self.prevQuaternion = q
                    else:
                        q = currQuat
                        self.prevQuaternion = q
                self.detected_gripper_pose = tfx.pose(p,
                                                      q,
                                                      frame=self.camera_frame,
                                                      stamp=image_time)
                self.quat_pub.publish(q)
                self.pose_pub.publish(
                    self.detected_gripper_pose.msg.PoseStamped())
                t_left = time.clock() - self.t0['left']
                t_right = time.clock() - self.t0['right']
                if self.show_time:
                    print "RUNNING TIME FROM LEFT IMAGE: " + str(t_left)
                    print "RUNNING TIME FROM RIGHT IMAGE: " + str(t_right)
                if self.print_messages:
                    print "WORKING FINE"
            else:
                #self.pose_pub.publish(self.prevQuaternion) #if the points aren't detected, continue publishing the last known location
                if self.print_messages:
                    print "POINTS NOT DETECTED"
        except (TypeError, ValueError) as e:
            if self.print_messages:
                print "CAUGHT ERROR"
                print e
            #self.pose_pub.publish(self.prevQuaternion) #if some kind of error comes up, continue publishing the last known location #FIXME we may want to change this behavior
            pass
        self.jointStates =  msg.arms[0].joints

    def getJointStates(self):
        return self.jointStates
        







#w.r.t. Link0
#down = tfx.tb_angles(0,66,0)
down = tfx.tb_angles(0,90,0)
rightArmDot = tfx.pose(tfx.point(-.117, -.020, -.123), down)
leftArmDot = tfx.pose(tfx.point(.012, -.034, -.119), down)
leftArmDotOther = tfx.pose(tfx.point(-.034, -.030, -.159), tfx.tb_angles(-9,56.4,-3.4))
# tfx.tb_angles(0,40,0)
leftArmFourthDot = tfx.pose(tfx.point(-.084,.003,-.157), down)

armDot = leftArmDot
arm = MyConstants.Arm.Left

def test_opencloseGripper(close=True,duration=2):
    rospy.init_node('gripper_control',anonymous=True)
    rospy.sleep(2)
    gripperControl = GripperControlClass(arm, tf.TransformListener())
    gripperControl.start()
    rospy.sleep(2)
    rospy.loginfo('Setting the gripper')
Esempio n. 31
0
def jointRequest(n_steps, endJointPositions, startPoses, endPoses, toolFrames, manips, approachDirs=None, approachDist=0.02):
    """
    approachDirs is a list of 3d vectors dictating approach direction
    w.r.t. 0_link
    To come from above, approachDir = np.array([0,0,1])
    """
    n_steps = n_steps if n_steps > 5 else 5
    approachDirs = approachDirs or [None for _ in range(len(endPoses))]
    
    request = {
        "basic_info" : {
            "n_steps" : n_steps,
            "manip" : "active",
            "start_fixed" : True
            },
        "costs" : [
                {
                    "type" : "joint_vel",
                    "params": {"coeffs" : [1]}
                    },
                {
                    "type" : "collision",
                    "params" : {
                        "coeffs" : [100],
                        "continuous" : True,
                        "dist_pen" : [0.001]
                        }
                    },
               {
                "type" : "collision",
                "params" : {
                    "coeffs" : [100],
                    "continuous" : False,
                    "dist_pen" : [0.001]
                    }
                },
                ],
        "constraints" : [
            {
                "type" : "joint", # joint-space target
                "params" : {"vals" : endJointPositions } # length of vals = # dofs of manip
                }
                
            ],
        "init_info" : {
            "type" : "straight_line",
            "endpoint" : endJointPositions
            }
        }
    
    for startPose, endPose, toolFrame, manip, approachDir in zip(startPoses, endPoses, toolFrames, manips, approachDirs):
            if approachDir is not None and tfx.point(np.array(endPose.position.list) - np.array(startPose.position.list)).norm > approachDist:
                #pose = tfx.pose(endPose)
                endPose = tfx.pose(endPose)
                approachPosition = endPose.position + approachDir/np.linalg.norm(approachDir)*approachDist
                pose = tfx.pose(approachPosition, endPose.orientation, frame=endPose.frame, stamp=endPose.stamp)
                
                print('endPosition: {0}'.format(endPose.position.list))
                print('approachPosition: {0}'.format(approachPosition.list))
            
                convPose = tfx.pose(transformRelativePoseForIk(manip, pose.matrix, pose.frame, toolFrame),frame=toolFrame)
            
                desPos = convPose.position.list
                desQuat = convPose.orientation
                wxyzQuat = [desQuat.w, desQuat.x, desQuat.y, desQuat.z]
                
                frame = toolFrame
                timestep = int(.75*n_steps) if int(.75*n_steps) < n_steps-2 else n_steps-3
                
                # cost or constraint?
                request["constraints"].append({
                            "type": "pose",
                            "name" : "target_pose",
                            "params" : {"xyz" : desPos,
                                        "wxyz" : wxyzQuat,
                                        "link" : frame,
                                        "rot_coeffs" : [0,0,0],
                                        "pos_coeffs" : [100000,100000,100000],
                                        "timestep" : timestep
                                        }
                            })

    return request
Esempio n. 32
0
def test_greedy(M=1000):
    random.seed(0)
        
    brett = pr2_sim.PR2('../envs/pr2-test.env.xml')
    env = brett.env
    arm = brett.rarm
    kinect = brett.r_kinect
    
    arm.set_posture('mantis')
    p = arm.get_pose()
    arm.set_pose(tfx.pose(p.position + [.5, 0, 0], tfx.tb_angles(0, 90, 0)))
    
    # .23 just on edge of range
    # .25 out of range
    p = kinect.get_pose()
    center = [p.position.x + .3, p.position.y, p.position.z]
    
    P = list()
    for i in xrange(M):
        pos = [0,0,0]
        for i in xrange(len(center)):
            pos[i] = random_within(center[i] - .025, center[i] + .025)
        particle = tfx.point(pos)
        P.append(particle)
        
    handles = list()
    for p in P:
        handles.append(utils.plot_point(env, p.array,color=[1,0,0]))
    
                
    eih_sys = EihSystem(env, arm, kinect, 'fov')
    kinect.render_on()
    time.sleep(1)
        
    x0 = arm.get_joint_values()
    U = [np.zeros(x0.shape)]
    
    try:
        t = 0
        while True:
            print('Iter: {0}'.format(t))
            t += 1
            
            arm.set_joint_values(x0)
            
            grad = eih_sys.cost_grad(x0, U, P, step=1e-3, use_discrete=False)[0]
            print('grad: {0}'.format(list(grad)))
            
            u_grad = -(2*(np.pi/180.))*grad/np.linalg.norm(grad, 2)
            x1 = x0 + u_grad
            
            arm.set_joint_values(x0)
            p0 = arm.get_pose()
            arm.set_joint_values(x1)
            p1 = arm.get_pose()
            
            delta_pos = .05*(p1.position - p0.position)/(p1.position - p0.position).norm
            clipped_quat = tft.quaternion_slerp(p0.tb_angles.to_quaternion(), p1.tb_angles.to_quaternion(), .5)
            
            p1_clipped = tfx.pose(p0.position + delta_pos, clipped_quat)
            arm.set_pose(p1_clipped)
            x1_clipped = arm.get_joint_values()
            u_t = x1_clipped - x0
            arm.set_joint_values(x0)
            
            x_tp1, P_tp1 = eih_sys.update_state_and_particles(x0, P, u_t, plot=True, add_noise=False)
            
            P = P_tp1
            x0 = x_tp1
            
            
            #utils.save_view(env, 'figures/eih_pf_{0}.png'.format(t))
            
    except KeyboardInterrupt:
        rave.RaveDestroy()
        print('Exiting')
        sys.exit()