chain.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Stuart Glaser, Wim Meeussen
31 
33 #include <kdl/tree.hpp>
35 
36 namespace pr2_mechanism_model {
37 
38 using namespace std;
39 
40 bool Chain::init(RobotState *robot_state, const std::string &root, const std::string &tip)
41 {
42 
43  robot_state_ = robot_state;
44 
45  // Constructs the kdl chain
46  KDL::Tree kdl_tree;
47  if (!kdl_parser::treeFromUrdfModel(robot_state->model_->robot_model_, kdl_tree)){
48  ROS_ERROR("Could not convert urdf into kdl tree");
49  return false;
50  }
51 
52  bool res;
53  try{
54  res = kdl_tree.getChain(root, tip, kdl_chain_);
55  }
56  catch(...){
57  res = false;
58  }
59  if (!res){
60  ROS_ERROR("Could not extract chain between %s and %s from kdl tree",
61  root.c_str(), tip.c_str());
62  return false;
63  }
64 
65 
66  // Pulls out all the joint indices
67  joints_.clear();
68  for (size_t i=0; i<kdl_chain_.getNrOfSegments(); i++){
69  if (kdl_chain_.getSegment(i).getJoint().getType() != KDL::Joint::None){
70  JointState* jnt = robot_state->getJointState(kdl_chain_.getSegment(i).getJoint().getName());
71  if (!jnt){
72  ROS_ERROR("Joint '%s' is not found in joint state vector", kdl_chain_.getSegment(i).getJoint().getName().c_str());
73  return false;
74  }
75  joints_.push_back(jnt);
76  }
77  }
78  ROS_DEBUG("Added %i joints", int(joints_.size()));
79 
80  return true;
81 }
82 
83 void Chain::getPositions(std::vector<double> &positions)
84 {
85  positions.resize(joints_.size());
86  for (unsigned int i = 0; i < joints_.size(); ++i)
87  {
88  positions[i] = joints_[i]->position_;
89  }
90 }
91 
92 void Chain::getVelocities(std::vector<double> &velocities)
93 {
94  velocities.resize(joints_.size());
95  for (unsigned int i = 0; i < joints_.size(); ++i)
96  {
97  velocities[i] = joints_[i]->velocity_;
98  }
99 }
100 
101 void Chain::getEfforts(std::vector<double> &efforts)
102 {
103  efforts.resize(joints_.size());
104  for (unsigned int i = 0; i < joints_.size(); ++i)
105  {
106  efforts[i] = joints_[i]->measured_effort_;
107  }
108 }
109 
111 {
112  for (unsigned int i = 0; i < joints_.size(); ++i)
113  {
114  if (!joints_[i]->calibrated_)
115  return false;
116  }
117  return true;
118 }
119 
121 {
122  chain = kdl_chain_;
123 }
124 
125 
127 {
128  assert(a.rows() == joints_.size());
129  for (unsigned int i = 0; i < joints_.size(); ++i)
130  a(i) = joints_[i]->position_;
131 }
132 
134 {
135  assert(a.q.rows() == joints_.size());
136  assert(a.qdot.rows() == joints_.size());
137  for (unsigned int i = 0; i < joints_.size(); ++i){
138  a.q(i) = joints_[i]->position_;
139  a.qdot(i) = joints_[i]->velocity_;
140  }
141 }
142 
144 {
145  assert(a.rows() == joints_.size());
146  for (unsigned int i = 0; i < joints_.size(); ++i)
147  a(i) = joints_[i]->measured_effort_;
148 }
149 
151 {
152  assert(a.rows() == joints_.size());
153  for (unsigned int i = 0; i < joints_.size(); ++i)
154  joints_[i]->commanded_effort_ = a(i);
155 }
156 
158 {
159  assert(a.rows() == joints_.size());
160  for (unsigned int i = 0; i < joints_.size(); ++i)
161  joints_[i]->commanded_effort_ += a(i);
162 }
163 
164 
165 JointState *Chain::getJoint(unsigned int actuated_joint_i)
166 {
167  if (actuated_joint_i >= joints_.size())
168  return NULL;
169  else
170  return joints_[actuated_joint_i];
171 }
172 
173 
174 
175 }
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
unsigned int rows() const
void getPositions(std::vector< double > &)
get the joint positions of the chain as a std vector
Definition: chain.cpp:83
void setEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a std vector
Definition: chain.cpp:150
void getVelocities(std::vector< double > &)
get the joint velocities of the chain as a std vector
Definition: chain.cpp:92
bool allCalibrated()
returns true if all the joints in the chain are calibrated
Definition: chain.cpp:110
void getEfforts(std::vector< double > &)
get the measured joint efforts of the chain as a std vector
Definition: chain.cpp:101
This class provides the controllers with an interface to the robot state.
Definition: robot.h:126
Robot * model_
The robot model containing the transmissions, urdf robot model, and hardware interface.
Definition: robot.h:133
JointState * getJointState(const std::string &name)
Get a joint state by name.
Definition: robot.cpp:194
JointState * getJoint(unsigned int actuated_joint_i)
get a joint state of an actuated joint of the chain.
Definition: chain.cpp:165
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
initialize the chain object
Definition: chain.cpp:40
#define ROS_ERROR(...)
urdf::Model robot_model_
The kinematic/dynamic model of the robot.
Definition: robot.h:91
void toKDL(KDL::Chain &chain)
get a kdl chain object that respresents the chain from root to tip
Definition: chain.cpp:120
void addEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a kdl jnt array
Definition: chain.cpp:157
#define ROS_DEBUG(...)


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Jun 10 2019 14:19:04