hrl_cody_arms: cody_arm_kinematics.py Source File

Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 # 
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 # 
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 # Author: Advait Jain
00029 
00030 
00031 import numpy as np, math
00032 #np.set_printoptions(precision=4, linewidth=80)
00033 
00034 import matplotlib.pyplot as pp
00035 
00036 import roslib; roslib.load_manifest('hrl_cody_arms')
00037 import PyKDL as kdl
00038 
00039 from equilibrium_point_control.hrl_arm import HRLArmKinematics
00040 import create_IK_guess_dict as cgd
00041 
00042 import hrl_lib.transforms as tr
00043 import hrl_lib.util as ut
00044 import hrl_lib.kdl_utils as ku
00045 
00046 
00047 class CodyArmKinematics(HRLArmKinematics):
00048     # arm - 'r' or 'l'
00049     def __init__(self, arm):
00050         HRLArmKinematics.__init__(self, n_jts = 7)
00051 
00052         # create joint limit dicts
00053         if arm == 'r':
00054             max_lim = np.radians([ 120.00, 122.15, 77.5, 144., 122.,  45.,  45.])
00055             min_lim = np.radians([ -47.61,  -20., -77.5,   0., -80., -45., -45.])
00056         else:
00057             max_lim = np.radians([ 120.00,   20.,  77.5, 144.,   80.,  45.,  45.])
00058             min_lim = np.radians([ -47.61, -122.15, -77.5,   0., -122., -45., -45.])
00059 
00060         self.joint_lim_dict = {}
00061         self.joint_lim_dict['max'] = max_lim
00062         self.joint_lim_dict['min'] = min_lim
00063 
00064         wrist_stub_length = 0.0135 + 0.04318 # wrist linkange and FT sensor lengths
00065         self.setup_kdl_chains(arm, wrist_stub_length)
00066 
00067         if arm == 'r':
00068             pkl_nm = 'q_guess_right_dict.pkl'
00069         else:
00070             pkl_nm = 'q_guess_left_dict.pkl'
00071           
00072         # Seems to pre-load the seed start values for IK. Set to none for the moment.
00073         pth = roslib.packages.get_pkg_dir('hrl_cody_arms') #roslib.rospack.rospackexec(['find', 'hrl_cody_arms'])
00074         q_guess_pkl = pth + '/src/hrl_cody_arms/'+pkl_nm
00075         self.q_guess_dict = None #ut.load_pickle(q_guess_pkl)
00076 
00077         self.arm_type = 'real' # for epc_skin_math
00078 
00079     #--------------- KDL stuff ----------------
00080     # KDL joint array to meka joint list. (7->7)
00081     def kdl_angles_to_meka(self, q_jnt_arr):
00082         if q_jnt_arr == None:
00083             return None
00084 
00085         q_rad = [0. for i in range(7)]
00086         q_rad[0] = -q_jnt_arr[0]
00087         q_rad[1] = -q_jnt_arr[1]
00088         q_rad[2] = -q_jnt_arr[2]
00089         q_rad[3] = -q_jnt_arr[3]
00090         q_rad[4] = -q_jnt_arr[4]
00091         q_rad[5] = -q_jnt_arr[5]
00092         q_rad[6] = -q_jnt_arr[6]
00093         return q_rad
00094 
00095     # meka joint list to KDL joint array (7->7)
00096     def meka_angles_to_kdl(self, q_list):
00097         if q_list == None:
00098             return None
00099 
00100         n_joints = len(q_list)
00101         q = kdl.JntArray(n_joints)
00102         q[0] = -q_list[0]
00103         q[1] = -q_list[1]
00104         q[2] = -q_list[2]
00105         q[3] = -q_list[3]
00106         q[4] = -q_list[4]
00107         q[5] = -q_list[5]
00108         q[6] = -q_list[6]
00109         return q
00110 
00111     def create_right_chain(self, end_effector_length):
00112         ch = kdl.Chain()
00113         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,-0.18493,0.))))
00114         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,-0.03175,0.))))
00115         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.00635,0.,-0.27795))))
00116         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,-0.27853))))
00117         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.,0.,0.))))
00118         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
00119         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,-end_effector_length))))
00120         return ch
00121 
00122     def create_left_chain(self, end_effector_length):
00123         ch = kdl.Chain()
00124         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.18493,0.))))
00125         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.03175,0.))))
00126         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.00635,0.,-0.27795))))
00127         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,-0.27853))))
00128         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.,0.,0.))))
00129         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
00130         ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,-end_effector_length))))
00131         return ch
00132 
00133     def create_solvers(self, ch):
00134          fk = kdl.ChainFkSolverPos_recursive(ch)
00135          ik_v = kdl.ChainIkSolverVel_pinv(ch)
00136          ik_p = kdl.ChainIkSolverPos_NR(ch, fk, ik_v)
00137          jac = kdl.ChainJntToJacSolver(ch)
00138          return fk, ik_v, ik_p, jac
00139 
00140     def setup_kdl_chains(self, arm, end_effector_length):
00141         if arm == 'r':
00142             #right arm
00143             ch = self.create_right_chain(end_effector_length)
00144             fk, ik_v, ik_p, jac = self.create_solvers(ch)
00145         else:
00146             #left arm
00147             ch = self.create_left_chain(end_effector_length)
00148             fk, ik_v, ik_p, jac = self.create_solvers(ch)
00149 
00150         kdl_chains = {}
00151         kdl_chains['chain'] = ch
00152         kdl_chains['nJnts'] = ch.getNrOfJoints()
00153         kdl_chains['fk_p'] = fk
00154         kdl_chains['ik_v'] = ik_v
00155         kdl_chains['ik_p'] = ik_p
00156         kdl_chains['jacobian_solver'] = jac
00157 
00158         #Add both chains to dictionary
00159         self.kdl_chains = kdl_chains
00160 
00161     def FK_kdl(self, q, link_number):
00162         fk_solver = self.kdl_chains['fk_p']
00163         endeffec_frame = kdl.Frame()
00164         kinematics_status = fk_solver.JntToCart(q, endeffec_frame,
00165                                                 link_number)
00166         if kinematics_status >= 0:
00167 #            print 'End effector transformation matrix:', endeffec_frame
00168             return endeffec_frame
00169         else:
00170             print 'Could not compute forward kinematics.'
00171             return None
00172 
00173     def IK_kdl(self,frame, q_init):
00174         nJnts = self.kdl_chains['nJnts']
00175         ik_solver = self.kdl_chains['ik_p']
00176         q = kdl.JntArray(nJnts)
00177         if ik_solver.CartToJnt(q_init,frame,q)>=0:
00178             for i in range(nJnts):
00179                 q[i] = tr.angle_within_mod180(q[i])
00180             return q
00181         else:
00182             return None
00183 
00184     #-------------- implementation of HRLArmKinematics -----------
00185 
00186     def FK_vanilla(self, q, link_number = 7):
00187         q = self.meka_angles_to_kdl(q)
00188         frame = self.FK_kdl(q, link_number)
00189         pos = frame.p
00190         pos = ku.kdl_vec_to_np(pos)
00191         m = frame.M
00192         rot = ku.kdl_rot_to_np(m)
00193         return pos, rot
00194 
00195 
00196     ## compute Jacobian at point pos. 
00197     # p is in the ground coord frame.
00198     # this is a wrapper to try and not
00199     # break things after changes to PR2
00200     # classes
00201     def jacobian(self, q, pos=None):
00202         return self.Jacobian(q, pos)
00203 
00204     ## compute Jacobian at point pos. 
00205     # p is in the ground coord frame.
00206     def Jacobian(self, q, pos=None):
00207         if pos == None:
00208             pos = self.FK(q)[0]
00209 
00210         ch = self.kdl_chains['chain']
00211         v_list = []
00212         w_list = []
00213 
00214         for i in xrange(self.n_jts):
00215             p, rot = self.FK_vanilla(q, i)
00216             r = pos - p
00217             z_idx = ch.getSegment(i).getJoint().getType() - 1
00218             z = rot[:, z_idx]
00219 
00220             # this is a nasty trick. The rotation matrix returned by
00221             # FK_vanilla describes the orientation of a frame of the
00222             # KDL chain in the base frame. It just so happens that the
00223             # way Advait defined the KDL chain, the axis of rotation
00224             # in KDL is the -ve of the axis of rotation on the real
00225             # robot for every joint.
00226             # Advait apologizes for this.
00227             z = -z
00228 
00229             v_list.append(np.matrix(np.cross(z.A1, r.A1)).T)
00230             w_list.append(z)
00231 
00232         J = np.row_stack((np.column_stack(v_list), np.column_stack(w_list)))
00233         return J
00234 
00235     def IK_vanilla(self, p, rot, q_guess=None):
00236         p_kdl = ku.np_vec_to_kdl(p)
00237         rot_kdl = ku.np_rot_to_kdl(rot)
00238         fr = kdl.Frame(rot_kdl, p_kdl)
00239 
00240         if q_guess == None:
00241             q_guess = cgd.find_good_config(p, self.q_guess_dict)
00242 
00243         q_guess = self.meka_angles_to_kdl(q_guess)
00244         q_res = self.IK_kdl(arm, fr, q_guess)
00245         q_res = self.kdl_angles_to_meka(arm, q_res)
00246         return q_res
00247 
00248     #----------- extra functions -----------------
00249 
00250     ## clamp joint angles to their physical limits.
00251     # @param q - list of 7 joint angles.
00252     # The joint limits for IK are larger that the physical limits.
00253     def clamp_to_joint_limits(self, q, delta_list=[0.,0.,0.,0.,0.,0.,0.]):
00254         min_arr, max_arr = self.get_joint_limits()
00255         q_arr = np.array(q)
00256         d_arr = np.array(delta_list)
00257         return np.clip(q_arr, min_arr-d_arr, max_arr+d_arr)
00258 
00259     def within_joint_limits(self, q, delta_list=[0.,0.,0.,0.,0.,0.,0.]):
00260         min_arr, max_arr = self.get_joint_limits()
00261         q_arr = np.array(q)
00262         d_arr = np.array(delta_list)
00263         return np.all((q_arr <= max_arr+d_arr, q_arr >= min_arr-d_arr))
00264 
00265     def get_joint_limits(self):
00266         return self.joint_lim_dict['min'], self.joint_lim_dict['max']
00267 
00268 
00269     #------------ 2D functions ------------
00270 
00271     # plot the arm using matplotlib.
00272     def plot_arm(self, q, color='b', alpha=1.):
00273         pts = [[0.,0.,0.]]
00274         for i in range(len(q)):
00275             p = self.FK(q, i+1)[0]
00276             pts.append(p.A1.tolist())
00277 
00278         pts_2d = np.array(pts)[:,0:2]
00279         direc_list = (pts_2d[1:] - pts_2d[:-1]).tolist()
00280 
00281         for i, d in enumerate(direc_list):
00282             d_vec = np.matrix(d).T
00283             d_vec = d_vec / np.linalg.norm(d_vec)
00284             w = np.cross(d_vec.A1, np.array([0., 0., 1.])) * 0.03/2
00285             x1 = pts_2d[i,0]
00286             y1 = pts_2d[i,1]
00287             x2 = pts_2d[i+1,0]
00288             y2 = pts_2d[i+1,1]
00289 
00290             x_data = [x1+w[0], x1-w[0], x2-w[0], x2+w[0], x1+w[0]]
00291             y_data = [y1+w[1], y1-w[1], y2-w[1], y2+w[1], y1+w[1]]
00292 
00293             l, = pp.plot(x_data, y_data, color+'-', alpha=alpha)
00294 
00295 
00296 
00297 
00298 
00299