Mismatch /effort_controllers/commands and /joint_states when simulate in gazebo

Help
1

I use “/effort_controllers/commands” to public torque to my robot but when i check /joint_states, i rcv different

image
image1250×334 25.3 KB

This is my urdf file

<?xml version="1.0" encoding="utf-8"?>

<robot name="cobot">
  
  <link name="world"/>

  <joint name="joint_base" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="world" />
    <child link="base_link" />
    <dynamics damping="0.01"/>
  </joint>

  <link name="base_link">
    <inertial>
      <origin
        xyz="-0.0010876 -1.2016E-18 0.046702"
        rpy="0 0 0" />
      <mass value="0.86292" />
      <inertia
          ixx="0.0013798"
          ixy="8.1806E-21"
          ixz="4.4445E-06"
          iyy="0.0014297"
          iyz="-2.3796E-20"
          izz="0.0012551" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/base_link.STL" />
      </geometry>
      <material name="">
        <color rgba="1 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_1" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="link_1" />
    <axis xyz="0 0 1" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="500"
      velocity="3" />
    <!--<dynamics damping="0.01" friction="10"/>-->
  </joint>  

  <link name="link_1">
    <inertial>
      <origin
        xyz="0.0051522 7.4529E-09 0.20457"
        rpy="0 0 0" />
      <mass value="1.1783" />
      <inertia
        ixx="0.0038719"
        ixy="6.8049E-10"
        ixz="-0.00027581"
        iyy="0.0043441"
        iyz="-1.5308E-10"
        izz="0.0014412" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_1.STL" />
      </geometry>
      <material name="">
        <color rgba="0.79216 1 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_2" type="revolute">
    <origin
      xyz="0 0 0.25"
      rpy="0 1.5708 0" />
    <parent
      link="link_1" />
    <child
      link="link_2" />
    <axis
      xyz="0 0 -1" />
    <limit
      lower="-1.572"
      upper="1.572"
      effort="300"
      velocity="3" />
    <!--<dynamics damping="0.01" friction="10"/>-->
  </joint>

  <link name="link_2">
    <inertial>
      <origin xyz="-0.061532 -2.8541E-09 0.13673" rpy="0 0 0" />
      <mass value="1.458" />
      <inertia
        ixx="0.0016388"
        ixy="1.4889E-09"
        ixz="0.00029331"
        iyy="0.0082794"
        iyz="-1.3586E-09"
        izz="0.0078853" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_2.STL" />
      </geometry>
      <material name="">
        <color rgba="1 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_2.STL" />
      </geometry>
    </collision>

  </link>

  <joint name="joint_3" type="revolute">
    <origin
      xyz="0 0 0.14"
      rpy="3.1416 1.5708 0" />
    <parent link="link_2" />
    <child link="link_3" />
    <axis xyz="0 0 -1" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="1000"
      velocity="3" />
    <!--<dynamics damping="0.01" friction="10"/>-->  
  </joint>

  <link name="link_3">
    <inertial>
      <origin
        xyz="-0.00059467 1.634E-08 0.32766"
        rpy="0 0 0" />
      <mass
        value="1.2819" />
      <inertia
        ixx="0.0064488"
        ixy="-2.3078E-10"
        ixz="5.515E-05"
        iyy="0.0066015"
        iyz="-2.122E-10"
        izz="0.0012324" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cobot_description/meshes/link_3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.79216 0.81961 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cobot_description/meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_4" type="revolute">
    <origin
      xyz="0 0 0.4"
      rpy="-1.5708 0 -1.5708" />
    <parent link="link_3" />
    <child link="link_4" />
    <axis xyz="0 0 -1" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="300"
      velocity="3" />
    <dynamics damping="0.01"/>
  </joint>

  <link name="link_4">
    <inertial>
      <origin
        xyz="0.023612 1.6277E-07 0.097183"
        rpy="0 0 0" />
      <mass
        value="0.80311" />
      <inertia
        ixx="0.00072649"
        ixy="6.7086E-10"
        ixz="-5.3406E-05"
        iyy="0.0018241"
        iyz="1.5562E-10"
        izz="0.0017783" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_4.STL" />
      </geometry>
      <material name="">
        <color rgba="1 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_5" type="revolute">
    <origin
      xyz="0 0 0.1"
      rpy="0 1.5708 0" />
    <parent link="link_4" />
    <child link="link_5" />
    <axis xyz="0 0 -1" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="300"
      velocity="3" />
    <!--<dynamics damping="0.01" friction="10"/>-->
  </joint>

  <link name="link_5">
    <inertial>
      <origin
        xyz="0.0021577 1.1159E-08 0.1987"
          py="0 0 0" />
      <mass
        value="0.79231" />
      <inertia
        ixx="0.0024391"
        ixy="-8.5635E-11"
        ixz="-8.7704E-05"
        iyy="0.0025491"
        iyz="-4.4408E-11"
        izz="0.00061425" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_5.STL" />
      </geometry>
      <material name="">
        <color rgba="0.79216 1 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_6" type="revolute">
    <origin
      xyz="0 0 0.25"
      rpy="-1.5708 0 1.5708" />
    <parent link="link_5" />
    <child link="link_6" />
    <axis xyz="0 0 1" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="500"
      velocity="3" />
    <!--<dynamics damping="0.01" friction="10"/>-->
  </joint>

  <link name="link_6">
    <inertial>
      <origin
        xyz="0.0087128 6.0871E-09 0.077268"
        rpy="0 0 0" />
      <mass value="0.41074" />
      <inertia
        ixx="0.00028889"
        ixy="1.6836E-10"
        ixz="-9.7782E-06"
        iyy="0.00047286"
        iyz="5.1955E-11"
        izz="0.00045602" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_6.STL" />
      </geometry>
      <material name="">
        <color rgba="0.79216 1 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cobot_description/meshes/link_6.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_7" type="revolute">
    <origin
      xyz="0 0 0.08"
      rpy="0 1.5708 0" />
    <parent link="link_6" />
    <child link="link_7" />
    <axis xyz="0 0 -1" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="300"
      velocity="3" />
    <!--<dynamics damping="0.01" friction="10"/>-->
  </joint>

  <link name="link_7">
    <inertial>
      <origin
        xyz="1.9429E-16 -0.0010943 0.08044"
        rpy="0 0 0" />
      <mass value="0.14277" />
      <inertia
        ixx="6.0814E-05"
        ixy="-6.9684E-20"
        ixz="-2.2715E-21"
        iyy="6.5672E-05"
        iyz="-6.8758E-08"
        izz="8.8469E-05" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_7.STL" />
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh filename="package://cobot_description/meshes/link_7.STL" />
      </geometry>
    </collision>
  </link>

  <!-- Base -->
  <gazebo reference="base_link">
    <material>Gazebo/Black</material>
    <selfCollide>True</selfCollide>
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>3.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  
  <!-- Link1 -->
  <gazebo reference="link_1">
    <material>Gazebo/Blue</material>
    <selfCollide>True</selfCollide>
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
      
  <!-- Link2 -->
  <gazebo reference="link_2">
    <material>Gazebo/Green</material>
    <selfCollide>True</selfCollide> 
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
      
  <!-- Link3 -->
  <gazebo reference="link_3">
    <material>Gazebo/Gray</material>
    <selfCollide>True</selfCollide>
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
      
  <!-- Link4 -->
  <gazebo reference="link_4">
    <material>Gazebo/Blue</material>
    <selfCollide>True</selfCollide> 
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
      
  <!-- Link5 -->
  <gazebo reference="link_5"> 
    <material>Gazebo/Green</material>
    <selfCollide>True</selfCollide>   
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
      
  <!-- Link6 -->
  <gazebo reference="link_6">
    <material>Gazebo/Blue</material>
    <selfCollide>True</selfCollide>
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  
  <!-- Link7 -->
  <gazebo reference="link_7">
    <material>Gazebo/Blue</material>
    <selfCollide>True</selfCollide>
    <!--<kp>1000000.0</kp>
    <kd>100.0</kd>-->
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>



  <!-- Ros2 Control Interface  -->

  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
      <parameters>/home/qy/robot_operation/reinforcement_ws/src/cobot_description/config/effort_controller.yaml</parameters>
    </plugin>
  </gazebo>
    
  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    
    <joint name="joint_1">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

    <joint name="joint_2">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

    <joint name="joint_3">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

    <joint name="joint_4">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

    <joint name="joint_5">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

    <joint name="joint_6">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

    <joint name="joint_7">
      <command_interface name="effort">
        <param name="min">-500</param>
        <param name="max">500</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/> 
    </joint>

  </ros2_control>

</robot>

and this is my node

#!/usr/bin/env python3

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

import math
import time
from time import time
from time import sleep

from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
from math import sin,cos,atan2,sqrt,fabs,pi



class CobotJointTorquesPublisher(Node):

        def __init__(self):
                super().__init__('Qybot_joint_torques_publisher')
                self.get_logger().info('Joint_toque_node has been started.')
                self.sub1 = self.create_subscription(JointState,'/joint_states',self.joint_callback,10)
                self.sub1  # prevent unused variable warning
                #Define publishers for each joint torque controller commands.
                self.pub1 = self.create_publisher(Float64MultiArray,'/effort_controllers/commands', 10)
                self.torque1 = float(input("Enter the desired torque for joint1 in Nm : "))
                self.torque2 = float(input("Enter the desired torque for joint2 in Nm : "))
                self.torque3 = float(input("Enter the desired torque for joint3 in Nm : "))
                self.torque4 = float(input("Enter the desired torque for joint4 in Nm : "))
                self.torque5 = float(input("Enter the desired torque for joint5 in Nm : "))
                self.torque6 = float(input("Enter the desired torque for joint6 in Nm : "))
                self.torque7 = float(input("Enter the desired torque for joint7 in Nm : "))
		
        def joint_callback(self, message):
        	
                msg = Float64MultiArray()
                msg.data = [self.torque1,self.torque2,self.torque3,self.torque4,self.torque5,self.torque6,self.torque7]

                self.pub1.publish(msg)
                self.get_logger().info('Publishing: "%s"' % msg.data)                        
                sleep(0.5)
                
	
def main(args = None):
        rclpy.init(args=args)

        rrbot_joint_torques_publisher = CobotJointTorquesPublisher()

        rclpy.spin(rrbot_joint_torques_publisher)

        rrbot_joint_torques_publisher.destroy_node()
        rclpy.shutdown()



#Main section of code that will continuously run unless rospy receives interuption (ie CTRL+C)
if __name__ == '__main__':
	try: 
		main()
	except rospy.ROSInterruptException: 
		pass
	rclpy.spin()
2

Could you please use the robotics stackexchange (successor of answers.gazebosim.org) for the technical questions related to the use of Gazebo Newest 'gazebo' Questions - Robotics Stack Exchange?

Thanks.