bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
{
setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model || !srdf)
{
ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
return false;
}
robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
if (!joint_model_group)
return false;
if(!joint_model_group->isChain())
{
ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
return false;
}
if(!joint_model_group->isSingleDOFJoints())
{
ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
return false;
}
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
{
ROS_ERROR_NAMED("kdl","Could not initialize tree object");
return false;
}
if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
{
ROS_ERROR_NAMED("kdl","Could not initialize chain object");
return false;
}
dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
{
if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
{
ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
}
}
fk_chain_info_.joint_names = ik_chain_info_.joint_names;
fk_chain_info_.limits = ik_chain_info_.limits;
if(!joint_model_group->hasLinkModel(getTipFrame()))
{
ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
return false;
}
ik_chain_info_.link_names.push_back(getTipFrame());
fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
joint_min_.resize(ik_chain_info_.limits.size());
joint_max_.resize(ik_chain_info_.limits.size());
for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
{
joint_min_(i) = ik_chain_info_.limits[i].min_position;
joint_max_(i) = ik_chain_info_.limits[i].max_position;
}
// Get Solver Parameters
int max_solver_iterations;
double epsilon;
bool position_ik;
private_handle.param("max_solver_iterations", max_solver_iterations, 500);
private_handle.param("epsilon", epsilon, 1e-5);
private_handle.param(group_name+"/position_only_ik", position_ik, false);
ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
private_handle.getNamespace().c_str(),
(group_name+"/position_only_ik").c_str());
if(position_ik)
ROS_INFO_NAMED("kdl","Using position only ik");
num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
// Check for mimic joints
std::vector<unsigned int> redundant_joints_map_index;
std::vector<JointMimic> mimic_joints;
unsigned int joint_counter = 0;
for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
{
const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
//first check whether it belongs to the set of active joints in the group
if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
{
JointMimic mimic_joint;
mimic_joint.reset(joint_counter);
mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
mimic_joint.active = true;
mimic_joints.push_back(mimic_joint);
++joint_counter;
continue;
}
if (joint_model_group->hasJointModel(jm->getName()))
{
if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
{
JointMimic mimic_joint;
mimic_joint.reset(joint_counter);
mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
mimic_joint.offset = jm->getMimicOffset();
mimic_joint.multiplier = jm->getMimicFactor();
mimic_joints.push_back(mimic_joint);
continue;
}
}
}
for (std::size_t i = 0; i < mimic_joints.size(); ++i)
{
if(!mimic_joints[i].active)
{
const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
for(std::size_t j=0; j < mimic_joints.size(); ++j)
{
if(mimic_joints[j].joint_name == joint_model->getName())
{
mimic_joints[i].map_index = mimic_joints[j].map_index;
}
}
}
}
mimic_joints_ = mimic_joints;
// Setup the joint state groups that we need
state_.reset(new robot_state::RobotState(robot_model_));
state_2_.reset(new robot_state::RobotState(robot_model_));
// Store things for when the set of redundant joints may change
position_ik_ = position_ik;
joint_model_group_ = joint_model_group;
max_solver_iterations_ = max_solver_iterations;
epsilon_ = epsilon;
active_ = true;
ROS_DEBUG_NAMED("kdl","KDL solver initialized");
return true;
}
bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
{
ROS_DEBUG("Initializing kdl solver");
setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
kinematic_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
if(!kinematic_model_->hasJointModelGroup(group_name))
{
ROS_ERROR("Kinematic model does not contain group %s",group_name.c_str());
return false;
}
robot_model::JointModelGroup* joint_model_group = kinematic_model_->getJointModelGroup(group_name);
if(!joint_model_group->isChain())
{
ROS_ERROR("Group is not a chain");
return false;
}
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
{
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!kdl_tree.getChain(base_frame_, tip_frame_, kdl_chain_))
{
ROS_ERROR("Could not initialize chain object");
return false;
}
dimension_ = joint_model_group->getVariableCount();
jnt_seed_state_.resize(dimension_);
jnt_pos_in_.resize(dimension_);
jnt_pos_out_.resize(dimension_);
ik_chain_info_.joint_names = joint_model_group->getJointModelNames();
ik_chain_info_.limits = joint_model_group->getVariableLimits();
fk_chain_info_.joint_names = ik_chain_info_.joint_names;
fk_chain_info_.limits = ik_chain_info_.limits;
if(!joint_model_group->hasLinkModel(tip_frame_))
{
ROS_ERROR("Could not find tip name in joint group");
return false;
}
ik_chain_info_.link_names.push_back(tip_frame_);
fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
joint_min_.resize(ik_chain_info_.limits.size());
joint_max_.resize(ik_chain_info_.limits.size());
for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
{
joint_min_(i) = ik_chain_info_.limits[i].min_position;
joint_max_(i) = ik_chain_info_.limits[i].max_position;
}
// Get Solver Parameters
int max_solver_iterations;
double epsilon;
private_handle.param("max_solver_iterations", max_solver_iterations, 500);
private_handle.param("epsilon", epsilon, 1e-5);
// Build Solvers
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
ik_solver_vel_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
ik_solver_pos_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_, joint_min_, joint_max_,*fk_solver_, *ik_solver_vel_, max_solver_iterations, epsilon));
// Setup the joint state groups that we need
kinematic_state_.reset(new robot_state::RobotState((const robot_model::RobotModelConstPtr) kinematic_model_));
kinematic_state_2_.reset(new robot_state::RobotState((const robot_model::RobotModelConstPtr) kinematic_model_));
active_ = true;
ROS_DEBUG("KDL solver initialized");
return true;
}
bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
{
setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model || !srdf)
{
ROS_ERROR("URDF and SRDF must be loaded for KDL kinematics solver to work.");
return false;
}
kinematic_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
if(!kinematic_model_->hasJointModelGroup(group_name))
{
ROS_ERROR("Kinematic model does not contain group %s", group_name.c_str());
return false;
}
robot_model::JointModelGroup* joint_model_group = kinematic_model_->getJointModelGroup(group_name);
if(!joint_model_group->isChain())
{
ROS_ERROR("Group '%s' is not a chain", group_name.c_str());
return false;
}
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
{
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!kdl_tree.getChain(base_frame_, tip_frame_, kdl_chain_))
{
ROS_ERROR("Could not initialize chain object");
return false;
}
dimension_ = joint_model_group->getVariableCount();
jnt_seed_state_.resize(dimension_);
jnt_pos_in_.resize(dimension_);
jnt_pos_out_.resize(dimension_);
ik_chain_info_.joint_names = joint_model_group->getJointModelNames();
ik_chain_info_.limits = joint_model_group->getVariableLimits();
fk_chain_info_.joint_names = ik_chain_info_.joint_names;
fk_chain_info_.limits = ik_chain_info_.limits;
if(!joint_model_group->hasLinkModel(tip_frame_))
{
ROS_ERROR("Could not find tip name in joint group '%s'", group_name.c_str());
return false;
}
ik_chain_info_.link_names.push_back(tip_frame_);
fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
joint_min_.resize(ik_chain_info_.limits.size());
joint_max_.resize(ik_chain_info_.limits.size());
for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
{
joint_min_(i) = ik_chain_info_.limits[i].min_position;
joint_max_(i) = ik_chain_info_.limits[i].max_position;
}
// Get Solver Parameters
int max_solver_iterations;
double epsilon;
bool position_ik;
private_handle.param("max_solver_iterations", max_solver_iterations, 500);
private_handle.param("epsilon", epsilon, 1e-5);
private_handle.param(group_name+"/position_only_ik", position_ik, false);
ROS_DEBUG("Looking in private handle: %s for param name: %s",
private_handle.getNamespace().c_str(),
(group_name+"/position_only_ik").c_str());
if(position_ik)
ROS_INFO("Using position only ik");
// Build Solvers
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
// Check for mimic joints
bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
if(!has_mimic_joints)
{
ik_solver_vel_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
ik_solver_pos_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_, joint_min_, joint_max_,*fk_solver_, *ik_solver_vel_, max_solver_iterations, epsilon));
}
else
{
std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
ik_solver_vel_.reset(new KDL::ChainIkSolverVel_pinv_mimic(kdl_chain_, joint_model_group->getMimicJointModels().size(), position_ik));
ik_solver_pos_.reset(new KDL::ChainIkSolverPos_NR_JL_Mimic(kdl_chain_, joint_min_, joint_max_,*fk_solver_, *ik_solver_vel_, max_solver_iterations, epsilon));
unsigned int joint_counter = 0;
for(std::size_t i=0; i < kdl_chain_.getNrOfSegments(); ++i)
{
//first check whether it belongs to the set of active joints in the group
if(joint_model_group->isActiveDOF(kdl_chain_.segments[i].getJoint().getName()))
{
kdl_kinematics_plugin::JointMimic mimic_joint;
mimic_joint.reset(joint_counter);
mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
mimic_joint.active = true;
mimic_joints.push_back(mimic_joint);
++joint_counter;
continue;
}
if(joint_model_group->hasJointModel(kdl_chain_.segments[i].getJoint().getName()))
{
if(joint_model_group->getJointModel(kdl_chain_.segments[i].getJoint().getName())->getMimic())
{
kdl_kinematics_plugin::JointMimic mimic_joint;
mimic_joint.reset(joint_counter);
mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
mimic_joint.offset = joint_model_group->getJointModel(kdl_chain_.segments[i].getJoint().getName())->getMimicOffset();
mimic_joint.multiplier = joint_model_group->getJointModel(kdl_chain_.segments[i].getJoint().getName())->getMimicFactor();
mimic_joints.push_back(mimic_joint);
continue;
}
}
}
for(std::size_t i=0; i < mimic_joints.size(); ++i)
{
if(!mimic_joints[i].active)
{
const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
for(std::size_t j=0; j < mimic_joints.size(); ++j)
{
if(mimic_joints[j].joint_name == joint_model->getName())
{
mimic_joints[i].map_index = mimic_joints[j].map_index;
}
}
}
}
KDL::ChainIkSolverVel_pinv_mimic* tmp_vel_solver = static_cast<KDL::ChainIkSolverVel_pinv_mimic*> (ik_solver_vel_.get());
KDL::ChainIkSolverPos_NR_JL_Mimic* tmp_pos_solver = static_cast<KDL::ChainIkSolverPos_NR_JL_Mimic*> (ik_solver_pos_.get());
tmp_vel_solver->setMimicJoints(mimic_joints);
tmp_pos_solver->setMimicJoints(mimic_joints);
}
// Setup the joint state groups that we need
kinematic_state_.reset(new robot_state::RobotState((const robot_model::RobotModelConstPtr) kinematic_model_));
kinematic_state_2_.reset(new robot_state::RobotState((const robot_model::RobotModelConstPtr) kinematic_model_));
active_ = true;
ROS_DEBUG("KDL solver initialized");
return true;
}
bool MoveItR2ChainKinematicsPlugin::initialize(const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
{
// Consciously NOT using the provided setValues function in the base
// This method removes the leading slash from base_frame and tip_frame, which wreaks havoc on the current R2 naming conventions for joints
/*robot_description_ = robot_description;
group_name_ = group_name;
base_frame_ = base_frame;
tip_frame_ = tip_frame;
if (base_frame_[0] != '/')
base_frame_ = "/" + base_frame_;
if (tip_frame_[0] != '/')
tip_frame_ = "/" + tip_frame_;
tip_frames_.push_back(tip_frame_);
search_discretization_ = search_discretization;
// DO NOT USE THIS FUNCTION-----> setValues(robot_description, group_name, base_frame, tip_frame, search_discretization); <---- DO NOT USE THIS*/
setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
ROS_INFO("Initializing MoveItR2ChainKinematics for group \"%s\" with base frame \"%s\" and tip frame \"%s\"", group_name_.c_str(), base_frame_.c_str(), tip_frame_.c_str());
// Loading URDF and SRDF
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model)
{
ROS_ERROR("URDF must be loaded for R2 kinematics solver to work.");
return false;
}
if (!srdf)
{
ROS_ERROR("SRDF must be loaded for R2 kinematics solver to work.");
return false;
}
// Load the robot kinematics and semantics (e.g., group information)
robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
// Extract the joint group
if(!robot_model_->hasJointModelGroup(group_name_))
{
ROS_ERROR("Kinematic model does not contain group \"%s\"", group_name_.c_str());
return false;
}
// Getting kinematic model for the joint group in question
robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name_);
KdlTreeIk tree_ik;
tree_ik.loadFromParam(robot_description_);
KDL::Chain chain;
if(!tree_ik.getTree().getChain(base_frame_, tip_frame_, chain))
{
ROS_ERROR("Failed to initialize kinematic chain with base %s and root %s", base_frame_.c_str(), tip_frame_.c_str());
return false;
}
ik_ = new KdlChainIk(chain);
fk_ = new KdlTreeFk();
// Initializing Ik and FK from URDF parameter
fk_->loadFromParam(robot_description_);
ik_->getJointNames(joint_names_);
// Setting joint limits from URDF
//std::map<std::string, double> min_joint_limits, max_joint_limits;
// Getting default joint values
std::map<std::string, double> default_joints;
robot_model_->getVariableDefaultPositions(default_joints);
// The total number of DOF
num_dofs_ = jmg->getVariableCount();
default_joint_positions_.resize(num_dofs_);
ROS_INFO("Initializing KdlChainIK for \"%s\" with %u DOFs", group_name_.c_str(), num_dofs_);
// The order of joint_names_ comes from in house kinematics, and is important to maintain.
for (size_t i = 0; i < joint_names_.size(); ++i)
{
//const moveit::core::VariableBounds& bounds = robot_model_->getVariableBounds(joint_names_[i]);
//std::pair<double, double> limits(bounds.min_position_, bounds.max_position_);
//ROS_INFO(" [%lu] - %s with range [%1.4f, %1.4f]", i, joint_names_[i].c_str(), limits.first, limits.second);
//ROS_INFO(" Attached to link: %s", robot_model_->getJointModel(joint_names_[i])->getChildLinkModel()->getName().c_str());
// Inserting into joint limit maps
//min_joint_limits[joint_names_[i]] = limits.first;
//max_joint_limits[joint_names_[i]] = limits.second;
// This joint is in the group
if (jmg->hasJointModel(joint_names_[i]))
{
group_joint_index_map_[joint_names_[i]] = i;
group_joints_.push_back(joint_names_[i]);
group_links_.push_back(robot_model_->getJointModel(joint_names_[i])->getChildLinkModel()->getName());
}
// Save default joint value
default_joint_positions_(i) = default_joints[joint_names_[i]];
}
return true;
}
bool SrvKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::vector<std::string>& tip_frames,
double search_discretization)
{
bool debug = false;
ROS_INFO_STREAM_NAMED("srv","SrvKinematicsPlugin initializing");
setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model || !srdf)
{
ROS_ERROR_NAMED("srv","URDF and SRDF must be loaded for SRV kinematics solver to work."); // TODO: is this true?
return false;
}
robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
joint_model_group_ = robot_model_->getJointModelGroup(group_name);
if (!joint_model_group_)
return false;
if (debug)
{
std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
std::cout << std::endl;
}
// Get the dimension of the planning group
dimension_ = joint_model_group_->getVariableCount();
ROS_INFO_STREAM_NAMED("srv","Dimension planning group '" << group_name << "': " << dimension_
<< ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
<< ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
// Copy joint names
for (std::size_t i=0; i < joint_model_group_->getJointModels().size(); ++i)
{
ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
}
if (debug)
{
ROS_ERROR_STREAM_NAMED("temp","tip links available:");
std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
}
// Make sure all the tip links are in the link_names vector
for (std::size_t i = 0; i < tip_frames_.size(); ++i)
{
if(!joint_model_group_->hasLinkModel(tip_frames_[i]))
{
ROS_ERROR_NAMED("srv","Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), group_name.c_str());
return false;
}
ik_group_info_.link_names.push_back(tip_frames_[i]);
}
// Choose what ROS service to send IK requests to
ROS_DEBUG_STREAM_NAMED("srv","Looking for ROS service name on rosparm server at location: " <<
private_handle.getNamespace() << "/" << group_name_ << "/kinematics_solver_service_name");
std::string ik_service_name;
private_handle.param(group_name_ + "/kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
// Setup the joint state groups that we need
robot_state_.reset(new robot_state::RobotState(robot_model_));
robot_state_->setToDefaultValues();
// Create the ROS service client
ros::NodeHandle nonprivate_handle("");
ik_service_client_ = boost::make_shared<ros::ServiceClient>(nonprivate_handle.serviceClient
<moveit_msgs::GetPositionIK>(ik_service_name));
if (!ik_service_client_->waitForExistence(ros::Duration(0.1))) // wait 0.1 seconds, blocking
ROS_WARN_STREAM_NAMED("srv","Unable to connect to ROS service client with name: " << ik_service_client_->getService());
else
ROS_INFO_STREAM_NAMED("srv","Service client started with ROS service name: " << ik_service_client_->getService());
active_ = true;
ROS_DEBUG_NAMED("srv","ROS service-based kinematics solver initialized");
return true;
}
