tilting_laser.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2008, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 import numpy
34 from numpy import matrix, vsplit, sin, cos, reshape
35 import rospy
36 from calibration_estimation.single_transform import SingleTransform
37 
38 # Primitive used to model a tilting laser platform.
39 
40 param_names = ['gearing']
41 
43 
44  def __init__(self, config ):
45  rospy.logdebug("Initializing tilting laser")
46  self._config = config
47  self._cov_dict = config['cov']
48 
49  param_vec = self.dict_to_params(config)
50  self.inflate(param_vec)
51 
52  def update_config(self, robot_params):
53  parent = robot_params.urdf.joint_map[self._config['joint']].parent
54  child = robot_params.urdf.joint_map[self._config['joint']].child
55  before_chain = robot_params.urdf.get_chain(robot_params.base_link, parent, links=False)
56  before_chain.append(self._config['joint'])
57  after_chain = robot_params.urdf.get_chain(child, self._config['frame_id'], links=False)
58  self._before_chain_Ts = [robot_params.transforms[transform_name] for transform_name in before_chain]
59  self._after_chain_Ts = [robot_params.transforms[transform_name] for transform_name in after_chain]
60 
61  def dict_to_params(self, config):
62  param_vec = matrix(numpy.zeros((1,1), float))
63  param_vec[0,0] = config['gearing']
64  return param_vec
65 
66  def params_to_config(self, param_vec):
67  return {'joint' : self._config['joint'],
68  'frame_id' : self._config['frame_id'],
69  "gearing" : float(param_vec[0,0]),
70  "cov" : self._cov_dict }
71 
72  def calc_free(self, free_config):
73  return [free_config['gearing'] == 1]
74 
75  # Convert column vector of params into config
76  def inflate(self, param_vec):
77  self._gearing = param_vec[0,0]
78 
79  # Return column vector of config
80  def deflate(self):
81  param_vec = matrix(numpy.zeros((1,1), float))
82  param_vec[0,0] = self._gearing
83  return param_vec
84 
85  # Returns # of params needed for inflation & deflation
86  def get_length(self):
87  return len(param_names)
88 
89  def compute_pose(self, joint_pos):
90  pose = matrix(numpy.eye(4))
91  for before_chain_T in self._before_chain_Ts:
92  pose = pose * before_chain_T.transform
93  pose = pose * SingleTransform([0, 0, 0, 0, joint_pos[0]*self._gearing, 0]).transform # TODO: remove assumption of Y axis
94  for after_chain_T in self._after_chain_Ts:
95  pose = pose * after_chain_T.transform
96  return pose
97 
98  # Given a single set of joint positions, project into 3D
99  # joint_pos - a single list that looks like [tilting_joint, pointing_angle, range]
100  # returns - 4x1 numpy matrix, with the resulting projected point (in homogenous coords)
101  def project_point_to_3D(self, joint_pos):
102  p = joint_pos[1] # Pointing Angle
103  r = joint_pos[2] # Range
104  ray = reshape(matrix([cos(p)*r, sin(p)*r, 0, 1]), (-1,1))
105 
106  result = self.compute_pose(joint_pos) * ray
107  return result
108 
109  # Take sets of joint positions, and project them into 3D
110  # joint_positions - should look like the following
111  # - [tilting_joint, pointing_angle, range] # Point 1
112  # - [tilting_joint, pointing_angle, range] # Point 2
113  # - :
114  # - [tilting_joint, pointing_angle, range] # Point N
115  # Return - 4xN numpy matrix of 3D points
116  def project_to_3D(self, joint_pos):
117  result = numpy.concatenate( [self.project_point_to_3D(x) for x in joint_pos], 1 )
118  return result
119 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Fri Apr 2 2021 02:12:53