ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hi,
Just found a non-constant method of the RobotModel class to access to JointModels. Here is the updated code :
bool set_variable_joints_bounds(const planning_scene::PlanningSceneConstPtr& scene,
std::string &l_arm_jmg_name_, const std::vector<std::pair< double, double > > &l_arm_limits,
std::string &r_arm_jmg_name_, const std::vector<std::pair< double, double > > &r_arm_limits)
{
bool _flag_l = false;
bool _flag_r = false;
robot_model::RobotModelConstPtr const_rm_= scene->getRobotModel();
robot_model::RobotModel rm = *const_rm_;
moveit::core::JointModelGroup* l_arm_jmg = rm.getJointModelGroup(l_arm_jmg_name_);
moveit::core::JointModelGroup* r_arm_jmg = rm.getJointModelGroup(r_arm_jmg_name_);
std::vector<std::string> l_arm_jm_list = l_arm_jmg->getActiveJointModelNames();
std::vector<std::string> r_arm_jm_list = r_arm_jmg->getActiveJointModelNames();
assert(l_arm_jm_list.size()==l_arm_limits.size());
assert(r_arm_jm_list.size()==r_arm_limits.size());
for (size_t i=0 ; i<l_arm_jm_list.size();i++)
{
moveit::core::JointModel* _jm = rm.getJointModel(l_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = l_arm_limits[i].first;
_jl[i].max_position = l_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(l_arm_jm_list.size()-1))
_flag_l =true;
}
for (size_t i=0 ; i<r_arm_jm_list.size();i++){
moveit::core::JointModel* _jm = rm.getJointModel(r_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = r_arm_limits[i].first;
_jl[i].max_position = r_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(r_arm_jm_list.size()-1))
_flag_r =true;
}
if (_flag_l && _flag_r)
return true;
else
return false;
}
The JointModel pointer in the for loops does not need to be constant here.
Now I need to declare this new robot_model instance as an attribute of the constraint sampler, but this is another problem.
2 | No.2 Revision |
Hi,
Just found a non-constant method of the RobotModel class to access to JointModels. Here is the updated code :
bool set_variable_joints_bounds(const robot_model::RobotModelConstPtr construct_limited_robot_model(const planning_scene::PlanningSceneConstPtr& scene,
std::string &l_arm_jmg_name_, const std::vector<std::pair< double, double > > &l_arm_limits,
std::string &r_arm_jmg_name_, const std::vector<std::pair< double, double > > &r_arm_limits)
{
&r_arm_limits){
bool _flag_l = false;
bool _flag_r = false;
robot_model::RobotModelConstPtr const_rm_= scene->getRobotModel();
robot_model::RobotModel rm = *const_rm_;
moveit::core::JointModelGroup* robot_model::RobotModelPtr _limited_robot_model_ptr (new robot_model::RobotModel(*const_rm_));
robot_model::JointModelGroup* l_arm_jmg = rm.getJointModelGroup(l_arm_jmg_name_);
moveit::core::JointModelGroup* _limited_robot_model_ptr->getJointModelGroup(l_arm_jmg_name_);
robot_model::JointModelGroup* r_arm_jmg = rm.getJointModelGroup(r_arm_jmg_name_);
_limited_robot_model_ptr->getJointModelGroup(r_arm_jmg_name_);
std::vector<std::string> l_arm_jm_list = l_arm_jmg->getActiveJointModelNames();
std::vector<std::string> r_arm_jm_list = r_arm_jmg->getActiveJointModelNames();
assert(l_arm_jm_list.size()==l_arm_limits.size());
assert(r_arm_jm_list.size()==r_arm_limits.size());
for (size_t i=0 ; i<l_arm_jm_list.size();i++)
{
moveit::core::JointModel* i<l_arm_jm_list.size();i++){
robot_model::JointModel* _jm = rm.getJointModel(l_arm_jm_list[i]);
_limited_robot_model_ptr->getJointModel(l_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = l_arm_limits[i].first;
_jl[i].max_position = l_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(l_arm_jm_list.size()-1))
_flag_l =true;
}
}
for (size_t i=0 ; i<r_arm_jm_list.size();i++){
moveit::core::JointModel* robot_model::JointModel* _jm = rm.getJointModel(r_arm_jm_list[i]);
_limited_robot_model_ptr->getJointModel(r_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = r_arm_limits[i].first;
_jl[i].max_position = r_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(r_arm_jm_list.size()-1))
_flag_r =true;
}
if (_flag_l && _flag_r)
return true;
_limited_robot_model_ptr;
else
return false;
}
nullptr;
}
The JointModel pointer in the for loops does not need to be constant here.
Now I need to declare this new robot_model instance as an attribute of the constraint sampler, but this is another problem.
3 | No.3 Revision |
Hi,
Just found a non-constant method of the RobotModel class to access to JointModels. Here is the updated code :
robot_model::RobotModelConstPtr construct_limited_robot_model(const planning_scene::PlanningSceneConstPtr& scene,
std::string &l_arm_jmg_name_, const std::vector<std::pair< double, double > > &l_arm_limits,
std::string &r_arm_jmg_name_, const std::vector<std::pair< double, double > > &r_arm_limits){
bool _flag_l = false;
bool _flag_r = false;
robot_model::RobotModelConstPtr const_rm_= scene->getRobotModel();
robot_model::RobotModelPtr _limited_robot_model_ptr (new robot_model::RobotModel(*const_rm_));
robot_model::JointModelGroup* l_arm_jmg = _limited_robot_model_ptr->getJointModelGroup(l_arm_jmg_name_);
robot_model::JointModelGroup* r_arm_jmg = _limited_robot_model_ptr->getJointModelGroup(r_arm_jmg_name_);
std::vector<std::string> l_arm_jm_list = l_arm_jmg->getActiveJointModelNames();
std::vector<std::string> r_arm_jm_list = r_arm_jmg->getActiveJointModelNames();
assert(l_arm_jm_list.size()==l_arm_limits.size());
assert(r_arm_jm_list.size()==r_arm_limits.size());
for (size_t i=0 ; i<l_arm_jm_list.size();i++){
robot_model::JointModel* _jm = _limited_robot_model_ptr->getJointModel(l_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = l_arm_limits[i].first;
_jl[i].max_position = l_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(l_arm_jm_list.size()-1))
_flag_l =true;
}
for (size_t i=0 ; i<r_arm_jm_list.size();i++){
robot_model::JointModel* _jm = _limited_robot_model_ptr->getJointModel(r_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = r_arm_limits[i].first;
_jl[i].max_position = r_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(r_arm_jm_list.size()-1))
_flag_r =true;
}
if (_flag_l && _flag_r)
return _limited_robot_model_ptr;
else
return nullptr;
}
}
The JointModel pointer in the for loops does not need to be constant here.
Now I need to declare this new robot_model instance as an attribute of the constraint sampler, but this is another problem. And for this using RobotModelConstPtr can be usefull.
4 | No.4 Revision |
Hi,
Just found a non-constant method of the RobotModel class to access to JointModels. Here is the updated code :
robot_model::RobotModelConstPtr construct_limited_robot_model(const planning_scene::PlanningSceneConstPtr& scene,
scene, std::string &l_arm_jmg_name_, const std::vector<std::pair< double, double > > &l_arm_limits,
&l_arm_limits, std::string &r_arm_jmg_name_, const std::vector<std::pair< double, double > > &r_arm_limits){
bool _flag_l = false;
bool _flag_r = false;
robot_model::RobotModelConstPtr const_rm_= scene->getRobotModel();
robot_model::RobotModelPtr _limited_robot_model_ptr (new robot_model::RobotModel(*const_rm_));
robot_model::JointModelGroup* l_arm_jmg = _limited_robot_model_ptr->getJointModelGroup(l_arm_jmg_name_);
robot_model::JointModelGroup* r_arm_jmg = _limited_robot_model_ptr->getJointModelGroup(r_arm_jmg_name_);
std::vector<std::string> l_arm_jm_list = l_arm_jmg->getActiveJointModelNames();
std::vector<std::string> r_arm_jm_list = r_arm_jmg->getActiveJointModelNames();
assert(l_arm_jm_list.size()==l_arm_limits.size());
assert(r_arm_jm_list.size()==r_arm_limits.size());
for (size_t i=0 ; i<l_arm_jm_list.size();i++){
robot_model::JointModel* _jm = _limited_robot_model_ptr->getJointModel(l_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = l_arm_limits[i].first;
_jl[i].max_position = l_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(l_arm_jm_list.size()-1))
_flag_l =true;
}
for (size_t i=0 ; i<r_arm_jm_list.size();i++){
robot_model::JointModel* _jm = _limited_robot_model_ptr->getJointModel(r_arm_jm_list[i]);
std::vector<moveit_msgs::JointLimits> _jl = _jm->getVariableBoundsMsg();
_jl[i].min_position = r_arm_limits[i].first;
_jl[i].max_position = r_arm_limits[i].second;
_jm->setVariableBounds(_jl);
if (i==(r_arm_jm_list.size()-1))
_flag_r =true;
}
if (_flag_l && _flag_r)
return _limited_robot_model_ptr;
else
return nullptr;
}
The JointModel pointer in the for loops does not need to be constant here.
Now I need to declare this new robot_model instance as an attribute of the constraint sampler, but this is another problem. And for this using RobotModelConstPtr can be usefull.