33 #include <kdl/tree.hpp> 43 robot_state_ = robot_state;
48 ROS_ERROR(
"Could not convert urdf into kdl tree");
54 res = kdl_tree.
getChain(root, tip, kdl_chain_);
60 ROS_ERROR(
"Could not extract chain between %s and %s from kdl tree",
61 root.c_str(), tip.c_str());
68 for (
size_t i=0; i<kdl_chain_.getNrOfSegments(); i++){
72 ROS_ERROR(
"Joint '%s' is not found in joint state vector", kdl_chain_.getSegment(i).getJoint().getName().c_str());
75 joints_.push_back(jnt);
78 ROS_DEBUG(
"Added %i joints",
int(joints_.size()));
85 positions.resize(joints_.size());
86 for (
unsigned int i = 0; i < joints_.size(); ++i)
88 positions[i] = joints_[i]->position_;
94 velocities.resize(joints_.size());
95 for (
unsigned int i = 0; i < joints_.size(); ++i)
97 velocities[i] = joints_[i]->velocity_;
103 efforts.resize(joints_.size());
104 for (
unsigned int i = 0; i < joints_.size(); ++i)
106 efforts[i] = joints_[i]->measured_effort_;
112 for (
unsigned int i = 0; i < joints_.size(); ++i)
114 if (!joints_[i]->calibrated_)
128 assert(a.
rows() == joints_.size());
129 for (
unsigned int i = 0; i < joints_.size(); ++i)
130 a(i) = joints_[i]->position_;
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_;
145 assert(a.
rows() == joints_.size());
146 for (
unsigned int i = 0; i < joints_.size(); ++i)
147 a(i) = joints_[i]->measured_effort_;
152 assert(a.
rows() == joints_.size());
153 for (
unsigned int i = 0; i < joints_.size(); ++i)
154 joints_[i]->commanded_effort_ = a(i);
159 assert(a.
rows() == joints_.size());
160 for (
unsigned int i = 0; i < joints_.size(); ++i)
161 joints_[i]->commanded_effort_ += a(i);
167 if (actuated_joint_i >= joints_.size())
170 return joints_[actuated_joint_i];
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
void setEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a std vector
void getVelocities(std::vector< double > &)
get the joint velocities of the chain as a std vector
bool allCalibrated()
returns true if all the joints in the chain are calibrated
void getEfforts(std::vector< double > &)
get the measured joint efforts of the chain as a std vector
This class provides the controllers with an interface to the robot state.
Robot * model_
The robot model containing the transmissions, urdf robot model, and hardware interface.
JointState * getJointState(const std::string &name)
Get a joint state by name.
JointState * getJoint(unsigned int actuated_joint_i)
get a joint state of an actuated joint of the chain.
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
urdf::Model robot_model_
The kinematic/dynamic model of the robot.
void toKDL(KDL::Chain &chain)
get a kdl chain object that respresents the chain from root to tip
void addEfforts(KDL::JntArray &)
set the commanded joint efforts of the chain as a kdl jnt array