Go to the documentation of this file.
55 ROS_ERROR(
"Mechanism Model received an invalid hardware interface");
61 ROS_ERROR(
"Mechanism Model failed to parse the URDF xml into a robot model");
67 "pr2_mechanism_model",
"pr2_mechanism_model::Transmission"));
70 TiXmlElement *xit = NULL;
71 for (xit = root->FirstChildElement(
"transmission"); xit;
72 xit = xit->NextSiblingElement(
"transmission"))
76 if(!xit->Attribute(
"type"))
78 if(!xit->FirstChildElement(
"type"))
continue;
79 type = xit->FirstChildElement(
"type")->GetText();
82 type = xit->Attribute(
"type");
83 if(type.substr(0, 3) !=
"pr2")
continue;
90 for(
unsigned int i = 0; i < classes.size(); ++i)
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());
104 catch (
const std::runtime_error &ex)
106 ROS_ERROR(
"Could not load class %s: %s", type.c_str(), ex.what());
110 ROS_ERROR(
"Unknown transmission type: %s", type.c_str());
111 else if (!t->initXml(xit,
this)){
112 ROS_ERROR(
"Failed to initialize transmission");
128 const std::string &name)
130 for (
unsigned int i = 0; i < v.size(); ++i)
132 if (v[i]->name_ == name)
167 unsigned int js_size = 0;
171 for (
unsigned int j = 0; j < t->actuator_names_.size(); ++j)
177 js_size += t->joint_names_.size();
182 unsigned int js_id = 0;
186 for (
unsigned int j = 0; j < t->joint_names_.size(); ++j)
197 ROS_WARN(
"No transmissions were specified in the robot description.");
199 ROS_WARN(
"None of the joints in the robot desription matches up to a motor. The robot is uncontrollable.");
214 std::map<std::string, JointState*>::const_iterator it =
joint_states_map_.find(name);
int getTransmissionIndex(const std::string &name) const
get the transmission index based on the transmission name. Returns -1 on failure
void propagateJointEffortToActuatorEffort()
Propagete the joint efforts, through the transmissions, to the actuator efforts.
ros::Time getTime()
Get the time when the current controller cycle was started.
RobotState(Robot *model)
constructor
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
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
get an actuator pointer based on the actuator name. Returns NULL on failure
void enforceSafety()
Modify the commanded_effort_ of each joint state so that the joint limits are satisfied.
std::vector< boost::shared_ptr< Transmission > > transmissions_
The list of transmissions.
void propagateActuatorPositionToJointPosition()
Propagete the actuator positions, through the transmissions, to the joint positions.
JointState * getJointState(const std::string &name)
Get a joint state by name.
Robot(pr2_hardware_interface::HardwareInterface *hw)
Constructor.
urdf::Model robot_model_
The kinematic/dynamic model of the robot.
int findIndexByName(const std::vector< boost::shared_ptr< T > > &v, const std::string &name)
This class provides the controllers with an interface to the robot model.
std::map< std::string, JointState * > joint_states_map_
pr2_hardware_interface::HardwareInterface * hw_
a pointer to the pr2 hardware interface. Only for advanced users
bool initXml(TiXmlElement *root)
Initialize the robot model form xml.
std::vector< JointState > joint_states_
The vector of joint states, in no particular order.
void propagateJointPositionToActuatorPosition()
Propagete the joint positions, through the transmissions, to the actuator positions.
bool isHalted()
Checks if one (or more) of the motors are halted.
std::vector< std::vector< pr2_mechanism_model::JointState * > > transmissions_out_
void zeroCommands()
Set the commanded_effort_ of each joint state to zero.
Robot * model_
The robot model containing the transmissions, urdf robot model, and hardware interface.
void propagateActuatorEffortToJointEffort()
Propagete the actuator efforts, through the transmissions, to the joint efforts.
std::vector< std::vector< pr2_hardware_interface::Actuator * > > transmissions_in_
URDF_EXPORT bool initXml(const tinyxml2::XMLDocument *xml)
Actuator * getActuator(const std::string &name) const
boost::shared_ptr< pluginlib::ClassLoader< pr2_mechanism_model::Transmission > > transmission_loader_
pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Mar 6 2023 03:49:17