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)