std::string getDOFName(KDL::Tree & tree, const unsigned int index) { std::string retVal = ""; for (KDL::SegmentMap::const_iterator it=tree.getSegments().begin(); it!=tree.getSegments().end(); ++it) { if( GetTreeElementQNr(it->second) == index ) { retVal = GetTreeElementSegment(it->second).getJoint().getName(); } } return retVal; }
std::vector<std::string> getLinkAttachedToFixedJoints(KDL::Tree & tree) { std::vector<std::string> ret; for(KDL::SegmentMap::const_iterator seg=tree.getSegments().begin(); seg != tree.getSegments().end(); seg++) { if( GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None ) { ret.push_back(GetTreeElementSegment(seg->second).getName()); } } return ret; }
bool framesFromKDLTree(const KDL::Tree& tree, std::vector<std::string>& framesNames, std::vector<std::string>& parentLinkNames) { framesNames.clear(); parentLinkNames.clear(); KDL::SegmentMap::iterator seg; KDL::SegmentMap segs; KDL::SegmentMap::const_iterator root_seg; root_seg = tree.getRootSegment(); segs = tree.getSegments(); for( seg = segs.begin(); seg != segs.end(); seg++ ) { if( GetTreeElementChildren(seg->second).size() == 0 && GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None && GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 ) { std::string frameName = GetTreeElementSegment(seg->second).getName(); std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName(); framesNames.push_back(frameName); parentLinkNames.push_back(parentLinkName); //also check parent KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second); if (parent.getJoint().getType() == KDL::Joint::None && parent.getInertia().getMass() == 0.0) { framesNames.push_back(parentLinkName); } } } return true; }
bool leatherman::getSegmentOfJoint(const KDL::Tree &tree, std::string joint, std::string &segment) { KDL::SegmentMap smap = tree.getSegments(); for(std::map<std::string, KDL::TreeElement>::const_iterator iter = smap.begin(); iter != smap.end(); ++iter) { if(iter->second.segment.getJoint().getName().compare(joint) == 0) { segment = iter->second.segment.getName(); return true; } } return false; }
bool kdl_urdf_tools::initialize_kinematics_from_urdf( const std::string &robot_description, const std::string &root_link, const std::string &tip_link, unsigned int &n_dof, KDL::Chain &kdl_chain, KDL::Tree &kdl_tree, urdf::Model &urdf_model) { if(robot_description.length() == 0) { ROS_ERROR("URDF string is empty."); return false; } // Construct an URDF model from the xml string urdf_model.initString(robot_description); // Get a KDL tree from the robot URDF if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){ ROS_ERROR("Failed to construct kdl tree"); return false; } // Populate the KDL chain if(!kdl_tree.getChain(root_link, tip_link, kdl_chain)) { ROS_ERROR_STREAM("Failed to get KDL chain from tree: "); ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link); ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints"); ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments"); ROS_ERROR_STREAM(" The segments are:"); KDL::SegmentMap segment_map = kdl_tree.getSegments(); KDL::SegmentMap::iterator it; for( it=segment_map.begin(); it != segment_map.end(); it++ ) { ROS_ERROR_STREAM( " "<<(*it).first); } return false; } // Store the number of degrees of freedom of the chain n_dof = kdl_chain.getNrOfJoints(); return true; }
bool JacoIKSolver::initFromURDF(const std::string urdf, const std::string root_name, const std::string tip_name, unsigned int max_iter, std::string error) { urdf::Model robot_model; KDL::Tree tree; if (!robot_model.initFile(urdf)) { error += "Could not initialize robot model"; return false; } if (!kdl_parser::treeFromFile(urdf, tree)) { error += "Could not initialize tree object"; return false; } if (tree.getSegment(root_name) == tree.getSegments().end()) { error += "Could not find root link '" + root_name + "'."; return false; } if (tree.getSegment(tip_name) == tree.getSegments().end()) { error += "Could not find tip link '" + tip_name + "'."; return false; } if (!tree.getChain(root_name, tip_name, chain_)) { error += "Could not initialize chain object"; return false; } // Get the joint limits from the robot model q_min_.resize(chain_.getNrOfJoints()); q_max_.resize(chain_.getNrOfJoints()); q_seed_.resize(chain_.getNrOfJoints()); joint_names_.resize(chain_.getNrOfJoints()); unsigned int j = 0; for(unsigned int i = 0; i < chain_.getNrOfSegments(); ++i) { const KDL::Joint& kdl_joint = chain_.getSegment(i).getJoint(); if (kdl_joint.getType() != KDL::Joint::None) { // std::cout << chain_.getSegment(i).getName() << " -> " << kdl_joint.getName() << " -> " << chain_.getSegment(i + 1).getName() << std::endl; boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(kdl_joint.getName()); if (joint && joint->limits) { q_min_(j) = joint->limits->lower; q_max_(j) = joint->limits->upper; q_seed_(j) = (q_min_(j) + q_max_(j)) / 2; } else { q_min_(j) = -1e9; q_max_(j) = 1e9; q_seed_(j) = 0; } joint_names_[j] = kdl_joint.getName(); ++j; } } ; fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_)); ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(chain_)); ik_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(chain_, q_min_, q_max_, *fk_solver_, *ik_vel_solver_, max_iter)); std::cout << "Using normal solver" << std::endl; jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_)); return true; }
bool treeToUrdfModel(const KDL::Tree& tree, const std::string & robot_name, urdf::ModelInterface& robot_model) { robot_model.clear(); robot_model.name_ = robot_name; //Add all links KDL::SegmentMap::iterator seg; KDL::SegmentMap segs; KDL::SegmentMap::const_iterator root_seg; root_seg = tree.getRootSegment(); segs = tree.getSegments(); for( seg = segs.begin(); seg != segs.end(); seg++ ) { if (robot_model.getLink(seg->first)) { std::cerr << "[ERR] link " << seg->first << " is not unique." << std::endl; robot_model.clear(); return false; } else { urdf::LinkPtr link; resetPtr(link, new urdf::Link); //add name link->name = seg->first; //insert link robot_model.links_.insert(make_pair(seg->first,link)); std::cerr << "[DEBUG] successfully added a new link " << link->name << std::endl; } //inserting joint //The fake root segment has no joint to add if( seg->first != root_seg->first ) { KDL::Joint jnt; jnt = GetTreeElementSegment(seg->second).getJoint(); if (robot_model.getJoint(jnt.getName())) { std::cerr << "[ERR] joint " << jnt.getName() << " is not unique." << std::endl; robot_model.clear(); return false; } else { urdf::JointPtr joint; urdf::LinkPtr link = robot_model.links_[seg->first]; //This variable will be set by toUrdf KDL::Frame H_new_old_successor; KDL::Frame H_new_old_predecessor = getH_new_old(GetTreeElementSegment(GetTreeElementParent(seg->second)->second)); urdf::resetPtr(joint, new urdf::Joint()); //convert joint *joint = toUrdf(jnt, GetTreeElementSegment(seg->second).getFrameToTip(),H_new_old_predecessor,H_new_old_successor); //insert parent joint->parent_link_name = GetTreeElementParent(seg->second)->first; //insert child joint->child_link_name = seg->first; //insert joint robot_model.joints_.insert(make_pair(seg->first,joint)); std::cerr << "[DEBUG] successfully added a new joint" << jnt.getName() << std::endl; //add inertial, taking in account an eventual change in the link frame resetPtr(link->inertial, new urdf::Inertial()); *(link->inertial) = toUrdf(H_new_old_successor * GetTreeElementSegment(seg->second).getInertia()); } } } // every link has children links and joints, but no parents, so we create a // local convenience data structure for keeping child->parent relations std::map<std::string, std::string> parent_link_tree; parent_link_tree.clear(); // building tree: name mapping //try //{ robot_model.initTree(parent_link_tree); //} /* catch(ParseError &e) { logError("Failed to build tree: %s", e.what()); robot_model.clear(); return false; }*/ // find the root link //try //{ robot_model.initRoot(parent_link_tree); //} /* catch(ParseError &e) { logError("Failed to find root link: %s", e.what()); robot_model.reset(); return false; } */ return true; }
bool Kinematics::build_chain(FINGERS f, const KDL::Tree &kdl_tree_){ std::string root_name, tip_name; se_finger finger_info; std::string finger_name; try{ finger_info = root_tip.at(f); root_name = finger_info.root_name; tip_name = finger_info.tip_name; }catch(const std::out_of_range& oor){ ROS_ERROR("Kinematics::build_chain no such finger type"); return false; } // joint_limits& joint_limits_ = finger_joint_limits.at(f); KDL::Chain& kdl_chain_ = kdl_chains[f]; finger_name = f_enum2str.at(f); // Populate the KDL chain if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_)) { ROS_ERROR_STREAM("Failed to get KDL chain from tree: "); ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name); ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints"); ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments"); ROS_ERROR_STREAM(" The segments are:"); KDL::SegmentMap segment_map = kdl_tree_.getSegments(); KDL::SegmentMap::iterator it; for( it=segment_map.begin(); it != segment_map.end(); it++ ) ROS_ERROR_STREAM( " "<<(*it).first); return false; } ROS_INFO("=== Loading %s KChain ===",finger_name.c_str()); ROS_INFO("root: %s\t tip: %s",root_name.c_str(),tip_name.c_str()); ROS_INFO("Number of segments: %d",kdl_chain_.getNrOfSegments()); ROS_INFO("Number of joints in chain: %d",kdl_chain_.getNrOfJoints()); // Parsing joint limits from urdf model /* boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name); boost::shared_ptr<const urdf::Joint> joint_; joint_limits_.min.resize(kdl_chain_.getNrOfJoints()); joint_limits_.max.resize(kdl_chain_.getNrOfJoints()); joint_limits_.center.resize(kdl_chain_.getNrOfJoints());*/ /*int index; for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++) { joint_ = model.getJoint(link_->parent_joint->name); index = kdl_chain_.getNrOfJoints() - i - 1; joint_limits_.min(index) = joint_->limits->lower; joint_limits_.max(index) = joint_->limits->upper; joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2; link_ = model.getLink(link_->getParent()->name); } */ /* std::cout<< "segment names" << std::endl; int index; for(std::size_t i = 0; i < kdl_chain_.getNrOfJoints();i++){ // std::cout<< kdl_chain_.segments[i].getName() << " " << kdl_chain_.segments[i].getJoint().getName() << " " << kdl_chain_.segments[i].getJoint().getTypeName() << std::endl; joint_ = model.getJoint(kdl_chain_.segments[i].getJoint().getName()); index = i; switch(joint_->type){ case urdf::Joint::REVOLUTE: { joint_limits_.min(i) = joint_->limits->lower; joint_limits_.max(i) = joint_->limits->upper; joint_limits_.center(i) = (joint_limits_.min(i) + joint_limits_.max(i))/2; ROS_INFO("joint name: %s \t type: %d \t %f %f",joint_->name.c_str(),joint_->type,joint_->limits->lower,joint_->limits->upper); break; } case urdf::Joint::FIXED: { ROS_INFO("joint name: %s \t type: %d",joint_->name.c_str(),joint_->type); break; } default: { std::cerr<< "Kinematics::build_chain no such joint type: " << joint_->type << " implemented" << std::endl; break; } } }*/ }
void drawMyTree(KDL::Tree& a_tree) { //graphviz stuff /****************************************/ Agraph_t *g; GVC_t *gvc; /* set up a graphviz context */ gvc = gvContext(); /* Create a simple digraph */ g = agopen("robot-structure", AGDIGRAPH); //create vector to hold nodes std::vector<Agnode_t*> nodeVector; nodeVector.resize(a_tree.getSegments().size()); // printf("size of segments in tree map %d\n", a_tree.getSegments().size()); // printf("size of segments in tree %d\n", a_tree.getNrOfSegments()); //create vector to hold edges std::vector<Agedge_t*> edgeVector; edgeVector.resize(a_tree.getNrOfJoints() + 1); int jointIndex = a_tree.getNrOfJoints() + 1; // printf("size of joint array %d %d\n", jointIndex, a_tree.getNrOfJoints()); int segmentIndex = 0; // fill in the node vector by iterating over tree segments for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter) { //it would have been very useful if one could access list of joints of a tree //list of segments is already possible int stringLength = iter->second.segment.getName().size(); char name[stringLength + 1]; strcpy(name, iter->second.segment.getName().c_str()); //q_nr returned is the same value for the root and the its child. this is a bug nodeVector[iter->second.q_nr] = agnode(g, name); agsafeset(nodeVector[iter->second.q_nr], "color", "red", ""); agsafeset(nodeVector[iter->second.q_nr], "shape", "box", ""); std::cout << "index parent " << iter->second.q_nr << std::endl; std::cout << "name parent " << iter->second.segment.getName() << std::endl; std::cout << "joint name parent " << iter->second.segment.getJoint().getName() << std::endl; std::cout << "joint type parent " << iter->second.segment.getJoint().getType() << std::endl; // if (iter->second.segment.getJoint().getType() == Joint::None) //equals to joint type None // { // int stringLength = iter->second.segment.getJoint().getName().size(); // char name[stringLength + 1]; // strcpy(name, iter->second.segment.getJoint().getName().c_str()); // edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[iter->second.q_nr]); // agsafeset(edgeVector[iter->second.q_nr], "label", name, ""); // } if (segmentIndex < a_tree.getSegments().size()) segmentIndex++; } //fill in edge vector by iterating over joints in the tree for (SegmentMap::const_iterator iter = a_tree.getSegments().begin(); iter != a_tree.getSegments().end(); ++iter) { //TODO: Fix node-edge connection relation int stringLength = iter->second.segment.getJoint().getName().size(); // std::cout << "Joint name " << iter->second.segment.getJoint().getName() << std::endl; char name[stringLength + 1]; strcpy(name, iter->second.segment.getJoint().getName().c_str()); for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIter = iter->second.children.begin(); childIter != iter->second.children.end(); childIter++) { edgeVector[iter->second.q_nr] = agedge(g, nodeVector[iter->second.q_nr], nodeVector[(*childIter)->second.q_nr]); agsafeset(edgeVector[iter->second.q_nr], "label", name, ""); } // if (jointIndex != 0) // { // edgeVector[jointIndex] = agedge(g, nodeVector[segmentIndex], nodeVector[jointIndex]); // agsafeset(edgeVector[jointIndex], "label", name, ""); // } } /* Compute a layout using layout engine from command line args */ // gvLayoutJobs(gvc, g); gvLayout(gvc, g, "dot"); /* Write the graph according to -T and -o options */ //gvRenderJobs(gvc, g); gvRenderFilename(gvc, g, "ps", "test-invdynamics2.ps"); /* Free layout data */ gvFreeLayout(gvc, g); /* Free graph structures */ agclose(g); gvFreeContext(gvc); /* close output file, free context, and return number of errors */ return; }
bool TreeKinematics::readJoints(urdf::Model &robot_model, KDL::Tree &kdl_tree, std::string &tree_root_name, unsigned int &nr_of_jnts, KDL::JntArray &joint_min, KDL::JntArray &joint_max, KDL::JntArray &joint_vel_max) { KDL::SegmentMap segmentMap; segmentMap = kdl_tree.getSegments(); tree_root_name = kdl_tree.getRootSegment()->second.segment.getName(); nr_of_jnts = kdl_tree.getNrOfJoints(); ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts ); joint_min.resize(nr_of_jnts); joint_max.resize(nr_of_jnts); joint_vel_max.resize(nr_of_jnts); info_.joint_names.resize(nr_of_jnts); info_.limits.resize(nr_of_jnts); // The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts // the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or // urdf::Joint::FIXED ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None."); boost::shared_ptr<const urdf::Joint> joint; for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it) { if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None) { // check, if joint can be found in the URDF model of the robot joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str()); if (!joint) { ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str()); return false; } // extract joint information if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() ); double lower = 0.0, upper = 0.0, vel_limit = 0.0; unsigned int has_pos_limits = 0, has_vel_limits = 0; if ( joint->type != urdf::Joint::CONTINUOUS ) { ROS_DEBUG("joint is not continuous."); lower = joint->limits->lower; upper = joint->limits->upper; has_pos_limits = 1; if (joint->limits->velocity) { has_vel_limits = 1; vel_limit = joint->limits->velocity; ROS_DEBUG("joint has following velocity limit: %f", vel_limit); } else { has_vel_limits = 0; vel_limit = 0.0; ROS_DEBUG("joint has no velocity limit."); } } else { ROS_DEBUG("joint is continuous."); lower = -M_PI; upper = M_PI; has_pos_limits = 0; if(joint->limits && joint->limits->velocity) { has_vel_limits = 1; vel_limit = joint->limits->velocity; ROS_DEBUG("joint has following velocity limit: %f", vel_limit); } else { has_vel_limits = 0; vel_limit = 0.0; ROS_DEBUG("joint has no velocity limit."); } } joint_min(seg_it->second.q_nr) = lower; joint_max(seg_it->second.q_nr) = upper; joint_vel_max(seg_it->second.q_nr) = vel_limit; ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit); info_.joint_names[seg_it->second.q_nr] = joint->name; info_.limits[seg_it->second.q_nr].joint_name = joint->name; info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits; info_.limits[seg_it->second.q_nr].min_position = lower; info_.limits[seg_it->second.q_nr].max_position = upper; info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits; info_.limits[seg_it->second.q_nr].max_velocity = vel_limit; } } } return true; }
void computeTemplatedDynamicsForTree(KDL::Tree& twoBranchTree, KDL::Vector& grav, std::vector<kdle::JointState>& jointState, std::vector<kdle::SegmentState>& linkState, std::vector<kdle::SegmentState>& linkState2) { printf("Templated dynamics values for Tree \n"); kdle::transform<kdle::kdl_tree_iterator, kdle::pose> _comp1; kdle::transform<kdle::kdl_tree_iterator, kdle::twist> _comp2; kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> _comp3; kdle::balance<kdle::kdl_tree_iterator, kdle::force> _comp4; kdle::project<kdle::kdl_tree_iterator, kdle::wrench> _comp5; #ifdef VERBOSE_CHECK_MAIN std::cout << "Transform initial state" << std::endl << linkState[0].X << std::endl; std::cout << "Twist initial state" << std::endl << linkState[0].Xdot << std::endl; std::cout << "Acc Twist initial state" << std::endl << linkState[0].Xdotdot << std::endl; std::cout << "Wrench initial state" << std::endl << linkState[0].F << std::endl << std::endl; #endif #ifdef VERBOSE_CHECK_MAIN std::cout << "Transform L1" << linkState[1].X << std::endl; std::cout << "Twist L1" << linkState[1].Xdot << std::endl; std::cout << "Acc Twist L1" << linkState[1].Xdotdot << std::endl; std::cout << "Wrench L1" << linkState[1].F << std::endl << std::endl; #endif #ifdef VERBOSE_CHECK_MAIN std::cout << "Transform L2" << linkState[2].X << std::endl; std::cout << "Twist L2" << linkState[2].Xdot << std::endl; std::cout << "Acc Twist L2" << linkState[2].Xdotdot << std::endl; std::cout << "Wrench L2" << linkState[2].F << std::endl << std::endl; #endif //typedef Composite<kdle::func_ptr(myTestComputation), kdle::func_ptr(myTestComputation) > compositeType0; typedef kdle::Composite< kdle::transform<kdle::kdl_tree_iterator, kdle::twist>, kdle::transform<kdle::kdl_tree_iterator, kdle::pose> > compositeType1; typedef kdle::Composite< kdle::balance<kdle::kdl_tree_iterator, kdle::force>, kdle::transform<kdle::kdl_tree_iterator, kdle::accTwist> > compositeType2; typedef kdle::Composite<compositeType2, compositeType1> compositeType3; // compositeType1 composite1 = kdle::compose(_comp2, _comp1); compositeType3 composite2 = kdle::compose(kdle::compose(_comp4, _comp3), kdle::compose(_comp2, _comp1)); //kdle::DFSPolicy<KDL::Tree> mypolicy; kdle::DFSPolicy<KDL::Tree, kdle::inward> mypolicy1; kdle::DFSPolicy<KDL::Tree, kdle::outward> mypolicy2; std::cout << std::endl << std::endl << "FORWARD TRAVERSAL" << std::endl << std::endl; // traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, jointState, linkState, linkState2); traverseGraph(twoBranchTree, composite2, mypolicy2)(jointState, linkState, linkState2); #ifdef VERBOSE_CHECK_MAIN std::cout << std::endl << std::endl << "LSTATE" << std::endl << std::endl; for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++) { std::cout << linkState[i].segmentName << std::endl; std::cout << std::endl << linkState[i].X << std::endl; std::cout << linkState[i].Xdot << std::endl; std::cout << linkState[i].Xdotdot << std::endl; std::cout << linkState[i].F << std::endl; } std::cout << std::endl << std::endl << "LSTATE2" << std::endl << std::endl; for (unsigned int i = 0; i < twoBranchTree.getNrOfSegments(); i++) { std::cout << linkState2[i].segmentName << std::endl; std::cout << std::endl << linkState2[i].X << std::endl; std::cout << linkState2[i].Xdot << std::endl; std::cout << linkState2[i].Xdotdot << std::endl; std::cout << linkState2[i].F << std::endl; } #endif std::vector<kdle::SegmentState> linkState3; linkState3.resize(twoBranchTree.getNrOfSegments()+1); std::cout << std::endl << std::endl << "REVERSE TRAVERSAL" << std::endl << std::endl; std::vector<kdle::JointState> jstate1; jstate1.resize(twoBranchTree.getNrOfSegments() + 1); traverseGraph(twoBranchTree, _comp5, mypolicy1)(jointState, jstate1, linkState2, linkState3); //version 1 traversal //traverseGraph(twoBranchTree, kdl_extensions::func_ptr(myTestComputation), mypolicy)(1, 2, 3); #ifdef VERBOSE_CHECK_MAIN std::cout << std::endl << std::endl << "LSTATE3" << std::endl << std::endl; for (KDL::SegmentMap::const_reverse_iterator iter = twoBranchTree.getSegments().rbegin(); iter != twoBranchTree.getSegments().rend(); ++iter) { std::cout << std::endl << iter->first << std::endl; std::cout << linkState3[iter->second.q_nr].X << std::endl; std::cout << linkState3[iter->second.q_nr].Xdot << std::endl; std::cout << linkState3[iter->second.q_nr].Xdotdot << std::endl; std::cout << linkState3[iter->second.q_nr].F << std::endl; std::cout << "Joint index and torque " << iter->second.q_nr << " " << jstate1[iter->second.q_nr].torque << std::endl; } #endif return; }
void IkSolver::init(const KDL::Tree& tree, const std::vector<std::string>& endpoint_names, const std::vector<EndpointCoupling>& endpoint_couplings) { // Preconditions assert(endpoint_names.size() == endpoint_couplings.size() && "endpoints and coupling vectors size mismatch"); // TODO: Verify that all endpoints are contained in tree // Enpoint names vector endpoint_names_ = endpoint_names; // Problem size const size_t q_dim = tree.getNrOfJoints(); // Joint space dimension size_t x_dim = 0; // Task space dimension, value assigned below // Populate coupled directions vector coupled_dirs_.resize(endpoint_names_.size()); for (size_t i = 0; i < coupled_dirs_.size(); ++i) { if ((endpoint_couplings[i] & Position) == Position) { coupled_dirs_[i].push_back(0); coupled_dirs_[i].push_back(1); coupled_dirs_[i].push_back(2); x_dim += 3; } if ((endpoint_couplings[i] & Orientation) == Orientation) { coupled_dirs_[i].push_back(3); coupled_dirs_[i].push_back(4); coupled_dirs_[i].push_back(5); x_dim += 3; } } // Initialize kinematics solvers fk_solver_.reset(new FkSolver(tree)); jac_solver_.reset(new JacSolver(tree)); inverter_.reset(new Inverter(x_dim, q_dim)); // Matrix inversion parameters TODO: Expose! inverter_->setLsInverseThreshold(1e-5); // NOTE: Magic values inverter_->setDlsInverseThreshold(1e-4); inverter_->setMaxDamping(0.05); // Default values of position solver parameters delta_twist_max_ = Eigen::NumTraits<double>::highest(); delta_joint_pos_max_ = Eigen::NumTraits<double>::highest(); velik_gain_ = 1.0; eps_ = Eigen::NumTraits<double>::epsilon(); max_iter_ = 1; // Populate map from joint names to KDL tree indices joint_name_to_idx_.clear(); const KDL::SegmentMap& tree_segments = tree.getSegments(); for (KDL::SegmentMap::const_iterator it = tree_segments.begin(); it != tree_segments.end(); ++it) { const KDL::Joint& joint = it->second.segment.getJoint(); if (joint.getType() != KDL::Joint::None) { joint_name_to_idx_[joint.getName()] = it->second.q_nr; } } // Joint space weights updater limits_avoider_.reset(new JointPositionLimitsAvoider(q_dim)); limits_avoider_->setSmoothing(0.8); // NOTE: Magic value // Preallocate IK resources delta_twist_ = VectorXd::Zero(x_dim); delta_q_ = VectorXd::Zero(q_dim); q_tmp_ = VectorXd::Zero(q_dim); jacobian_ = Eigen::MatrixXd(x_dim, q_dim); jacobian_tmp_ = KDL::Jacobian(q_dim); Wx_ = VectorXd::Ones(x_dim); q_posture_ = KDL::JntArray(q_dim); nullspace_projector_ = Eigen::MatrixXd(q_dim, q_dim); identity_qdim_ = Eigen::MatrixXd::Identity(q_dim, q_dim); }