robot.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 /*
31  * Author: Stuart Glaser, Wim Meeussen
32  */
33 
36 #include <tinyxml.h>
37 #include <urdf/model.h>
40 
41 
42 using namespace pr2_mechanism_model;
43 using namespace pr2_hardware_interface;
44 
45 
47  :hw_(hw)
48 {}
49 
50 
51 bool Robot::initXml(TiXmlElement *root)
52 {
53  // check if current time is valid
54  if (!hw_){
55  ROS_ERROR("Mechanism Model received an invalid hardware interface");
56  return false;
57  }
58 
59  // Parses the xml into a robot model
60  if (!robot_model_.initXml(root)){
61  ROS_ERROR("Mechanism Model failed to parse the URDF xml into a robot model");
62  return false;
63  }
64 
65  // Creates the plugin loader for transmissions.
67  "pr2_mechanism_model", "pr2_mechanism_model::Transmission"));
68 
69  // Constructs the transmissions by parsing custom xml.
70  TiXmlElement *xit = NULL;
71  for (xit = root->FirstChildElement("transmission"); xit;
72  xit = xit->NextSiblingElement("transmission"))
73  {
74  // for the old and new transmission style
75  std::string type;
76  if(!xit->Attribute("type"))
77  {
78  if(!xit->FirstChildElement("type")) continue;
79  type = xit->FirstChildElement("type")->GetText();
80  }
81  else
82  type = xit->Attribute("type");
83  if(type.substr(0, 3) != "pr2") continue;
85  try {
86  // Backwards compatibility for using non-namespaced controller types
87  if (!transmission_loader_->isClassAvailable(type))
88  {
89  std::vector<std::string> classes = transmission_loader_->getDeclaredClasses();
90  for(unsigned int i = 0; i < classes.size(); ++i)
91  {
92  if(type == transmission_loader_->getName(classes[i]))
93  {
94  ROS_WARN("The deprecated transmission type %s was not found. Using the namespaced version %s instead. "
95  "Please update your urdf file to use the namespaced version.",
96  type.c_str(), classes[i].c_str());
97  type = classes[i];
98  break;
99  }
100  }
101  }
102  t = transmission_loader_->createInstance(type);
103  }
104  catch (const std::runtime_error &ex)
105  {
106  ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
107  }
108 
109  if (!t)
110  ROS_ERROR("Unknown transmission type: %s", type.c_str());
111  else if (!t->initXml(xit, this)){
112  ROS_ERROR("Failed to initialize transmission");
113  }
114  else // Success!
115  transmissions_.push_back(t);
116  }
117 
118  return true;
119 }
120 
122 {
123  return hw_->current_time_;
124 }
125 
126 template <class T>
127 int findIndexByName(const std::vector<boost::shared_ptr<T> >& v,
128  const std::string &name)
129 {
130  for (unsigned int i = 0; i < v.size(); ++i)
131  {
132  if (v[i]->name_ == name)
133  return i;
134  }
135  return -1;
136 }
137 
138 int Robot::getTransmissionIndex(const std::string &name) const
139 {
140  return findIndexByName(transmissions_, name);
141 }
142 
143 Actuator* Robot::getActuator(const std::string &name) const
144 {
145  return hw_->getActuator(name);
146 }
147 
149 {
150  int i = getTransmissionIndex(name);
151  return i >= 0 ? transmissions_[i] : boost::shared_ptr<Transmission>();
152 }
153 
154 
155 
156 
157 
159  : model_(model)
160 {
161  assert(model_);
162 
163  transmissions_in_.resize(model->transmissions_.size());
164  transmissions_out_.resize(model->transmissions_.size());
165 
166  // Creates a joint state for each transmission
167  unsigned int js_size = 0;
168  for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
169  {
171  for (unsigned int j = 0; j < t->actuator_names_.size(); ++j)
172  {
173  Actuator *act = model_->getActuator(t->actuator_names_[j]);
174  assert(act != NULL);
175  transmissions_in_[i].push_back(act);
176  }
177  js_size += t->joint_names_.size();
178  }
179 
180  // Wires up the transmissions to the joint state
181  joint_states_.resize(js_size);
182  unsigned int js_id = 0;
183  for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
184  {
186  for (unsigned int j = 0; j < t->joint_names_.size(); ++j)
187  {
188  joint_states_[js_id].joint_ = model_->robot_model_.getJoint(t->joint_names_[j]);
189  joint_states_map_[t->joint_names_[j]] = &(joint_states_[js_id]);
190  transmissions_out_[i].push_back(&(joint_states_[js_id]));
191  js_id++;
192  }
193  }
194 
195  // warnings
196  if (model_->transmissions_.empty())
197  ROS_WARN("No transmissions were specified in the robot description.");
198  if (js_size == 0)
199  ROS_WARN("None of the joints in the robot desription matches up to a motor. The robot is uncontrollable.");
200 }
201 
202 
203 JointState *RobotState::getJointState(const std::string &name)
204 {
205  std::map<std::string, JointState*>::iterator it = joint_states_map_.find(name);
206  if (it == joint_states_map_.end())
207  return NULL;
208  else
209  return it->second;
210 }
211 
212 const JointState *RobotState::getJointState(const std::string &name) const
213 {
214  std::map<std::string, JointState*>::const_iterator it = joint_states_map_.find(name);
215  if (it == joint_states_map_.end())
216  return NULL;
217  else
218  return it->second;
219 }
220 
222 {
223  for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
224  {
225  model_->transmissions_[i]->propagatePosition(transmissions_in_[i],
226  transmissions_out_[i]);
227  }
228 
229  for (unsigned int i = 0; i < joint_states_.size(); i++)
230  {
231  joint_states_[i].joint_statistics_.update(&(joint_states_[i]));
232  }
233 }
234 
236 {
237  for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
238  {
239  model_->transmissions_[i]->propagateEffort(transmissions_out_[i],
240  transmissions_in_[i]);
241  }
242 }
243 
245 {
246  for (unsigned int t = 0; t < transmissions_in_.size(); ++t){
247  for (unsigned int a = 0; a < transmissions_in_[t].size(); a++){
248  if (transmissions_in_[t][a]->state_.halted_)
249  return true;
250  }
251  }
252 
253  return false;
254 }
255 
257 {
258  for (unsigned int i = 0; i < joint_states_.size(); ++i)
259  {
260  joint_states_[i].enforceLimits();
261  }
262 }
263 
265 {
266  for (unsigned int i = 0; i < joint_states_.size(); ++i)
267  joint_states_[i].commanded_effort_ = 0;
268 }
269 
271 {
272  for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
273  {
274  model_->transmissions_[i]->propagatePositionBackwards(transmissions_out_[i],
275  transmissions_in_[i]);
276  }
277 }
278 
280 {
281  for (unsigned int i = 0; i < model_->transmissions_.size(); ++i)
282  {
283  model_->transmissions_[i]->propagateEffortBackwards(transmissions_in_[i],
284  transmissions_out_[i]);
285  }
286 }
287 
288 
pr2_hardware_interface
pr2_mechanism_model::Robot::getTransmissionIndex
int getTransmissionIndex(const std::string &name) const
get the transmission index based on the transmission name. Returns -1 on failure
Definition: robot.cpp:138
pr2_mechanism_model::RobotState::propagateJointEffortToActuatorEffort
void propagateJointEffortToActuatorEffort()
Propagete the joint efforts, through the transmissions, to the actuator efforts.
Definition: robot.cpp:235
pr2_mechanism_model::JointState
Definition: joint.h:103
robot.h
pr2_mechanism_model::Robot::getTime
ros::Time getTime()
Get the time when the current controller cycle was started.
Definition: robot.cpp:121
boost::shared_ptr
pr2_hardware_interface::HardwareInterface::current_time_
ros::Time current_time_
pr2_mechanism_model
Definition: chain.h:41
pr2_mechanism_model::RobotState::RobotState
RobotState(Robot *model)
constructor
Definition: robot.cpp:158
pr2_mechanism_model::Robot::getTransmission
boost::shared_ptr< pr2_mechanism_model::Transmission > getTransmission(const std::string &name) const
get a transmission pointer based on the transmission name. Returns NULL on failure
Definition: robot.cpp:148
pr2_mechanism_model::Robot::getActuator
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
get an actuator pointer based on the actuator name. Returns NULL on failure
Definition: robot.cpp:143
pr2_mechanism_model::RobotState::enforceSafety
void enforceSafety()
Modify the commanded_effort_ of each joint state so that the joint limits are satisfied.
Definition: robot.cpp:256
pr2_mechanism_model::Robot::transmissions_
std::vector< boost::shared_ptr< Transmission > > transmissions_
The list of transmissions.
Definition: robot.h:94
pr2_hardware_interface::HardwareInterface
hardware_interface.h
pr2_mechanism_model::RobotState::propagateActuatorPositionToJointPosition
void propagateActuatorPositionToJointPosition()
Propagete the actuator positions, through the transmissions, to the joint positions.
Definition: robot.cpp:221
pr2_mechanism_model::RobotState::getJointState
JointState * getJointState(const std::string &name)
Get a joint state by name.
Definition: robot.cpp:203
model.h
pr2_mechanism_model::Robot::Robot
Robot(pr2_hardware_interface::HardwareInterface *hw)
Constructor.
Definition: robot.cpp:46
ROS_ERROR
#define ROS_ERROR(...)
ROS_WARN
#define ROS_WARN(...)
pr2_mechanism_model::Robot::robot_model_
urdf::Model robot_model_
The kinematic/dynamic model of the robot.
Definition: robot.h:91
findIndexByName
int findIndexByName(const std::vector< boost::shared_ptr< T > > &v, const std::string &name)
Definition: robot.cpp:127
transmission.h
pr2_mechanism_model::Robot
This class provides the controllers with an interface to the robot model.
Definition: robot.h:78
pluginlib::ClassLoader
Definition: robot.h:62
class_loader.hpp
pr2_mechanism_model::RobotState::joint_states_map_
std::map< std::string, JointState * > joint_states_map_
Definition: robot.h:181
pr2_mechanism_model::Robot::hw_
pr2_hardware_interface::HardwareInterface * hw_
a pointer to the pr2 hardware interface. Only for advanced users
Definition: robot.h:109
pr2_mechanism_model::Robot::initXml
bool initXml(TiXmlElement *root)
Initialize the robot model form xml.
Definition: robot.cpp:51
pr2_mechanism_model::RobotState::joint_states_
std::vector< JointState > joint_states_
The vector of joint states, in no particular order.
Definition: robot.h:136
pr2_mechanism_model::RobotState::propagateJointPositionToActuatorPosition
void propagateJointPositionToActuatorPosition()
Propagete the joint positions, through the transmissions, to the actuator positions.
Definition: robot.cpp:270
ros::Time
pr2_mechanism_model::RobotState::isHalted
bool isHalted()
Checks if one (or more) of the motors are halted.
Definition: robot.cpp:244
pr2_mechanism_model::RobotState::transmissions_out_
std::vector< std::vector< pr2_mechanism_model::JointState * > > transmissions_out_
Definition: robot.h:159
pr2_mechanism_model::RobotState::zeroCommands
void zeroCommands()
Set the commanded_effort_ of each joint state to zero.
Definition: robot.cpp:264
pr2_mechanism_model::RobotState::model_
Robot * model_
The robot model containing the transmissions, urdf robot model, and hardware interface.
Definition: robot.h:133
pr2_mechanism_model::RobotState::propagateActuatorEffortToJointEffort
void propagateActuatorEffortToJointEffort()
Propagete the actuator efforts, through the transmissions, to the joint efforts.
Definition: robot.cpp:279
pr2_mechanism_model::RobotState::transmissions_in_
std::vector< std::vector< pr2_hardware_interface::Actuator * > > transmissions_in_
Definition: robot.h:145
urdf::Model::initXml
URDF_EXPORT bool initXml(const tinyxml2::XMLDocument *xml)
pr2_hardware_interface::Actuator
pr2_hardware_interface::HardwareInterface::getActuator
Actuator * getActuator(const std::string &name) const
pr2_mechanism_model::Robot::transmission_loader_
boost::shared_ptr< pluginlib::ClassLoader< pr2_mechanism_model::Transmission > > transmission_loader_
Definition: robot.h:112


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Mar 6 2023 03:49:17