joint_chain.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2008-2011, 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, pi, array
35 import rospy
36 
37 class JointChain:
38  '''
39  This represents a chain of actuated joints
40  parameters = [gearing*num_of_joints]
41  '''
42  def __init__(self, config):
43  self.root = config['root']
44  self.tip = config['tip']
45  self._joints = config['joints']
46  self._active = config['active_joints']
47  self._M = len(config['active_joints'])
48 
49  rospy.logdebug("Initializing joint chain with [%u] links", self._M)
50 
51  self._transforms = config['transforms']
52  self._axis = numpy.matrix([ eval(str(x)) for x in config['axis']], float)
53  self._cov_dict = config['cov']
54  self._gearing = config['gearing']
55 
56  def calc_free(self, free_config):
57  return [x == 1 for x in free_config['gearing']]
58 
59  def params_to_config(self, param_vec):
60  config_dict = {}
61  config_dict['axis'] = self._axis.tolist()
62  config_dict['gearing'] = (array(param_vec)[:,0]).tolist()
63  config_dict['cov'] = self._cov_dict
64  config_dict['root'] = self.root
65  config_dict['tip'] = self.tip
66  return config_dict
67 
68  # Convert column vector of params into config
69  def inflate(self, param_vec):
70  self._gearing = array(param_vec)[:,0].tolist()
71 
72  # Return column vector of config
73  def deflate(self):
74  return matrix(self._gearing).T
75 
76  # Returns # of params needed for inflation & deflation
77  def get_length(self):
78  return self._M
79 
80  # Returns 4x4 numpy matrix of the pose of the tip of
81  # the specified link num. Assumes the last link's tip
82  # when link_num < 0
83  def fk(self, chain_state, link_num=-1):
84  if link_num < 0:
85  link_num = self._M
86 
87  pos_trimmed = chain_state.position[0:(link_num+1)]
88  gearing_trimmed = self._gearing[0:(link_num+1)]
89 
90  pos_scaled = [cur_pos * cur_gearing for cur_pos, cur_gearing in zip(pos_trimmed, gearing_trimmed)]
91 
92  T = matrix([ [ 1, 0, 0, 0 ],
93  [ 0, 1, 0, 0 ],
94  [ 0, 0, 1, 0 ],
95  [ 0, 0, 0, 1 ] ])
96 
97  m = 0
98  for joint_name in self._joints:
99  if m > link_num:
100  break
101  transform = self._transforms[joint_name]
102  if joint_name in self._active:
103  k = int(self._axis[0,m])
104  p = transform.deflate()
105  p = p.T.tolist()[0]
106  if k > 0:
107  p[k-1] = p[k-1] + pos_scaled[m]
108  else:
109  p[abs(k)-1] = p[abs(k)-1] - pos_scaled[m]
110  T = T * link_T( p )
111  m += 1
112  else:
113  T = T * transform.transform
114 
115  return T
116 
117 def link_T(link_params):
118  r = link_params[3]
119  p = link_params[4]
120  y = link_params[5]
121 
122  T_roll = matrix([ [ 1, 0, 0, 0 ],
123  [ 0, cos(r), -sin(r), 0 ],
124  [ 0, sin(r), cos(r), 0 ],
125  [ 0, 0, 0, 1 ] ])
126 
127  T_pitch = matrix([ [ cos(p), 0, sin(p), 0 ],
128  [ 0, 1, 0, 0 ],
129  [ -sin(p), 0, cos(p), 0 ],
130  [ 0, 0, 0, 1 ] ])
131 
132  T_yaw = matrix([ [ cos(y), -sin(y), 0, 0 ],
133  [ sin(y), cos(y), 0, 0 ],
134  [ 0, 0, 1, 0 ],
135  [ 0, 0, 0, 1 ] ])
136 
137 
138  T_trans = matrix([ [ 1, 0, 0, link_params[0] ],
139  [ 0, 1, 0, link_params[1] ],
140  [ 0, 0, 1, link_params[2] ],
141  [ 0, 0, 0, 1 ] ])
142 
143  return T_trans * T_roll * T_pitch * T_yaw
144 
def fk(self, chain_state, link_num=-1)
Definition: joint_chain.py:83


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:16