ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Problem with ROS control and Gazebo.

asked 2019-11-04 14:01:26 -0500

MichaelDUEE gravatar image

updated 2019-11-06 15:59:02 -0500

Hello,

I have a problem with controlling a URDF that I exported from SolidWorks. (Ubuntu 16.04 , Kinetic, Gazebo 7.x) I followed this tutorial and I wanted to implemented on my robot. All the controllers are starting correctly so as the Gazebo simulation also the Node publish the data correctly I have checked it with echo-ing the topic and with different values for the data. Is there a chance not working because the PID values ?

All the transmissions look like this :

<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Joint_1">
  <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
  <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  <mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

The controller is like this (for all joints) :

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: Joint_1
    pid: {p: 100.0, i: 0.01, d: 10.0}

And I have this node:

rospy.init_node('ArmMovement')
pub1=rospy.Publisher("/rrbot/joint1_position_controller/command",Float64,queue_size=10 )
rate = rospy.Rate(50)
ArmCor1= Float64()
ArmCor1.data=0
pub1.publish(ArmCor1)
rate.sleep()

Edit#1 : Part of URDF for the Joint_1:

 <joint name="Joint_1" type="revolute">
<origin
  xyz="0 0 -0.008"
  rpy="1.5708 0 0" />
<parent link="base_link" />
<child link="Link_1" />
<axis
  xyz="0 1 0" />
<limit
  lower="0"
  upper="3.14"
  effort="0"
  velocity="0" />
 </joint>
edit retag flag offensive close merge delete

Comments

What is the behavior that you are seeing? IS it vibrating?

pmuthu2s gravatar image pmuthu2s  ( 2019-11-04 14:48:07 -0500 )edit

@pmuthu2s It has a little vibration but I believe it occurs because the gripper of the arm is touching the ground. There is no movement further than that.

MichaelDUEE gravatar image MichaelDUEE  ( 2019-11-04 16:28:30 -0500 )edit

Are there any warnings or errors when running your simulation? Is Gazebo loading your controllers correctly, is it publishing on the topic "/rrbot/joint1_position_controller/state" and subscribing to "/rrbot/joint1_position_controller/command" as expected?

Jasmin gravatar image Jasmin  ( 2019-11-06 13:07:35 -0500 )edit

No I dont have any warning or errors. The controllers are starting correclty and the message is successfully transimitted. @Jasmin

MichaelDUEE gravatar image MichaelDUEE  ( 2019-11-06 14:36:44 -0500 )edit

Could you post your urdf or at least the joint definition part?

Jasmin gravatar image Jasmin  ( 2019-11-06 15:43:50 -0500 )edit

@Jasmin. Of course, I have edited the question!

MichaelDUEE gravatar image MichaelDUEE  ( 2019-11-06 15:56:21 -0500 )edit
1

That helps! The joint will never move with a velocity="0". You need to set non zero effort and velocity values, try something like:

<limit
  lower="0"
  upper="3.14"
  effort="2"
  velocity="2.0" />
Jasmin gravatar image Jasmin  ( 2019-11-07 01:42:37 -0500 )edit

@Jasmin Thank you a lot for your help, it actually works!!

MichaelDUEE gravatar image MichaelDUEE  ( 2019-11-12 13:59:02 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2019-11-12 15:40:18 -0500

MichaelDUEE gravatar image

Thank you guys for your help, it actually work after your comments. This was my Joint_1 :

 <joint name="Joint_1" type="revolute">
 <origin
 xyz="0 0 -0.008"
 rpy="1.5708 0 0" />
<parent link="base_link" />
<child link="Link_1" />
<axis
xyz="0 1 0" />
<limit
 lower="0"
 upper="3.14"
 effort="0"
 velocity="0" />
</joint>

I changed the limit section to this :

<limit
lower="0"
upper="3.14"
effort="2"
velocity="2.0" />

I have other problems like a very annoying shiver (that comes from effort value I think) but It is not for this topic.

edit flag offensive delete link more
1

answered 2019-11-05 01:47:10 -0500

gvdhoorn gravatar image

updated 2019-11-05 01:53:05 -0500

Seeing this:

rospy.init_node('ArmMovement')
pub1=rospy.Publisher("/rrbot/joint1_position_controller/command",Float64,,queue_size=10 )
rate = rospy.Rate(50)
ArmCor1= Float64()
ArmCor1.data=0
pub1.publish(ArmCor1)
rate.sleep()

It's likely that there simply isn't any chance for the Subscriber (on Gazebo's / ros_control's side) to actually receive the message. rate.sleep() is single-shot. There is no periodicity here, so after 1/50 seconds (ie: 20 milliseconds), your program terminates and the Publisher is destroyed with it.

In those 20 milliseconds, the Subscriber needs to:

  1. become aware of your Publisher
  2. register interest with the ROS master
  3. request a subscription
  4. set up the actual network connection
  5. negotiate with the Publisher to see whether it can actually exchange messages
  6. complete negotiation, and only then
  7. can it receive the message

Just to see whether this is indeed the problem, at a rospy.sleep(5) after the pub1=rospy.Publisher(..) line.

If things start working, you'll want to either make your script periodic, or use a state-based approach (ie: check there are subscribers) with rospy.Publisher.get_num_connections().

edit flag offensive delete link more

Comments

@gvdhoorn I made a while loop and still got the same results. I used echo to see if the message was published and the message was published.

MichaelDUEE gravatar image MichaelDUEE  ( 2019-11-05 02:26:41 -0500 )edit

Then I would suggest to not use a loop, but check with get_num_connections() you actually have subscribers. This would both make things state-based, as well as verify that you're publishing to the correct topic.

gvdhoorn gravatar image gvdhoorn  ( 2019-11-07 01:40:23 -0500 )edit

Question Tools

Stats

Asked: 2019-11-04 14:01:26 -0500

Seen: 1,881 times

Last updated: Nov 12 '19