bool Kinematics::readJoints(urdf::Model &robot_model) {
num_joints = 0;
// get joint maxs and mins
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint;
while (link && link->name != root_name) {
joint = robot_model.getJoint(link->parent_joint->name);
if (!joint) {
ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
return false;
}
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
num_joints++;
}
link = robot_model.getLink(link->getParent()->name);
}
joint_min.resize(num_joints);
joint_max.resize(num_joints);
info.joint_names.resize(num_joints);
info.link_names.resize(num_joints);
info.limits.resize(num_joints);
link = robot_model.getLink(tip_name);
unsigned int i = 0;
while (link && i < num_joints) {
joint = robot_model.getJoint(link->parent_joint->name);
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS ) {
lower = joint->limits->lower;
upper = joint->limits->upper;
hasLimits = 1;
} else {
lower = -M_PI;
upper = M_PI;
hasLimits = 0;
}
int index = num_joints - i -1;
joint_min.data[index] = lower;
joint_max.data[index] = upper;
info.joint_names[index] = joint->name;
info.link_names[index] = link->name;
info.limits[index].joint_name = joint->name;
info.limits[index].has_position_limits = hasLimits;
info.limits[index].min_position = lower;
info.limits[index].max_position = upper;
i++;
}
link = robot_model.getLink(link->getParent()->name);
}
return true;
}
bool getChainInfoFromRobotModel(urdf::Model &robot_model,
const std::string &root_name,
const std::string &tip_name,
kinematics_msgs::KinematicSolverInfo &chain_info)
{
// get joint maxs and mins
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint;
while (link && link->name != root_name)
{
joint = robot_model.getJoint(link->parent_joint->name);
if (!joint)
{
ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
return false;
}
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
lower = joint->limits->lower;
upper = joint->limits->upper;
hasLimits = 1;
}
else
{
lower = -M_PI;
upper = M_PI;
hasLimits = 0;
}
chain_info.joint_names.push_back(joint->name);
motion_planning_msgs::JointLimits limits;
limits.joint_name = joint->name;
limits.has_position_limits = hasLimits;
limits.min_position = lower;
limits.max_position = upper;
chain_info.limits.push_back(limits);
}
link = robot_model.getLink(link->getParent()->name);
}
link = robot_model.getLink(tip_name);
if(link)
chain_info.link_names.push_back(tip_name);
std::reverse(chain_info.limits.begin(),chain_info.limits.end());
std::reverse(chain_info.joint_names.begin(),chain_info.joint_names.end());
return true;
}
bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
// Reset root mass to zero and give it the proper name
mBodies[0].mMass = 0;
// Set gravity to ROS standards
gravity << 0, 0, -9.81;
std::vector<boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);
if(oldRoot == newRoot || !newRoot)
process(*oldRoot, 0, Math::SpatialTransform());
else
processReverse(*newRoot, 0, 0, Math::SpatialTransform());
// Rename the root body to our URDF root
mBodyNameMap.erase("ROOT");
mBodyNameMap[root] = 0;
return true;
}
bool FkLookup::addRoot(const urdf::Model &model, const std::string &root){
boost::shared_ptr<KDL::Tree> tree;
tree_map::iterator it = roots.find(root);
if(it != roots.end()){
ROS_WARN_STREAM("Link " << root << " already processed");
return false;
}
if(!model.getLink(root)){
ROS_ERROR_STREAM("Model does not include link '" << root << "'");
return false;
}
tree.reset(new KDL::Tree());
kdl_parser::treeFromUrdfModel(model, *tree);
roots.insert(std::make_pair(root,tree));
crawl_and_add_links(model.getLink(root)->child_links,root,tips);
return !tips.empty();
}
SolverInfoProcessor::SolverInfoProcessor(const urdf::Model &robot_model,
const std::string &tip_name,
const std::string &root_name)
{
// setup the IK solver information which contains joint names, joint limits and so on
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
while (link) {
// check if we have already reached the final link
if (link->name == root_name) break;
// if we have reached the last joint the root frame is not in the chain
// then we cannot build the chain
if (!link->parent_joint) {
ROS_ERROR("The provided root link is not in the chain");
ROS_ASSERT(false);
}
// process the joint
boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
if (!joint) {
ROS_ERROR("Could not find joint: %s", link->parent_joint->name.c_str());
ROS_ASSERT(false);
}
// add the joint to the chain
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_DEBUG("Joint axis: %f, %f, %f", joint->axis.x, joint->axis.y, joint->axis.z);
addJointToChainInfo(link->parent_joint, _solver_info);
}
link = robot_model.getLink(link->getParent()->name);
}
_solver_info.link_names.push_back(tip_name);
// We expect order from root to tip, so reverse the order
std::reverse(_solver_info.limits.begin(), _solver_info.limits.end());
std::reverse(_solver_info.joint_names.begin(), _solver_info.joint_names.end());
std::reverse(_solver_info.link_names.begin(), _solver_info.link_names.end());
}
bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
// Display debug information
#if DISPLAY_DEBUG_INFO
ROS_ERROR("Initialising RBDL model of '%s' with root link '%s'...", model.getName().c_str(), root.c_str());
#endif
// Save the root link name
m_nameURDFRoot = model.getRoot()->name;
m_indexURDFRoot = (unsigned int) -1;
// Reset root mass to zero and give it the proper name
mBodies[0].mMass = 0;
// Set gravity to ROS standards
gravity << 0, 0, -9.81;
std::vector<boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);
if(oldRoot == newRoot || !newRoot)
process(*oldRoot, 0, Math::SpatialTransform());
else
processReverse(*newRoot, 0, NULL, Math::SpatialTransform());
// Rename the root body to our URDF root
mBodyNameMap.erase("ROOT");
mBodyNameMap[root] = 0;
// Display debug information
#if DISPLAY_DEBUG_INFO
ROS_INFO("URDF root link is '%s' and was detected at RBDL body index %u", m_nameURDFRoot.c_str(), m_indexURDFRoot);
#endif
return true;
}
