ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 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.

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.

problem. And for this using RobotModelConstPtr can be usefull.

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.

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.