tree.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, PAL Robotics S.L.
3  * All rights reserved.
4  *
5  * Copyright (c) 2008, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Author: Marcus Liebhardt
9  *
10  * This class has been derived from the chain class in the
11  * pr2_mechanism_model package in the pr2_mechanism stack for ROS
12  * written by Stuart Glaser and Wim Meeussen.
13  *
14  * Redistribution and use in source and binary forms, with or without
15  * modification, are permitted provided that the following conditions are met:
16  *
17  * * Redistributions of source code must retain the above copyright
18  * notice, this list of conditions and the following disclaimer.
19  * * Redistributions in binary form must reproduce the above copyright
20  * notice, this list of conditions and the following disclaimer in the
21  * documentation and/or other materials provided with the distribution.
22  * * Neither the name of the Willow Garage, Inc. nor the names of its
23  * contributors may be used to endorse or promote products derived from
24  * this software without specific prior written permission.
25  *
26  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
27  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
28  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
29  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
30  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
31  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
32  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
33  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
34  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
35  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  */
38 
39 #ifndef PR2_MECHANISM_MODEL_TREE_H
40 #define PR2_MECHANISM_MODEL_TREE_H
41 
42 #include <ros/ros.h>
43 #include <kdl/chain.hpp>
44 #include <kdl/tree.hpp>
45 #include <kdl/jntarray.hpp>
46 #include <kdl/jntarrayvel.hpp>
48 
49 namespace pr2_mechanism_model
50 {
51 
52 class Tree
53 {
54 public:
55  Tree() : kdl_tree_() {};
56  ~Tree() {};
57 
67  bool init(RobotState *robot_state);
68 
70  void getPositions(KDL::JntArray&) const;
71 
73  template <class Vec>
74  void getPositions(Vec&) const;
75 
77  void getVelocities(KDL::JntArrayVel&) const;
78 
80  template <class Vec>
81  void getVelocities(Vec&) const;
82 
84  void getEfforts(KDL::JntArray&) const;
85 
87  template <class Vec>
88  void getEfforts(Vec&) const;
89 
91  void setEfforts(const KDL::JntArray&);
92 
94  template <class Vec>
95  void setEfforts(const Vec&);
96 
98  void addEfforts(const KDL::JntArray&);
99 
101  template <class Vec>
102  void addEfforts(const Vec&);
103 
105  bool allCalibrated() const;
106 
108  void toKdl(KDL::Tree&) const;
109 
111  JointState* getJoint(unsigned int) const;
112 
114  int size() const;
115 
116 private:
117  KDL::Tree kdl_tree_;
119  std::vector<JointState*> joints_;
120 };
121 
122 inline void Tree::getPositions(KDL::JntArray& positions) const
123 {
124  assert(positions.rows() == joints_.size());
125  for (unsigned int i = 0; i < joints_.size(); ++i)
126  positions(i) = joints_[i]->position_;
127 }
128 
129 template <class Vec>
130 inline void Tree::getPositions(Vec &v) const
131 {
132  assert((int)v.size() == (int)joints_.size());
133  for (size_t i = 0; i < joints_.size(); ++i)
134  v[i] = joints_[i]->position_;
135 }
136 
137 inline void Tree::getVelocities(KDL::JntArrayVel& velocities) const
138 {
139  assert(velocities.q.rows() == joints_.size());
140  assert(velocities.qdot.rows() == joints_.size());
141  for (unsigned int i = 0; i < joints_.size(); ++i)
142  {
143  velocities.q(i) = joints_[i]->position_;
144  velocities.qdot(i) = joints_[i]->velocity_;
145  }
146 }
147 
148 template <class Vec>
149 inline void Tree::getVelocities(Vec &v) const
150 {
151  assert((int)v.size() == (int)joints_.size());
152  for (size_t i = 0; i < joints_.size(); ++i)
153  v[i] = joints_[i]->velocity_;
154 }
155 
156 inline void Tree::getEfforts(KDL::JntArray& efforts) const
157 {
158  assert(efforts.rows() == joints_.size());
159  for (unsigned int i = 0; i < joints_.size(); ++i)
160  efforts(i) = joints_[i]->measured_effort_;
161 }
162 
163 template <class Vec>
164 inline void Tree::getEfforts(Vec &v) const
165 {
166  assert((int)v.size() == (int)joints_.size());
167  for (size_t i = 0; i < joints_.size(); ++i)
168  v[i] = joints_[i]->measured_effort_;
169 }
170 
171 inline void Tree::setEfforts(const KDL::JntArray& efforts)
172 {
173  assert(efforts.rows() == joints_.size());
174  for (unsigned int i = 0; i < joints_.size(); ++i)
175  joints_[i]->commanded_effort_ = efforts(i);
176 }
177 
178 template <class Vec>
179 inline void Tree::setEfforts(const Vec& v)
180 {
181  assert((int)v.size() == (int)joints_.size());
182  for (size_t i = 0; i < joints_.size(); ++i)
183  joints_[i]->commanded_effort_ = v[i];
184 }
185 
186 inline void Tree::addEfforts(const KDL::JntArray& efforts)
187 {
188  assert(efforts.rows() == joints_.size());
189  for (unsigned int i = 0; i < joints_.size(); ++i)
190  joints_[i]->commanded_effort_ += efforts(i);
191 }
192 
193 template <class Vec>
194 inline void Tree::addEfforts(const Vec& v)
195 {
196  assert((int)v.size() == (int)joints_.size());
197  for (size_t i = 0; i < joints_.size(); ++i)
198  joints_[i]->commanded_effort_ += v[i];
199 }
200 
201 inline bool Tree::allCalibrated() const
202 {
203  for (unsigned int i = 0; i < joints_.size(); ++i)
204  {
205  if (!joints_[i]->calibrated_)
206  return false;
207  }
208  return true;
209 }
210 
211 inline void Tree::toKdl(KDL::Tree &tree) const
212 {
213  tree = kdl_tree_;
214 }
215 
216 inline JointState* Tree::getJoint(unsigned int actuated_joint_i) const
217 {
218  if (actuated_joint_i >= joints_.size())
219  return NULL;
220  else
221  return joints_[actuated_joint_i];
222 }
223 
224 inline int Tree::size() const
225 {
226  return joints_.size();
227 }
228 
229 } // namespace
230 
231 #endif /* PR2_MECHANISM_MODEL_TREE_H */
pr2_mechanism_model::Tree::getEfforts
void getEfforts(KDL::JntArray &) const
get the measured joint efforts of the tree's joints as a KDL::JntArray
Definition: tree.h:156
pr2_mechanism_model::JointState
Definition: joint.h:103
robot.h
pr2_mechanism_model::Tree::setEfforts
void setEfforts(const KDL::JntArray &)
set the commanded joint efforts of the tree's joints from a KDL::JntArray
Definition: tree.h:171
pr2_mechanism_model
Definition: chain.h:41
ros.h
pr2_mechanism_model::Tree::init
bool init(RobotState *robot_state)
initializes the tree object The initializer's most important functionality is to create a vector of j...
Definition: tree.cpp:45
pr2_mechanism_model::Tree::getPositions
void getPositions(KDL::JntArray &) const
get the position of the joints of the tree as a KDL::JntArray
Definition: tree.h:122
pr2_mechanism_model::Tree::Tree
Tree()
Definition: tree.h:55
pr2_mechanism_model::Tree::toKdl
void toKdl(KDL::Tree &) const
get a KDL::Tree object that respresents the tree
Definition: tree.h:211
pr2_mechanism_model::Tree
Definition: tree.h:52
pr2_mechanism_model::RobotState
This class provides the controllers with an interface to the robot state.
Definition: robot.h:126
pr2_mechanism_model::Tree::joints_
std::vector< JointState * > joints_
a vector of pointers to joint states; includes only the ones that can be actuated (not fixed joints)
Definition: tree.h:119
pr2_mechanism_model::Tree::allCalibrated
bool allCalibrated() const
returns true, if all the joints in the tree are calibrated
Definition: tree.h:201
pr2_mechanism_model::Tree::size
int size() const
returns the number of actuated joints in the tree
Definition: tree.h:224
pr2_mechanism_model::Tree::kdl_tree_
KDL::Tree kdl_tree_
Definition: tree.h:117
pr2_mechanism_model::Tree::getJoint
JointState * getJoint(unsigned int) const
returns a pointer to the joint state of a joint in the list of the tree's actuated joints (index star...
Definition: tree.h:216
pr2_mechanism_model::Tree::~Tree
~Tree()
Definition: tree.h:56
pr2_mechanism_model::Tree::getVelocities
void getVelocities(KDL::JntArrayVel &) const
get the velocities of the joints of the tree as a KDL::JntArrayVel (fills in the positions,...
Definition: tree.h:137
pr2_mechanism_model::Tree::addEfforts
void addEfforts(const KDL::JntArray &)
add to the commanded joint efforts of the tree's joints from a KDL::JntArray
Definition: tree.h:186


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Fri May 6 2022 02:50:08