bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description,
const std::string& group_name,
const std::string& base_name,
const std::string& tip_name,
double search_discretization)
{
setValues(robot_description, group_name, base_name, tip_name,search_discretization);
urdf::Model robot_model;
std::string xml_string;
ros::NodeHandle private_handle("~/"+group_name);
dimension_ = 7;
while(!loadRobotModel(private_handle,robot_model,xml_string) && private_handle.ok())
{
ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
ros::Duration(0.5).sleep();
}
ROS_DEBUG("Loading KDL Tree");
if(!getKDLChain(xml_string,base_frame_,tip_frame_,kdl_chain_))
{
active_ = false;
ROS_ERROR("Could not load kdl tree");
}
ROS_DEBUG("Advertising services");
jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
private_handle.param<int>("free_angle",free_angle_,2);
pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model, base_frame_,tip_frame_, search_discretization_,free_angle_));
if(!pr2_arm_ik_solver_->active_)
{
ROS_ERROR("Could not load ik");
active_ = false;
}
else
{
pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
fk_solver_info_.joint_names = ik_solver_info_.joint_names;
for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
}
for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
}
for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
{
ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
}
ROS_DEBUG("PR2KinematicsPlugin::active for %s",group_name.c_str());
active_ = true;
}
pr2_arm_ik_solver_->setFreeAngle(2);
return active_;
}
bool GaffaArmKinematicsPlugin::initialize(std::string name)
{
urdf::Model robot_model;
std::string tip_name, xml_string;
ros::NodeHandle private_handle("~/"+name);
dimension_ = 7;
while(!loadRobotModel(private_handle,robot_model,root_name_,tip_name,xml_string) && private_handle.ok())
{
ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
ros::Duration(0.5).sleep();
}
ROS_INFO("Loading KDL Tree");
if(!getKDLChain(xml_string,root_name_,tip_name,kdl_chain_))
{
active_ = false;
ROS_ERROR("Could not load kdl tree");
}
ROS_INFO("Advertising services");
jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
private_handle.param<int>("free_angle",free_angle_,2);
private_handle.param<double>("search_discretization",search_discretization_,0.01);
gaffa_arm_ik_solver_.reset(new gaffa_arm_kinematics::GaffaArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
if(!gaffa_arm_ik_solver_->active_)
{
ROS_ERROR("Could not load ik");
active_ = false;
}
else
{
gaffa_arm_ik_solver_->getSolverInfo(ik_solver_info_);
gaffa_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
fk_solver_info_.joint_names = ik_solver_info_.joint_names;
for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
{
ROS_INFO("GaffaKinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
}
for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
{
ROS_INFO("GaffaKinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
}
for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
{
ROS_INFO("GaffaKinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
}
ROS_INFO("GaffaKinematics::active");
active_ = true;
}
return active_;
}
TEST(MoveArm, goToJointGoal)
{
ros::NodeHandle nh;
ros::NodeHandle private_handle("~");
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
boost::thread spin_thread(&spinThread);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalB;
std::vector<std::string> names(7);
names[0] = "r_shoulder_pan_joint";
names[1] = "r_shoulder_lift_joint";
names[2] = "r_upper_arm_roll_joint";
names[3] = "r_elbow_flex_joint";
names[4] = "r_forearm_roll_joint";
names[5] = "r_wrist_flex_joint";
names[6] = "r_wrist_roll_joint";
goalB.motion_plan_request.group_name = "right_arm";
private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
{
// goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.stamp = ros::Time::now();
// goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.frame_id = "base_link";
goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
}
goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = 0.15;
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalB);
finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::ABORTED);
EXPECT_TRUE(success);
ROS_INFO("Action finished: %s",state.toString().c_str());
}
ros::shutdown();
spin_thread.join();
}
bool KDLArmKinematicsPlugin::initialize(std::string name)
{
// Get URDF XML
std::string urdf_xml, full_urdf_xml;
ros::NodeHandle node_handle;
ros::NodeHandle private_handle("~"+name);
node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG("Reading xml file from parameter server");
std::string result;
if (!node_handle.getParam(full_urdf_xml, result)) {
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
// Get Root and Tip From Parameter Service
if (!private_handle.getParam("root_name", root_name_)) {
ROS_FATAL("GenericIK: No root name found on parameter server");
return false;
}
if (!private_handle.getParam("tip_name", tip_name_)) {
ROS_FATAL("GenericIK: No tip name found on parameter server");
return false;
}
// Load and Read Models
if (!loadModel(result)) {
ROS_FATAL("Could not load models!");
return false;
}
// Get Solver Parameters
int max_iterations;
double epsilon;
private_handle.param("max_solver_iterations", max_iterations, 1000);
private_handle.param("max_search_iterations", max_search_iterations_, 1000);
private_handle.param("epsilon", epsilon, 1e-2);
// 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_iterations, epsilon));
active_ = true;
return true;
}
bool getChainInfo(const std::string &name,
kinematics_msgs::KinematicSolverInfo &chain_info)
{
std::string urdf_xml, full_urdf_xml, root_name, tip_name;
ros::NodeHandle node_handle;
ros::NodeHandle private_handle("~"+name);
node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG("Reading xml file from parameter server");
std::string result;
if (!node_handle.getParam(full_urdf_xml, result))
{
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
return false;
}
// Get Root and Tip From Parameter Service
if (!private_handle.getParam("root_name", root_name))
{
ROS_FATAL("GenericIK: No root name found on parameter server");
return false;
}
if (!private_handle.getParam("tip_name", tip_name))
{
ROS_FATAL("GenericIK: No tip name found on parameter server");
return false;
}
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initString(result))
{
ROS_FATAL("Could not initialize robot model");
return -1;
}
if (!getChainInfoFromRobotModel(robot_model,root_name,tip_name,chain_info))
{
ROS_FATAL("Could not read information about the joints");
return false;
}
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_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 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;
}
TEST(MoveArm, goToPoseGoal)
{
ros::NodeHandle nh;
ros::NodeHandle private_handle("~");
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
boost::thread spin_thread(&spinThread);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalA;
goalA.motion_plan_request.group_name = "right_arm";
goalA.motion_plan_request.num_planning_attempts = 1;
private_handle.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
goalA.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.15;
goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.95;
goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices.resize(1);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].x = 0.15;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].y = -0.95;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].z = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.x = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.y = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.z = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.2;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
/* move arm should send the pose constraint straight to the planner */
goalA.disable_ik = true;
std::vector<std::string> names(7);
names[0] = "r_shoulder_pan_joint";
names[1] = "r_shoulder_lift_joint";
names[2] = "r_upper_arm_roll_joint";
names[3] = "r_elbow_flex_joint";
names[4] = "r_forearm_roll_joint";
names[5] = "r_wrist_flex_joint";
names[6] = "r_wrist_roll_joint";
int num_test_attempts = 0;
int max_attempts = 5;
bool success = false;
while (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalA);
finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
actionlib::SimpleClientGoalState state = move_arm.getState();
success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if ((!finished_within_time || !success) && num_test_attempts < max_attempts)
{
move_arm.cancelAllGoals();
ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
num_test_attempts++;
}
else
{
if(!success)
{
ROS_INFO("Action unsuccessful");
move_arm.cancelAllGoals();
}
ROS_INFO("Action finished: %s",state.toString().c_str());
break;
}
}
EXPECT_TRUE(success);
ros::shutdown();
spin_thread.join();
}