How to run jog_arm on UR5

[gachiemchiep]

Hello @AndyZe
Thank you for your great work.

I follow your tutorial at moveit_jog_arm but unfortunately, the robot didn't move at all. Here's a quick summary of what I'm trying for now.

• What I want to achieve
  Run jog_arm on gazebo and real UR5 robot

• What I'm trying now
  Run jog_arm on gazebo simulation.

• Detail

Follow tutorial at moveit_jog_arm , i can click "plan and execute" to move the robot to selected pose.

Create a custom "arm_controller_ur5.yaml" as follow

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
      elbow_joint: {trajectory: 0.1, goal: 0.1}
      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10
joint_group_position_controller:
  # type: position_controllers/JointGroupPositionController
  type: velocity_controllers/JointGroupVelocityController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint

I don't have the SpaceNavigator joystick so I used example jogging commands as in the tutorial

rostopic pub -r 100 /jog_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 0.0
    y: 0.01
    z: -0.05
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

The /jog_server/delta_jog_cmds topic does exist and has incoming message

(base) jil@jil-dev-truong:catkin_ws$ rostopic hz /jog_server/delta_jog_cmds
subscribed to [/jog_server/delta_jog_cmds]
WARNING: may be using simulated time
average rate: 100.101

But the robot didn't move at all. There isn't any error message, so I don't know what is wrong for now.

Would you mind take a look at this, and give me some help?
Thank you.

[caiobarrosv]

Did you have any error related to the change in joint_group_position_controller?

When I change from position_controllers/JointGroupPositionController to velocity_controllers/JointGroupVelocityController the following error appears when I try to launch ur5.launch

Could not load controller 'joint_group_position_controller' because controller type
'velocity_controllers/JointGroupVelocityController' does not exist

[AndyZe]

I'm guessing the yaml file you created isn't actually being used. Did you modify any of the launch files to load that yaml?

Instead of creating a new yaml file, what if you try adding your controller to the yaml that already exists? If you're using ur_modern_driver, the yaml to modify is here:

https://github.com/ros-industrial/ur_modern_driver/blob/master/config/ur5_controllers.yaml

Also, did you clone the package from this repo or did you get it from https://github.com/ros-planning/moveit/tree/master/moveit_experimental/moveit_jog_arm? You shouldn't use it from this repo anymore.

[AndyZe]

@caiobarrosv did you try sudo apt install ros-melodic-velocity-controllers?

@gachiemchiep what happens if you do rosservice call /controller_manager/list_controllers?

[gachiemchiep]

I'm guessing the yaml file you created isn't actually being used. Did you modify any of the launch files to load that yaml?

Instead of creating a new yaml file, what if you try adding your controller to the yaml that already exists? If you're using ur_modern_driver, the yaml to modify is here:

https://github.com/ros-industrial/ur_modern_driver/blob/master/config/ur5_controllers.yaml

Also, did you clone the package from this repo or did you get it from https://github.com/ros-planning/moveit/tree/master/moveit_experimental/moveit_jog_arm? You shouldn't use it from this repo anymore.

Hello @AndyZe
I use the source code pull from https://github.com/ros-planning/moveit/tree/master/moveit_experimental/moveit_jog_arm

based on https://github.com/ros-industrial/ur_modern_driver/blob/master/config/ur5_controllers.yaml
i didchange the arm_controller_ur5.yaml into following.

arm_controller:
  type: velocity_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
      elbow_joint: {trajectory: 0.1, goal: 0.1}
      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  125
  action_monitor_rate: 10
  gains:
    #!!These values are useable, but maybe not optimal
    shoulder_pan_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
    shoulder_lift_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
    elbow_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
    wrist_1_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
    wrist_2_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
    wrist_3_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
joint_group_position_controller:
  type: velocity_controllers/JointGroupPositionController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint

Even i installed theros-melodic-velocity-controllers i still have the same errors as @caiobarrosv .

[ERROR] [1577588824.370483067, 0.408000000]: Could not find joint 'shoulder_pan_joint' in 'hardware_interface::VelocityJointInterface'.
[ERROR] [1577588824.370568868, 0.408000000]: Failed to initialize the controller
[ERROR] [1577588824.370594169, 0.408000000]: Initializing controller 'arm_controller' failed
Error when loading 'arm_controller'
[ERROR] [1577588824.374547718, 0.412000000]: Could not start controller with name 'arm_controller' because no controller with this name exists

The ourput of rosservice call is as followed

(ros-melodic-py2) gachiemchiep:catkin_ws$ rosservice call /controller_manager/list_controllers
controller: 
  - 
    name: "joint_state_controller"
    state: "running"
    type: "joint_state_controller/JointStateController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::JointStateInterface"
        resources: []

Maybe this is a little awkward, but would you mind share your "arm_controller_ur5.yaml"? This is the only bottleneck now.

[Abduoit]

@gachiemchiep Did you solve the issue, could you please share the arm_controller_ur5.yaml file ?

[gachiemchiep]

@Abduoit
Sorry, I still couldn't solve the issue.

[Abduoit]

@gachiemchiep
I could solve the issue, you need to switch to velocity control as seen below. However, ur5, in gazebo, is not moving smoothly when it is attached with husky.

abdu@abdu-uoit:~/devel_ws$ rosservice call /controller_manager/list_controllers
controller: 
  - 
    name: arm_controller
    state: running
    type: velocity_controllers/JointTrajectoryController
    hardware_interface: hardware_interface::VelocityJointInterface
    resources: ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
  - 
    name: joint_state_controller
    state: running
    type: joint_state_controller/JointStateController
    hardware_interface: hardware_interface::JointStateInterface
    resources: []