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));
     }
   }
 }
}

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;
}

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;
}

 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);
 }

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;
 }

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;
}

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;
}

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);
}