def rigidTransformMessageFromFrame(transform):
'''
Given a vtkTransform, returns a bot_core.rigid_transform_t message
'''
msg = bot_core.rigid_transform_t()
msg.trans, msg.quat = transformUtils.poseFromTransform(transform)
return msg
def rigidTransformMessageFromFrame(transform):
'''
Given a vtkTransform, returns a bot_core.rigid_transform_t message
'''
msg = bot_core.rigid_transform_t()
msg.trans, msg.quat = transformUtils.poseFromTransform(transform)
return msg
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 __init__(self, mode=Mode.translating, safe=True):
self.mode = mode
self.safe = safe # Don't send atlas behavior commands (to ensure that the robot never starts walking accidentally when running tests)
self.lc = lcm.LCM()
if self.mode == Mode.plotting:
self.gl = gl
else:
self.gl = None
self.bdi_step_queue_in = []
self.delivered_index = None
self.use_spec = True
self.drift_from_plan = np.zeros((3, 1))
self.behavior = Behavior.BDI_STEPPING
self.T_local_to_localbdi = bot_core.rigid_transform_t()
self.T_local_to_localbdi.trans = np.zeros(3)
self.T_local_to_localbdi.quat = ut.rpy2quat([0, 0, 0])
self.last_params = None
self.executing = False
self.last_footstep_plan_time = -np.inf
def __init__(self, mode=Mode.translating, safe=True):
self.mode = mode
self.safe = safe # Don't send atlas behavior commands (to ensure that the robot never starts walking accidentally when running tests)
self.lc = lcm.LCM()
if self.mode == Mode.plotting:
self.gl = gl
else:
self.gl = None
self.bdi_step_queue_in = []
self.delivered_index = None
self.use_spec = True
self.drift_from_plan = np.zeros((3,1))
self.behavior = Behavior.BDI_STEPPING
self.T_local_to_localbdi = bot_core.rigid_transform_t()
self.T_local_to_localbdi.trans = np.zeros(3)
self.T_local_to_localbdi.quat = ut.rpy2quat([0,0,0])
self.last_params = None
self.executing = False
self.last_footstep_plan_time = -np.inf
#!/usr/bin/python
import lcm
import drc
from bot_core import rigid_transform_t
import sys
import time
if len(sys.argv) != 9:
print "Usage: publish_transform <channel> <x> <y> <z> <w> <x> <y> <z>"
channel = sys.argv[1]
x = float(sys.argv[2])
y = float(sys.argv[3])
z = float(sys.argv[4])
ww = float(sys.argv[5])
wx = float(sys.argv[6])
wy = float(sys.argv[7])
wz = float(sys.argv[8])
lc = lcm.LCM()
msg = rigid_transform_t()
msg.utime = 0
msg.trans = [x, y, z]
msg.quat = [ww, wx, wy, wz]
while (1):
lc.publish(channel, msg.encode())
time.sleep(0.1)
