def __init__(self,cam):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if(not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.cam = cam
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.tl = tf.TransformListener()
self.xbox = XboxController()
def __init__(self):
self.xbox = XboxController()
self.robot = hsrb_interface.Robot()
self.whole_body = self.robot.get('whole_body')
self.gripper = self.robot.get('gripper')
self.pubTwist = rospy.Publisher('hsrb/command_velocity',
Twist,
queue_size=1)
def __init__(self, com=None, inject_noise=False, noise_scale=1.0):
self.alpha = noise_scale
self.xbox = XboxController()
self.com = com
self.i_n = inject_noise
if (self.i_n):
self.cov_matrix = pickle.load(
open(self.com.Options.stats_dir + 'cov_matrix.p', 'rb'))
self.pubTwist = rospy.Publisher('hsrb/command_velocity',
Twist,
queue_size=1)
self.record_actions = np.zeros(2)
self.curr_action = np.zeros(3)
self.twist_applied = None
class JoyStick_X():
def __init__(self, com=None, inject_noise=False, noise_scale=1.0):
self.alpha = noise_scale
self.xbox = XboxController()
self.com = com
self.i_n = inject_noise
if (self.i_n):
self.cov_matrix = pickle.load(
open(self.com.Options.stats_dir + 'cov_matrix.p', 'rb'))
self.pubTwist = rospy.Publisher('hsrb/command_velocity',
Twist,
queue_size=1)
self.record_actions = np.zeros(2)
self.curr_action = np.zeros(3)
self.twist_applied = None
def apply_control(self):
control_state = self.xbox.getControllerState()
noise_action = None
left_joystick = control_state['left_stick']
right_joystick = control_state['right_stick']
d_pad = control_state['d_pad']
twist = Twist()
self.curr_action = np.array(
[left_joystick[0], left_joystick[1], right_joystick[1]])
twist = self.com.format_twist(self.curr_action)
if (self.i_n and LA.norm(self.curr_action) > 2e-3):
noise_action = multivariate_normal(self.curr_action,
self.alpha * self.cov_matrix)
twist = self.com.format_twist(noise_action)
print d_pad
self.record_actions = np.array([d_pad[0], d_pad[1]])
self.twist_applied = twist
self.pubTwist.publish(twist)
return self.curr_action, noise_action, rospy.Time.now()
def get_current_control(self):
return self.curr_action
def get_current_twist(self):
return self.twist_applied
def get_record_actions(self):
return self.record_actions
def get_record_actions_passive(self):
control_state = self.xbox.getControllerState()
d_pad = control_state['d_pad']
return np.array([d_pad[0], d_pad[1]])
class JoyStick_X():
def __init__(self):
self.xbox = XboxController()
self.robot = hsrb_interface.Robot()
self.whole_body = self.robot.get('whole_body')
self.gripper = self.robot.get('gripper')
self.pubTwist = rospy.Publisher('hsrb/command_velocity',
Twist,
queue_size=1)
def format_twist(self, pos):
twist = Twist()
gain = -1.0
if (np.abs(pos[1]) < 2e-3):
pos[1] = 0.0
twist.linear.x = gain * pos[1] #+ self.noise*normal()
twist.linear.y = gain * pos[0] #+ self.noise*normal()
twist.angular.z = gain * pos[2] #+ self.noise*normal(
return twist
def apply_control(self):
control_state = self.xbox.getControllerState()
noise_action = None
left_joystick = control_state['left_stick']
right_joystick = control_state['right_stick']
d_pad = control_state['d_pad']
twist = Twist()
self.curr_action = np.array(
[left_joystick[0], left_joystick[1], right_joystick[1]])
twist = self.format_twist(self.curr_action)
self.record_actions = np.array([d_pad[0], d_pad[1]])
self.twist_applied = twist
print twist
self.pubTwist.publish(twist)
if (self.record_actions[1] < -0.1):
print "Move To Home"
self.gripper.command(1.2)
self.whole_body.move_to_neutral()
def get_current_control(self):
return self.curr_action
def get_current_twist(self):
return self.twist_applied
def get_record_actions(self):
return self.record_actions
def get_record_actions_passive(self):
control_state = self.xbox.getControllerState()
d_pad = control_state['d_pad']
return np.array([d_pad[0], d_pad[1]])
class InitialSampler(object):
def __init__(self,cam):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if(not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.cam = cam
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.tl = tf.TransformListener()
self.xbox = XboxController()
def debug_images(self,p_1,p_2):
c_img = self.cam.read_color_data()
p_1i = (int(p_1[0]),int(p_1[1]))
p_2i = (int(p_2[0]),int(p_2[1]))
cv2.line(c_img,p_1i,p_2i,(0,0,255),thickness = 10)
cv2.imshow('debug',c_img)
cv2.waitKey(300)
#IPython.embed()
def project_to_rgbd(self,trans):
M_t = tf.transformations.translation_matrix(trans)
M_R = self.get_map_to_rgbd()
M_cam_trans = np.matmul(LA.inv(M_R),M_t)
return M_cam_trans[0:3,3]
def make_projection(self,t_1,t_2):
###GO FROM MAP TO RGBD###
t_1 = self.project_to_rgbd(t_1)
t_2 = self.project_to_rgbd(t_2)
p_1 = self.pcm.project3dToPixel(t_1)
p_2 = self.pcm.project3dToPixel(t_2)
self.debug_images(p_1,p_2)
def debug_broadcast(self,pose,name):
while True:
self.br.sendTransform((pose[0], pose[1], pose[2]),
tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=0.0),
rospy.Time.now(),
name,
#'head_rgbd_sensor_link')
'rgbd_sensor_rgb_frame_map')
def get_map_to_rgbd(self):
not_found = True
while not_found:
try:
pose = self.tl.lookupTransform('map','rgbd_sensor_rgb_frame_map', rospy.Time(0))
not_found = False
except:
rospy.logerr("waiting for pose")
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
return M
def get_postion(self,name):
not_found = True
while not_found:
try:
pose = self.tl.lookupTransform('map',name, rospy.Time(0))
not_found = False
except:
rospy.logerr("waiting for pose")
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
return pose[0]
def look_up_transform(self,count):
transforms = self.tl.getFrameStrings()
for transform in transforms:
current_grasp = 'bed_i_'+str(count)
if current_grasp in transform:
print 'got here'
pose = self.tl.lookupTransform('rgbd_sensor_rgb_frame_map',transform, rospy.Time(0))
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
return M
def sample_corners(self):
head_up = self.get_postion("head_up")
head_down = self.get_postion("head_down")
bottom_up = self.get_postion("bottom_up")
bottom_down = self.get_postion("bottom_down")
# Get true midpoints of table edges
# (i.e. don't assume bottom_up and head_up have the same y value
# in case sensor image is tilted)
middle_up = np.array([(bottom_up[0] + head_up[0])/2,
(bottom_up[1] + head_up[1])/2,
bottom_down[2]])
middle_down = np.array([(bottom_down[0] + head_down[0])/2,
(bottom_down[1] + head_down[1])/2,
bottom_down[2]] )
bottom_middle = np.array([(bottom_down[0] + bottom_up[0])/2,
(bottom_down[1] + bottom_up[1])/2,
bottom_down[2]] )
head_middle = np.array([(head_down[0] + head_up[0])/2,
(head_down[1] + head_up[1])/2,
bottom_down[2]])
center = np.array([(bottom_middle[0] + head_middle[0])/2,
(middle_down[1] + middle_up[1])/2,
bottom_down[2]])
# Generate random point in sensor frame for the closer corner
# Draws from gaussian distribution
u_down = np.random.uniform(low=0.0,high=0.5)
v_down = np.random.uniform(low=0.3,high=1.0)
x_down = center[0] + u_down*LA.norm(center - bottom_middle)
y_down = center[1] + v_down*LA.norm(center - middle_down)
down_corner = (x_down, y_down, center[2])
# Generate random point in sensor frame for the further corner
# Draws from gaussian distribution
u_up = np.random.uniform(low=0.0,high=0.5)
v_up = np.random.uniform(low=0.3,high=1.0)
x_up = center[0] - u_up*LA.norm(center - bottom_middle)
y_up = center[1] - v_up*LA.norm(center - middle_up)
up_corner = (x_up, y_up, center[2])
print "CENTER ",center
if center[1] < 0.0 or center[2] < 0.0:
raise "ROBOT TRANSFROM INCORRECT"
print "UP CORNER ", up_corner
print "DOWN CORNER ", down_corner
return down_corner, up_corner
def sample_initial_state(self):
down_corner, up_corner = self.sample_corners()
button = 1.0
while button > -0.1:
control_state = self.xbox.getControllerState()
d_pad = control_state['d_pad']
button = d_pad[1]
self.make_projection(down_corner,up_corner)
return down_corner, up_corner