I use “/effort_controllers/commands” to public torque to my robot but when i check /joint_states, i rcv different
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()