Xacros, macros and using properties for moveit with the Universal Robots [closed]

edit

Hello all,

Background/Context: I am in the process of writing a xacro file that calls x2 URs. A UR10 and a UR5, each with different end effectors on them.

My ideal goal is to control both arms with Moveit and have them move at the same time as well as plan paths that will not result in collisions with each other. The end-effectors on these robots will be working within close proximity together on a process. So collision avoidance will be important.

From what I've read and understood, moveit will only be able to plan for two manipulators if they are tied to the same "robot". I can't have two instances of moveit running and have them plan with respect to each other, so I am trying to import both arms into a global frame as a single arm.

I am able to build the xacro and use check_urdf to build the link trees and verify they look correct from the world frame, to each parallel arm and their respective manipulators.

My main issue: When I try and use the MoveIt setup assistant, I see this preview. As you can see the UR5(the one on the left) is being stretched out to the link lengths of the UR10.

When I swap their import order in the high level xacro file I see:

As you can see, the UR10 (right again) is now crunched up to have the link lengths of the UR5.

I am importing these URDF xacro models FROM Universal Robot's ROS package. Namely these two files:

https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_description/urdf/ur10.urdf.xacro

https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_description/urdf/ur5.urdf.xacro

I think that there is some sort of inheritance issue due to the usage of the property tag in the XMLs:

From the UR5 urdf:

<property name="ur5_d1" value="0.089159" />
  <property name="ur5_a2" value="-0.42500" />
  <property name="ur5_a3" value="-0.39225" />
  <property name="ur5_d4" value="0.10915" />
  <property name="ur5_d5" value="0.09465" />
  <property name="ur5_d6" value="0.0823" />

  <!-- Arbitrary offsets for shoulder/elbow joints -->
  <property name="shoulder_offset" value="0.13585" />  <!-- measured from model -->
  <property name="elbow_offset" value="-0.1197" /> <!-- measured from model -->

  <!-- link lengths used in model -->
  <property name="shoulder_height" value="${ur5_d1}" />
  <property name="upper_arm_length" value="${-ur5_a2}" />
  <property name="forearm_length" value="${-ur5_a3}" />
  <property name="wrist_1_length" value="${ur5_d4 - elbow_offset - shoulder_offset}" />
  <property name="wrist_2_length" value="${ur5_d5}" />
  <property name="wrist_3_length" value="${ur5_d6}" />

and the UR10:

 <!-- Kinematic model -->
  <!-- Properties from urcontrol.conf -->
  <property name="ur10_d1" value="0.1273" />
  <property name="ur10_a2" value="-0.612" />
  <property name="ur10_a3" value="-0.5723" />
  <property name="ur10_d4" value="0.163941" />
  <property name="ur10_d5" value="0.1157" />
  <property name="ur10_d6" value="0.0922" />

  <!-- Arbitrary offsets for shoulder/elbow joints -->
  <property name="shoulder_offset" value="0.220941" />  <!-- measured from model -->
  <property name="elbow_offset" value="-0.1719" /> <!-- measured from model -->

  <!-- link lengths used in model -->
  <property name="shoulder_height" value="${ur10_d1}" />
  <property name="upper_arm_length" value="${-ur10_a2}" />
  <property name="forearm_length" value="${-ur10_a3}" />
  <property name ...