def go_angle(self, target_odom, tolerance=math.radians(5.), max_ang_vel=math.radians(20.), func=None):
self.tl.waitForTransform('base_footprint', 'odom_combined', rospy.Time(), rospy.Duration(10))
rate = rospy.Rate(100)
k = math.radians(80)
#current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2]
#target_odom = current_ang_odom + delta_ang
while not rospy.is_shutdown():
robot_odom = tfu.transform('base_footprint', 'odom_combined', self.tl)
current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2]
diff = ut.standard_rad(current_ang_odom - target_odom)
p = k * diff
#print diff
if func != None:
func(diff)
if np.abs(diff) < tolerance:
break
tw = gm.Twist()
vels = [p, np.sign(p) * max_ang_vel]
tw.angular.z = vels[np.argmin(np.abs(vels))]
#rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z)))
self.tw_pub.publish(tw)
#rospy.loginfo('commanded %s' % str(tw))
rate.sleep()
rospy.loginfo('finished %.3f' % math.degrees(diff))
return diff
def go_angle(self,
target_odom,
tolerance=math.radians(5.),
max_ang_vel=math.radians(20.),
func=None):
self.tl.waitForTransform('base_footprint', 'odom_combined',
rospy.Time(), rospy.Duration(10))
rate = rospy.Rate(100)
k = math.radians(80)
#current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2]
#target_odom = current_ang_odom + delta_ang
while not rospy.is_shutdown():
robot_odom = tfu.transform('base_footprint', 'odom_combined',
self.tl)
current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3],
'sxyz')[2]
diff = ut.standard_rad(current_ang_odom - target_odom)
p = k * diff
#print diff
if func != None:
func(diff)
if np.abs(diff) < tolerance:
break
tw = gm.Twist()
vels = [p, np.sign(p) * max_ang_vel]
tw.angular.z = vels[np.argmin(np.abs(vels))]
#rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z)))
self.tw_pub.publish(tw)
#rospy.loginfo('commanded %s' % str(tw))
rate.sleep()
rospy.loginfo('finished %.3f' % math.degrees(diff))
return diff
def diff_arm_pose(pose1, pose2):
pcpy = pose2.copy()
pcpy[4,0] = unwrap2(pose1[4,0], pose2[4,0])
pcpy[6,0] = unwrap2(pose1[6,0], pose2[6,0])
diff = pose1 - pose2
for i in range(pose1.shape[0]):
diff[i,0] = ut.standard_rad(diff[i,0])
return diff
def diff_arm_pose(pose1, pose2):
pcpy = pose2.copy()
pcpy[4, 0] = unwrap2(pose1[4, 0], pose2[4, 0])
pcpy[6, 0] = unwrap2(pose1[6, 0], pose2[6, 0])
diff = pose1 - pose2
for i in range(pose1.shape[0]):
diff[i, 0] = ut.standard_rad(diff[i, 0])
return diff
def go_angle(self, target_base, tolerance=math.radians(5.), max_ang_vel=math.radians(20.), func=None):
self.tl.waitForTransform('odom_combined', 'base_footprint', rospy.Time(), rospy.Duration(10))
rate = rospy.Rate(100)
k = math.radians(80)
od_T_bf = tfu.transform('odom_combined', 'base_footprint', self.tl)
target_odom_mat = od_T_bf * tfu.tf_as_matrix( ([0, 0, 0.], tr.quaternion_from_euler(0, 0, target_base)) )
target_odom = tr.euler_from_quaternion(tfu.matrix_as_tf(target_odom_mat)[1])[2]
#target_odom = (od_T_bf * np.row_stack([target_base, np.matrix([0,1.]).T]))[0:2,0]
#current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2]
#target_odom = current_ang_odom + delta_ang
robot_odom = tfu.transform('odom_combined', 'base_footprint', self.tl)
current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2]
diff = ut.standard_rad(current_ang_odom - target_odom)
rospy.loginfo('go_angle: target %.2f' % np.degrees(target_odom))
rospy.loginfo('go_angle: current %.2f' % np.degrees(current_ang_odom))
rospy.loginfo('go_angle: diff %.2f' % np.degrees(diff))
while not rospy.is_shutdown():
robot_odom = tfu.transform('odom_combined', 'base_footprint', self.tl)
current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2]
diff = ut.standard_rad(target_odom - current_ang_odom)
rospy.loginfo('go_angle: current %.2f diff %.2f' % (np.degrees(current_ang_odom), np.degrees(diff)))
p = k * diff
if func != None:
func(diff)
if np.abs(diff) < tolerance:
break
if self.go_ang_server.is_preempt_requested():
self.go_ang_server.set_preempted()
break
tw = gm.Twist()
vels = [p, np.sign(p) * max_ang_vel]
tw.angular.z = vels[np.argmin(np.abs(vels))]
#rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z)))
self.tw_pub.publish(tw)
#rospy.loginfo('commanded %s' % str(tw))
rate.sleep()
rospy.loginfo('go_ang: finished %.3f' % math.degrees(diff))
return diff
def extract_object_localization_features2(start_conditions, tflistener,
arm_used, p_base_map):
mid_contact_bf, jstate_msgs = find_contacts_and_fk(tflistener, arm_used)
model_surf_loc, model_surf_descriptors, surf_loc3d_pro, point_cloud_2d_pro = find3d_surf(
start_conditions)
#Find frame
surf_loc3d_bf = (np.linalg.inv(start_conditions['pro_T_bf']) \
* np.row_stack((surf_loc3d_pro, 1+np.zeros((1,surf_loc3d_pro.shape[1])))))[0:3,:]
frame_bf = create_frame(surf_loc3d_bf, p=np.matrix([-1, 0, 0.]).T)
center_bf = np.mean(surf_loc3d_bf, 1)
#Find out what the SURF features point to in this new frame
bf_R_pro = (start_conditions['pro_T_bf'][0:3, 0:3]).T
bf_R_obj = frame_bf
x_bf = frame_bf[:, 0]
x_pro = bf_R_pro.T * x_bf
x_ang_pro = math.atan2(x_pro[1, 0], x_pro[0, 0])
center_pro = tfu.transform_points(start_conditions['pro_T_bf'], center_bf)
center2d_pro = start_conditions['camera_info'].project(center_pro)
surf_directions = []
surf_dir_center = []
for loc, lap, size, direction, hess in model_surf_loc:
surf_directions.append(
ut.standard_rad(np.radians(direction) - x_ang_pro))
direction_to_center = center2d_pro - np.matrix(loc).T
surf_dir_center.append(direction_to_center)
surf_dir_center = np.column_stack(surf_dir_center)
return {
'contact_bf': mid_contact_bf,
'surf_loc3d_pro': surf_loc3d_pro,
'surf_loc2d_pro': model_surf_loc,
'point_cloud_2d_pro': point_cloud_2d_pro,
'surf_directions': surf_directions, #Orientation
'surf_pose_dir2d': surf_dir_center, #Position
'descriptors': model_surf_descriptors,
'jtransforms': jstate_msgs,
'frame_bf': frame_bf,
'center_bf': center_bf
}
def extract_object_localization_features2(start_conditions, tflistener, arm_used, p_base_map):
mid_contact_bf, jstate_msgs = find_contacts_and_fk(tflistener, arm_used)
model_surf_loc, model_surf_descriptors, surf_loc3d_pro, point_cloud_2d_pro = find3d_surf(start_conditions)
#Find frame
surf_loc3d_bf = (np.linalg.inv(start_conditions['pro_T_bf']) \
* np.row_stack((surf_loc3d_pro, 1+np.zeros((1,surf_loc3d_pro.shape[1])))))[0:3,:]
frame_bf = create_frame(surf_loc3d_bf, p=np.matrix([-1, 0, 0.]).T)
center_bf = np.mean(surf_loc3d_bf, 1)
#Find out what the SURF features point to in this new frame
bf_R_pro = (start_conditions['pro_T_bf'][0:3, 0:3]).T
bf_R_obj = frame_bf
x_bf = frame_bf[:,0]
x_pro = bf_R_pro.T * x_bf
x_ang_pro = math.atan2(x_pro[1,0], x_pro[0,0])
center_pro = tfu.transform_points(start_conditions['pro_T_bf'], center_bf)
center2d_pro = start_conditions['camera_info'].project(center_pro)
surf_directions = []
surf_dir_center = []
for loc, lap, size, direction, hess in model_surf_loc:
surf_directions.append(ut.standard_rad(np.radians(direction) - x_ang_pro))
direction_to_center = center2d_pro - np.matrix(loc).T
surf_dir_center.append(direction_to_center)
surf_dir_center = np.column_stack(surf_dir_center)
return {
'contact_bf': mid_contact_bf,
'surf_loc3d_pro': surf_loc3d_pro,
'surf_loc2d_pro': model_surf_loc,
'point_cloud_2d_pro': point_cloud_2d_pro,
'surf_directions': surf_directions, #Orientation
'surf_pose_dir2d': surf_dir_center, #Position
'descriptors': model_surf_descriptors,
'jtransforms': jstate_msgs,
'frame_bf': frame_bf,
'center_bf': center_bf
}
def go_ang(self, ang, speed):
dt = math.radians(ang)
if dt > 0:
sign = -1
elif dt < 0:
sign = 1
else:
sign = 0
self.tl.waitForTransform('base_footprint', 'odom_combined',
rospy.Time(), rospy.Duration(10))
p0_base = tfu.transform('base_footprint', 'odom_combined',
self.tl) # \
start_ang = tr.euler_from_matrix(p0_base[0:3, 0:3], 'sxyz')[2]
r = rospy.Rate(100)
dist_so_far = 0.
last_ang = start_ang
while not rospy.is_shutdown():
pcurrent_base = tfu.transform('base_footprint', 'odom_combined',
self.tl) #\
current_ang = tr.euler_from_matrix(pcurrent_base[0:3, 0:3],
'sxyz')[2]
dist_so_far = dist_so_far + (ut.standard_rad(current_ang -
last_ang))
if dt > 0 and dist_so_far > dt:
rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
break
elif dt < 0 and dist_so_far < dt:
rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
break
elif dt == 0:
rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
break
tw = gm.Twist()
tw.angular.z = math.radians(speed * sign)
self.tw_pub.publish(tw)
r.sleep()
last_ang = current_ang
def go_ang(self, ang, speed):
dt = math.radians(ang)
if dt > 0:
sign = -1
elif dt < 0:
sign = 1
else:
sign = 0
self.tl.waitForTransform("base_footprint", "odom_combined", rospy.Time(), rospy.Duration(10))
p0_base = tfu.transform("base_footprint", "odom_combined", self.tl) # \
start_ang = tr.euler_from_matrix(p0_base[0:3, 0:3], "sxyz")[2]
r = rospy.Rate(100)
dist_so_far = 0.0
last_ang = start_ang
while not rospy.is_shutdown():
pcurrent_base = tfu.transform("base_footprint", "odom_combined", self.tl) # \
current_ang = tr.euler_from_matrix(pcurrent_base[0:3, 0:3], "sxyz")[2]
dist_so_far = dist_so_far + (ut.standard_rad(current_ang - last_ang))
if dt > 0 and dist_so_far > dt:
rospy.loginfo("stopped! %f %f" % (dist_so_far, dt))
break
elif dt < 0 and dist_so_far < dt:
rospy.loginfo("stopped! %f %f" % (dist_so_far, dt))
break
elif dt == 0:
rospy.loginfo("stopped! %f %f" % (dist_so_far, dt))
break
tw = gm.Twist()
tw.angular.z = math.radians(speed * sign)
self.tw_pub.publish(tw)
r.sleep()
last_ang = current_ang
