Help! I can't get jog_arm to work

[blubbi321]

Hey there,
very glad I found your code. However, Im struggling to get it to run on the UR5 model available at [1]. Not sure whether its the right model at all though .. if so people seem to have renamed a lot of the frames. Anyways it would be nice if jog_arm supported that model :)

[1] http://wiki.ros.org/ur5_moveit_config

[AndyZe]

That's a good idea but it will take me a while to test on a stock UR5. Good news is, I'm 99% sure it will work for you. It's been used on very old and very new models.

The only changes should be in ./config/jog_settings.yaml, and it's probably just a matter of removing the prefixes (delete "right_"). Also, rostopic list might help you find the right topic names.

You could also display the tf frames in RViz. I'm not sure what "right_ur5_base_link" should change to... Maybe "base_link" or "ur5_base_link"

move_group_name should be "manipulator"

[blubbi321]

Thanks for your quick reply! I think I have it almost working, however I cant seem to figure out where to publish the JointTrajectory Messages to.

Just for reference what I've done before is to run

roslaunch ur_gazebo ur5.launch limited:=true
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true limited:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true

as suggested at the bottom of the readme here https://github.com/ros-industrial/universal_robot

My jog_settings.yaml currently contains

jog_arm_server:
  simulation: false # Whether the robot is started in simulation environment
  collision_check: false # Check collisions?
  command_in_topic:  jog_arm_server/delta_jog_cmds
  command_frame:  base_link  # TF frame that incoming cmds are given in
  incoming_command_timeout:  5  # Stop jogging if X seconds elapse without a new cmd
  joint_topic:  joint_states
  move_group_name:  manipulator
  singularity_threshold:  5.5  # Slow down when the condition number hits this (close to singularity)
  hard_stop_singularity_threshold: 12. # Stop when the condition number hits this
  command_out_topic:  arm_controller/atm_joint_speed
  planning_frame:  base_link
  low_pass_filter_coeff:  2.  # Larger --> trust the filtered data more, trust the measurements less.
  publish_period:  0.01  # 1/Nominal publish rate [seconds]
  scale:
    linear:  0.0004  # Max linear velocity. Meters per publish_period. Units is [m/s]
    rotational:  0.0008  # Max angular velocity. Rads per publish_period. Units is [rad/s]
  # Publish boolean warnings to this topic
  warning_topic:  jog_arm_server/warning

I cant see how I would use rostopic list to find out the topic name that I should publish to so that it gets subscribed my the right entity.

[AndyZe]

OK, so you're simulating in Gazebo. Here's the config file I use for Gazebo simulation:

jog_arm_server:
simulation: true # Whether the robot is started in simulation environment
collision_check: true # Check collisions?
command_in_topic: jog_arm_server/delta_jog_cmds
command_frame: spacenav # TF frame that incoming cmds are given in
incoming_command_timeout: 20 # Stop jogging if X seconds elapse without a new cmd
joint_topic: joint_states
move_group_name: manipulator
singularity_threshold: 5.5 # Slow down when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 10. # Stop when the condition number hits this
command_out_topic: arm_controller/command
planning_frame: base_link
low_pass_filter_coeff: 2. # Larger c --> trust the filtered data more, trust the measurements less.
publish_period: 0.01 # 1/Nominal publish rate [seconds]
scale:
linear: .004
rotational: .008
warning_topic: jog_arm_server/warning

Some differences:

• simulation should be set to 'true' for Gazebo simulation
• command_out_topic is different
• my command_frame is different because I'm using a SpaceNav controller.

You'll probably see that it's a little jerky in Gazebo, and that's due to the slow ros_control action server. If you run it on a hardware robot, it should be completely smooth.

[AndyZe]

Does rostopic list show a topic called arm_controller/command?

[baba5000]

Hi i'm trying to use your package with a UR3 but i have this error :

[ERROR] [1534428209.921992408, 644.454000000]: Client [/gazebo] wants topic /arm_controller/command to have datatype/md5sum [trajectory_msgs/JointTrajectory/65b4f94a94d1ed67169da35a02f33d3f], but our version has [std_msgs/String/992ce8a1687cec8c8bd883ec73ca41d1]. Dropping connection.`

[AndyZe]

Hi @baba5000,

Can you quickly check that you are on the 'kinetic' branch? If you are on the 'kinetic-urscript' branch, it would explain that error.

[baba5000]

Thank you, it was the problem ! So, it works well with Gazebo now but not with the real UR3. I obtain this when i use rostopic list :

/attached_collision_object
/clicked_point
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/follow_joint_trajectory/cancel
/follow_joint_trajectory/feedback
/follow_joint_trajectory/goal
/follow_joint_trajectory/result
/follow_joint_trajectory/status
/initialpose
/joint_states
/joint_temperature
/move_base_simple/goal
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/recognized_object_array
/rosout
/rosout_agg
/rviz/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz/motionplanning_planning_scene_monitor/parameter_updates
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/tf
/tf_static
/tool_velocity
/trajectory_execution_event
/ur_driver/URScript
/ur_driver/io_states
/ur_driver/robot_status
/wrench

With gazebo the topic 'arm_controller/command' appears but not here. I think I don't use the appropriate command_out_topic but I don't know what is the right topic to put.

[AndyZe]

I don't see an obvious JointTrajectory message topic on that list. Can you go through and see if any of the message types are `JointTrajectory'? Like,
rostopic info /tool_velocity
You can also run rqt_graph to see what topics are going to the robot controller.

If there isn't any JointTrajectory message, then you can use the kinetic-urscript branch on the hardware robot, and the kinetic branch for simulation. The only difference is kinetic-urscript sends strings instead of JointTrajectory msgs.

Can I ask what UR driver you're using? Is it this ur_modern_driver package? Thanks!

[blubbi321]

Hey there .. its been a while and I almost have things working now (or at least to me it looks like that ;))
Still trying to get the UR5 arm to jog in rviz/gazebo ..

What I did:

• used the package from https://wiki.ros.org/ur_gazebo (installed the packages via apt on kinetic)
• (just to test if everything is fine) path planning via the rviz interface works
• checked out and built a recent version of jog_arm
• config

gazebo: true # Whether the robot is started in a Gazebo simulation environment
collision_check: true # Check collisions?
command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale: # Only used if command_in_type=="unitless"
  linear:  0.004  # Max linear velocity. Meters per publish_period.
  rotational:  0.008  # Max angular velocity. Rads per publish_period.
  joint: 0.05  # Max joint angular/linear velocity. Rads or Meters per publish period.

# topics
cartesian_command_in_topic:  jog_arm_server/delta_jog_cmds # Topic for xyz commands
joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for angle commands

command_frame: ee_link   # TF frame that incoming cmds are given in

incoming_command_timeout:  20  # Stop jogging if X seconds elapse without a new cmd
joint_topic:  joint_states

move_group_name:  manipulator  # Often 'manipulator' or 'arm'

lower_singularity_threshold:  30  # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity
hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity
lower_collision_proximity_threshold: 0.1 # Start decelerating when a collision is this far [m]
hard_stop_collision_proximity_threshold: 0.005 # Stop when a collision is this far [m]

planning_frame: base_link  # The MoveIt! planning frame. Often 'base_link'

low_pass_filter_coeff: 2.  # Larger-> more smoothing to jog commands, but more lag.
#publish_period: 0.008  # 1/Nominal publish rate [seconds]
publish_period: 0.01  # 1/Nominal publish rate [seconds]
publish_delay: 0.005  # delay between calculation and execution start of command
collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often.

# Publish boolean warnings to this topic
warning_topic: jog_arm_server/warning
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.

command_out_topic: arm_controller/command

# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController)
# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
command_out_type: trajectory_msgs/JointTrajectory
# Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false

In the meantime I also got myself a spacemouse and this seems to emit the control messages correctly on the corresponding topics. However, the robot arm control only works to a degree, ie the arm shakes madly (in Gazebo) when I move the mouse. After mouse movement it is still roughly in the same position as before.

Messages on the jog_arm output in this state report "Close to a singularity. Halting." while the UR5 gazebo stdout spams "Dropping first 2 trajectory point(s) out of 30, as they occur before the current time. First valid point will be reached in 0.005s" when there is spacemouse input.

Now Im wondering whether or not Im using the correct command_frame and if my control messages might be asking for too large motions.

Here is the output of rostopic list for reference:

/arm_controller/command
/arm_controller/follow_joint_trajectory/cancel
/arm_controller/follow_joint_trajectory/feedback
/arm_controller/follow_joint_trajectory/goal
/arm_controller/follow_joint_trajectory/result
/arm_controller/follow_joint_trajectory/status
/arm_controller/state
/attached_collision_object
/calibrated
/clock
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/joint_states
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/rosout
/rosout_agg
/tf
/tf_static
/trajectory_execution_event

[Obbart]

Hi, I'm trying to use jog_arm (branch kinetic) in cooperation with ur_modern_driver, ros_control and moveIT to move a UR10e arm. Using rviz and moveit APIs everything works fine and the arm moves as it should.
I wrote a simple node that publishes twistStamped messages on /jog_arm_server/delta_jog_cmds to test the setup but the robot does not move.
Using rqt i noticed that there is no connection between jog_arm_server and /pos_based_pos_traj_controller/command and even if the node publishes at a rate of 5Hz i get the message

[ WARN] [1550076563.892017326]: Stale or zero command. Try a larger 'incoming_command_timeout' parameter

rostopic echo shows that the messages are correctly published to the jog_arm_server.
Here is my jog_settings.yaml file:

gazebo: false                                        # Whether the robot is started in a Gazebo simulation environment
collision_check: true                                 # Check collisions?
command_in_type: "unitless"                         # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale: # Only used if command_in_type=="unitless"
  linear:  0.003                                      # Max linear velocity. Meters per publish_period.
  rotational:  0.006                                  # Max angular velocity. Rads per publish_period.
  joint: 0.01                                          # Max joint angular/linear velocity. Rads or Meters per publish period.
cartesian_command_in_topic:  /jog_arm_server/delta_jog_cmds         # Topic for xyz commands
joint_command_in_topic: /jog_arm_server/joint_delta_jog_cmds     # Topic for angle commands
command_frame: world                                  # TF frame that incoming cmds are given in
incoming_command_timeout: 30                         # Stop jogging if X seconds elapse without a new cmd
joint_topic:  joint_states
move_group_name:  manipulator                          # Often 'manipulator' or 'arm'
lower_singularity_threshold:  30                      # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity
hard_stop_singularity_threshold: 45                 # Stop when the condition number hits this. Larger --> closer to singularity
lower_collision_proximity_threshold: 0.1             # Start decelerating when a collision is this far [m]
hard_stop_collision_proximity_threshold: 0.005         # Stop when a collision is this far [m]
planning_frame: world                                  # The MoveIt! planning frame. Often 'base_link'
low_pass_filter_coeff: 2.0                            # Larger-> more smoothing to jog commands, but more lag.
publish_period: 0.008                                   # 1/Nominal publish rate [seconds]
publish_delay: 0.005                                   # delay between calculation and execution start of command
collision_check_rate: 5                              # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Publish boolean warnings to this topic
warning_topic: jog_arm_server/warning
joint_limit_margin: 0.1                             # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
command_out_topic: /pos_based_pos_traj_controller/command
# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController)
# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
command_out_type: trajectory_msgs/JointTrajectory
# Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false

and here the connections when the test node is down:

I'm new to ROS and I'm probably missing something obvious...
Thank you in advance

[AndyZe]

@blubbi321 I can help a little. It does sound like you are very close.

Messages on the jog_arm output in this state report "Close to a singularity. Halting." while the UR5 gazebo stdout spams "Dropping first 2 trajectory point(s) out of 30, as they occur before the current time. First valid point will be reached in 0.005s" when there is spacemouse input.

Gazebo probably launches with the arm fully extended, right? All joint angles at 0. That's why you get this singularity warning. You can plan/execute a move in RViz to a better position and this warning should go away.

In Gazebo, you'll always get this warning about points being dropped. No need to worry about it.

[AndyZe]

@Obbart I think you are the first to try this on an E-series. 👍

It's suspicious on the rqt_plot that the jog_arm_server has connections to pickup/action_topics and place/action_topics. Why would that be?

Does your command have a proper time stamp? Is it all zeros?

If those suggestions ^ don't help, I would try debugging with a print statement at line ~1044 of jog_arm_server.cpp. In JogROSInterface::deltaCartesianCmdCB. Something like:

ROS_ERROR_STREAM(msg);

That will at least let you know it's receiving the message correctly.

[blubbi321]

@AndyZe Thanks for your help - and the encouragement! Got it to work now on the UR5 :) (changed scale limits and the command_frame)
Will see if I can get it to work on my custom arm next!

[Obbart]

It's suspicious on the rqt_plot that the jog_arm_server has connections to pickup/action_topics and place/action_topics. Why would that be?

I really don't know, every time I start jog_arm these connections appear on the graph, I suppose that is an unexpected behavior since I have not modified the package and I'm not using pick/place actions anywere.
However I managed to get it working now, timestamps were part of the problem.
Now the robot is moving but not as expected; sending always the same delta command of 0.05 m/s, in x direction for example, the motion is very slow.
I changed the jog_settings.yaml using really high values but nothing changes.
I'm working with URsim for these tests, to be safe..

command_in_type: "speed_units"                         # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale: # Only used if command_in_type=="unitless"
  linear:  0.1                                      # Max linear velocity. Meters per publish_period.
  rotational:  0.5                                 # Max angular velocity. Rads per publish_period.
  joint: 0.5

[EdwardAbrosimov]

Hello! So, after reading the entire discussion, I still set up the jog_arm to work with ur3 and ps4 joystick. That's what I'm doing:

1. Launch this .launch-file:

<launch>
  <include file="$(find ur_gazebo)/launch/ur3.launch">
    <arg name="limited" value="true"/>
  </include>

  <include file="$(find ur3_moveit_config)/launch/ur3_moveit_planning_execution.launch">
		<arg name="sim" value="true"/>    
		<arg name="limited" value="true"/>
  </include>

  <include file="$(find ur3_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
  </include> 
</launch>

2. I wrote a node similar to xbox_to_twist.cpp and the corresponding launch-file. In the node I do about the following:

    geometry_msgs::TwistStamped twist;
    twist.header.stamp = ros::Time::now();
    twist.twist.linear.x = msg->axes[3]; // axis_Right_stick_horizontal
    twist.twist.linear.y = msg->axes[4]; // axis_Right_stick_vertical
    twist.twist.linear.z = msg->axes[7]; // axis_D_PAD_vertical

    twist.twist.angular.x = msg->axes[0]; // axis_Left_stick_horizontal
    twist.twist.angular.y = msg->axes[1]; // axis_Left_stick_verital
    twist.twist.angular.z = msg->axes[6]; // axis_D_PAD_horizontal

3. And my config file for jog_node is:

gazebo: true # Whether the robot is started in a Gazebo simulation environment
collision_check: true # Check collisions?

command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale: # Only used if command_in_type=="unitless"
  linear:  0.01  # Max linear velocity. Meters per publish_period.
  rotational:  0.01  # Max angular velocity. Rads per publish_period.
  joint: 0.05  # Max joint angular/linear velocity. Rads or Meters per publish period.

cartesian_command_in_topic:  jog_arm_server/delta_jog_cmds # Topic for xyz commands
joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for angle commands

command_frame:  base_link  # TF frame that incoming cmds are given in
incoming_command_timeout:  10  # Stop jogging if X seconds elapse without a new cmd
joint_topic:  joint_states
move_group_name: manipulator  # Often 'manipulator' or 'arm'
lower_singularity_threshold:  30  # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity
hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity
lower_collision_proximity_threshold: 0.1 # Start decelerating when a collision is this far [m]
hard_stop_collision_proximity_threshold: 0.005 # Stop when a collision is this far [m]

planning_frame: base_link  # The MoveIt! planning frame. Often 'base_link'
low_pass_filter_coeff: 2.  # Larger-> more smoothing to jog commands, but more lag.
publish_period: 0.008  # 1/Nominal publish rate [seconds]
publish_delay: 0.005  # delay between calculation and execution start of command
collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Publish boolean warnings to this topic
warning_topic: jog_arm_server/warning
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
command_out_topic: arm_controller/command
# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController)
# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
command_out_type: trajectory_msgs/JointTrajectory
# Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false

It works fine, but first you need to move to a position away from the singularities. I move to the "up" (in Rviz->MotionPlanning->Planning->Query->Goal State->up.) position and then move down a little along the z axis.