Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "pr2_mechanism_model/chain.h"
00033 #include <kdl/tree.hpp>
00034 #include <kdl_parser/kdl_parser.hpp>
00035
00036 namespace pr2_mechanism_model {
00037
00038 using namespace std;
00039
00040 bool Chain::init(RobotState *robot_state, const std::string &root, const std::string &tip)
00041 {
00042
00043 robot_state_ = robot_state;
00044
00045
00046 KDL::Tree kdl_tree;
00047 if (!kdl_parser::treeFromUrdfModel(robot_state->model_->robot_model_, kdl_tree)){
00048 ROS_ERROR("Could not convert urdf into kdl tree");
00049 return false;
00050 }
00051
00052 bool res;
00053 try{
00054 res = kdl_tree.getChain(root, tip, kdl_chain_);
00055 }
00056 catch(...){
00057 res = false;
00058 }
00059 if (!res){
00060 ROS_ERROR("Could not extract chain between %s and %s from kdl tree",
00061 root.c_str(), tip.c_str());
00062 return false;
00063 }
00064
00065
00066
00067 joints_.clear();
00068 for (size_t i=0; i<kdl_chain_.getNrOfSegments(); i++){
00069 if (kdl_chain_.getSegment(i).getJoint().getType() != KDL::Joint::None){
00070 JointState* jnt = robot_state->getJointState(kdl_chain_.getSegment(i).getJoint().getName());
00071 if (!jnt){
00072 ROS_ERROR("Joint '%s' is not found in joint state vector", kdl_chain_.getSegment(i).getJoint().getName().c_str());
00073 return false;
00074 }
00075 joints_.push_back(jnt);
00076 }
00077 }
00078 ROS_DEBUG("Added %i joints", int(joints_.size()));
00079
00080 return true;
00081 }
00082
00083 void Chain::getPositions(std::vector<double> &positions)
00084 {
00085 positions.resize(joints_.size());
00086 for (unsigned int i = 0; i < joints_.size(); ++i)
00087 {
00088 positions[i] = joints_[i]->position_;
00089 }
00090 }
00091
00092 void Chain::getVelocities(std::vector<double> &velocities)
00093 {
00094 velocities.resize(joints_.size());
00095 for (unsigned int i = 0; i < joints_.size(); ++i)
00096 {
00097 velocities[i] = joints_[i]->velocity_;
00098 }
00099 }
00100
00101 void Chain::getEfforts(std::vector<double> &efforts)
00102 {
00103 efforts.resize(joints_.size());
00104 for (unsigned int i = 0; i < joints_.size(); ++i)
00105 {
00106 efforts[i] = joints_[i]->measured_effort_;
00107 }
00108 }
00109
00110 bool Chain::allCalibrated()
00111 {
00112 for (unsigned int i = 0; i < joints_.size(); ++i)
00113 {
00114 if (!joints_[i]->calibrated_)
00115 return false;
00116 }
00117 return true;
00118 }
00119
00120 void Chain::toKDL(KDL::Chain &chain)
00121 {
00122 chain = kdl_chain_;
00123 }
00124
00125
00126 void Chain::getPositions(KDL::JntArray& a)
00127 {
00128 assert(a.rows() == joints_.size());
00129 for (unsigned int i = 0; i < joints_.size(); ++i)
00130 a(i) = joints_[i]->position_;
00131 }
00132
00133 void Chain::getVelocities(KDL::JntArrayVel& a)
00134 {
00135 assert(a.q.rows() == joints_.size());
00136 assert(a.qdot.rows() == joints_.size());
00137 for (unsigned int i = 0; i < joints_.size(); ++i){
00138 a.q(i) = joints_[i]->position_;
00139 a.qdot(i) = joints_[i]->velocity_;
00140 }
00141 }
00142
00143 void Chain::getEfforts(KDL::JntArray& a)
00144 {
00145 assert(a.rows() == joints_.size());
00146 for (unsigned int i = 0; i < joints_.size(); ++i)
00147 a(i) = joints_[i]->measured_effort_;
00148 }
00149
00150 void Chain::setEfforts(KDL::JntArray& a)
00151 {
00152 assert(a.rows() == joints_.size());
00153 for (unsigned int i = 0; i < joints_.size(); ++i)
00154 joints_[i]->commanded_effort_ = a(i);
00155 }
00156
00157 void Chain::addEfforts(KDL::JntArray& a)
00158 {
00159 assert(a.rows() == joints_.size());
00160 for (unsigned int i = 0; i < joints_.size(); ++i)
00161 joints_[i]->commanded_effort_ += a(i);
00162 }
00163
00164
00165 JointState *Chain::getJoint(unsigned int actuated_joint_i)
00166 {
00167 if (actuated_joint_i >= joints_.size())
00168 return NULL;
00169 else
00170 return joints_[actuated_joint_i];
00171 }
00172
00173
00174
00175 }