kr6r900sixx_moveit_rsi_convenience not working for a KUKA kr10r1100

[cschindlbeck]

I am not able to run a real KUKA Agilus with the kr6r900sixx_moveit_rsi_convenience package f77934c5321ec71bfceb49f3e4d16aa93db90733. I have a KUKA kr10r1100 but the joint names etc should be the same, so i don't think this is causing the problem. The only difference are two link lengths (Link 2 and Link 4) After launching the file $ roslaunch kuka_kr6r900sixx_moveit_config moveit_planning_execution_rsi.launch sim:=false robot_ip:=KUKA_KRC4_IP i don't see Waiting for robot which is what i would expect from However, the kuka_hardware_interface (kuka_rsi_hw_interface/kuka_hardware_interface_node) is started according to ROS.

I have tested the connection and connectivity to the real robot with

$ roscore 
$ roslaunch kuka_rsi_hw_interface test_hardware_agilus.launch sim:=false
$ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller

and it works as expected.

[gavanderhoorn]

Can you tell me which commit you're at? Make sure to have checked out the correct branch, then do a git rev-parse HEAD.

Also, please re-run with:

roslaunch --screen kuka_kr6r900sixx_moveit_config moveit_planning_execution_rsi.launch sim:=false robot_ip:=KUKA_KRC4_IP

and copy/paste all console output in a reply to this issue (format it as code please).

[cschindlbeck]

I'm at f77934c5321ec71bfceb49f3e4d16aa93db90733

 git branch
* (detached from gavanderhoorn/kr6r900sixx_moveit_rsi_convenience)
  indigo-devel
  moveit_test

Edit: I just noticed /robot_ip_address and /rsi/address are the same, is that correct, shouldn't they be distinct ( /rsi/address = HOST-PC)?

* /robot_ip_address: <KUKA_KRC4_IP>
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /rsi/address: <KUKA_KRC4_IP>

See:

robot_interface_streaming.launch (line 52)

  <rosparam subst_value="true">
    rsi:
      address: <host_ip>
      port: $(arg robot_port)
  </rosparam>

[gavanderhoorn]

Is KUKA_KRC4_IP something that resolves to an IP?

[cschindlbeck]

I just changed that for here, in my code there is a hardcoded IP address at that place

edit: replaced KUKA_KRC4_IP with typical notation

[gavanderhoorn]

Ah ok, I typically format that as <KUKA_KRC4_IP> in those cases.

And yes, there is a mistake in the launch file, rsi/address should be the local address, not the address of the robot.

Could you hard-code that in the launch file (here) to test, and report back if that works?

[gavanderhoorn]

We might want to rename those parameters to reflect the fact that they configure the local socket, not something on the robot.

[gavanderhoorn]

57.

[cschindlbeck]

Ok so i hard-coded the host-ip in the launch file. It still doesn't work to move the robot via moveit, however i suspect a different error. The robot is moved to (0,-90,90,0,0,90,0) first in the ros_rsi.src file. The default pose in moveit is at (0,0,0,0,0,0). Executing a path results in a huge discrepancy which the KUKA controller prompts: too much kinetic energy in A2,A3,A5. Shouldn't moveit read the current robot state from the encoders?

Basically i get an error: Stop because $SPEED_LIMIT_TEACH_MODE has been exceeded.

[gavanderhoorn]

Shouldn't moveit read the current robot state from the encoders?

yes, it should.

Could you post a screenshot showing the pose of the robot in RViz right after you've started up everything? So before you try to do any planning? Does that correspond to the pose of your real hw?

Could you also include the output of:

rostopic echo -n1 /joint_states

and (only need a single set of lines from the output):

rosrun tf tf_echo base tool0

[gavanderhoorn]

Thanks for debugging this with me btw. This is exactly why I wanted to make sure those launch files actually work, before merging them into the main development branches.

[tingelst]

This is due to the velocity limits in the moveit config is specified based on the maximum velocity of the manipulator. Reduce the max velocity using the velocity scaling factor in the move_group.

[gavanderhoorn]

Also a possibility, but as the convenience wrapper launch file remaps some topics, I'd like to make sure I didn't botch up something. If the initial state of the robot doesn't correspond to what RViz shows, I think we need to solve that first.

[cschindlbeck]

rostopic echo -n1 /joint_states:

header: 
  seq: 850
  stamp: 
    secs: 1468318055
    nsecs: 557080030
  frame_id: ''
name: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6']
position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
velocity: []
effort: []

rosrun tf tf_echo base tool0:

At time 1468318064.457
- Translation: [0.980, 0.000, 0.435]
- Rotation: in Quaternion [0.000, 0.707, 0.000, 0.707]
            in RPY (radian) [0.000, 1.571, 0.000]
            in RPY (degree) [0.000, 90.000, 0.000]

Actual robot position is at (0,-90,90,0,90,0)

Moveit:

KUKA SmartPad: Stop by %CORRECTION function Originator: A2

[gavanderhoorn]

@tingelst: any idea?

Seems strange that joint_states shows [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], while physical hw is at (0,-90,90,0,90,0)?

[gavanderhoorn]

@cschindlbeck wrote:

Actual robot position is at (0,-90,90,0,90,0)

and what are the Cartesian coordinates of the flange / tcp?

[gavanderhoorn]

@cschindlbeck: also: you wrote things worked for you when using the rqt_joint_trajectory_controller. Could you try that again, and then check joint_states and the tf_echo output?

[cschindlbeck]

Hm when i use the rqt_joint_trajectory_controller i see the correct values with rostopic echo -n1 /joint_states :

header: 
  seq: 1547
  stamp: 
    secs: 9157
    nsecs: 117042708
  frame_id: ''
name: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6']
position: [3.490658503988659e-06, -1.5699987113267353, 1.5699969659974833, 1.7453292519943296e-06, 1.570002201985239, 4.0142572795869576e-05]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

[tingelst]

My best guess is the topic remapping in the kuka_hardware_interface_node here

https://github.com/gavanderhoorn/kuka_experimental/blob/kr6r900sixx_moveit_rsi_convenience/kuka_rsi_hw_interface/launch/robot_interface_streaming.launch

[cschindlbeck]

Sorry, i must have done something wrong:

This is my output of rostopic echo /joint_states with moveit now

header: 
  seq: 815
  stamp: 
    secs: 10702
    nsecs: 490011353
  frame_id: ''
name: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6']
position: [1.7453292519943296e-06, -1.5707928361363925, 1.5707910908071407, -1.7453292519943296e-06, 1.5708033081119046, 6.632251157578452e-05]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

When want to move the robot i get the following errors:

Maximum kinetic energy A2, A3, A5 and Command gear torque A2, A3, A5 and Command motor torque A2, A3, A5

I still bet this is due to a false init pose: Here an updated screenshot

[gavanderhoorn]

@cschindlbeck: ok, so just to make sure: with the convenience launch files you get the right joint states, but the MoveIt start (?) / goal (?) state doesn't correspond to your robot?

@tingelst: hm, but that doesn't remap the joint_states topic, and @cschindlbeck sees the correct joint states now.

[gavanderhoorn]

@cschindlbeck: the 'stretched out' robot is the MoveIt! query goal state. Could you enable visualisation of the query start state and check whether it corresponds to the current robot state (the transparent robot)?

If it doesn't, could you try clicking the Update button on the Planning tab under Select Start State (make sure to set it to current first).

These are all not solutions, I'm just trying to get some insight here.

[cschindlbeck]

I can report a minor success, i have made the robot move with MoveIt! To do so i had to move the solid model in MoveIt / rViz located at all-zeros manually by drag-and-drop to the translucent pose "init". Therefore, the robot does not want to move large distances (see kinetic energy error). I still think MoveIt doesn't see the init pose? Isn't translucent goal state and solid start state? My robot moved very fast although i specified Velocity Scaling: 0,01 and Accelartion Scaling 0,01 and then came to a halt, again $SPEED_LIMIT_TEACH_MODE has been exceeded. So the velocity scaling did not work as expected.

**Abbreviations:

• "init": (0,-90,90,0,0,90,0)
• "all-zeros": (0,0,0,0,0,0,0)

[gavanderhoorn]

@cschindlbeck wrote:

I still think MoveIt doesn't see the init pose? Isn't translucent goal state and solid start state?

No (assuming default settings): the translucent one is the current state based on joint states (so should reflect the real hardware). The 'solid' one should be the Query Goal State.

Could you check my previous two comments and confirm things there?

[cschindlbeck]

Yeah sorry, it was my bad, jumps and abortions by the KUKA controller were cause by not setting the start state accordingly. Therefore it jumped at the initial state. It works now as expected, thanks alot @gavanderhoorn and @tingelst for your time and effort into this project.

It would be convenient to include the start pose in the srdf:

<group_state name="init" group="manipulator">
    <joint name="joint_a1" value="0" />
    <joint name="joint_a2" value="-1.5708" />
    <joint name="joint_a3" value="1.5708" />
    <joint name="joint_a4" value="0" />
    <joint name="joint_a5" value="1.5708" />
    <joint name="joint_a6" value="0" />
</group_state>

Maybe you could include that and commit / push or even merge to main repo. The rsi_address needs to be hardcoded and is still faulty in the current commit, but i think this issue is adressed in #57 such that i can close this one.

Thanks a lot everyone!

[gavanderhoorn]

Thanks for reporting back @cschindlbeck. Glad that it turned out to be a user problem in the end, but it might be a good thing to document this somewhere.

[gavanderhoorn]

I'll definitely need to fix the incorrect IP set for rsi/address.

re: named pose in moveit cfg: yes, I figured that'd be a good idea as well. But it is heavily dependent on what users configure as the initial pose in the RSI cfg files though.

[Prawee682]

I am doing moveit rviz with KUKA KR16, I use RSI but I don't use KRC4 which I use KRC2. This method (means used robot_interface_streaming.launch of RSI and create moveit_planning_execution.launch) can be used?

If it works, I need your help. I'd like to know how the code needs to be modified to make it work with KRC2.