jogging with real UR3

[EdwardAbrosimov]

I was able to set up a jog_arm for my simulation of ur3 #39 , but now, when I try to run it all on a real robot, it does not work. The main question is how to make the MoveIt! to do jogging with the help of a jyotik. Any ideas? Has anyone tried jog arm on a real robot?

I use ubntu18.04, ROS Melodic, ur_modern_driver and ur3_moveit_config.

Here is my .launch:

  <include file="$(find ur_modern_driver)/launch/ur3_bringup.launch">
    <arg name="robot_ip" value="192.168.102.100"/>
  </include>

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

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

and my jog_arm config;

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.001  # Max linear velocity. Meters per publish_period.
  rotational:  0.001  # Max angular velocity. Rads per publish_period.
  joint: 0.005  # 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.004  # 1/Nominal publish rate [seconds]
publish_delay: 0.002  # 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

I know that “command_out_topic: arm_controller / command” is the wrong decision, because I am not connected to anything in node_grath, but I don’t know where to send messages.

I tried to write a node that would take my trajectory_msgs / JointTrajectory, create trajectory_msgs / FollowJointTrajectoryActionGoal from them and send it to the same topic as for the move_group node after planning ("/follow_joint_trajectory/goal"), but it did not work.

I would appreciate any help.

[machinekoder]

You might take a look at the examples: https://github.com/UTNuclearRoboticsPublic/jog_arm/tree/kinetic/jog_arm/src/jog_arm/teleop_examples

[EdwardAbrosimov]

Yes, it was for this example that I wrote a node for the ps4 joystick:

#include "geometry_msgs/TwistStamped.h"
#include "jog_msgs/JogJoint.h"
#include "ros/ros.h"
#include "sensor_msgs/Joy.h"

namespace to_twist
{
class ps4ToTwist
{
public:
  ps4ToTwist() : spinner_(1)
  {
    joy_sub_ = n_.subscribe("joy", 1, &ps4ToTwist::joyCallback, this);
    twist_pub_ = n_.advertise<geometry_msgs::TwistStamped>("jog_arm_server/delta_jog_cmds", 1);
    joint_delta_pub_ = n_.advertise<jog_msgs::JogJoint>("jog_arm_server/joint_delta_jog_cmds", 1);

    spinner_.start();
    ros::waitForShutdown();
  };

private:
  ros::NodeHandle n_;
  ros::Subscriber joy_sub_;
  ros::Publisher twist_pub_, joint_delta_pub_;
  ros::AsyncSpinner spinner_;

  // Convert incoming joy commands to TwistStamped commands for jogging
  void joyCallback(const sensor_msgs::Joy::ConstPtr& msg)
  {
    // Cartesian jogging
    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

    // Joint jogging
    jog_msgs::JogJoint joint_deltas;
    // This example is for a Universal Robot UR3."elbow_joint" is the base joint.
    joint_deltas.joint_names.push_back("elbow_joint");
    // Button 5 (R1): positive
    // Button 4 (L1): negative
    joint_deltas.deltas.push_back(msg->buttons[5] - msg->buttons[4]);
    joint_deltas.header.stamp = ros::Time::now();

    twist_pub_.publish(twist);
    joint_delta_pub_.publish(joint_deltas);
  }
};
}  // end to_twist namespace

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ps4_to_twist");

  to_twist::ps4ToTwist to_twist;

  return 0;
}

It works in a simulator.
But the problem is that it doesn't work on a real robot. I renamed
command_out_topic: arm_controller / command to command_out_topic: follow_joint_trajectory / command, but the topic / follow_joint_trajectory / goal still empty anyway

[AndyZe]

Did you search for other likely command topics?
rostopic list | grep command

It sounds like you are using this launch file:
roslaunch ur_modern_driver ur3_bringup.launch robot_ip:=ROBOT_IP_ADDRESS

Try switching to the ros_control version:
roslaunch ur_modern_driver ur3_ros_control.launch robot_ip:=ROBOT_IP_ADDRESS

You should make sure that the velocity_controllers/JointGroupVelocityController controller type is active. You can switch to it by:

rosservice call controller_manager/switch_controllers start_this_controller stop_this_controller

^ That might be slightly wrong, I'm working from memory. Remember, you can use tab to see valid commands.

My jog settings for a UR3 are attached. Your command_out_topic might different -- use grep to find the right topic.

ur3_jog.yaml.txt

[EdwardAbrosimov]

Damn right, now it's works!

1. In ur3_ros_control need need to stop all controllers, except joint_group_vel_controller. Of course you can do this with rosrun rqt_controller_manager rqt_controller_manager.
2. In my case:

command_out_topic: joint_group_vel_controller/command
command_out_type: std_msgs/Float64MultiArray

and publish only joint velocity.
3. Launch this (obviously with yours right robot_ip):

<?xml version="1.0"?>
<launch>
  <include file="$(find ur_modern_driver)/launch/ur3_ros_control.launch">
    <arg name="robot_ip" value="192.168.102.100"/>
  </include>

  <!-- Launch moveit -->
  <include file="$(find ur3_moveit_config)/launch/move_group.launch">
    <arg name="limited" value="true"/>
  </include>

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

</launch>

And you jog_with_something launch-file.

P.S. Sometimes Rviz falls, but this is a completely different question. Thanks again for the right direction @AndyZe )