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
