Failure to use Moveit package to control KR6R900 via RSI interface

[monykhosrvi]

I have successfully managed to move the robot after establishing an RSI interface and installing RT-Preeemt kernel for my PC.

in order to move the robot I load an Moveit! RViz Plugin package built with KR6R900 xacro file.

after that I launch
roslaunch kuka_rsi_hw_interface test_hardware_interface.launch sim:=false

[ INFO] [1521192109.017223355]: Got connection from robot
[INFO] [WallTime: 1521192109.024914] Started controllers: position_trajectory_controller, joint_state_controller

by establishing the connection and giving to real time priority to kuka_rsi_hw_interface
without problem, I can move each joint using:
rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller

after establishing the RSI connection, the 3D model of the robot in Moveit interface, shows the initial position of the robot somewhere strange (all joints at 0).

this problem, results in failing the possibility to command the robot from Moveit, as it can not find a feasible solution.

[ERROR] [1521191669.111218235]: Found empty JointState message
[ERROR] [1521191669.111253879]: Found empty JointState message
[ INFO] [1521191669.135948573]: Found a contact between 'link_1' (type 'Robot link') and 'link_4' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1521191669.136117037]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1521191669.136177080]: Start state appears to be in collision with respect to group ALL_HAND
[ WARN] [1521191669.440121515]: Unable to find a valid state nearby the start state (using jiggle fraction of 0.050000 and 100 sampling attempts). Passing the original planning request to the planner.
[ERROR] [1521191669.440678395]: Found empty JointState message
[ERROR] [1521191669.456379933]: Found empty JointState message
[ INFO] [1521191669.458346701]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1521191669.459705318]: No planner specified. Using default.
[ INFO] [1521191669.471234226]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1521191669.474916260]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1521191669.476581331]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.477486431]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.481797118]: LBKPIECE1: Motion planning start tree could not be initialized!
[ INFO] [1521191669.483058267]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.483480308]: LBKPIECE1: Skipping invalid start state (invalid state)
[ WARN] [1521191669.484615941]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.487371590]: LBKPIECE1: Motion planning start tree could not be initialized!
[ERROR] [1521191669.488962577]: LBKPIECE1: Motion planning start tree could not be initialized!
[ INFO] [1521191669.489875834]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.490197610]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.490518865]: LBKPIECE1: Motion planning start tree could not be initialized!
[ WARN] [1521191669.491006016]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.018775 seconds
[ INFO] [1521191669.491432781]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.491616590]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.491778512]: LBKPIECE1: Motion planning start tree could not be initialized!
[ INFO] [1521191669.491981629]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.492134078]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.492289074]: LBKPIECE1: Motion planning start tree could not be initialized!
[ INFO] [1521191669.492444350]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.492609990]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.493308512]: LBKPIECE1: Motion planning start tree could not be initialized!
[ INFO] [1521191669.493532308]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.493711371]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.493868313]: LBKPIECE1: Motion planning start tree could not be initialized!
[ WARN] [1521191669.494043061]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.002787 seconds
[ INFO] [1521191669.494279450]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.494441234]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.494595569]: LBKPIECE1: Motion planning start tree could not be initialized!
[ INFO] [1521191669.494876843]: LBKPIECE1: Attempting to use default projection.
[ WARN] [1521191669.495049480]: LBKPIECE1: Skipping invalid start state (invalid state)
[ERROR] [1521191669.495211510]: LBKPIECE1: Motion planning start tree could not be initialized!
[ WARN] [1521191669.495393061]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.001210 seconds
[ INFO] [1521191669.502281036]: Unable to solve the planning problem
[ INFO] [1521191669.531462806]: ABORTED: No motion plan found. No execution attempted.

initial position defined in my ros_rsi.src is

; Move to start position
PTP {A1 0, A2 -90, A3 90, A4 0, A5 90, A6 0}

the same as the setting in the XACRO file. exapmle:

    <joint name="${prefix}joint_a2" type="revolute">
      <origin xyz="0.025 0 0" rpy="0 -1.5708 0"/>
      <parent link="${prefix}link_1"/>
      <child link="${prefix}link_2"/>
      <axis xyz="0 1 0"/>
      <limit effort="0" lower="${-DEG2RAD * 190}" upper="${DEG2RAD * 45}" velocity="${DEG2RAD * 300}"/>

the parameter "robot_description" is not loaded in the test launch file as it has been already loaded in the Moveit package.

Does anyone know that what the problem is ?

[gavanderhoorn]

Please revert the changes you made to the xacro macro. If the driver works correctly, there is no need to make any changes.

In fact, I have a suspicion that those changes are what is causing you trouble.

[monykhosrvi]

thanks @gavanderhoorn, I will modify the xacro macro file as soon as I get back to the Lab and will update the post afterwards.

[monykhosrvi]

I reverted the xacro macro to the default value (fully stretched)and now when I launch the moveit plug-in the robot posture is shown correctly.

when i try to move the robot from Rviz gui, plan and execute it, the robot in Rviz moves but the actual robot does not move accordingly.

is it due to the fake trajectory execution and my launch file ?

[gavanderhoorn]

Yes, most likely. The fact that MoveIt's robot state display also doesn't correspond to the actual state of your robot probably indicates that the launch file you use is not correctly setup (ie: MoveIt is not using the JointState msgs from your driver, but an internal dummy model).

[monykhosrvi]

would you please give more hints about composing a correct launch file to load the robot description, Rviz , Moveit etc. ?

[gavanderhoorn]

Two things (or datastreams really) are important here:

1. joint states going into moveit
2. trajectory coming out of moveit

For the first, you'll need:

1. an entity publishing sensor_msgs/JointState msgs.
2. moveit configured to listen to the topic that is used there

For the second:

1. make moveit not use the fake controllers
2. make sure the driver required the appropriate action server

For both of these, you could take at #17 for some examples, specifically config/controllers.yaml and the moveit_planning_execution.launch.

See also the Create a MoveIt Package for an Industrial Robot tutorial, specifically the Update Configuration Files and following sections.

---

Edit: you could also take a look at gavanderhoorn/kuka_experimental/kr6r900sixx_moveit_rsi_convenience/kuka_kr6r900sixx_moveit_config, but note that it's really just an example, and especially the way the RSI interface is 'integrated' is not something you should copy.

[monykhosrvi]

@gavanderhoorn thanks for the instruction ,

I have gone through the steps:
after launching:
roslaunch kuka_kr6r900sixx_moveit_config moveit_planning_execution.launch sim=false

roslaunch kuka_rsi_hw_interface test_hardware_interface.launch sim:=false
and establishing RSI connection, after moving the robot in Rviz, and plan and execute it, I receive the below error:

[ INFO] [1523016012.883187659]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016012.884364606]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016012.885047970]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016012.885503426]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016012.885688650]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016012.886930985]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016012.887198952]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016012.887754779]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016012.887859642]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016012.987797546]: LBKPIECE1: Created 68 (29 start + 39 goal) states in 58 cells (29 start (29 on boundary) + 29 goal (29 on boundary))
[ INFO] [1523016012.990716361]: LBKPIECE1: Created 128 (90 start + 38 goal) states in 117 cells (89 start (89 on boundary) + 28 goal (28 on boundary))
[ INFO] [1523016013.065994998]: LBKPIECE1: Created 120 (41 start + 79 goal) states in 108 cells (41 start (41 on boundary) + 67 goal (67 on boundary))
[ INFO] [1523016013.095896603]: LBKPIECE1: Created 154 (42 start + 112 goal) states in 140 cells (40 start (40 on boundary) + 100 goal (100 on boundary))
[ INFO] [1523016013.177916020]: ParallelPlan::solve(): Solution found by one or more threads in 0.294600 seconds
[ INFO] [1523016013.179070097]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016013.179665727]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016013.180324094]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016013.180620750]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016013.181445499]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016013.181831020]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016013.184236936]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016013.184696869]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016013.318927581]: LBKPIECE1: Created 95 (76 start + 19 goal) states in 82 cells (74 start (74 on boundary) + 8 goal (8 on boundary))
[ INFO] [1523016013.328627150]: LBKPIECE1: Created 125 (45 start + 80 goal) states in 113 cells (45 start (45 on boundary) + 68 goal (68 on boundary))
[ INFO] [1523016013.363589478]: LBKPIECE1: Created 117 (30 start + 87 goal) states in 106 cells (30 start (30 on boundary) + 76 goal (76 on boundary))
[ INFO] [1523016013.408516813]: LBKPIECE1: Created 132 (48 start + 84 goal) states in 121 cells (47 start (47 on boundary) + 74 goal (74 on boundary))
[ INFO] [1523016013.838916916]: ParallelPlan::solve(): Solution found by one or more threads in 0.659913 seconds
[ INFO] [1523016013.839942501]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016013.840462603]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016013.841032940]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1523016013.841761686]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1523016013.861060423]: LBKPIECE1: Created 36 (13 start + 23 goal) states in 26 cells (13 start (13 on boundary) + 13 goal (13 on boundary))
[ INFO] [1523016013.883196836]: LBKPIECE1: Created 61 (27 start + 34 goal) states in 51 cells (26 start (26 on boundary) + 25 goal (25 on boundary))
[ INFO] [1523016014.058510812]: ParallelPlan::solve(): Solution found by one or more threads in 0.218627 seconds
[ INFO] [1523016014.059166113]: SimpleSetup: Path simplification took 0.000432 seconds and changed from 3 to 2 states
[ INFO] [1523016014.061910797]: Returned 0 controllers in list
[ERROR] [1523016014.062450396]: Unable to identify any set of controllers that can actuate the specified joints: [ joint_a1 joint_a2 joint_a3 joint_a4 joint_a5 joint_a6 ]
[ERROR] [1523016014.062698106]: Known controllers and their joints:

[ERROR] [1523016014.062975857]: Apparently trajectory initialization failed
[ INFO] [1523016014.083216778]: ABORTED: Solution found but controller failed during execution

my controllers.yaml is as:

controller_list:
  - name: "position_trajectory_controller"
    action_ns: controller_manager
    default: true
    type: FollowJointTrajectory
    joints: [joint_a1, joint_a2, joint_a3, joint_a4, joint_a5, joint_a6]

when I move the robot via rqt_joint_trajectory_controller I can see the robot moving also in Rviz.

[gavanderhoorn]

Do you see any errors or warning when starting moveit_planning_execution.launch about not being able to find certain action servers?

I'm not sure immediately what your issue is, but I would verify that first.

---

The example moveit config I linked you to follows the "ROS-Industrial specifications" for robot drivers. In this particular case that means that there are certain expectations about locations of action server topics and namespaces.

The controllers.yaml you show does not follow those conventions, and if you're reusing the Moveit config I linked to then that will not work. That's why I emphasised that these files are just examples, you'll still need to make sure that the topics etc match.

You could take a look at kuka_kr6r900sixx_moveit_config/config/controllers.yaml and see whether a setup like that works better. Note the absence of the name (or: the zero-length name).

[monykhosrvi]

Problem solved!

updating the controllers.yaml as:

controller_list:
  - name: ""
    action_ns: /position_trajectory_controller/follow_joint_trajectory/
    type: FollowJointTrajectory
    joints: [joint_a1, joint_a2, joint_a3, joint_a4, joint_a5, joint_a6]

@gavanderhoorn thank you again for you help.

[gavanderhoorn]

I'm not sure where the additional namespace is coming from, but if it works for you, then +1.

[gavanderhoorn]

Seeing as you've appeared to have resolved your problems, I'm closing this issue.