tilting_laser.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 import numpy
00034 from numpy import matrix, vsplit, sin, cos, reshape
00035 import rospy
00036 from calibration_estimation.single_transform import SingleTransform
00037 
00038 # Primitive used to model a tilting laser platform.
00039 
00040 param_names = ['gearing']
00041 
00042 class TiltingLaser:
00043 
00044     def __init__(self, config ):
00045         rospy.logdebug("Initializing tilting laser")
00046         self._config = config
00047         self._cov_dict = config['cov']
00048 
00049         param_vec = self.dict_to_params(config)
00050         self.inflate(param_vec)
00051 
00052     def update_config(self, robot_params):
00053         parent = robot_params.urdf.joints[self._config['joint']].parent
00054         child = robot_params.urdf.joints[self._config['joint']].child
00055         before_chain = robot_params.urdf.get_chain(robot_params.base_link, parent, links=False)
00056         before_chain.append(self._config['joint'])
00057         after_chain = robot_params.urdf.get_chain(child, self._config['frame_id'], links=False)
00058         self._before_chain_Ts = [robot_params.transforms[transform_name] for transform_name in before_chain]
00059         self._after_chain_Ts  = [robot_params.transforms[transform_name] for transform_name in after_chain]
00060 
00061     def dict_to_params(self, config):
00062         param_vec = matrix(numpy.zeros((1,1), float))
00063         param_vec[0,0] = config['gearing']
00064         return param_vec
00065 
00066     def params_to_config(self, param_vec):
00067         return {'joint'     : self._config['joint'],
00068                 'frame_id'  : self._config['frame_id'],
00069                 "gearing"   : float(param_vec[0,0]),
00070                 "cov"       : self._cov_dict }
00071 
00072     def calc_free(self, free_config):
00073         return [free_config['gearing'] == 1]
00074 
00075     # Convert column vector of params into config
00076     def inflate(self, param_vec):
00077         self._gearing = param_vec[0,0]
00078 
00079     # Return column vector of config
00080     def deflate(self):
00081         param_vec = matrix(numpy.zeros((1,1), float))
00082         param_vec[0,0] = self._gearing
00083         return param_vec
00084 
00085     # Returns # of params needed for inflation & deflation
00086     def get_length(self):
00087         return len(param_names)
00088 
00089     def compute_pose(self, joint_pos):
00090         pose = matrix(numpy.eye(4))
00091         for before_chain_T in self._before_chain_Ts:
00092             pose = pose * before_chain_T.transform
00093         pose = pose * SingleTransform([0, 0, 0, 0, joint_pos[0]*self._gearing, 0]).transform # TODO: remove assumption of Y axis
00094         for after_chain_T in self._after_chain_Ts:
00095             pose = pose * after_chain_T.transform
00096         return pose
00097 
00098     # Given a single set of joint positions, project into 3D
00099     # joint_pos - a single list that looks like [tilting_joint, pointing_angle, range]
00100     # returns - 4x1 numpy matrix, with the resulting projected point (in homogenous coords)
00101     def project_point_to_3D(self, joint_pos):
00102         p = joint_pos[1] # Pointing Angle
00103         r = joint_pos[2] # Range
00104         ray = reshape(matrix([cos(p)*r, sin(p)*r, 0, 1]), (-1,1))
00105 
00106         result = self.compute_pose(joint_pos) * ray
00107         return result
00108 
00109     # Take sets of joint positions, and project them into 3D
00110     # joint_positions - should look like the following
00111     #   - [tilting_joint, pointing_angle, range]   # Point 1
00112     #   - [tilting_joint, pointing_angle, range]   # Point 2
00113     #   -                     :
00114     #   - [tilting_joint, pointing_angle, range]   # Point N
00115     # Return - 4xN numpy matrix of 3D points
00116     def project_to_3D(self, joint_pos):
00117         result = numpy.concatenate( [self.project_point_to_3D(x) for x in joint_pos], 1 )
00118         return result
00119 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Sun Oct 5 2014 22:44:09