43 bool load_kinematics_solvers)
69 bool canSpecifyPosition(
const robot_model::JointModel* jmodel,
const unsigned int index)
72 if (jmodel->getType() == robot_model::JointModel::PLANAR && index == 2)
73 ROS_ERROR(
"Cannot specify position limits for orientation of planar joint '%s'", jmodel->getName().c_str());
74 else if (jmodel->getType() == robot_model::JointModel::FLOATING && index > 2)
75 ROS_ERROR(
"Cannot specify position limits for orientation of floating joint '%s'", jmodel->getName().c_str());
76 else if (jmodel->getType() == robot_model::JointModel::REVOLUTE &&
77 static_cast<const robot_model::RevoluteJointModel*
>(jmodel)->isContinuous())
78 ROS_ERROR(
"Cannot specify position limits for continuous joint '%s'", jmodel->getName().c_str());
111 for (std::size_t i = 0; i <
model_->getJointModels().size(); ++i)
113 robot_model::JointModel* jmodel =
model_->getJointModels()[i];
114 std::vector<moveit_msgs::JointLimits> jlim = jmodel->getVariableBoundsMsg();
115 for (std::size_t j = 0; j < jlim.size(); ++j)
117 std::string prefix =
rdf_loader_->getRobotDescription() +
"_planning/joint_limits/" + jlim[j].joint_name +
"/";
120 if (nh.
getParam(prefix +
"max_position", max_position))
122 if (canSpecifyPosition(jmodel, j))
124 jlim[j].has_position_limits =
true;
125 jlim[j].max_position = max_position;
129 if (nh.
getParam(prefix +
"min_position", min_position))
131 if (canSpecifyPosition(jmodel, j))
133 jlim[j].has_position_limits =
true;
134 jlim[j].min_position = min_position;
138 if (nh.
getParam(prefix +
"max_velocity", max_velocity))
140 jlim[j].has_velocity_limits =
true;
141 jlim[j].max_velocity = max_velocity;
144 if (nh.
getParam(prefix +
"has_velocity_limits", has_vel_limits))
145 jlim[j].has_velocity_limits = has_vel_limits;
148 if (nh.
getParam(prefix +
"max_acceleration", max_acc))
150 jlim[j].has_acceleration_limits =
true;
151 jlim[j].max_acceleration = max_acc;
154 if (nh.
getParam(prefix +
"has_acceleration_limits", has_acc_limits))
155 jlim[j].has_acceleration_limits = has_acc_limits;
157 jmodel->setVariableBounds(jlim);
169 const kinematics_plugin_loader::KinematicsPluginLoaderPtr& kloader)
184 std::stringstream ss;
185 std::copy(groups.begin(), groups.end(), std::ostream_iterator<std::string>(ss,
" "));
186 ROS_DEBUG_STREAM(
"Loaded information about the following groups: '" << ss.str() <<
"'");
187 if (groups.empty() && !
model_->getJointModelGroups().empty())
188 ROS_WARN(
"No kinematics plugins defined. Fill and load kinematics.yaml!");
190 std::map<std::string, robot_model::SolverAllocatorFn> imap;
191 for (std::size_t i = 0; i < groups.size(); ++i)
194 if (!
model_->hasJointModelGroup(groups[i]))
197 const robot_model::JointModelGroup* jmg =
model_->getJointModelGroup(groups[i]);
199 kinematics::KinematicsBasePtr solver = kinematics_allocator(jmg);
202 std::string error_msg;
203 if (solver->supportsGroup(jmg, &error_msg))
205 imap[groups[i]] = kinematics_allocator;
209 ROS_ERROR(
"Kinematics solver %s does not support joint group %s. Error: %s",
typeid(*solver).name(),
210 groups[i].c_str(), error_msg.c_str());
215 ROS_ERROR(
"Kinematics solver could not be instantiated for joint group %s.", groups[i].c_str());
218 model_->setKinematicsAllocators(imap);
222 for (std::map<std::string, double>::const_iterator it = timeout.begin(); it != timeout.end(); ++it)
224 if (!
model_->hasJointModelGroup(it->first))
226 robot_model::JointModelGroup* jmg =
model_->getJointModelGroup(it->first);
227 jmg->setDefaultIKTimeout(it->second);
231 const std::map<std::string, unsigned int>& attempts =
kinematics_loader_->getIKAttempts();
232 for (std::map<std::string, unsigned int>::const_iterator it = attempts.begin(); it != attempts.end(); ++it)
234 if (!
model_->hasJointModelGroup(it->first))
236 robot_model::JointModelGroup* jmg =
model_->getJointModelGroup(it->first);
237 jmg->setDefaultIKAttempts(it->second);
void loadKinematicsSolvers(const kinematics_plugin_loader::KinematicsPluginLoaderPtr &kloader=kinematics_plugin_loader::KinematicsPluginLoaderPtr())
Load the kinematics solvers into the kinematic model. This is done by default, unless disabled explic...
#define ROS_DEBUG_STREAM_NAMED(name, args)
TiXmlDocument * urdf_doc_
The parsed XML content of the URDF and SRDF documents.
kinematics_plugin_loader::KinematicsPluginLoaderPtr kinematics_loader_
std::string robot_description_
The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter nam...
bool load_kinematics_solvers_
Flag indicating whether the kinematics solvers should be loaded as well, using specified ROS paramete...
void configure(const Options &opt)
Structure that encodes the options to be passed to the RobotModelLoader constructor.
boost::shared_ptr< Model > ModelSharedPtr
#define ROS_DEBUG_STREAM(args)
rdf_loader::RDFLoaderPtr rdf_loader_
robot_model::RobotModelPtr model_
RobotModelLoader(const Options &opt=Options())
Default constructor.
TiXmlDocument * srdf_doc_
bool getParam(const std::string &key, std::string &s) const
std::string urdf_string_
The string content of the URDF and SRDF documents. Loading from string is attempted only if loading f...
Helper class for loading kinematics solvers.