0
$\begingroup$

Rosanswers logo

Hello,

we have a Staubli tx2_40 industrial robot with a CS8 controller and want to control it using ros. I created the xacro/urdf description, a moveit_config and a _gazebo package.

I use ros melodic on ubuntu 18.04.

Using the val3 driver i can connect to the CS8 controller and read the joint positions. Rviz visualizes the robot state. Following this tutorial (https://industrial-training-master.readthedocs.io/en/melodic/_source/session3/Build-a-Moveit!-Package.html) I created the staubli_tx2_40_planning_execution.launch. When launching this file I get the error that the action client is not connected.

[ WARN] [1622624709.062673834]: Waiting for arm_controller/follow_joint_trajectory to come up ... [ERROR] [1622624721.063007243]: Action client not connected: arm_controller/follow_joint_trajectory [ INFO] [1622624721.074742909]: Returned 0 controllers in list

I read about this problems in other questions. However I was not abl eto fix the rpoblem from that. Using gazebo as robot simulation works. In this configuration I am able to use moveit to plan trajectories and move the simulated robot.

The problem occurs when using indusrtial_robot_simulator or the val3 driver with the industrial_robot_client. This is the launch file:

<launch>
  <!-- The planning and execution components of MoveIt! configured to run -->
  <!-- using the ROS-Industrial interface. -->

  <!-- Non-standard joint names:
       - Create a file [robot_moveit_config]/config/joint_names.yaml
           controller_joint_names: [joint_1, joint_2, ... joint_N]
       - Update with joint names for your robot (in order expected by rbt controller)
       - and uncomment the following line: -->
  <rosparam command="load" file="$(find staubli_tx2_40_support)/config/joint_names.yaml"/>

  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
  <!--  - if sim=false, a robot_ip argument is required -->
  <arg name="sim" default="true" />
  <arg name="robot_ip" unless="$(arg sim)" />
  <arg name="use_industrial_robot_simulator" default="false"/>

  <!-- load the robot_description parameter before launching ROS-I nodes -->
  <include file="$(find staubli_tx2_40_moveit_config)/launch/planning_context.launch" >
   <arg name="load_robot_description" value="true" />
  </include>

  <!-- run the robot simulator and action interface nodes -->
  <group if="$(arg sim)">

    <group unless="$(arg use_industrial_robot_simulator)">
      <include file="$(find staubli_tx2_40_gazebo)/launch/staubli_tx2_40.launch" />
      <remap from="/follow_joint_trajectory" to="/arm_controller/follow_joint_trajectory"/>
    </group>

    <group if="$(arg use_industrial_robot_simulator)">
      <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
      <!-- publish the robot state (tf transforms) -->
      <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    </group>
  </group>

  <!-- run the "real robot" interface nodes -->
  <!--   - this typically includes: robot_state, motion_interface, and joint_trajectory_action nodes -->
  <!--   - replace these calls with appropriate robot-specific calls or launch files -->
  <group unless="$(arg sim)">
    <remap from="follow_joint_trajectory" to="joint_trajectory_action"/>
    <include file="$(find staubli_val3_driver)/launch/robot_interface_streaming.launch" >
      <arg name="robot_ip" value="$(arg robot_ip)"/>
    </include>
  </group>

  <include file="$(find staubli_tx2_40_moveit_config)/launch/move_group.launch">
    <arg name="publish_monitored_planning_scene" value="true" />
  </include>

 <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find staubli_tx2_40_moveit_config)/launch/moveit_rviz.launch">
    <arg name="rviz_config" value="$(find staubli_tx2_40_moveit_config)/launch/moveit.rviz"/>
  </include>

</launch>

This ist the output produced:

process[rosout-1]: started with pid [7059]
started core service [/rosout]
process[industrial_robot_simulator-2]: started with pid [7067]
process[joint_trajectory_action-3]: started with pid [7068]
process[robot_state_publisher-4]: started with pid [7069]
process[move_group-5]: started with pid [7070]
process[rviz_kaba_All_Series_7019_5421043519905626114-6]: started with pid [7076]
[ INFO] [1622623635.142176440]: Loading robot model 'staubli_tx2_40'...
[ WARN] [1622623635.142741175]: Skipping virtual joint 'FixedBase' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1622623635.142773904]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1622623635.194746205]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ INFO] [1622623635.196384182]: IK Using joint link_1 -3.14159 3.14159
[ INFO] [1622623635.196401766]: IK Using joint link_2 -2.18166 2.18166
[ INFO] [1622623635.196411470]: IK Using joint link_3 -2.40855 2.40855
[ INFO] [1622623635.196418858]: IK Using joint link_4 -4.71239 4.71239
[ INFO] [1622623635.196427415]: IK Using joint link_5 -2.0944 2.33001
[ INFO] [1622623635.196435571]: IK Using joint link_6 -4.71239 4.71239
[ INFO] [1622623635.196482942]: Looking in common namespaces for param name: arm/position_only_ik
[ INFO] [1622623635.197170328]: Looking in common namespaces for param name: arm/solve_type
[ INFO] [1622623635.197803875]: Using solve type Speed
[ INFO] [1622623635.228551897]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1622623635.229847210]: MoveGroup debug mode is OFF
Starting planning scene monitors...

[ INFO] [1622623635.229863712]: Starting planning scene monitor
[ INFO] [1622623635.230942894]: Listening to '/planning_scene'
[ INFO] [1622623635.230966784]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1622623635.231925917]: Listening to '/collision_object'
[ INFO] [1622623635.232983758]: Listening to '/planning_scene_world' for planning scene world geometry
[ERROR] [1622623635.233391553]: No sensor plugin specified for octomap updater 0; ignoring.
[ INFO] [1622623635.239756828]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1622623635.255029006]: Initializing OMPL interface using ROS parameters
[ INFO] [1622623635.266106468]: Using planning interface 'OMPL'
[ INFO] [1622623635.268001324]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1622623635.268247878]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1622623635.268514222]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1622623635.268749556]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1622623635.268968498]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1622623635.269185637]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1622623635.269231531]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1622623635.269251135]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1622623635.269266150]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1622623635.269273070]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1622623635.269292096]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1622623637.343343643]: rviz version 1.13.16
[ INFO] [1622623637.343383654]: compiled against Qt version 5.9.5
[ INFO] [1622623637.343394525]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1622623637.351185110]: Forcing OpenGl version 0.
[ WARN] [1622623640.277582609]: Waiting for arm_controller/follow_joint_trajectory to come up
[ INFO] [1622623643.641388991]: Stereo is NOT SUPPORTED
[ INFO] [1622623643.641432617]: OpenGL device: llvmpipe (LLVM 10.0.0, 256 bits)
[ INFO] [1622623643.641446991]: OpenGl version: 3,1 (GLSL 1,4).
[ WARN] [1622623646.277772362]: Waiting for arm_controller/follow_joint_trajectory to come up
[ INFO] [1622623647.324861354]: Loading robot model 'staubli_tx2_40'...
[ WARN] [1622623647.324919440]: Skipping virtual joint 'FixedBase' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1622623647.324949700]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1622623647.355366926]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ INFO] [1622623647.357584572]: IK Using joint link_1 -3.14159 3.14159
[ INFO] [1622623647.357630147]: IK Using joint link_2 -2.18166 2.18166
[ INFO] [1622623647.357684453]: IK Using joint link_3 -2.40855 2.40855
[ INFO] [1622623647.357722110]: IK Using joint link_4 -4.71239 4.71239
[ INFO] [1622623647.357758813]: IK Using joint link_5 -2.0944 2.33001
[ INFO] [1622623647.357773965]: IK Using joint link_6 -4.71239 4.71239
[ INFO] [1622623647.357792963]: Looking in common namespaces for param name: arm/position_only_ik
[ INFO] [1622623647.358561581]: Looking in common namespaces for param name: arm/solve_type
[ INFO] [1622623647.359281448]: Using solve type Speed
[ INFO] [1622623647.482876181]: Starting planning scene monitor
[ INFO] [1622623647.485085213]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1622623647.596309276]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ERROR] [1622623652.277927833]: Action client not connected: arm_controller/follow_joint_trajectory
[ INFO] [1622623652.287814525]: Returned 0 controllers in list
[ INFO] [1622623652.295099531]: Trajectory execution is managing controllers
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'...
[ INFO] [1622623652.314526758]: waitForService: Service [/get_planning_scene] is now available.
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
[ INFO] [1622623652.316195231]: Constructing new MoveGroup connection for group 'arm' in namespace ''
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1622623652.342410303]: 

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

[ INFO] [1622623652.342450758]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1622623652.342468612]: MoveGroup context initialization complete

You can start planning now!

I have the rosgraph as .dot and png but not allowed to upload files (>5).

This is the _moveit_controller_manager.launch.xml:

<launch>

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find staubli_tx2_40_moveit_config)/config/ros_controllers.yaml"/>
</launch>

This is the ros_controllers.yaml:

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: arm
  joint_model_group_pose: home
# 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_a1
    - joint_a2
    - joint_a3
    - joint_a4
    - joint_a5
    - joint_a6
  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_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - joint_a1
      - joint_a2
      - joint_a3
      - joint_a4
      - joint_a5
      - joint_a6

This is the robot srdf:

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="staubli_tx2_40">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="arm">
        <chain base_link="base_link" tip_link="tool0" />
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="home" group="arm">
        <joint name="joint_a1" value="0" />
        <joint name="joint_a2" value="0" />
        <joint name="joint_a3" value="0" />
        <joint name="joint_a4" value="0" />
        <joint name="joint_a5" value="0" />
        <joint name="joint_a6" value="0" />
    </group_state>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="FixedBase" type="fixed" parent_frame="world" child_link="base_link" />
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
    <disable_collisions link1="base_link" link2="link_2" reason="Never" />
    <disable_collisions link1="base_link" link2="link_3" reason="Never" />
    <disable_collisions link1="link_1" link2="link_2" reason="Adjacent" />
    <disable_collisions link1="link_2" link2="link_3" reason="Adjacent" />
    <disable_collisions link1="link_2" link2="link_4" reason="Never" />
    <disable_collisions link1="link_2" link2="link_5" reason="Never" />
    <disable_collisions link1="link_2" link2="link_6" reason="Never" />
    <disable_collisions link1="link_3" link2="link_4" reason="Adjacent" />
    <disable_collisions link1="link_3" link2="link_5" reason="Never" />
    <disable_collisions link1="link_3" link2="link_6" reason="Never" />
    <disable_collisions link1="link_4" link2="link_5" reason="Adjacent" />
    <disable_collisions link1="link_5" link2="link_6" reason="Adjacent" />
</robot>

Do I have to spawn a controller from ros control manually (I tried that but it didn't work so far) or ist the industrial_robot_client already doing that? Wehen I try to spawn a controller manually I get the error/ warning:

[INFO] [1622627768.107235]: Controller Spawner: Waiting for service controller_manager/load_controller
[WARN] [1622627798.216071]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[controller_spawner-4] process has finished cleanly

Can you explain how to use the val3 driver with the ros_industrial_core repository to make it work?

Thank you very much in advance!


Originally posted by bkaiser on ROS Answers with karma: 3 on 2021-06-02

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

How to get Staubli Val3 Driver to work with industrial_robot_simulator/ industrial_robot_client

at face value, this question doesn't really make sense. The industrial_robot_simulator is a Python ROS node which does not interact with real hw. So it cannot do anything with staubli_val3_driver.

The staubli_val3_driver)/launch/robot_interface_streaming.launch you already found starts all the required parts:

  1. the robot_state node, which publishes JointState messages
  2. the motion_streaming_interface node, which relays JointTrajectorys
  3. the joint_trajectory_action node, which exposes the FollowJointTrajectory action server

The industrial_robot_simulator combines all of those nodes in one single script, and pretends it's a real driver -- with the same ROS API (topics, services, actions).

MoveIt talks to the joint_trajectory_action node; it's essentially a FollowJointTrajectory client, if you configure it as such. That's what currently not working in your setup. Likely because the configuration on MoveIt's side is incorrect.

The ros_controllers.yaml file is a bit of a mess (not your fault). It's really unfortunate it mixes ros_control based controller and hardware interface configuration with "regular" controller interfaces in MoveIt. It makes it really difficult to diagnose these kinds of problems, if you don't already know what's going wrong.

staubli_val3_driver is not a ros_control based driver, so you cannot spawn controllers in it, nor is any of the ros_control or hardware_interface configuration used with it.

Your ros_controllers.yaml file tells MoveIt it should expect the FollowJointTrajectory server on arm_controller/follow_joint_trajectory, but the driver does not start the server there. It starts it on /follow_joint_trajectory. You'll either have to remap from arm_controller/follow_joint_trajectory to /follow_joint_trajectory -- as you already do for the staubli_tx2_40_gazebo)/launch/staubli_tx2_40.launch file -- or update the controller_list entry in your ros_controllers.yaml file to look like this (from fanuc_cr7ia_moveit_config/config/controllers.yaml, which uses the exact same ROS API):

controller_list:
  - name: ""
    action_ns: joint_trajectory_action
    type: FollowJointTrajectory
    joints: [...]

Some additional comments:

  • Staubli joints aren't named joint_aN (with N a natural number), but joint_N (with N a natural number) in robot support packages (look at the .xacros in staubli_tx2_60_support fi)
  • it's uncommon to mix real driver, industrial_robot_simulator and Gazebo usage in a single moveit_planning_execution.launch file
  • it's uncommon to repeat the robot variant name in the name of the moveit_planning_execution.launch file: it's already hosted by a package with the variant name in it, so there's no need to repeat it again (ie: staubli_tx2_40_moveit_config contains moveit_planning_execution.launch)

Originally posted by gvdhoorn with karma: 86574 on 2021-06-02

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by bkaiser on 2021-06-02:
Thank you very much for the quick reply. I will try to adopt the addressed changes and implement the nameing conventions. In addition, I will edit the title of the question.

Comment by gvdhoorn on 2021-06-02:
And a PR (or two) adding the staubli_tx2_40_support and related packages to ros-industrial/staubli_experimental would also be very much appreciated of course :)

Comment by bkaiser on 2021-06-02:
Once everything is working I will gladly contribute it.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.