Applying consecutive torques to joints using Joint::SetForce in Gazebo Model? [closed]

edit

Hi,

I have a Barrett WAM model in gazebo with ROS indigo, and I have been trying to make the arm execute random trajectories one after another. Say, if I have to execute 100 such random trajectories, I need to actuate each joint with a random torques at each iteration. Once the robot has moved in response to the applied torques, I need to update the torques on each joint by again sending random values at the next iteration. To begin with I am trying to move around a single joint randomly. I can easily achieve this using rosservice call /gazebo/apply_joint_effort, but I need to do this automatically with a plugin. Following the gazebo plugin tutorials, I have written a simple plugin to achieve the same. I am applying 2 opposing torques on a single joint as follows:

public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
   this->model->GetJoint("j1_joint")->SetForce(0,10); 
   this->model->GetJoint("j1_joint")->SetForce(0,-5);  
  //this->model->GetJoint("j2_joint")->SetForce(0,50); 
}

I expected that the j1_joint would move a distance corresponding to 10 Nm in one direction, then move a distance of 5 Nm in the other direction. But I am not able to perceive the two movements separately, and the joint just moves a small distance in a single direction which I think is 10-5=5 units in that direction. I understand that the forces are accumulating and at first I thought I am able to see only one movement because there is no delay between the application of the two torques. But on adding a delay I found that the simulation completely freezes. On further inspection it came to light that actually the plugin is executed from beginning to end at launch even before the gazebo models show up. So, we are left with the net torque on each joint.

So, how to enable the applied torques to have their effect on the joint one by one instead of just having the net effect? Is it possible to achieve this using this kind of a plugin? Or should I need to look into ros controllers with publisher subscriber framework to achieve what I want? I want the plugin to dynamically control the simulation by repeatedly applying different torques but it is not happening. I am a novice in gazebo and ROS, so I am not getting any about how to proceed. Any help is much appreciated.

Thanks in advance.