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