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++){
69 if (kdl_chain_.getSegment(i).getJoint().getType() != KDL::Joint::None){
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];