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


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