full_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 from sensor_msgs.msg import JointState
34 from numpy import matrix
35 import numpy
36 
38  '''
39  Wraps a full set of transforms, including a joint_chain
40 
41  root
42  \
43  fixed links before -- chain -- fixed links after
44  '''
45  def __init__(self, chain_id, tip, root=None):
46  self.chain_id = chain_id
47  self.root = root
48  self.tip = tip
50 
51  def update_config(self, robot_params):
52  if self.root == None:
53  self.root = robot_params.base_link
54  try:
55  chain = robot_params.chains[self.chain_id]
56  before_chain = robot_params.urdf.get_chain(self.root, chain.root, links=False)
57  full_chain = robot_params.urdf.get_chain(chain.root, chain.tip)
58  try:
59  after_chain = robot_params.urdf.get_chain(chain.tip, self.tip, links=False)
60  link_num = -1
61  except:
62  # using only part of the chain, have to calculate link_num
63  tip_chain = robot_params.urdf.get_chain(chain.root,self.tip)
64  new_root = full_chain[0]
65  i = 1
66  link_num = -1
67  while i < len(full_chain):
68  if full_chain[i] in tip_chain:
69  if full_chain[i] in chain._active:
70  link_num += 1
71  new_root = full_chain[i+1]
72  i += 1
73  after_chain = robot_params.urdf.get_chain(new_root, self.tip, links=False)
74  except KeyError:
75  chain = None
76  before_chain = robot_params.urdf.get_chain(self.root, self.tip, links=False)
77  after_chain = []
78  link_num = None
79  before_chain_Ts = [robot_params.transforms[transform_name] for transform_name in before_chain]
80  after_chain_Ts = [robot_params.transforms[transform_name] for transform_name in after_chain]
81  self.calc_block.update_config(before_chain_Ts, chain, link_num, after_chain_Ts)
82 
84  """
85  Build a dictionary that defines which parameters will in fact affect a measurement for a sensor using this chain.
86  """
87  sparsity = dict()
88  sparsity['transforms'] = {}
89  sparsity['chains'] = {}
90  if self.chain_id is not None:
91  for cur_transform in ( self.calc_block._before_chain_Ts + \
92  self.calc_block._chain._transforms.values() + \
93  self.calc_block._after_chain_Ts ):
94  sparsity['transforms'][cur_transform._name] = [1, 1, 1, 1, 1, 1]
95  sparsity['chains'][self.chain_id] = {}
96  link_num = self.calc_block._link_num
97  if link_num < 0:
98  link_num = self.calc_block._chain._M
99  sparsity['chains'][self.chain_id]['gearing'] = [1 for i in range(link_num)]
100  else:
101  for cur_transform in ( self.calc_block._before_chain_Ts + \
102  self.calc_block._after_chain_Ts ):
103  sparsity['transforms'][cur_transform._name] = [1, 1, 1, 1, 1, 1]
104  return sparsity
105 
107  def update_config(self, before_chain_Ts, chain, link_num, after_chain_Ts):
108  self._before_chain_Ts = before_chain_Ts
109  self._chain = chain
110  self._link_num = link_num
111  self._after_chain_Ts = after_chain_Ts
112 
113  def fk(self, chain_state):
114  pose = matrix(numpy.eye(4))
115 
116  # Apply the 'before chain' transforms
117  for before_chain_T in self._before_chain_Ts:
118  pose = pose * before_chain_T.transform
119 
120  # Apply the Chain
121  if self._chain is not None:
122  chain_T = self._chain.fk(chain_state, self._link_num)
123  pose = pose * chain_T
124 
125  # Apply the 'after chain' transforms
126  for after_chain_T in self._after_chain_Ts:
127  pose = pose * after_chain_T.transform
128 
129  return pose
130 
def update_config(self, before_chain_Ts, chain, link_num, after_chain_Ts)
Definition: full_chain.py:107
def __init__(self, chain_id, tip, root=None)
Definition: full_chain.py:45


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