How to Get Velocity of a joint using API ?

edit

Hi Akshay5059

Here is a program i wrote some time ago. As i recall it works. let me know if you have any problems with it. I do not know about base pointers i just remember getting this to work:

#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
//#include <physics/World.hh> //added to use getsimtime
#include <common/common.hh>
#include <stdio.h>
#include <unistd.h> // sleep function you added yourself
#include <gazebo/physics/Joint.hh> //These two lines you added to use the function getforcetorque
#include <gazebo/physics/JointWrench.hh> //These two lines you added to use the function getforcetorque

int a=0;
int b;
double angle;
double currenttime;
double PI = 3.141592654;
namespace gazebo
{  
/*  class HelloWorld : public WorldPlugin
  {
    public: HelloWorld() : WorldPlugin() 
        {
          printf("Hello World!\n");
        }

    public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
        {
        }

  };*/
  class SetJoints : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
    {
      // Store the pointer to the model
      this->model = _parent;

      this->j2_controller = new physics::JointController(model);

      //this->last_update_time_ = this->model->GetWorld()->GetSimTime(); //Trying to get simtime. not needed.

      this->joint1_ = this->model->GetJoint("my_joint1");
      this->joint2_ = this->model->GetJoint("my_joint2");

      //physics::JointPtr joint = this->model->GetJoint("my_joint");
      //physics::JointWrench wrench = joint->GetForceTorque(0);

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&SetJoints::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      common::Time current_time = this->model->GetWorld()->GetSimTime();//getting simtime
      // Apply a small linear velocity to the model.
      //this->model->SetLinearVel(math::Vector3(.03, 0, 0));
      std::string j2name("my_joint");  

      double current_angle = this->joint1_->GetAngle(0).Radian();
      double current_vel = this->joint1_->GetVelocity(0);
      physics::JointWrench wrench1 = this->joint1_->GetForceTorque(0);
      //double forcetorque = this->joint_->GetForceTorque(0);
      //double current_forcetorque = this->joint_->GetForce(0); //doesn't work yet. try using GetForce

      //a=0;
      //b=45;
      //angle=current_time.Double();
      //angle=a/1000.;
      float test = wrench1.body2Force[2];

      angle=current_angle+PI;
      printf("Hello World! %lf\n",test);
//      printf("Hello World!\n");
      //j2_controller->SetJointPosition(this->model->GetJoint("my_joint"), angle);
      //usleep(1);
      a++;


    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    private: physics::JointController * j2_controller;

    //common::Time last_update_time_; Not needed

    //Things for saving joint_ in.
    physics::JointPtr joint1_;
    physics::JointPtr joint2_;
    //physics::JointPtr joint;

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(SetJoints)
}