velocity got from /joint_states is wrong?

[itfanr]

With hardware_interface/EffortJointInterface and gazebo7, I published the effort commands as follows:

import rospy
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped, PoseArray, Pose
from std_msgs.msg import Float64

class CmdPub(object):
    def __init__(self):
        self.joints_pub=rospy.Publisher('my_effort_controller/command', Float64, queue_size=1)

    def function_pub_effort_elfin5(self, effort):       
        self.joints_pub.publish(effort)

if __name__=='__main__':
    rospy.init_node('cmd_pub', anonymous=True)
    cp = CmdPub()

    effort = -6
    while True:
        effort = -1 * effort
        cp.function_pub_effort_elfin5(effort)
        rospy.sleep(5)
        print "effort ", effort             

    rospy.spin()

And I subscribed /joint_states and plot position and velocity line:

rospy.Subscriber('/joint_states', JointState, joint_states_cb)

The line I got is:

But It seems the velocity is not currect. I caculated the velocity and got this:

Are there some error with these codes:

void GazeboRosJointStatePublisher::publishJointStates() {
    ros::Time current_time = ros::Time::now();

    joint_state_.header.stamp = current_time;
    joint_state_.name.resize ( joints_.size() );
    joint_state_.position.resize ( joints_.size() );
    joint_state_.velocity.resize ( joints_.size() );

    for ( int i = 0; i < joints_.size(); i++ ) {
        physics::JointPtr joint = joints_[i];
        double velocity = joint->GetVelocity( 0 );
#if GAZEBO_MAJOR_VERSION >= 8
        double position = joint->Position ( 0 );
#else
        double position = joint->GetAngle ( 0 ).Radian();
#endif
        joint_state_.name[i] = joint->GetName();
        joint_state_.position[i] = position;
        joint_state_.velocity[i] = velocity;
    }
    joint_state_publisher_.publish ( joint_state_ );
}

[sahandrez]

Any updates on this? I have also noticed there is an offset in the published velocities in Gazebo 9.

[dbworth]

Hi @itfanr @sahandrez I think we are also seeing errors in rotational velocity since upgrading from Gazebo7 to Gazebo9.
I'm not sure if the bug is from Gazebo directly, or from code in "gazebo_ros_pkgs". Can someone compare the output from Gazebo Vs. ROS ?

@itfanr it is a bit difficult to understand your post (above). Could you please label your graph plots with "applied force" or "output" or "gazebo" or "computed result" ?

[itfanr]

@dbworth I donot have the image and the code now. Sorry about that.

[matthias-mayr]

I think that we encounter a similar issue. I described it in answers.gazebosim.org: https://answers.gazebosim.org/question/26191/ft-sensor-joint-velocity-and-joint-effort-readings-can-be-wrong/

In our case, the joint velocity and joint effort readings as well as the values from the FT sensor plugin are off close to singularities.

[peci1]

In case anyone wants to debug this, I'll post the code path for hinge joints:

https://github.com/gazebosim/gazebo-classic/blob/7ccef40e24831eb5cd974b489ee279fc064eacc4/gazebo/physics/ode/ODEHingeJoint.cc#L138

https://github.com/gazebosim/gazebo-classic/blob/7ccef40e24831eb5cd974b489ee279fc064eacc4/deps/opende/src/joints/hinge.cpp#L372

This uses difference of body->avel for the two bodies connected by joint. body->avel is only written in the step functions:

https://github.com/gazebosim/gazebo-classic/blob/7ccef40e24831eb5cd974b489ee279fc064eacc4/deps/opende/src/quickstep_update_bodies.cpp#L277

https://github.com/gazebosim/gazebo-classic/blob/7ccef40e24831eb5cd974b489ee279fc064eacc4/deps/opende/src/step.cpp#L859

https://github.com/gazebosim/gazebo-classic/blob/7ccef40e24831eb5cd974b489ee279fc064eacc4/deps/opende/src/robuststep.cpp#L2042

So it really seems the velocities are a result of the physics computation. Now the question is why.