Second instance of robot model for constraints planning

edit

Hi every one!

I'm working on a dual_arm planner to use 2 UR-10 for manipulation. I'm coding it in c++, but i'm still learning every day about coding, so please be gentle :)

Using ROS Indigo on Ubuntu 14.04

In fact I'm trying to create a second instance of the robot_model used in the planning_scene by moveit, and use it for a constraint sampler that check that the two end effectors are keeping the same relative position/orientation during the manipulation.

I need this second robot_model because the UR's range for each joint is very large, and the trac_ik solver (used for an RRT algorythm) can have unexpected behavior if he find multiple IK solution (crossing singularities between two consecutive state of the robot).

Limiting the robot joint bounds is a good solution to avoid those plannig problem, but doing it in the urdf file is to restrictive for the appliation of the robot. So I'm trying to set this second robot model to be able to set a JointBoundsVector with ROS Param.

And here is my problem :

the JointModelGroup extracted from the second instance of the robot_model just give me constants method to extract joint models (witch contains JointsLimit.msg), or JointBoundsVector, or anything else that could allow me to modify the robot model object.

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)
{
    moveit::core::RobotModelConstPtr const_rm_= scene->getRobotModel();
    moveit::core::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->getJointModelNames();
    std::vector<std::string> r_arm_jm_list = r_arm_jmg->getJointModelNames();
    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++){
       const robot_model::JointModel* _jm = l_arm_jmg->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);
    }

}

I understand that those methods are constant for a good reason : some ROS services just look at the robot_model during there inittialisation. But I don want to modify the planning scene, I just need a alterable version of the robot_model ... and don't want to modify the moveit package for it ^^

So if some one has an idea, I'll be pleased to try it ! Thanks for reading