Facing issues when trying to configure denso VS060 with moveit servo on ROS-1

[TheGodOfWar007]

Hi,

I am trying to set up the robot (a Denso VS060) for compliance control tasks with ROS, for which, I wanted to configure the MoveIt Servo package to work with the current version of denso_robot_ros. (MoveIt Servo Docs).

Before I proceed, here is some information about the environment I am using:

ROS Distro: Noetic
OS: Ubuntu 20.04 LTS
RAM: 16GB

MoveIt servo needs specific type of controllers from the ROS-Control package to be setup in the robot's moveit config, namely: JointGroupPositionControl and JointGroupVelocityControl. These controllers in turn need the PositionJointControl and VelocityJointControl hardware interfaces enabled for the robot respectively. However, the package only has the PositionJointControl interface configured in the URDF which just leaves the 1st JointGroupPositionControlas the option. This leads us to my issue: Replacing the defaultJointTrajectoryControllerby theJointGroupPositionControlcontroller in list of supported controllers indenso_robot_control.yaml` doesn't spawn the controller and the robot cannot be moved through the MoveIt GUI in Rviz.

I launch the robot in simulation mode through the following command:

roslaunch denso_robot_bringup vs060_bringup.launch

This is my denso_robot_control.yaml file:

 vs060:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 125

  arm_controller:
    type: "position_controllers/JointGroupPositionController"
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6

I have left the controllers.yaml to the default configuration, since I thought the same low level controller should work:

controller_manager_ns: controller_manager
controller_list:
  - name: vs060/arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]

This is the logger output on launch:

[ WARN] [1673811889.757424733]: Falling back to using the move_group node's namespace (deprecated Melodic behavior).
[ INFO] [1673811889.769159458]: Loading robot model 'vs060'...
[ INFO] [1673811889.769223151]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1673811889.968867560]: rviz version 1.14.19
[ INFO] [1673811889.969430313]: compiled against Qt version 5.12.8
[ INFO] [1673811889.969558900]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1673811889.990951562]: Forcing OpenGl version 0.
[INFO] [1673811890.242193, 0.000000]: Waiting for /clock to be available...
[ INFO] [1673811890.432015818]: Stereo is NOT SUPPORTED
[ INFO] [1673811890.432114512]: OpenGL device: Mesa Intel(R) UHD Graphics 620 (KBL GT2)
[ INFO] [1673811890.432151594]: OpenGl version: 4.6 (GLSL 4.6) limited to GLSL 1.4 on Mesa system.
[ INFO] [1673811890.586501037]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1673811890.588268914]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1673811890.682577868]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1673811890.684026664]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1673811890.754668153]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1673811890.757298645]: Listening to 'joint_states' for joint states
[ INFO] [1673811890.760935340]: Listening to '/attached_collision_object' for attached collision objects
[ INFO] [1673811890.760977918]: Starting planning scene monitor
[ INFO] [1673811890.762763493]: Listening to '/planning_scene'
[ INFO] [1673811890.762803142]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1673811890.764492832]: Listening to '/collision_object'
[ INFO] [1673811890.766635997]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1673811891.226304621, 0.003000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1673811891.251994570, 0.027000000]: Physics dynamic reconfigure ready.
[INFO] [1673811891.496387, 0.266000]: /clock is published. Proceeding to load the controller(s).
[INFO] [1673811891.498240, 0.267000]: Controller Spawner: Waiting for service controller_manager/load_controller
[spawn_urdf-5] process has finished cleanly
log file: /home/neelaksh/.ros/log/11df97d6-950d-11ed-8f52-49efd8f26302/spawn_urdf-5*.log
[ INFO] [1673811892.578225224, 0.364000000]: Loading gazebo_ros_control plugin
[ INFO] [1673811892.578354027, 0.364000000]: Starting gazebo_ros_control plugin in namespace: /vs060
[ INFO] [1673811892.578880402, 0.364000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [/robot_description] on the ROS param server.
[ERROR] [1673811892.693412669, 0.364000000]: No p gain specified for pid.  Namespace: /vs060/gazebo_ros_control/pid_gains/joint_1
[ERROR] [1673811892.694172037, 0.364000000]: No p gain specified for pid.  Namespace: /vs060/gazebo_ros_control/pid_gains/joint_2
[ERROR] [1673811892.694896561, 0.364000000]: No p gain specified for pid.  Namespace: /vs060/gazebo_ros_control/pid_gains/joint_3
[ERROR] [1673811892.695630012, 0.364000000]: No p gain specified for pid.  Namespace: /vs060/gazebo_ros_control/pid_gains/joint_4
[ERROR] [1673811892.696352135, 0.364000000]: No p gain specified for pid.  Namespace: /vs060/gazebo_ros_control/pid_gains/joint_5
[ERROR] [1673811892.697015979, 0.364000000]: No p gain specified for pid.  Namespace: /vs060/gazebo_ros_control/pid_gains/joint_6
[ INFO] [1673811892.701814784, 0.364000000]: Loaded gazebo_ros_control.
[ INFO] [1673811892.704449577, 0.367000000]: Loading planning pipeline ''
[INFO] [1673811892.706600, 0.369000]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1673811892.709431, 0.372000]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1673811892.711928, 0.374000]: Loading controller: joint_state_controller
[INFO] [1673811892.724390, 0.387000]: Loading controller: arm_controller
[ INFO] [1673811892.756673112, 0.419000000]: Using planning interface 'OMPL'
[ INFO] [1673811892.760983282, 0.424000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[INFO] [1673811892.761330, 0.424000]: Controller Spawner: Loaded controllers: joint_state_controller, arm_controller
[ INFO] [1673811892.761427895, 0.424000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1673811892.761804085, 0.424000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1673811892.762604304, 0.425000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1673811892.763087140, 0.426000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1673811892.763535951, 0.426000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1673811892.763612816, 0.426000000]: Using planning request adapter 'Add Time Optimal Parameterization'
[ INFO] [1673811892.763661011, 0.426000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1673811892.763691683, 0.426000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1673811892.763724563, 0.426000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1673811892.763759762, 0.426000000]: Using planning request adapter 'Fix Start State Path Constraints'
[INFO] [1673811892.768180, 0.431000]: Started controllers: joint_state_controller, arm_controller
[ INFO] [1673811894.729083611, 2.345000000]: Loading robot model 'vs060'...
[ INFO] [1673811894.729161155, 2.345000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1673811896.374986747, 3.949000000]: Starting planning scene monitor
[ INFO] [1673811896.379436405, 3.953000000]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1673811897.033363227, 4.595000000]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ WARN] [1673811897.904253795, 5.446000000]: Waiting for vs060/arm_controller/follow_joint_trajectory to come up
[ INFO] [1673811902.050558053, 9.474000000]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
[ INFO] [1673811902.050760674, 9.474000000]: Constructing new MoveGroup connection for group 'arm' in namespace ''
[ WARN] [1673811904.057450145, 11.446000000]: Waiting for vs060/arm_controller/follow_joint_trajectory to come up
[ERROR] [1673811910.184540524, 17.447000000]: Action client not connected: vs060/arm_controller/follow_joint_trajectory
[ INFO] [1673811910.196207945, 17.458000000]: Returned 0 controllers in list
[ INFO] [1673811910.228441745, 17.490000000]: Trajectory execution is managing controllers
[ INFO] [1673811910.228514137, 17.490000000]: MoveGroup debug mode is OFF
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1673811910.401372631, 17.656000000]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1673811910.402979645, 17.658000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1673811910.403041923, 17.658000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1673811911.459208309, 18.700000000]: Ready to take commands for planning group arm.

In the code block above, observe the error:

[ WARN] [1673811904.057450145, 11.446000000]: Waiting for vs060/arm_controller/follow_joint_trajectory to come up
[ERROR] [1673811910.184540524, 17.447000000]: Action client not connected: vs060/arm_controller/follow_joint_trajectory
[ INFO] [1673811910.196207945, 17.458000000]: Returned 0 controllers in list

The "action client not connected" error wasn't appearing before I replaced the controller. And now there is no controller spawned. Note that the PID gains error doesn't affect the output with the JointTrajectoryController so I am hoping that it can be ignored, unless it is mandatory for JointGroupPositionController, something I am not aware of. However, on checking the list of available controllers through rosservice call /vs060/controller_manager/list_controllers I get the following output:

controller: 
  - 
    name: "joint_state_controller"
    state: "running"
    type: "joint_state_controller/JointStateController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::JointStateInterface"
        resources: []
  - 
    name: "arm_controller"
    state: "running"
    type: "position_controllers/JointGroupPositionController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::PositionJointInterface"
        resources: 
          - joint_1
          - joint_2
          - joint_3
          - joint_4
          - joint_5
          - joint_6

One can see that the JointGroupPositionController is shown as running so I am really confused why it couldn't spawn through the controller_spawner. I am new to ROS-Control and the relatively extinct documentation of some of the ROS controllers doesn't help much. I will really appreciate it if I can get some help with a solution to this problem. Any leads to the right docs or forum to look at in case this is not the right place to ask about this will be really helpful too or in case I missed some good reference or documentation about ros controllers somewhere.

Thank you.

[DENSO-FASupport]

Dear @TheGodOfWar007 san,
Thank you for using Denso Robot.
I apologize for this late reply.
DENSO WAVE supports hydro, indigo, kineteic and melodic.
Noetic is not supported by DENSO WAVE yet.
Therefore, an error may have occurred.

I have a question.
The "action client not connected" error wasn't appearing before I replaced the controller.
But does this mean that the error did not occur before the controller was replaced, but it started to occur after the replacement?

I'm sorry to trouble you, but I appreciate your cooperation.
Thank you very much!

[TheGodOfWar007]

Hi. Thank you for replying @DENSO-FASupport . Yes the error wasn't appearing before I replaced the controller everything was working absolutely fine. I am able to get the robot to track waypoints using the default moveit pipeline from the repository this error only appears after changing the controller type.

Thank you for your time!

[DENSO-FASupport]

Dear @TheGodOfWar007 san,
Thank you for replying.
I am able to get the robot to track waypoints using the default moveit pipeline from the repository this error only appears after changing the controller type.
Does after changing the controller type mean hardware replacement? Or is it a launch file switch?

If it means a launch file switch, I think the file paths may be different.
The file vs060_bringup.launch is a sample file so anyone can run it, but if you have created another controller, could you create a new description file in ROSConverter and run it with the new path and new name of the launch file?
If you have any questions, could you feel free to contact us?
Thank you very much!

[TheGodOfWar007]

So my changing the controller I meant that I simple changed the type of ros-controller I was using. I apologize if my explanation was a bit vague earlier, I'll try to explain again: I wanted to change the ros-controller being used by the robot from: position_controllers/JointTrajectoryController to position_controllers/JointGroupPositionController in order to enable the MoveIt Servo node which needs the latter as mentioned in the MoveIt docs. I wanted to do this in order to enable real-time servoing with the VS060 arm, however, whenever I switch the controller to position_controllers/JointGroupPositionController, the controller does spawn but the \move_group node throws the action client not connected error. I am not changing the launch files, I am simply changing the ros-controller type in the denso_robot_control.yaml file. The controller_spawner node, according my understanding (feel free to correct me if I am wrong), should handle all the paths and node launches required for the specified ros-controller to bring up. But when MoveIt tries to launch the lower level controller, i.e. FollowJointTrajectory from the controllers.yaml file, it fails to do so when the controller type is position_controllers/JointGroupPositionController. It doesn't fail if I use the default position_controllers/JointTrajectoryController, with all the namespaces being correct, as can be inferred from the controllers.yaml and denso_robot_control.yaml files in the first post, I am unable to pinpoint why the lower level controller fails to spawn. So I am not making any hardware change, I am just changing the ros-controller MoveIt is supposed to use. I will really appreciate it if I could get some clarity on why this error is coming up?

In the meantime, I will try the ROSConverter solution. Thank you for your time and consideration @DENSO-FASupport.

EDIT: While mentioning FollowJointTrajectory in the comment above I referred to it as the lower level controller move it uses. It was a misunderstanding on my part. It is not a lower level controller it is just the action server the JointTrajectoryController uses to communicate with the robot's hardware interface. I have realized that the JointGroupPositionController might not even need an action server in the first place, it just needs to be spawned and moveit_servo's nodes should be able to find it's command topic. However, I am not sure how the denso_robot_control nodes will be able to interface with another controller. Some clarity on this will be very helpful. Thank you.

[DENSO-FASupport]

Dear @TheGodOfWar007 san,
Thank you for your kind instruction.
I apologize for this late reply.
If the sample VS060 was able to run before you changed the type of ros-controller in your Noetic environment, I think Noetic is able to do in MoveIt.
controlles.yaml is a file you don't need to edit.
https://github.com/DENSORobot/denso_robot_ros/blob/master/denso_robot_moveit_config/config/vs060_config/controllers.yaml
If you want to control another robot type, create a new one with ROSConverter.
■Method
https://wiki.ros.org/denso_robot_ros/Tutorials/How%20to%20control%20an%20RC8%20with%20MoveIt%21
https://wiki.ros.org/denso_robot_ros/ROSConverter

The Joint_limits.yaml file can get information from the controller with AcquireVelAcc.
https://github.com/DENSORobot/denso_robot_ros/blob/master/denso_robot_moveit_config/config/vs060_config/joint_limits.yaml

https://www.denso-wave.com/en/robot/download/application/rost.html
https://wiki.ros.org/denso_robot_ros/AcquireVelAcc
https://wiki.ros.org/denso_robot_ros/ROSConverter

Could you let me know should there be any issues?
I'm sorry to trouble you, but I appreciate your cooperation.
Thank you very much!

[TheGodOfWar007]

Thank you for the directives. I will post an update soon once I have exhausted all the suggested options.

[LnBdd]

Hi @TheGodOfWar007,
did you manage to solve the issue and get the servoing up and running?

I am pretty much in the same situation right now just using ROS melodic and a VS050 with a RC8A. I was able to make the servoing work in a gazebo simulation environment. However, the real robot does not react to the servoing commands at all. Checking all available nodes, topics, and their graph, the only obvious difference is the denso_robot_control node which replaces the gazebo node. Both are subscribed to the arm_controller/command topic (where the servo_server publishes the commands) and both publish to the same arm_controller/follow_joint_trajectory/XXX topics. I assume that the commands get stuck somewhere within the denso_robot_control node but was not able to figure it out, yet.
Checking the available services (rosservice list) for the controller_manager service and then checking the available controllers with rosservice call vs050_H2O2/controller_manager/list_controllers leads to the same output for sim and for the real robot:

controller: 
  - 
    name: "joint_state_controller"
    state: "running"
    type: "joint_state_controller/JointStateController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::JointStateInterface"
        resources: []
  - 
    name: "arm_controller"
    state: "running"
    type: "position_controllers/JointTrajectoryController"
    claimed_resources: 
      - 
        hardware_interface: "hardware_interface::PositionJointInterface"
        resources: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]

Any ideas on how to get the servoing to work?

Thanks a lot,
Leon

---

denso_robot_control.yaml:

vs050_H2O2: #arm_controller:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 100  

  # Position - Joint Position Trajectory Controllers -------------------
  arm_controller: #position_trajectory_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    constraints:
      goal_time: 2.0                  # Defaults to zero
      stopped_velocity_tolerance: 0.1 # Defaults to 0.01
      joint1:
        trajectory: 0
        goal: 0.2
      joint2:
        trajectory: 0
        goal: 0.2
      joint3:
        trajectory: 0
        goal: 0.2
      joint4:
        trajectory: 0
        goal: 0.2
      joint5:
        trajectory: 0
        goal: 0.2
      joint6:
        trajectory: 0
        goal: 0.2

    state_publish_rate:  50 # Defaults to 50
    action_monitor_rate: 20 # Defaults to 20

  # Position Controllers ---------------------------------------
  j1_position_controller:
    type: position_controllers/JointPositionController
    joint: joint_1
    pid: {p: 1.0, i: 0.0, d: 0.0}
  j2_position_controller:
    type: position_controllers/JointPositionController
    joint: joint_2
    pid: {p: 1.0, i: 0.0, d: 0.0}
  j3_position_controller:
    type: position_controllers/JointPositionController
    joint: joint_3
    pid: {p: 1.0, i: 0.0, d: 0.0}
  j4_position_controller:
    type: position_controllers/JointPositionController
    joint: joint_4
    pid: {p: 1.0, i: 0.0, d: 0.0}
  j5_position_controller:
    type: position_controllers/JointPositionController
    joint: joint_5
    pid: {p: 1.0, i: 0.0, d: 0.0}
  joint_6_position_controller:
    type: position_controllers/JointPositionController
    joint: joint_6
    pid: {p: 1.0, i: 0.0, d: 0.0}
 

initial:  # Define initial robot poses.
  - group: Arm
    pose: Ready_Video

ros_control.yaml (loaded through move_group.launch for sim and for real robot):

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: Arm
  joint_model_group_pose: Ready_Video
# Settings for ros_control_boilerplate control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
  joints:
    - joint_1
    - joint_2
    - joint_3
    - joint_4
    - joint_5
    - joint_6
  sim_control_mode: 1  # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

controller_manager_ns: controller_manager
controller_list:
  - name: vs050_H2O2/arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
#position_controllers/JointTrajectoryController
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    gains:
      joint_1:
        p: 100
        d: 1
        i: 1
        i_clamp: 1
      joint_2:
        p: 100
        d: 1
        i: 1
        i_clamp: 1
      joint_3:
        p: 100
        d: 1
        i: 1
        i_clamp: 1
      joint_4:
        p: 100
        d: 1
        i: 1
        i_clamp: 1
      joint_5:
        p: 100
        d: 1
        i: 1
        i_clamp: 1
      joint_6:
        p: 100
        d: 1
        i: 1
        i_clamp: 1

[TheGodOfWar007]

Hi @LnBdd ,

Sorry for the late reply, I just had a look at your issue. So in my case I was able to get the servo package to move the robot. However, I believe the behavior I am getting from moveit_servo wasn't as desired. I could only move the robot with collision checks turned off. I believe that I had ignored the relevant collision checks in the srdf file so I couldn't pin point the issue. Coming back to the point as to how I got the servoing to work:

What I did was use the JointGroupPositionController instead. I wrote my own YAML file listing the controllers and made a launch file of my own derived from the denso launch files. However, I later tried using the default controller i.e. the joint trajectory controller too, and I remember that it worked back then. I have forgotten some of the specifics since I am not working on the project anymore. However, I'll suggest disabling the action server, and disable spawning of the moveit planning and trajectory execution services & pipelines since you don't need them. This can be done by removing the corresponding launch includes and node launches from the denso_robot_ros package. If you want I can share an example launch file (the one I used for this). You have to still launch the move group and the planning scene since they are also used by moveit_servo for collision checks. Try cross checking your implementation.

If this doesn't solve it can you share the exact error message you receive when you run it on real hardware, if one is thrown? It'll be good if you can share more details about the exact error you are facing.