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