joint_chain.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008-2011, 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, pi, array
00035 import rospy
00036 
00037 class JointChain:
00038     '''
00039     This represents a chain of actuated joints
00040     parameters = [gearing*num_of_joints]
00041     '''
00042     def __init__(self, config):
00043         self.root = config['root']
00044         self.tip = config['tip']
00045         self._joints = config['joints']
00046         self._active = config['active_joints']
00047         self._M = len(config['active_joints'])
00048 
00049         rospy.logdebug("Initializing joint chain with [%u] links", self._M)
00050 
00051         self._transforms = config['transforms']
00052         self._axis = numpy.matrix([ eval(str(x)) for x in config['axis']], float)
00053         self._cov_dict = config['cov']
00054         self._gearing = config['gearing']
00055 
00056     def calc_free(self, free_config):
00057         return [x == 1 for x in free_config['gearing']]
00058 
00059     def params_to_config(self, param_vec):
00060         config_dict = {}
00061         config_dict['axis'] = self._axis.tolist()
00062         config_dict['gearing'] = (array(param_vec)[:,0]).tolist()
00063         config_dict['cov'] = self._cov_dict
00064         config_dict['root'] = self.root
00065         config_dict['tip'] = self.tip
00066         return config_dict
00067 
00068     # Convert column vector of params into config
00069     def inflate(self, param_vec):
00070         self._gearing = array(param_vec)[:,0].tolist()
00071 
00072     # Return column vector of config
00073     def deflate(self):
00074         return matrix(self._gearing).T
00075 
00076     # Returns # of params needed for inflation & deflation
00077     def get_length(self):
00078         return self._M
00079 
00080     # Returns 4x4 numpy matrix of the pose of the tip of
00081     # the specified link num. Assumes the last link's tip
00082     # when link_num < 0
00083     def fk(self, chain_state, link_num=-1):
00084         if link_num < 0:
00085             link_num = self._M
00086 
00087         pos_trimmed = chain_state.position[0:(link_num+1)]
00088         gearing_trimmed = self._gearing[0:(link_num+1)]
00089 
00090         pos_scaled = [cur_pos * cur_gearing for cur_pos, cur_gearing in zip(pos_trimmed, gearing_trimmed)]
00091         
00092         T = matrix([ [ 1, 0, 0, 0 ],
00093                      [ 0, 1, 0, 0 ],
00094                      [ 0, 0, 1, 0 ],
00095                      [ 0, 0, 0, 1 ] ])
00096 
00097         m = 0
00098         for joint_name in self._joints:
00099             if m > link_num:
00100                 break
00101             transform = self._transforms[joint_name]
00102             if joint_name in self._active:
00103                 k = int(self._axis[0,m])
00104                 p = transform.deflate()
00105                 p = p.T.tolist()[0]
00106                 if k > 0:
00107                     p[k-1] = p[k-1] + pos_scaled[m]
00108                 else:
00109                     p[abs(k)-1] = p[abs(k)-1] - pos_scaled[m]
00110                 T = T * link_T( p )
00111                 m += 1
00112             else:
00113                 T = T * transform.transform                
00114 
00115         return T
00116 
00117 def link_T(link_params):
00118     r = link_params[3]
00119     p = link_params[4]
00120     y = link_params[5]
00121 
00122     T_roll =  matrix([ [ 1,   0,       0,    0 ],
00123                        [ 0, cos(r), -sin(r), 0 ],
00124                        [ 0, sin(r),  cos(r), 0 ],
00125                        [ 0,   0,       0,    1 ] ])
00126 
00127     T_pitch = matrix([ [  cos(p), 0, sin(p), 0 ],
00128                        [    0,    1,   0,    0 ],
00129                        [ -sin(p), 0, cos(p), 0 ],
00130                        [    0,    0,   0,    1 ] ])
00131 
00132     T_yaw =   matrix([ [ cos(y), -sin(y), 0, 0 ],
00133                        [ sin(y),  cos(y), 0, 0 ],
00134                        [   0,       0,    1, 0 ],
00135                        [   0,       0,    0, 1 ] ])
00136 
00137 
00138     T_trans = matrix([ [ 1, 0, 0, link_params[0] ],
00139                        [ 0, 1, 0, link_params[1] ],
00140                        [ 0, 0, 1, link_params[2] ],
00141                        [ 0, 0, 0, 1 ] ])
00142 
00143     return T_trans * T_roll * T_pitch * T_yaw
00144 


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