Robotics StackExchange | Archived questions

velocity got from /joint_states is wrong?

velocity got from /joint_states is error?

Also asked here : https://github.com/ros-simulation/gazebo_ros_pkgs/issues/830

With hardware_interface/EffortJointInterface, 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:

figure_1

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

figure_1-2

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_ );
}

Asked by itfanr on 2018-10-08 00:12:05 UTC

Comments

Answers