attachtool
It is usually the case that we need to attach a tool to the robot and command it in Cartesian Space referring to the tool tip frame, or consider it with an kinematic solver to command it in Joint Space.
To do so, we need to extend the kinematic chain of the robot to take the tool into account.
That needs to be done both on the ROSCORE and the SUNRISE side, at least if you want to have control of a frame attached to the tool tip. Else, you could command the robot within the flange frame.
To give a better understading, we provide a small extension to the iiwa_stack, HERE.
It contains the robot description of an IIWA with a simple tool attached to it, HERE.
You could basically clone that package and adjust it to your needs.
The tool used is a very simple pointy tool that is pretty much useless, but its simple geometry helps to use it as an example. The tool was 3D printed and designed by us, here is a picture.
You might notice in the ROS package that we use names like "iiwa_tool" or "tool_link", "tool_joint".
We suggest you to use the same convention for your own tool, for example if you want to attach a gripper to the robot you might want to use "iiwa_gripper", "gripper_link" and "gripper_joint". Both on the ROSCORE and the SUNRISE side.
The core of the robot representation is the URDF file.
We need to extend the one used for a "mint" IIWA, adding the tool.
You can have a look at the result HERE
First, we give a name to our new robotic description. If our tool name is gripper, it is going to be iiwa7_gripper.
(here we use an iiwa7, but you could also use an iiwa14).
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="iiwa7_tool">Then, we define a base coordinate system in 3D space.
You could go for something simple, just defining the origin to be at 0,0,0 :
<link name="world"/>Or add some complexity. For example, our robot is placed on top of a wheeled cart, so we add its geometry to the environment and place the robot on top of that.
<link name="world">
<visual>
<origin xyz="0 0 0.42" rpy="0 0 0"/>
<geometry>
<box size="0.68 0.605 0.84"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0.42" rpy="0 0 0"/>
<geometry>
<box size="0.68 0.605 0.84"/>
</geometry>
</collision>
</link>This is both useful during visualization and it also allows MoveIt! to take that geometry into account during the trajectory generation.
Next, we add our robot into the scene :
<xacro:arg name="hardware_interface" default="PositionJointInterface"/>
<xacro:arg name="robot_name" default="iiwa"/>
<xacro:iiwa7 hardware_interface="$(arg hardware_interface)" robot_name="$(arg robot_name)" parent="world">
<origin xyz="0 0 0.84" rpy="0 0 0"/>
</xacro:iiwa7>The macro for the iiwa needs two parameters, the first two lines add a default value for those.
We know that our IIWA TCP frame is iiwa_link_ee, what we need next to to attach additional frames/links to it accordingly to the tool we are going to use. We need to define dummy joints between the links, just to have a well defined kinematic chain.
<!-- Here we define a dummy joint between the tip of the iiwa7 and the base of the tool.
There is no offset between the two, that means the tool is attached at the robot flange. -->
<joint name="tool_joint" type="fixed">
<parent link="iiwa_link_ee" />
<child link = "tool_link" />
<origin xyz="0 0 0" rpy="0 ${PI/2.0} 0" />
</joint>
<!-- Here we define the geometry of the tool. We designed the tool ourselves, so we have a mesh file that represents it.
Else, one can define it using a geometric representation that approximate it, like a cylinder (see commented lines) -->
<link name="tool_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://iiwa_tool_description/meshes/visual/tool.stl"/>
<!-- <cylinder length="0.1275" radius="0.020"/> -->
</geometry>
<material name="Grey"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://iiwa_tool_description/meshes/visual/tool.stl"/>
<!-- <cylinder length="0.1275" radius="0.020"/> -->
</geometry>
<material name="Grey"/>
</collision>
</link>
<!-- Here we define another dummy joint.
It is placed at the end of the tool (12.75 cm), so we can then attach a frame at its TCP for control -->
<joint name="tool_tip_joint" type="fixed">
<parent link="tool_link" />
<child link = "tool_link_ee" />
<origin xyz="0 0 0.1275" rpy="0 ${-PI/2.0} 0" />
</joint>
<!-- The TCP frame is here defined -->
<link name="tool_link_ee"/>We have now a frame tool_link_ee at the tip of the tool, which in this case is just 12.75 cm long.
Other tools might have a complex kinematic chains themselves, you might have already a URDF xacro macro that can be added as the one from the IIWA.
When commanding the robot with MoveIt! you can refer at tool_iiwa_ee as the endeffector link.
Additionally, it is a good idea to add another link.
Since ROS/MoveIt! convention is to have the x axis upward, we rotated the last frame to achieve that.
KUKA convention is different (Z upward), so if we want to send direct Cartesian commands referring to the tool tip frame to the KUKA controller, we need a frame for that.
<joint name="tool_tip_joint_kuka" type="fixed">
<parent link="tool_link" />
<child link = "tool_link_ee_kuka" />
<origin xyz="${tool_link_offset}" rpy="0 0 0" />
</joint>
<link name="tool_link_ee_kuka"/>Now we also have tool_link_ee_kuka, same position as tool_link_ee, but orientation accordingly to KUKA convention. Now, we will create the same frame in SUNRISE, to have a coherent configuration and allow a seamless control.
On Sunrise Workbench, you can create a Tool as described in the KUKA manuals.
Give it the same name that you use in the URDF, in our case "tool" (but for example "gripper" for a gripper).
Then, define a frame for it's tip (in our case, it is just a translation along z of 12.75 cm) and name it as the one in the URDF, for us : "tool_link_ee_kuka".
The last step is quite important, if you follow this convention you can just set the name of the tool you are going to use as a ROS parameter.
The Java code will look at it and use the respectively frame for Cartesian commands.
You just have to set a ROS parameter name /iiwa/toolName with the appropriate name.
You can do it by commandline : rosparam set iiwa/toolName tool
But it is better to do it from a launch file :
<arg name="tool_name" default="tool"/>
<param name="/iiwa/toolName" type="string" value="$(arg tool_name)" />For example you can use gripper instead of tool as argument value, on the Java side a frame named gripper_link_ee_kuka will be used (if defined correctly as described above).
❗ IMPORTANT ❗
You should create a launch file to load the robot description of the iiwa with the tool, an example is HERE.
Copy that file, rename it with the proper tool name and replace "iiwa_tool" with the proper name (e.g. iiwa_gripper).