KDL::JntArray KDLArmKinematicsPlugin::getRandomConfiguration()
{
KDL::JntArray jnt_array;
jnt_array.resize(dimension_);
for(unsigned int i=0; i < dimension_; i++)
jnt_array(i) = genRandomNumber(joint_min_(i),joint_max_(i));
return jnt_array;
}
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;
}
