Beispiel #1
46
void testJacobian(std::string group_name, std::string base_link, std::string tip_link)
{
 SCOPED_TRACE(group_name + ": " + base_link + " to " + tip_link);
 
 srand ( time(NULL) ); // initialize random seed:
 rdf_loader::RDFLoader model_loader("robot_description");
 robot_model::RobotModelPtr kinematic_model;
 kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));

 robot_state::RobotStatePtr kinematic_state;
 kinematic_state.reset(new robot_state::RobotState(kinematic_model));
 kinematic_state->setToDefaultValues();

 const moveit::core::JointModelGroup* joint_state_group = kinematic_state->getRobotModel()->getJointModelGroup(group_name);

 std::string link_name = tip_link;
 std::vector<double> joint_angles(7,0.0);
 geometry_msgs::Point ref_position;
 Eigen::MatrixXd jacobian;
 Eigen::Vector3d point(0.0,0.0,0.0);
 kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
 ASSERT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name),point,jacobian));

 KDL::Tree tree;
 if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
 {
   ROS_ERROR("Could not initialize tree object");
 }
 KDL::Chain kdl_chain;
 std::string base_frame(base_link);
 std::string tip_frame(tip_link);
 if (!tree.getChain(base_frame, tip_frame, kdl_chain))
 {
   ROS_ERROR("Could not initialize chain object");
 }
 KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
 KDL::Jacobian jacobian_kdl(7);
 KDL::JntArray q_in(7);
 EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);

 unsigned int NUM_TESTS = 10000;
 for(unsigned int i=0; i < NUM_TESTS; i++)
 {
   for(int j=0; j < 7; j++)
   {
     q_in(j) = gen_rand(-M_PI,M_PI);
     joint_angles[j] = q_in(j);
   }
   EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
   kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
   EXPECT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name), point, jacobian));
   for(unsigned int k=0; k < 6; k++)
   {
     for(unsigned int m=0; m < 7; m++)
     {
       EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
     }
   }
 }
}
Beispiel #2
0
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;
}
TEST(JacobianSolver, solver)
{
 srand ( time(NULL) ); // initialize random seed: 
 rdf_loader::RDFLoader model_loader("robot_description");
 robot_model::RobotModelPtr kinematic_model;
 kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));

 robot_state::RobotStatePtr kinematic_state;
 kinematic_state.reset(new robot_state::RobotState(kinematic_model));
 kinematic_state->setToDefaultValues();

 robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm");
 
 std::string link_name = "r_wrist_roll_link";
 std::vector<double> joint_angles(7,0.0); 
 geometry_msgs::Point ref_position;
 Eigen::MatrixXd jacobian;
 Eigen::Vector3d point(0.0,0.0,0.0);
 joint_state_group->setVariableValues(joint_angles);
 ASSERT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));

 KDL::Tree tree;
 if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree)) 
 {
   ROS_ERROR("Could not initialize tree object");
 }
 KDL::Chain kdl_chain;
 std::string base_frame("torso_lift_link");
 std::string tip_frame("r_wrist_roll_link");
 if (!tree.getChain(base_frame, tip_frame, kdl_chain)) 
 {
   ROS_ERROR("Could not initialize chain object");
 }
 KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
 KDL::Jacobian jacobian_kdl(7);
 KDL::JntArray q_in(7);
 EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);

 unsigned int NUM_TESTS = 1000000;
 for(unsigned int i=0; i < NUM_TESTS; i++)
 {
   for(int j=0; j < 7; j++)
   {
     q_in(j) = gen_rand(-M_PI,M_PI);
     joint_angles[j] = q_in(j);
   }
   EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
   joint_state_group->setVariableValues(joint_angles);
   EXPECT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
   for(unsigned int k=0; k < 6; k++)
   {
     for(unsigned int m=0; m < 7; m++)
     {
       EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
     }
   }
 }
}
osg::ref_ptr<osg::Node> RobotModel::makeOsg( boost::shared_ptr<urdf::ModelInterface> urdf_model ){
    //Parse also to KDL
    KDL::Tree tree;
    kdl_parser::treeFromUrdfModel(*urdf_model, tree);

    //
    // Here we perform a full traversal throu the kinematic tree
    // hereby we go depth first
    //
    boost::shared_ptr<const urdf::Link> urdf_link; //Temp Storage for current urdf link
    KDL::Segment kdl_segment; //Temp Storage for urrent KDL link (same as URDF, but already parsed to KDL)
    osg::ref_ptr<osg::Node> hook = 0; //Node (from previous segment) to hook up next segment to

    std::vector<boost::shared_ptr<const urdf::Link> > link_buffer; //Buffer for links we still need to visit
    //used after LIFO principle
    std::vector<osg::ref_ptr<osg::Node> > hook_buffer;                  //Same as above but for hook. The top most
    //element here corresponds to the hook of the
    //previous depth level in the tree.
    link_buffer.push_back(urdf_model->getRoot()); //Initialize buffers with root
    hook_buffer.push_back(original_root_);
    original_root_name_ = urdf_model->getRoot()->name;
    while(!link_buffer.empty()){
        //get current node in buffer
        urdf_link = link_buffer.back();
        link_buffer.pop_back();

        //FIXME: This is hacky solution to prevent from links being added twice. There should be a better one
        if(std::find (segmentNames_.begin(), segmentNames_.end(), urdf_link->name) != segmentNames_.end())
            continue;

        //expand node
        link_buffer.reserve(link_buffer.size() + std::distance(urdf_link->child_links.begin(), urdf_link->child_links.end()));
        link_buffer.insert(link_buffer.end(), urdf_link->child_links.begin(), urdf_link->child_links.end());

        //create osg link
        hook = hook_buffer.back();
        hook_buffer.pop_back();
        kdl_segment = tree.getSegment(urdf_link->name)->second.segment;
        osg::ref_ptr<osg::Node> new_hook = makeOsg2(kdl_segment,
                                       *urdf_link, hook->asGroup());

        //Also store names of links and joints
        segmentNames_.push_back(kdl_segment.getName());
        if(kdl_segment.getJoint().getType() != KDL::Joint::None)
            jointNames_.push_back(kdl_segment.getJoint().getName());

        //Append hooks
        for(uint i=0; i<urdf_link->child_links.size(); i++)
            hook_buffer.push_back(new_hook);
    }
    relocateRoot(urdf_model->getRoot()->name);

    
    return root_;
}
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;
}
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;
}
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;
}
Beispiel #8
0
bool leatherman::getChainTip(const KDL::Tree &tree, const std::vector<std::string> &segments, std::string chain_root, std::string &chain_tip)
{
  KDL::Chain chain;

  // compute # of links each link would include if tip of chain
  for(size_t i = 0; i < segments.size(); ++i)
  {
    // create chain with link i as the tip
    if (!tree.getChain(chain_root, segments[i], chain))
    {
      ROS_ERROR("Failed to fetch the KDL chain. This code only supports a set of segments that live within a single kinematic chain. (root: %s, tip: %s)", chain_root.c_str(), segments[i].c_str());
      return false;
    }

    int index;
    size_t num_segments_included = 0;
    for(size_t j = 0; j < segments.size(); ++j)
    {
      if(leatherman::getSegmentIndex(chain, segments[j], index))
        num_segments_included++;
    }
    
    if(num_segments_included == segments.size())
    { 
      chain_tip = segments[i];
      return true;
    }
  }
  return false;
}
 bool getKDLChain(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
 {
   // create robot chain from root to tip
   KDL::Tree tree;
   if (!kdl_parser::treeFromString(xml_string, tree))
   {
     ROS_ERROR("Could not initialize tree object");
     return false;
   }
   if (!tree.getChain(root_name, tip_name, kdl_chain))
   {
     ROS_ERROR("Could not initialize chain object");
     return false;
   }
   return true;
 }
bool getKDLChain(const urdf::ModelInterface& model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
{
  // create robot chain from root to tip
  KDL::Tree tree;
  if (!kdl_parser::treeFromUrdfModel(model, tree))
  {
    ROS_ERROR("Could not initialize tree object");
    return false;
  }
  if (!tree.getChain(root_name, tip_name, kdl_chain))
  {
    ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
    return false;
  }
  return true;
}
void computeRNEDynamicsForChain(KDL::Tree& a_tree, const std::string& rootLink, const std::string& tipLink, KDL::Vector& grav,
                                std::vector<kdle::JointState>& jointState, std::vector<kdle::SegmentState>& linkState)
{
    KDL::Chain achain;
    a_tree.getChain(rootLink, tipLink, achain);

    KDL::JntArray q(achain.getNrOfJoints());
    KDL::JntArray q_dot(achain.getNrOfJoints());
    KDL::JntArray q_dotdot(achain.getNrOfJoints());
    JntArray torques(achain.getNrOfJoints());
    KDL::Wrenches f_ext;
    f_ext.resize(achain.getNrOfSegments());

    std::cout << endl << endl;
    printf("RNE dynamics values \n");

    KDL::ChainIdSolver_RNE *rneDynamics = new ChainIdSolver_RNE(achain, -grav);
    
    for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
    {
        q(i) = jointState[i].q;
        q_dot(i) = jointState[i].qdot;
        q_dotdot(i) = jointState[i].qdotdot;
        printf("q, qdot %f, %f\n", q(i), q_dot(i));
    }

    
    rneDynamics->CartToJnt(q, q_dot, q_dotdot, f_ext, torques);

    for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
    {
        printf("index, q, qdot, torques %d, %f, %f, %f\n", i, q(i), q_dot(i), torques(i));
    }
    return;
}
Beispiel #12
0
int main(int argc, char **argv)
{
	ros::init(argc, argv, "cob_ik_solver");
	ros::NodeHandle n;
	ros::NodeHandle node;
	std::string robot_desc_string;
	node.param("/robot_description", robot_desc_string, string());
	if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
	      ROS_ERROR("Failed to construct kdl tree");
      	      return false;
	}
	my_tree.getChain("base_link","arm_7_link", chain);


	ros::ServiceServer get_ik_service = n.advertiseService("get_ik", ik_solve);
	ros::ServiceServer get_constraint_aware_ik_service = n.advertiseService("get_constraint_aware_ik", constraint_aware_ik_solve);
	ros::ServiceServer get_ik_solver_info_service = n.advertiseService("get_ik_solver_info", getIKSolverInfo);
	ros::ServiceServer get_fk_service = n.advertiseService("get_fk", fk_solve);
	ros::ServiceServer get_fk_tcp_service = n.advertiseService("get_fk_tcp", fk_solve_TCP);
	ros::ServiceServer get_fk_all_service = n.advertiseService("get_fk_all", fk_solve_all);
	ros::ServiceServer get_fk_solver_info_service = n.advertiseService("get_fk_solver_info", getFKSolverInfo);


	ROS_INFO("IK Server Running.");
	ros::spin();

	return 0;
}
 static inline YAML::Node extract(const std::string& start_link, const std::string& end_link, const KDL::Tree& robot_tree)
 {
     KDL::Chain chain;
     if (!robot_tree.getChain(start_link, end_link, chain))
     {
         throw InvalidChain(start_link, end_link);
     }
     return extract(start_link, end_link, chain);
 }
Beispiel #14
0
KDL::Chain ArmKinematics::readChainFromUrdf(const urdf::ModelInterface& robot_model,
    const std::string &root_name, const std::string &tip_name)
{
  KDL::Tree tree;
  KDL::Chain chain;

  if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
    cout << "Could not parse robot model into a kdl_tree.\n";
    throw;
  }

  if (!tree.getChain(root_name, tip_name, chain)) {
    cout << "Could not initialize chain object\n";
    throw;
  }

  return chain; 
}
 bool isBaseLinkFake(const KDL::Tree & tree)
 {
     KDL::SegmentMap::const_iterator root = tree.getRootSegment();
     bool return_value;
     if (GetTreeElementChildren(root->second).size() == 1 && GetTreeElementSegment(GetTreeElementChildren(root->second)[0]->second).getJoint().getType() == Joint::None) {
         return_value = true;
     } else {
         return_value = false;
     }
     return return_value;
 }
Beispiel #16
0
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
    KDL::Chain chain;
    tree.getChain(root_name, tip_name, chain);
    solver =  new KDL::ChainFkSolverPos_recursive(chain);
    unsigned int  num = chain.getNrOfSegments();
    for(unsigned int i = 0; i < num; ++i){
	const KDL::Joint &joint = chain.getSegment(i).getJoint();
	if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
    }
    ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
/* Method to load all the values from the parameter server
 *  @returns true is successful
 */
bool Kinematics::loadModel(const std::string xml) {
  urdf::Model robot_model;
  KDL::Tree tree;
  if (!robot_model.initString(xml)) {
    ROS_FATAL("Could not initialize robot model");
    return -1;
  }
  if (!kdl_parser::treeFromString(xml, tree)) {
    ROS_ERROR("Could not initialize tree object");
    return false;
  }
  if (!tree.getChain(root_name, tip_name, chain)) {
    ROS_ERROR("Could not initialize chain object for root_name %s and tip_name %s",root_name.c_str(), tip_name.c_str());
    return false;
  }
  if (!readJoints(robot_model)) {
    ROS_FATAL("Could not read information about the joints");
    return false;
  }

  return true;
}
Beispiel #18
0
bool BodyKinematics::loadModel(const std::string xml){
	//Construct tree with kdl_parser
	KDL::Tree tree;

	if (!kdl_parser::treeFromString(xml, tree)) {
		ROS_ERROR("Could not initialize tree object");
		return false;
	}
	ROS_INFO("Construct tree");

	//Get coxa and leg_center frames via segments (for calculating vectors)
	std::map<std::string,KDL::TreeElement>::const_iterator segments_iter;
	std::string link_name_result;
	for (int i=0; i<num_legs; i++){
		link_name_result = root_name + suffixes[i];
		segments_iter = tree.getSegment(link_name_result);
		frames.push_back((*segments_iter).second.segment.getFrameToTip());
	}
	for (int i=0; i<num_legs; i++){
		link_name_result = tip_name + suffixes[i];
		segments_iter = tree.getSegment(link_name_result);
		frames.push_back((*segments_iter).second.segment.getFrameToTip());
	}
	ROS_INFO("Get frames");

	//Vector iterators
	for (int i=0; i<num_legs; i++){
		frames[i] = frames[i] * frames[i+num_legs];
	}
	frames.resize(num_legs);

	for (int i=0; i<num_legs; i++){
		for (int j = 0; j < num_joints; j++) {
			legs.joints_state[i].joint[j] = 0;
		}
	}

	return true;
}
Beispiel #19
0
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 LegKinematics::loadModel(const std::string xml) {
    KDL::Tree tree;
    KDL::Chain chain;
    std::string tip_name_result;

    if (!kdl_parser::treeFromString(xml, tree)) {
        ROS_ERROR("Could not initialize tree object");
        return false;
    }
    ROS_INFO("Construct tree");

    for (int i=0; i<num_legs; i++){
    	tip_name_result = tip_name + suffixes[i];
		if (!tree.getChain(root_name, tip_name_result, chain)) {
			ROS_ERROR("Could not initialize chain_%s object", suffixes[i].c_str());
			return false;
		}
		chains_ptr[i] = new KDL::Chain(chain);
    }
    ROS_INFO("Construct chains");

    return true;
}
bool ExcavaROBArmKinematics::loadModel(const std::string xml) {
    urdf::Model robot_model;
    KDL::Tree tree;

    if (!robot_model.initString(xml)) {
	ROS_FATAL("Could not initialize robot model");
	return false;
    }
    if (!kdl_parser::treeFromString(xml, tree)) {
        ROS_ERROR("Could not initialize tree object");
        return false;
    }
    if (!tree.getChain(root_name_, tip_name_, arm_chain_)) {
        ROS_ERROR("Could not initialize chain object");
        return false;
    }
    if (!readJoints(robot_model, tip_name_, root_name_, &num_joints_arm_)) {
        ROS_FATAL("Could not read information about the joints");
        return false;
    }

    return true;
}
TorqueEstimationTree::TorqueEstimationTree(KDL::Tree& icub_kdl,
                                          std::vector< kdl_format_io::FTSensorData > ft_sensors,
                                          std::vector< std::string > ft_serialization,
                                          std::string fixed_link, unsigned int verbose)
{
    yarp::sig::Vector q_max(icub_kdl.getNrOfJoints(),1000.0);
    yarp::sig::Vector q_min(icub_kdl.getNrOfJoints(),-1000.0);

    KDL::CoDyCo::TreeSerialization serial(icub_kdl);

    std::vector<std::string> dof_serialization;

    for(int i = 0; i < serial.getNrOfDOFs(); i++ )
    {
        dof_serialization.push_back(serial.getDOFName(i));
    }

    TorqueEstimationConstructor(icub_kdl,ft_sensors,dof_serialization,ft_serialization,q_min,q_max,fixed_link,verbose);
}
void LaserJointProjector::configure(const KDL::Tree& tree,
                                    const std::string& root, const std::string& tip)
{
  bool success;
  success = tree.getChain(root, tip, chain_);
  if (!success)
    ROS_ERROR("Error extracting chain from [%s] to [%s]\n", root.c_str(), tip.c_str());

  KDL::Segment angle_segment("laser_angle_segment",
                             KDL::Joint("laser_angle_joint", KDL::Joint::RotZ));
  KDL::Segment range_segment("laser_range_segment",
                             KDL::Joint("laser_range_joint", KDL::Joint::TransX));

  chain_.addSegment(angle_segment);
  chain_.addSegment(range_segment);

  for (unsigned int i=0; i < chain_.segments.size(); i++)
  {
    printf("%2u) %s -> %s\n", i, chain_.segments[i].getName().c_str(),
                                  chain_.segments[i].getJoint().getName().c_str());
  }

  solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
}
Beispiel #24
0
int main(int argc, char **argv)
{
  ros::init(argc, argv, "cartesian");
  ros::NodeHandle n;
  ros::NodeHandle n_private("~");

  string robot_desc;
  n.getParam("robot_description", robot_desc);
  KDL::Tree tree;
  if (!kdl_parser::treeFromString(robot_desc, tree))
  {
    ROS_ERROR("failed to extract kdl tree from xml robot description");
    return 1;
  }
  if (!tree.getNrOfSegments())
  {
    ROS_ERROR("empty tree. sad.");
    return 1;
  }
  KDL::Chain chain;
  if (!tree.getChain("torso_link", "tool_link", chain))
  {
    ROS_ERROR("couldn't pull arm chain from robot model");
    return 1;
  }
  ROS_INFO("parsed tree successfully");
  ///////////////////////////////////////////////////////////////////////

  //ros::Subscriber target_sub = n.subscribe("ik_request", 1, ik_request_cb);
  ros::Subscriber joint_sub = n.subscribe("joint_states", 1, joint_cb);
  ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("target_joints", 1);
  ros::ServiceServer ik_param_srv = n.advertiseService("arm_ik_params", 
                                                       ik_params_cb);
  g_joint_pub = &joint_pub; // ugly ugly
  tf::TransformBroadcaster tf_broadcaster;
  tf::TransformListener tf_listener;
  g_tf_broadcaster = &tf_broadcaster;
  g_tf_listener = &tf_listener;
  ros::Rate loop_rate(30);
  geometry_msgs::TransformStamped world_trans;
  world_trans.header.frame_id = "world";
  world_trans.child_frame_id = "torso_link";
  world_trans.transform.translation.x = 0;
  world_trans.transform.translation.y = 0;
  world_trans.transform.translation.z = 0;
  world_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0,0,0);

  g_js.name.resize(7);
  g_js.position.resize(7);
  g_js.name[0] = "shoulder1";
  g_js.name[1] = "shoulder2";
  g_js.name[2] = "shoulder3";
  g_js.name[3] = "elbow";
  g_js.name[4] = "wrist1";
  g_js.name[5] = "wrist2";
  g_js.name[6] = "wrist3";
  for (int i = 0; i < 7; i++)
    g_js.position[i] = 0;

  KDL::TreeFkSolverPosFull_recursive fk_solver(tree);
  g_fk_solver = &fk_solver;
  KDL::SegmentMap::const_iterator root_seg = tree.getRootSegment();
  string tree_root_name = root_seg->first;
  printf("root: %s\n", tree_root_name.c_str());
  KDL::ChainFkSolverPos_recursive fk_solver_chain(chain);
  KDL::ChainIkSolverVel_pinv ik_solver_vel(chain);
  KDL::JntArray q_min(7), q_max(7);
  for (int i = 0; i < 7; i++)
  {
    q_min.data[i] = g_joint_min[i];
    q_max.data[i] = g_joint_max[i];
  }
  KDL::ChainIkSolverPos_NR_JL ik_solver_pos(chain, q_min, q_max,
                                            fk_solver_chain, ik_solver_vel, 
                                            100, 1e-6);
  //KDL::ChainIkSolverPos_NR ik_solver_pos(chain, fk_solver_chain, ik_solver_vel, 100, 1e-6);
  g_ik_solver = &ik_solver_pos;

  KDL::ChainJntToJacSolver jac_solver(chain);
  g_jac_solver = &jac_solver;

  //boost::scoped_ptr<KDL::TreeFkSolverPosFull_recursive> fk_solver;

  g_pose.resize(7);
  g_pose[0] = .3;
  g_pose[1] = -.3;
  g_pose[2] = -.6;
  g_pose[3] = 0;
  g_pose[4] = 0;
  g_pose[5] = 0;
  g_pose[6] = 0;
  //tf::Transform t_tool = fk_tool(g_pose);
  //double d_pose = 0, d_pose_inc = 0.01;

  // set origin to be something we can comfortably reach
  //g_target_origin = fk_tool(g_pose);
  //btQuaternion target_quat;
  //target_quat.setEuler(1.57, -1.57, 0);
  //g_target_origin.setRotation(target_quat);
  //g_target_origin = btTransform(btQuaternion::getIdentity(), btVector3(0, 0.1, 0));
  //tf::Transform(btQuaternion::getIdentity(), btVector3(0, 0, 0));

  std::vector<double> j_ik;
  j_ik.resize(7);
  while (ros::ok())
  {
    loop_rate.sleep();
    ros::spinOnce();

    world_trans.header.stamp = ros::Time::now();
    tf_broadcaster.sendTransform(world_trans);

    if (g_actual_js.position.size() >= 7)
    {
      for (int i = 0; i < 7; i++)
        j_ik[i] = g_actual_js.position[i];
    }

    tf::StampedTransform t;
    try
    {
      g_tf_listener->lookupTransform("torso_link", "ik_target",
                                     ros::Time(0), t);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s", ex.what());
      continue;
    }
    if (ik_tool(t, j_ik, g_posture, g_posture_gain))
    {
      g_js.header.stamp = ros::Time::now();
      g_js.position = j_ik;
      g_joint_pub->publish(g_js);
    }
    ros::spinOnce();

/*
    double x = t.getOrigin().x(), y = t.getOrigin().y(), z = t.getOrigin().z();
    double roll, pitch, yaw;
    btMatrix3x3(t.getRotation()).getRPY(roll, pitch, yaw);
    printf("%.3f %.3f %.3f   %.3f %.3f %.3f\n", x, y, z, roll, pitch, yaw);
*/
    //std::vector<double> j_ik = pose;
    //js.position[2] += 1;
    //j_ik = pose;js.position;
    //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
    //       j_ik[0], j_ik[1], j_ik[2], j_ik[3], j_ik[4], j_ik[5], j_ik[6]);
    //j_ik[2] += posture;
    //j_ik[3] += posture;
    //j_ik[4] += posture;
#if 0
    tf::Transform t_bump(btQuaternion::getIdentity(),
                         btVector3(d_pose, 0, 0));
    //tf::Transform t_target = t_tool * t_bump;

    ik_tool(t_tool * t_bump, j_ik);
#endif


    //double x = t.getOrigin().x(), y = t.getOrigin().y(), z = t.getOrigin().z();
    /*
    printf("%.3f   %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
           posture,
           j_ik[0], j_ik[1], j_ik[2], j_ik[3], j_ik[4], j_ik[5], j_ik[6]);
    printf("\n");
    */
/*
    js.header.stamp = ros::Time::now();
    js.position = j_ik;
    joint_pub.publish(js);

    d_pose += d_pose_inc;
    if (d_pose > 0.28)
      d_pose_inc = -0.005;
    else if (d_pose < -0.30)
      d_pose_inc = 0.005;
    printf("%f %f\n", d_pose, d_pose_inc);
*/
  }
  return 0;
}
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;
}
void testFwdKinConsistency(std::string modelFilePath)
{
    std::cout << "Testing " << modelFilePath << std::endl;


    // Load iDynTree model and compute a default traversal
    ModelLoader loader;
    loader.loadModelFromFile(modelFilePath);
    iDynTree::Model model = loader.model();
    iDynTree::Traversal traversal;
    model.computeFullTreeTraversal(traversal);

    // Load KDL tree
    KDL::Tree tree;
    treeFromUrdfFile(modelFilePath,tree);
    KDL::CoDyCo::UndirectedTree undirected_tree(tree);
    KDL::CoDyCo::Traversal kdl_traversal;
    undirected_tree.compute_traversal(kdl_traversal);

    // A KDL::Tree only supports 0 and 1 DOFs joints, and
    // KDL::Tree::getNrOfJoints does not count the 0 dof joints, so
    // it is actually equivalent to iDynTree::Model::getNrOfDOFs
    ASSERT_EQUAL_DOUBLE(tree.getNrOfJoints(),model.getNrOfDOFs());

    // Input : joint positions and base position with respect to world
    iDynTree::FreeFloatingPos baseJntPos(model);
    iDynTree::FreeFloatingVel baseJntVel(model);
    iDynTree::FreeFloatingAcc baseJntAcc(model);

    baseJntPos.worldBasePos() =  getRandomTransform();
    baseJntVel.baseVel() = getRandomTwist();
    baseJntAcc.baseAcc() =  getRandomTwist();

    for(unsigned int jnt=0; jnt < baseJntPos.getNrOfPosCoords(); jnt++)
    {
        baseJntPos.jointPos()(jnt) = getRandomDouble();
        baseJntVel.jointVel()(jnt) = getRandomDouble();
        baseJntAcc.jointAcc()(jnt) = getRandomDouble();
    }


    // Build a map between KDL joints and iDynTree joints because we are not sure
    // that the joint indices will match
    std::vector<int> kdl2idyntree_joints;
    kdl2idyntree_joints.resize(undirected_tree.getNrOfDOFs());

    for(unsigned int dofIndex=0; dofIndex < undirected_tree.getNrOfDOFs(); dofIndex++)
    {
        std::string dofName = undirected_tree.getJunction(dofIndex)->getName();
        int idyntreeJointIndex = model.getJointIndex(dofName);
        kdl2idyntree_joints[dofIndex] = idyntreeJointIndex;
    }


    KDL::JntArray jntKDL(undirected_tree.getNrOfDOFs());
    KDL::JntArray jntVelKDL(undirected_tree.getNrOfDOFs());
    KDL::JntArray jntAccKDL(undirected_tree.getNrOfDOFs());

    KDL::Frame  worldBaseKDL;
    KDL::Twist  baseVelKDL;
    KDL::Twist  baseAccKDL;

    ToKDL(baseJntPos,worldBaseKDL,jntKDL,kdl2idyntree_joints);
    ToKDL(baseJntVel,baseVelKDL,jntVelKDL,kdl2idyntree_joints);
    ToKDL(baseJntAcc,baseAccKDL,jntAccKDL,kdl2idyntree_joints);

    // Output : link (for iDynTree) or frame positions with respect to world
    std::vector<KDL::Frame> framePositions(undirected_tree.getNrOfLinks());

    iDynTree::LinkPositions linkPositions(model);


    // Build a map between KDL links and iDynTree links because we are not sure
    // that the link indices will match
    std::vector<int> idynTree2KDL_links;
    idynTree2KDL_links.resize(model.getNrOfLinks());

    for(unsigned int linkIndex=0; linkIndex < model.getNrOfLinks(); linkIndex++)
    {
        std::string linkName = model.getLinkName(linkIndex);
        int kdlLinkIndex = undirected_tree.getLink(linkName)->getLinkIndex();
        idynTree2KDL_links[linkIndex] = kdlLinkIndex;
    }

    // Compute position forward kinematics with old KDL code and with the new iDynTree code
    KDL::CoDyCo::getFramesLoop(undirected_tree,
                               jntKDL,
                               kdl_traversal,
                               framePositions,
                               worldBaseKDL);

    ForwardPositionKinematics(model,traversal,baseJntPos,linkPositions);

    // Check results
    for(unsigned int travEl = 0; travEl  < traversal.getNrOfVisitedLinks(); travEl++)
    {
        LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
        ASSERT_EQUAL_TRANSFORM_TOL(linkPositions(linkIndex),ToiDynTree(framePositions[idynTree2KDL_links[linkIndex]]),1e-1);
    }

    // Compution velocity & acceleration kinematics
    std::vector<KDL::Twist> kdlLinkVel(undirected_tree.getNrOfLinks());
    std::vector<KDL::Twist> kdlLinkAcc(undirected_tree.getNrOfLinks());
    KDL::CoDyCo::rneaKinematicLoop(undirected_tree,jntKDL,jntVelKDL,jntAccKDL,kdl_traversal,
                                                   baseVelKDL,baseAccKDL,kdlLinkVel,kdlLinkAcc);

    iDynTree::LinkVelArray linksVels(model);
    iDynTree::LinkAccArray linksAcc(model);

    ForwardVelAccKinematics(model,traversal,baseJntPos,baseJntVel,baseJntAcc,linksVels,linksAcc);

    // Check results
    for(unsigned int travEl = 0; travEl  < traversal.getNrOfVisitedLinks(); travEl++)
    {
        LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();

        ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
                            ToiDynTree(kdlLinkVel[idynTree2KDL_links[linkIndex]]).asVector());
        ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
                            ToiDynTree(kdlLinkAcc[idynTree2KDL_links[linkIndex]]).asVector());
    }

    // Compute position, velocity and acceleration
    LinkPositions linksPos_check(model);
    iDynTree::LinkVelArray linksVels_check(model);
    iDynTree::LinkAccArray linksAcc_check(model);

    ForwardPosVelAccKinematics(model,traversal,baseJntPos, baseJntVel, baseJntAcc,
                               linksPos_check,linksVels_check,linksAcc_check);

    for(unsigned int travEl = 0; travEl  < traversal.getNrOfVisitedLinks(); travEl++)
    {
        LinkIndex linkIndex = traversal.getLink(travEl)->getIndex();
        ASSERT_EQUAL_TRANSFORM(linkPositions(linkIndex),
                               linksPos_check(linkIndex));
        ASSERT_EQUAL_VECTOR(linksVels(linkIndex).asVector(),
                            linksVels_check(linkIndex).asVector());
        ASSERT_EQUAL_VECTOR(linksAcc(linkIndex).asVector(),
                             linksAcc(linkIndex).asVector());
    }

    // Compute torques (i.e. inverse dynamics)
    LinkNetExternalWrenches fExt(model);
    LinkInternalWrenches f(model);
    FreeFloatingGeneralizedTorques baseWrenchJointTorques(model);
    KDL::JntArray jntTorques(model.getNrOfDOFs());
    KDL::Wrench   baseWrench;

    std::vector<KDL::Wrench> fExtKDL(undirected_tree.getNrOfLinks());
    std::vector<KDL::Wrench> fKDL(undirected_tree.getNrOfLinks());

    // First set to zero all the fExtKDL, fKDL wrenches : then
    // the following loop will set the one that correspond to "real"
    // iDynTree links to the same value we used in iDynTree
    for(unsigned int linkKDL = 0; linkKDL < undirected_tree.getNrOfLinks(); linkKDL++)
    {
        fKDL[linkKDL]    = KDL::Wrench::Zero();
        fExtKDL[linkKDL] = KDL::Wrench::Zero();
    }

    for(unsigned int link = 0; link < model.getNrOfLinks(); link++ )
    {
        fExt(link) = getRandomWrench();
        fExtKDL[idynTree2KDL_links[link]] = ToKDL(fExt(link));
        fKDL[idynTree2KDL_links[link]] = KDL::Wrench::Zero();
    }

    bool ok = RNEADynamicPhase(model,traversal,
                               baseJntPos.jointPos(),
                               linksVels,linksAcc,fExt,f,baseWrenchJointTorques);

    int retVal = KDL::CoDyCo::rneaDynamicLoop(undirected_tree,jntKDL,kdl_traversal,
                                 kdlLinkVel,kdlLinkAcc,
                                 fExtKDL,fKDL,jntTorques,baseWrench);

    ASSERT_EQUAL_DOUBLE(ok,true);
    ASSERT_EQUAL_DOUBLE(retVal,0);

    /***
     * This check is commented out because KDL::CoDyCo::rneaDynamicLoop returned
     * a reverse baseWrench.. still need to be investigated.
     * (The - is necessary for consistency with the mass matrix..)
     *
     */
    //ASSERT_EQUAL_VECTOR(baseWrenchJointTorques.baseWrench().asVector(),
    //                    ToiDynTree(baseWrench).asVector());

    for(int link = 0; link < model.getNrOfLinks(); link++ )
    {
        ASSERT_EQUAL_VECTOR(f(link).asVector(),ToiDynTree(fKDL[idynTree2KDL_links[link]]).asVector());
    }

    for(int dof = 0; dof < model.getNrOfDOFs(); dof++ )
    {
        ASSERT_EQUAL_DOUBLE(jntTorques(dof),baseWrenchJointTorques.jointTorques()(kdl2idyntree_joints[dof]));
    }

    // Check mass matrix
    iDynTree::FreeFloatingMassMatrix massMatrix(model);
    iDynTree::LinkInertias crbis(model);
    ok = CompositeRigidBodyAlgorithm(model,traversal,
                                     baseJntPos.jointPos(),
                                     crbis,massMatrix);

    KDL::CoDyCo::FloatingJntSpaceInertiaMatrix massMatrixKDL(undirected_tree.getNrOfDOFs()+6);
    std::vector<KDL::RigidBodyInertia> Ic(undirected_tree.getNrOfLinks());

    retVal = KDL::CoDyCo::crba_floating_base_loop(undirected_tree,kdl_traversal,jntKDL,Ic,massMatrixKDL);

    ASSERT_IS_TRUE(ok);
    ASSERT_EQUAL_DOUBLE_TOL(retVal,0,1e-8);

    // Check composite rigid body inertias
    for(int link = 0; link < model.getNrOfLinks(); link++ )
    {
         ASSERT_EQUAL_MATRIX(crbis(link).asMatrix(),ToiDynTree(Ic[idynTree2KDL_links[link]]).asMatrix());
    }

    std::cerr << "iDynTree " << massMatrix.toString() << std::endl;
    std::cerr << "massMatrix " << massMatrixKDL.data << std::endl;


    // Check CRBA algorithm
    for(int ii=0; ii < model.getNrOfDOFs()+6; ii++ )
    {
        for( int jj=0; jj < model.getNrOfDOFs()+6; jj++ )
        {
            int idyntreeII = kdl2idyntreeFloatingDOFMapping(ii,kdl2idyntree_joints);
            int idyntreeJJ = kdl2idyntreeFloatingDOFMapping(jj,kdl2idyntree_joints);
            ASSERT_EQUAL_DOUBLE_TOL(massMatrix(idyntreeII,idyntreeJJ),massMatrixKDL(ii,jj),1e-8);
        }
    }

    return;
}
  SNS_IK::SNS_IK(const std::string& base_link, const std::string& tip_link,
                 const std::string& URDF_param, double loopPeriod, double eps,
                 sns_ik::VelocitySolveType type) :
    m_initialized(false),
    m_eps(eps),
    m_loopPeriod(loopPeriod),
    m_nullspaceGain(1.0),
    m_solvetype(type)
  {
    ros::NodeHandle node_handle("~");
    urdf::Model robot_model;
    std::string xml_string;
    std::string urdf_xml, full_urdf_xml;
    node_handle.param("urdf_param",urdf_xml,URDF_param);
    node_handle.searchParam(urdf_xml,full_urdf_xml);

    ROS_DEBUG_NAMED("sns_ik","Reading xml file from parameter server");
    if (!node_handle.getParam(full_urdf_xml, xml_string)) {
      ROS_FATAL_NAMED("sns_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
      return;
    }

    node_handle.param(full_urdf_xml, xml_string, std::string());
    robot_model.initString(xml_string);

    ROS_DEBUG_STREAM_NAMED("sns_ik","Reading joints and links from URDF");
    KDL::Tree tree;
    if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
      ROS_FATAL("Failed to extract kdl tree from xml robot description.");
      return;
    }

    if(!tree.getChain(base_link, tip_link, m_chain)) {
      ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
    }

    std::vector<KDL::Segment> chain_segments = m_chain.segments;
    boost::shared_ptr<const urdf::Joint> joint;
    m_lower_bounds.resize(m_chain.getNrOfJoints());
    m_upper_bounds.resize(m_chain.getNrOfJoints());
    m_velocity.resize(m_chain.getNrOfJoints());
    m_acceleration.resize(m_chain.getNrOfJoints());
    m_jointNames.resize(m_chain.getNrOfJoints());

    unsigned int joint_num=0;
    for(std::size_t i = 0; i < chain_segments.size(); ++i) {
      joint = robot_model.getJoint(chain_segments[i].getJoint().getName());
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
        double lower=0; //TODO Better default values? Error if these arent found?
        double upper=0;
        double velocity=0;
        double acceleration=0;

        if ( joint->type == urdf::Joint::CONTINUOUS ) {
            lower=std::numeric_limits<float>::lowest();
            upper=std::numeric_limits<float>::max();
        } else {
          if(joint->safety) {
            lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
            upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
          } else {
            lower = joint->limits->lower;
            upper = joint->limits->upper;
          }
          velocity = std::fabs(joint->limits->velocity);
        }
        // Checking the Param server for limit modifications
        // and acceleration limits
        std::string prefix = urdf_xml + "_planning/joint_limits/" + joint->name + "/";
        double ul;
        if(node_handle.getParam(prefix + "max_position", ul)){
          upper = std::min(upper, ul);
        }
        double ll;
        if(node_handle.getParam(prefix + "min_position", ll)){
          lower = std::max(lower, ll);
        }
        double vel;
        if(node_handle.getParam(prefix + "max_velocity", vel)){
          if (velocity > 0)
            velocity = std::min(velocity, std::fabs(vel));
          else
            velocity = std::fabs(vel);
        }
        node_handle.getParam(prefix + "max_acceleration", acceleration);

        m_lower_bounds(joint_num)=lower;
        m_upper_bounds(joint_num)=upper;
        m_velocity(joint_num) = velocity;
        m_acceleration(joint_num) = std::fabs(acceleration);
        m_jointNames[joint_num] = joint->name;

        ROS_INFO("sns_ik: Using joint %s lb: %.3f, ub: %.3f, v: %.3f, a: %.3f", joint->name.c_str(),
                 m_lower_bounds(joint_num), m_upper_bounds(joint_num), m_velocity(joint_num), m_acceleration(joint_num));
        joint_num++;
      }
    }
    initialize();
  }
Beispiel #28
0
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;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "leg_kinematics");
    KDL::Tree tree;
    KDL::Chain chain;
    ros::NodeHandle node;
    KDL::ChainFkSolverPos_recursive* fk_solver;
    KDL::HP_ChainIkSolverPos_NR_JL *ik_solver_pos;
    KDL::ChainIkSolverVel_pinv* ik_solver_vel;

    std::string robot_desc_string;
    node.param("robot_description", robot_desc_string, std::string("robot_description"));
    if (!kdl_parser::treeFromString(robot_desc_string, tree)) {
        ROS_ERROR("Failed to construct kdl tree");
        return false;
    }

    if (!tree.getChain("base_link", "tibia_foot_r1", chain)) {
        ROS_ERROR("Failed to construct kdl chain");
        return false;
    }
    ROS_INFO("Construct kdl chain");

//		unsigned int nj = chain.getNrOfJoints();
//		unsigned int js = chain.getNrOfSegments();
//		std_msgs::String msg;
//		std::stringstream ss;
//		ss << "# J: " << nj << "  # S: " << js;
//		msg.data = ss.str();
//		ROS_INFO("Construct kdl tree");
//		ROS_INFO("%s", msg.data.c_str()) ;

    fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
    ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
//	Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);
//	matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;
//	ik_solver_vel -> setWeightTS(matrix_Mx);
    KDL::JntArray joint_min(chain.getNrOfJoints());
    KDL::JntArray joint_max(chain.getNrOfJoints());
    joint_min(0) = -1.57;
    joint_min(1) = -1.57;
    joint_min(2) = -1.57;
    joint_max (0) = 1.57;
    joint_max (1) = 1.57;
    joint_max (2) = 1.57;
    ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max,
            *fk_solver, *ik_solver_vel, 20, 0.0001);

    KDL::JntArray q_init(chain.getNrOfJoints());
    q_init (0) = 0;
    q_init (1) = 0;
    q_init (2) = 0;
    KDL::JntArray q_out(chain.getNrOfJoints());

//	KDL::Segment my_seg = chain.getSegment(3);
//	KDL::Frame my_fr;
//
//	if (fk_solver.JntToCart(q_init, my_fr) >= 0){
//
//			std_msgs::String msg;
//			std::stringstream ss;
//			ss << "# 0: " << my_fr.p[0] << "  # 1: " << my_fr.p[1] << "  # 2: " << my_fr.p[2];
//			msg.data = ss.str();
//			ROS_INFO("Construct kdl tree");
//			ROS_INFO("%s", msg.data.c_str()) ;
//		}

    double x = 0.16509, y = -0.18567, z = 0.053603;  //0.16509; -0.18567; 0.053603
    KDL::Frame p_in (KDL::Vector(0.12291, -0.085841, -0.16766));

    int ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out);
    if (ik_valid >= 0) {
        ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2));
    }
    else {
        ROS_ERROR("IK not found");
    }

    return 0;
}
Beispiel #30
0
bool ik_solve(kinematics_msgs::GetPositionIK::Request  &req,
         kinematics_msgs::GetPositionIK::Response &res )
{
	ROS_INFO("get_ik_service has been called!");
	
	if(req.ik_request.ik_link_name.length() == 0)
		my_tree.getChain("base_link","arm_7_link", chain);
	else
		my_tree.getChain("base_link",req.ik_request.ik_link_name, chain);
	
	
	
	unsigned int nj = chain.getNrOfJoints();
	
	JntArray q_min(nj);
	JntArray q_max(nj);
	
	//ToDo: get joint limits from robot_description on parameter_server or use /cob_arm_kinematics/get_ik_solver_info!!!
	for(int i = 0; i < nj; i+=2)
	{
		q_min(i) =-2.9670;		//adjusted due to cob_description/lbr.urdf.xacro
		q_max(i) = 2.9670;
	}
	for(int i = 1; i < nj; i+=2)
	{
		q_min(i) =-2.0943951;	//adjusted due to cob_description/lbr.urdf.xacro
		q_max(i) = 2.0943951;
	}
	
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
	ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
	ChainIkSolverPos_NR_JL iksolverpos(chain, q_min, q_max, fksolver1,iksolver1v,1000,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

	JntArray q(nj);
	JntArray q_init(nj);
	for(int i = 0; i < nj; i++)
		q_init(i) = req.ik_request.ik_seed_state.joint_state.position[i];
	Frame F_dest;
	Frame F_ist;
	fksolver1.JntToCart(q_init, F_ist);
	tf::PoseMsgToKDL(req.ik_request.pose_stamped.pose, F_dest);
	std::cout << "Getting Goal\n";
	std::cout << F_dest <<"\n";
	std::cout << "Calculated Position out of Seed-Configuration:\n";
	std::cout << F_ist <<"\n";

	//uhr-fm: here comes the actual IK-solver-call -> could be replaced by analytical-IK-solver (cob)
	int ret = iksolverpos.CartToJnt(q_init,F_dest,q);
	
	//res.solution.joint_state.name = req.ik_request.ik_seed_state.joint_state.name;	
	res.solution.joint_state.name.resize(nj);
	res.solution.joint_state.name[0]="arm_1_joint";
	res.solution.joint_state.name[1]="arm_2_joint";
	res.solution.joint_state.name[2]="arm_3_joint";
	res.solution.joint_state.name[3]="arm_4_joint";
	res.solution.joint_state.name[4]="arm_5_joint";
	res.solution.joint_state.name[5]="arm_6_joint";
	res.solution.joint_state.name[6]="arm_7_joint";
	
	res.solution.joint_state.position.resize(nj);
	res.solution.joint_state.velocity.resize(nj);
	res.solution.joint_state.effort.resize(nj);
	if(ret < 0)
	{
		//res.error_code.val = 0;
		res.error_code.val = res.error_code.NO_IK_SOLUTION;
		ROS_INFO("Inverse Kinematic found no solution");
		std::cout << "RET: " << ret << std::endl;
		for(int i = 0; i < nj; i++)
		{
			res.solution.joint_state.position[i] = q_init(i);
			res.solution.joint_state.velocity[i] = 0.0;
			res.solution.joint_state.effort[i] = 0.0;
		}
	}
	else
	{
		ROS_INFO("Inverse Kinematic found a solution");
		//res.error_code.val = 1;
		res.error_code.val = res.error_code.SUCCESS;
		for(int i = 0; i < nj; i++)
		{
			res.solution.joint_state.position[i] = q(i);
			res.solution.joint_state.velocity[i] = 0.0;
			res.solution.joint_state.effort[i] = 0.0;
		}
	}
	//std::cout << "q_init\n";
	ROS_INFO("q_init: %f %f %f %f %f %f %f", q_init(0), q_init(1), q_init(2), q_init(3), q_init(4), q_init(5), q_init(6));
	ROS_INFO("q_out: %f %f %f %f %f %f %f", q(0), q(1), q(2), q(3), q(4), q(5), q(6));		
	//std::cout << "Solved with " << ret << " as return\n";
	//std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6)  << "\n";	

	return true;
}