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
00033
00034
00037 #include <collision_proximity_planner/chomp_robot_model.h>
00038 #include <kdl_parser/kdl_parser.hpp>
00039 #include <ros/ros.h>
00040 #include <cstdio>
00041 #include <iostream>
00042 #include <visualization_msgs/MarkerArray.h>
00043
00044 using namespace std;
00045 using namespace arm_navigation_msgs;
00046
00047 namespace chomp
00048 {
00049
00050 ChompRobotModel::ChompRobotModel():
00051 node_handle_("~")
00052 {
00053 }
00054
00055 ChompRobotModel::~ChompRobotModel()
00056 {
00057 }
00058
00059 bool ChompRobotModel::init(const planning_environment::CollisionModels* models)
00060 {
00061 reference_frame_ = models->getRobotFrameId();
00062
00063
00064 string urdf_string;
00065 if (!node_handle_.getParam(models->getDescription(), urdf_string))
00066 {
00067 return false;
00068 }
00069
00070
00071 double joint_update_limit;
00072 node_handle_.param("joint_update_limit", joint_update_limit, 0.05);
00073
00074
00075 if (!kdl_parser::treeFromString(urdf_string, kdl_tree_))
00076 {
00077 ROS_ERROR("Failed to construct KDL tree from URDF.");
00078 return false;
00079 }
00080 num_kdl_joints_ = kdl_tree_.getNrOfJoints();
00081
00082
00083
00084 KDL::SegmentMap segment_map = kdl_tree_.getSegments();
00085
00086 for (KDL::SegmentMap::const_iterator it = segment_map.begin(); it != segment_map.end(); ++it)
00087 {
00088 if (it->second.segment.getJoint().getType() != KDL::Joint::None)
00089 {
00090 std::string joint_name = it->second.segment.getJoint().getName();
00091 std::string segment_name = it->first;
00092 joint_segment_mapping_.insert(make_pair(joint_name, segment_name));
00093 }
00094 }
00095
00096
00097 fk_solver_ = new KDL::TreeFkSolverJointPosAxis(kdl_tree_, reference_frame_);
00098
00099 kdl_number_to_urdf_name_.resize(num_kdl_joints_);
00100
00101
00102 for (map<string, string>::iterator it = joint_segment_mapping_.begin(); it!= joint_segment_mapping_.end(); ++it)
00103 {
00104 std::string joint_name = it->first;
00105 std::string segment_name = it->second;
00106
00107 segment_joint_mapping_.insert(make_pair(segment_name, joint_name));
00108 int kdl_number = kdl_tree_.getSegment(segment_name)->second.q_nr;
00109 if (kdl_tree_.getSegment(segment_name)->second.segment.getJoint().getType() != KDL::Joint::None)
00110 {
00111
00112 kdl_number_to_urdf_name_[kdl_number] = joint_name;
00113 urdf_name_to_kdl_number_.insert(make_pair(joint_name, kdl_number));
00114 }
00115 }
00116
00117
00118 const std::map<std::string, planning_models::KinematicModel::JointModelGroup*> groups = models->getKinematicModel()->getJointModelGroupMap();
00119 for(std::map<std::string, planning_models::KinematicModel::JointModelGroup*>::const_iterator it = groups.begin();
00120 it != groups.end();
00121 it++) {
00122 ChompPlanningGroup group;
00123 ROS_INFO_STREAM("Making group for " << it->first);
00124 group.name_ = it->first;
00125 int num_links = it->second->getGroupLinkModels().size();
00126 group.num_joints_ = 0;
00127 group.link_names_.resize(num_links);
00128 std::vector<bool> active_joints;
00129 active_joints.resize(num_kdl_joints_, false);
00130 for (unsigned int i=0; i < it->second->getJointModelNames().size(); i++)
00131 {
00132 std::string joint_name = it->second->getJointModelNames()[i];
00133 map<string, string>::iterator link_name_it = joint_segment_mapping_.find(joint_name);
00134 if (link_name_it == joint_segment_mapping_.end())
00135 {
00136 ROS_WARN("Joint name %s did not have containing KDL segment.", joint_name.c_str());
00137 continue;
00138 }
00139 std::string link_name = link_name_it->second;
00140 group.link_names_[i] = link_name;
00141 const KDL::Segment* segment = &(kdl_tree_.getSegment(link_name)->second.segment);
00142 KDL::Joint::JointType joint_type = segment->getJoint().getType();
00143 if (joint_type != KDL::Joint::None)
00144 {
00145 ChompJoint joint;
00146 joint.chomp_joint_index_ = group.num_joints_;
00147 joint.kdl_joint_index_ = kdl_tree_.getSegment(link_name)->second.q_nr;
00148 joint.kdl_joint_ = &(segment->getJoint());
00149 joint.link_name_ = link_name;
00150 joint.joint_name_ = segment_joint_mapping_[link_name];
00151 joint.joint_update_limit_ = joint_update_limit;
00152 const planning_models::KinematicModel::JointModel* kin_model_joint = models->getKinematicModel()->getJointModel(joint.joint_name_);
00153 if (const planning_models::KinematicModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(kin_model_joint))
00154 {
00155 joint.wrap_around_ = revolute_joint->continuous_;
00156 joint.has_joint_limits_ = !(joint.wrap_around_);
00157 std::pair<double,double> bounds;
00158 revolute_joint->getVariableBounds(revolute_joint->getName(), bounds);
00159 joint.joint_limit_min_ = bounds.first;
00160 joint.joint_limit_max_ = bounds.second;
00161 ROS_DEBUG_STREAM("Setting bounds for joint " << revolute_joint->getName() << " to " << joint.joint_limit_min_ << " " << joint.joint_limit_max_);
00162 }
00163 else if (const planning_models::KinematicModel::PrismaticJointModel* prismatic_joint = dynamic_cast<const planning_models::KinematicModel::PrismaticJointModel*>(kin_model_joint))
00164 {
00165 joint.wrap_around_ = false;
00166 joint.has_joint_limits_ = true;
00167 std::pair<double,double> bounds;
00168 prismatic_joint->getVariableBounds(prismatic_joint->getName(), bounds);
00169 joint.joint_limit_min_ = bounds.first;
00170 joint.joint_limit_max_ = bounds.second;
00171 ROS_DEBUG_STREAM("Setting bounds for joint " << prismatic_joint->getName() << " to " << joint.joint_limit_min_ << " " << joint.joint_limit_max_);
00172 }
00173 else
00174 {
00175 ROS_WARN("CHOMP cannot handle floating or planar joints yet.");
00176 }
00177
00178 group.num_joints_++;
00179 group.chomp_joints_.push_back(joint);
00180 active_joints[joint.kdl_joint_index_] = true;
00181 }
00182
00183 }
00184 group.fk_solver_.reset(new KDL::TreeFkSolverJointPosAxisPartial(kdl_tree_, reference_frame_, active_joints));
00185 planning_groups_.insert(make_pair(it->first, group));
00186 }
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214 ROS_INFO("Initialized chomp robot model in %s reference frame.", reference_frame_.c_str());
00215
00216 return true;
00217 }
00218
00219 void ChompRobotModel::getLinkInformation(const std::string link_name, std::vector<int>& active_joints, int& segment_number)
00220 {
00221 ROS_DEBUG_STREAM("Link info for " << link_name);
00222
00223
00224 active_joints.clear();
00225 KDL::SegmentMap::const_iterator segment_iter = kdl_tree_.getSegment(link_name);
00226
00227
00228 while (segment_iter != kdl_tree_.getRootSegment())
00229 {
00230 KDL::Joint::JointType joint_type = segment_iter->second.segment.getJoint().getType();
00231 if (joint_type != KDL::Joint::None)
00232 {
00233 active_joints.push_back(segment_iter->second.q_nr);
00234 ROS_DEBUG_STREAM("Adding parent " << segment_iter->second.segment.getJoint().getName());
00235 }
00236 segment_iter = segment_iter->second.parent;
00237 }
00238 ROS_DEBUG(" ");
00239
00240 segment_number = fk_solver_->segmentNameToIndex(link_name);
00241
00242 }
00243
00244 void ChompRobotModel::getActiveJointInformation(const std::string &link_name,
00245 std::vector<int>& active_joints,
00246 int& segment_number)
00247 {
00248 KDL::SegmentMap::const_iterator segment_iter = kdl_tree_.getSegment(link_name);
00249
00250 while (segment_iter != kdl_tree_.getRootSegment())
00251 {
00252 KDL::Joint::JointType joint_type = segment_iter->second.segment.getJoint().getType();
00253 if (joint_type != KDL::Joint::None)
00254 {
00255 active_joints.push_back(segment_iter->second.q_nr);
00256 ROS_DEBUG_STREAM("Adding parent " << segment_iter->second.segment.getJoint().getName());
00257 }
00258 segment_iter = segment_iter->second.parent;
00259 }
00260 ROS_DEBUG(" ");
00261 segment_number = fk_solver_->segmentNameToIndex(link_name);
00262 }
00263
00264 }