search

UTNuclearRoboticsPublic / jog_arm

A real-time robot arm jogger.
45 stars 22 forks source link

Help! I can't get jog_arm to work #39

Open blubbi321 opened 6 years ago

blubbi321 commented 6 years ago

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 commented 6 years ago

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/jogsettings.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 commented 6 years ago

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 commented 6 years ago

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:

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 commented 6 years ago

Does rostopic list show a topic called arm_controller/command?

baba5000 commented 6 years ago

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]. Droppingconnection.`

AndyZe commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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 commented 5 years ago

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:

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 commented 5 years ago

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: rosgraph

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

AndyZe commented 5 years ago

@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 commented 5 years ago

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

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 commented 5 years ago

@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 commented 5 years ago

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 commented 5 years ago

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.