def on_est_robot_state(channel, data): m = robot_state_t.decode(data) s.last_utime = m.utime s.status.utime = m.utime if (s.manip_until_utime > m.utime): # manip plan still being executed s.status.execution_status = 0 # EXECUTION_STATUS_EXECUTING s.status.plan_type = 8 # manip time_remaining = (s.manip_until_utime - m.utime) * 1E-6 if (m.utime - s.last_utime_printf > 0.1 * 1E6): print "manip, time remaining: ", time_remaining s.last_utime_printf = m.utime elif (s.walk_until_utime > m.utime): # manip plan still being executed s.status.execution_status = 0 # EXECUTION_STATUS_EXECUTING s.status.plan_type = 2 # walking time_remaining = (s.walk_until_utime - m.utime) * 1E-6 if (m.utime - s.last_utime_printf > 0.1 * 1E6): print "walking, time remaining: ", time_remaining s.last_utime_printf = m.utime else: # manip or walking plan not being executed s.status.plan_type = 1 # standing s.status.execution_status = 2 # NO PLAN lc.publish("PLAN_EXECUTION_STATUS", s.status.encode())
def box_state_handler(channel, data): if (have_gt_transform): latest_state = robot_state_t.decode(data) last_transform = numpy.identity(4) last_transform[0:3, 3] = numpy.array([ latest_state.pose.translation.x, latest_state.pose.translation.y, latest_state.pose.translation.z ]) last_transform[0:3, 0:3] = quaternion_matrix( numpy.array([ latest_state.pose.rotation.w, latest_state.pose.rotation.x, latest_state.pose.rotation.y, latest_state.pose.rotation.z ]))[0:3, 0:3] kinect2local = numpy.dot(robot2local, numpy.linalg.inv(kinect2robot)) local2kinect = numpy.linalg.inv(kinect2local) err_local = numpy.dot(numpy.linalg.inv(gt_transform), last_transform) # figure out corrections in local frame, then transfer to kinect frmae rotation_error = numpy.dot(gt_transform[0:3, 0:3], numpy.linalg.inv(last_transform[0:3, 0:3])) extra_correction = -numpy.dot( rotation_error, kinect2local[0:3, 3]) + kinect2local[0:3, 3] print extra_correction translation_error = gt_transform[0:3, 3] - last_transform[0:3, 3] print translation_error print numpy.dot(rotation_error, last_transform[0:3, 3]) - last_transform[0:3, 3] local_correction = numpy.identity(4) local_correction[0:3, 3] = translation_error local_correction[0:3, 0:3] = rotation_error print "local corr", local_correction correction_in_camera = numpy.identity(4) #correction_in_camera[0:3, 0:3] = numpy.linalg.inv(rotation_error) correction_in_camera[0:3, 3] = -numpy.dot(local2kinect[0:3, 0:3], local_correction[0:3, 3]) print "camera corr", correction_in_camera new_local2kinect = numpy.dot(correction_in_camera, local2kinect) # undo each transform in sequence final_transform = numpy.dot(new_local2kinect, robot2local) print kinect2robot print final_transform print "\n\n\n" msg = rigid_transform_t() msg.utime = 0 msg.trans = final_transform[0:3, 3] msg.quat = quaternion_from_matrix(final_transform[0:3, 0:3]) print msg.trans, msg.quat lc.publish("GT_CAMERA_OFFSET", msg.encode())
def box_gt_handler(channel, data): global have_gt_transform global gt_transform latest_gt = robot_state_t.decode(data) gt_transform = numpy.identity(4) gt_transform[0:3, 3] = numpy.array([ latest_gt.pose.translation.x, latest_gt.pose.translation.y, latest_gt.pose.translation.z ]) gt_transform[0:3, 0:3] = quaternion_matrix( numpy.array([ latest_gt.pose.rotation.w, latest_gt.pose.rotation.x, latest_gt.pose.rotation.y, latest_gt.pose.rotation.z ]))[0:3, 0:3] have_gt_transform = True
def on_est_robot_state(channel, data): m = robot_state_t.decode(data) s.last_utime = m.utime s.status.utime = m.utime if (s.manip_until_utime > m.utime): # manip plan still being executed s.status.execution_status = 0 # EXECUTION_STATUS_EXECUTING s.status.plan_type = 8 # manip time_remaining = (s.manip_until_utime - m.utime)*1E-6 print "manip, time remaining: ", time_remaining elif (s.walk_until_utime > m.utime): # manip plan still being executed s.status.execution_status = 0 # EXECUTION_STATUS_EXECUTING s.status.plan_type = 2 # walking time_remaining = (s.walk_until_utime - m.utime)*1E-6 print "walking, time remaining: ", time_remaining else: # manip or walking plan not being executed s.status.plan_type = 1 # standing s.status.execution_status = 2 # NO PLAN lc.publish("PLAN_EXECUTION_STATUS", s.status.encode())
def on_est_robot_state(channel, data): m = robot_state_t.decode(data) s.last_utime = m.utime