UR Kinematics malfunctions if URDF base link is different

[atyshka]

I have a UR10 on a mobile base and I've created a new URDF with the UR atop my robot. In the SRDF I have base_footprint (the root of this urdf) specified as a virtual joint. I wouldn't mind planning in the UR base frame, but that's not an option as MoveIt only works if it's planning in the URDF base frame. Here's the problem: when I attempt to plan to the cursor location, the kinematics plugin plans to the cursor location in the base_link frame instead of the base_link frame. I thought I did something wrong with my moveit config but sure enough, everything works fine if I use the KDL plugin. Since it seems this plugin is frame-unaware, is there something in the plugin code I need to change to get this to function properly? See the videos below:

Correct behavior in KDL:

In UR Kinematics:

[atyshka]

UPDATE: After digging through the code, I found this in ur_moveit_plugin.cpp:

bool URKinematicsPlugin::initialize(const std::string &robot_description, const std::string& group_name, const std::string& base_frame, const std::string& tip_frame, double search_discretization) { ROS_INFO("Kinematics base frame %s", base_frame); setValues(robot_description, group_name, base_frame, tip_frame, search_discretization); ... more stuff ... }

If I change the code to call setValues with my "base_footprint" frame instead of the frame passed in initialize(), I get correct behavior. It would seem the issue is that initialize is called with the wrong base frame. But how can that be if the KDL Plugin works fine?

[atyshka]

@kphawkins When you wrote this plugin, you clearly intended for base frames other than the UR to be used. I noticed around line 608 in ur_moveit_plugin that you have some transform code. Can you provide some insight on how you configured your srdf with this to get a working result?

[shaun-edwards]

Related: moveit/moveit#1014

In short, KDL is able to handle other frames, but that's not supposed to happen. There is some discussion related to this in the PR I reference above.

[atyshka]

@shaun-edwards I did check out that pr. What do you mean by KDL is able to handle other frames, but that’s not supposed to happen? There should be a way to get this to work on a mobile base. I don’t mind if I can only plan in the UR link frames, it’s just that somehow the frames are getting mixed up in Rviz. The original KDL packaged with moveit handles the mobile base just fine, it’s something with UR KinematicsPlugin that doesn’t work right.

[shaun-edwards]

See the discussion here: moveit/moveit#1014 (comment)

[atyshka]

@shaun-edwards In that post @gavanderhoorn argues that in general the IK/FK shouldn’t be expected to handle other frames than it was designed for. I get that. My problem is with the fact that UR KinematicsPlugin claims to be able to do this. The lines around 608 in ur_moveit_plugin are specifically in there to handle extra stuff in the URDF. The first post in the PR actually mentions this “shim layer” in ur kinematics.

[kphawkins]

Check out the implementation here: https://github.com/gt-ros-pkg/excel-util/tree/hydro/excel_moveit_config My application used a single linear rail joint between the base link and the root of the UR kinematic chain called table_rail_joint. I think the SRDF here might help guide you.

…

On Tue, Oct 30, 2018 at 4:31 PM atyshka ***@***.***> wrote: @shaun-edwards <https://github.com/shaun-edwards> In that post @gavanderhoorn <https://github.com/gavanderhoorn> argues that in general the IK/FK shouldn’t be expected to handle other frames than it was designed for. I get that. My problem is with the fact that UR KinematicsPlugin *claims* to be able to do this. The lines around 608 in ur_moveit_plugin are specifically in there to handle extra stuff in the URDF. The first post in the PR actually mentions this “shim layer” in ur kinematics. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#389 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAYlK9TeBN696s272O6QxVAR0XYO-J3wks5uqLcygaJpZM4X86Pw> .

[atyshka]

@kphawkins thanks for the info. Here is my SRDF. I have one group that includes everything from the base of my mobile base to the tip of the UR10 and another that includes gripper stuff. Please note that I have remapped the ur10 base link from base_link to ur10_base_link, and base link is now the base of my whole robot. I’ve changed all references in ur_kinematics to ur10_base_link too. Theoretically this is exactly the same as your “excel” planning group.

[UmutUz]

@atyshka did you ever find a fix for that?