Set joints angles by model plugin

edit

Hello,I am using gazebo for my thesis project. I am trying to get information about the movement of a robotic arm. I would like to know what is the best way to set joints angles without animation. What I am doing is trying all of the possible logic combinations in each joint (e.g. one combinations is like -> joint1: no move, joint2: move + , joint3: move -, ...) and getting the end-effort position. I used the setAngle method and the jointController, at the beginning both solutions works fine but in three or four updateWorldEvent calls the robot start to fly and to separate their links. I am using Gazebo 1.9 standalone.

SUMMARY 1.-which method do you recommend me to set the angle? I would like to show you a piece of the code, this code is in the updateWorldEvent of my model plugin and set the different combinations.

for(int i = 0 ; i<this->combinations.size();i++)
  {
for(int j  = 0; j< this->joints_.size(); j++)
     {
  this->jointController->SetJointPosition(this->joints_[j],this->joints_[j]->GetAngle(0).Radian()+(combinations[i][j]*moveProportion_));
  this->jointController->Update();
  //this->joints_[j]->SetAngle(0,this->joints_[j]->GetAngle(0).Radian()+(combinations[i][j]*moveProportion_));
  //this->joints_[j]->Update();
}   
    this->model_->Update();actualizeDistance();
if(bestDistance > this->distance)
{
  bestState = physics::ModelState(this->model_);
  bestDistance = this->distance;
}   
this->model_->SetState(this->actualState);
this->model_->Update();
  }

Thanks for reading.