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)
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)
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)
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)
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
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())
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
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])
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
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
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
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
])
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 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
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
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
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()
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 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)
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)
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
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')
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
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()
