Skip to content

Commit

Permalink
Merge pull request #39 from code-iai/noetic
Browse files Browse the repository at this point in the history
Noetic support
  • Loading branch information
ichumuh authored Jul 20, 2023
2 parents bf4d596 + 2a4b9da commit 6b4635f
Show file tree
Hide file tree
Showing 6 changed files with 62 additions and 82 deletions.
10 changes: 5 additions & 5 deletions iai_boxy_base_2/robots/base.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="2.0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.03" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<visual>
<geometry>
Expand All @@ -64,7 +64,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="2.0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.03" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<visual>
<geometry>
Expand All @@ -90,7 +90,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="2.0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.03" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<visual>
<geometry>
Expand All @@ -116,7 +116,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="2.0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.03" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<visual>
<geometry>
Expand Down Expand Up @@ -252,7 +252,7 @@
<geometry>
<box size="0.6 0.553 0.02" />
</geometry>
<material name="White" />
<material name="Grey" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
Expand Down
33 changes: 5 additions & 28 deletions iai_boxy_description/launch/display.launch
Original file line number Diff line number Diff line change
@@ -1,31 +1,8 @@
<launch>
<arg
name="model" />
<arg
name="gui"
default="True" />
<arg name="urdf-name" default="boxy_description.urdf.xacro"/>
<arg name="urdf-path" value="$(find iai_boxy_description)/robots/$(arg urdf-name)"/>
<arg name="gui" default="True" />
<include file="$(find iai_boxy_description)/launch/upload_boxy.launch" />

<include file="$(find iai_boxy_description)/launch/upload_boxy.launch">
<arg name="urdf-name" value="$(arg urdf-name)"/>
<arg name="urdf-path" value="$(arg urdf-path)"/>
</include>

<param
name="use_gui"
value="$(arg gui)" />
<node
name="joint_state_publisher"
pkg="joint_state_publisher"
type="joint_state_publisher" />
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="state_publisher" />
<!--node
name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find boxy_description)/urdf.rviz" /-->
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find iai_boxy_description)/urdf.rviz" />
</launch>
2 changes: 1 addition & 1 deletion iai_boxy_description/robots/boxy_description.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,6 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="boxy_description">

<xacro:include filename="$(find iai_boxy_description)/urdf/boxy_description.urdf.xacro"/>
<boxy/>
<xacro:boxy/>

</robot>
70 changes: 36 additions & 34 deletions iai_boxy_description/urdf/boxy_description.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
<xacro:property name="torso_joint_z" value="0.410"/>
<!-- /Property definitions e.g. constants -->
<xacro:include filename="$(find iai_boxy_base)/robots/base.URDF"/>
<xacro:property name="M_PI" value="${pi}" />
<xacro:macro name="boxy">
<!-- virtual joints to model odometry -->
<link name="odom"/>
Expand Down Expand Up @@ -89,9 +90,9 @@
<!-- NEW NECK ASSEMBLY -->
<!-- mounting plate of UR3 -->
<xacro:property name="ur3_prefix" value="neck_"/>
<adapter_boxy_ur3_xacro prefix="${ur3_prefix}" parent="base_link">
<xacro:adapter_boxy_ur3_xacro prefix="${ur3_prefix}" parent="base_link">
<origin xyz="0.175 -0.227 1.48" rpy="0 0 0"/>
</adapter_boxy_ur3_xacro>
</xacro:adapter_boxy_ur3_xacro>
<xacro:ur3_robot prefix="${ur3_prefix}" joint_limited="true" transmission_hw_interface="$(arg transmission_hw_interface)" kinematics_file="${load_yaml('$(arg kinematics_config)')}"/>

<joint name="${ur3_prefix}base_mounting_joint" type="fixed">
Expand All @@ -101,13 +102,13 @@
</joint>

<!-- The following URDF has frame_in / frame_out, so it is placed at the parent -->
<adapter_iso50_kinect2_xacro parent="neck_tool0" prefix="neck">
<xacro:adapter_iso50_kinect2_xacro parent="neck_tool0" prefix="neck">
<origin xyz="0 0 0" rpy="0 0 -1.570795"/>
</adapter_iso50_kinect2_xacro>
</xacro:adapter_iso50_kinect2_xacro>

<kinect2_xacro parent="neck_adapter_iso50_kinect2_frame_out" prefix="head_mount">
<xacro:kinect2_xacro parent="neck_adapter_iso50_kinect2_frame_out" prefix="head_mount">
<origin xyz="-0.031284 0.098627 0.023544" rpy="4.728550 6.279385 1.578295"/>
</kinect2_xacro>
</xacro:kinect2_xacro>


<!-- JOINT to connect the base with the torso -->
Expand All @@ -121,33 +122,33 @@
<xacro:include filename="$(find iai_boxy_torso)/robots/torso.URDF"/>

<!--FIXME: triangle_left_arm_link should be rotated, and not the origin of the arm (which right now is calib_left_arm_base_link -->
<kuka_lwr_arm parent="triangle_left_arm_link" prefix="left_" color_postfix="">
<xacro:kuka_lwr_arm parent="triangle_left_arm_link" prefix="left_" color_postfix="">
<!-- From CAD -->
<!--origin xyz="0 0 0" rpy="0 0 1.047198"/-->
<!-- from calibration on 20160903 -->
<origin xyz="-0.006185 0.004080 -0.010726" rpy="6.276354 6.281653 1.074679"/>
</kuka_lwr_arm>
<lwr4_cabling_adaptor parent="left_arm_flange_link" prefix="left_arm_">
</xacro:kuka_lwr_arm>
<xacro:lwr4_cabling_adaptor parent="left_arm_flange_link" prefix="left_arm_">
<origin xyz="0 0 0.01" rpy="0 0 0"/>
</lwr4_cabling_adaptor>
</xacro:lwr4_cabling_adaptor>

<!-- The following URDFs have frame_in / frame_out, so they are simply placed at their parent -->
<kms_40_xacro parent="left_arm_cabling_adaptor_link" prefix="left_arm_">
<xacro:kms_40_xacro parent="left_arm_cabling_adaptor_link" prefix="left_arm_">
<origin xyz="0 0 0" rpy="0 0 0"/>
<!--places the kms40 frame 13mm from the parent -->
</kms_40_xacro>
<adapter_kms40_fwk050_xacro parent="left_arm_kms40_frame_out" prefix="left_arm_">
</xacro:kms_40_xacro>
<xacro:adapter_kms40_fwk050_xacro parent="left_arm_kms40_frame_out" prefix="left_arm_">
<origin xyz="0 0 0" rpy="0 0 0"/>
</adapter_kms40_fwk050_xacro>
<fwk_fwa_050_xacro parent="left_arm_adapter_kms40_fwk050_frame_out" prefix="left_arm_">
</xacro:adapter_kms40_fwk050_xacro>
<xacro:fwk_fwa_050_xacro parent="left_arm_adapter_kms40_fwk050_frame_out" prefix="left_arm_">
<origin xyz="0 0 0" rpy="0 0 0"/>
</fwk_fwa_050_xacro>
<adapter_fwa050_wsg50_xacro parent="left_arm_fwk_fwa_050_frame_out" prefix="left_arm_">
</xacro:fwk_fwa_050_xacro>
<xacro:adapter_fwa050_wsg50_xacro parent="left_arm_fwk_fwa_050_frame_out" prefix="left_arm_">
<origin xyz="0 0 0.0" rpy="0 0 0"/>
</adapter_fwa050_wsg50_xacro>
<wsg_50_xacro name="left_gripper" parent="left_arm_adapter_fwa050_wsg50_frame_out">
</xacro:adapter_fwa050_wsg50_xacro>
<xacro:wsg_50_xacro name="left_gripper" parent="left_arm_adapter_fwa050_wsg50_frame_out">
<origin xyz="0 0 0" rpy="0 0 0"/>
</wsg_50_xacro>
</xacro:wsg_50_xacro>

<link name="left_wrist_optical_frame_x">
<inertial>
Expand Down Expand Up @@ -259,29 +260,30 @@
<origin xyz="0 0 0" rpy="0 0 2.35619183661"/>
</wsg_50_xacro-->
<!--This is a 60deg rotation to calib_right_arm_base_link -->
<kuka_lwr_arm parent="triangle_right_arm_link" prefix="right_" color_postfix="">
<xacro:kuka_lwr_arm parent="triangle_right_arm_link" prefix="right_" color_postfix="">
<origin xyz="0 0 0" rpy="0 0 -1.047198"/>
</kuka_lwr_arm>
<lwr4_cabling_adaptor parent="right_arm_flange_link" prefix="right_arm_">
</xacro:kuka_lwr_arm>
<xacro:lwr4_cabling_adaptor parent="right_arm_flange_link" prefix="right_arm_">
<origin xyz="0 0 0.01" rpy="0 0 0"/>
</lwr4_cabling_adaptor>
</xacro:lwr4_cabling_adaptor>

<!-- The following URDFs have frame_in / frame_out, so they are simply placed at their parent -->
<kms_40_xacro parent="right_arm_cabling_adaptor_link" prefix="right_arm_">
<xacro:kms_40_xacro parent="right_arm_cabling_adaptor_link" prefix="right_arm_">
<origin xyz="0 0 0" rpy="0 0 3.14159265359"/>
<!--places the kms40 frame 13mm from the parent -->
</kms_40_xacro>
<adapter_kms40_fwk050_xacro parent="right_arm_kms40_frame_out" prefix="right_arm_">
</xacro:kms_40_xacro>
<xacro:adapter_kms40_fwk050_xacro parent="right_arm_kms40_frame_out" prefix="right_arm_">
<origin xyz="0 0 0" rpy="0 0 0"/>
</adapter_kms40_fwk050_xacro>
<fwk_fwa_050_xacro parent="right_arm_adapter_kms40_fwk050_frame_out" prefix="right_arm_">
</xacro:adapter_kms40_fwk050_xacro>
<xacro:fwk_fwa_050_xacro parent="right_arm_adapter_kms40_fwk050_frame_out" prefix="right_arm_">
<origin xyz="0 0 0" rpy="0 0 0"/>
</fwk_fwa_050_xacro>
<adapter_fwa050_wsg50_xacro parent="right_arm_fwk_fwa_050_frame_out" prefix="right_arm_">
</xacro:fwk_fwa_050_xacro>
<xacro:adapter_fwa050_wsg50_xacro parent="right_arm_fwk_fwa_050_frame_out" prefix="right_arm_">
<origin xyz="0 0 0.0" rpy="0 0 0"/>
</adapter_fwa050_wsg50_xacro>
<wsg_50_xacro name="right_gripper" parent="right_arm_adapter_fwa050_wsg50_frame_out">
</xacro:adapter_fwa050_wsg50_xacro>
<xacro:wsg_50_xacro name="right_gripper" parent="right_arm_adapter_fwa050_wsg50_frame_out">
<origin xyz="0 0 0" rpy="0 0 3.14159265359"/>
</wsg_50_xacro>
</xacro:wsg_50_xacro>
</xacro:macro>

</robot>
11 changes: 6 additions & 5 deletions iai_kuka_lwr4_description/defs/util_defs.xml
Original file line number Diff line number Diff line change
@@ -1,27 +1,28 @@
<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://www.ros.org/wiki/xacro">

<property name="M_PI" value="3.1415926535897931" />

<!--
Little helper macro to define the inertia matrix needed
for links.
-->
<macro name="cuboid_inertia_def" params="width height length mass">
<xacro:macro name="cuboid_inertia_def" params="width height length mass">
<inertia ixx="${mass * (height * height + length * length) / 12}"
iyy="${mass * (width * width + length * length) / 12}"
izz="${mass * (width * width + height * height) / 12}"
ixy="0" iyz="0" ixz="0"/>
</macro>
</xacro:macro>

<!-- length is along the y-axis! -->
<macro name="cylinder_inertia_def" params="radius length mass">
<xacro:macro name="cylinder_inertia_def" params="radius length mass">
<inertia ixx="${mass * (3 * radius * radius + length * length) / 12}"
iyy="${mass * radius* radius / 2}"
izz="${mass * (3 * radius * radius + length * length) / 12}"
ixy="0" iyz="0" ixz="0"/>
</macro>
</xacro:macro>

</robot>
18 changes: 9 additions & 9 deletions iai_kuka_lwr4_description/urdf/kuka_lwr_arm.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
<inertial>
<mass value="${arm_elem_base_mass}"/>
<origin xyz="0 0 0.055" rpy="0 0 0"/>
<cylinder_inertia_def radius="0.06" length="0.11"
<xacro:cylinder_inertia_def radius="0.06" length="0.11"
mass="${arm_elem_base_mass}"/>
</inertial>

Expand Down Expand Up @@ -73,7 +73,7 @@
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0 0.130"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.260"
<xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.260"
mass="${arm_elem_link_mass}"/>
</inertial>

Expand Down Expand Up @@ -123,7 +123,7 @@
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0.06 ${0.130 - 0.06}"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.260"
<xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.260"
mass="${arm_elem_link_mass}"/>
</inertial>

Expand Down Expand Up @@ -173,7 +173,7 @@
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0.06 0.130"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.260"
<xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.260"
mass="${arm_elem_link_mass}"/>
</inertial>

Expand Down Expand Up @@ -223,7 +223,7 @@
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 -0.06 ${0.130 - 0.06}"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.2600"
<xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.2600"
mass="${arm_elem_link_mass}"/>
</inertial>

Expand Down Expand Up @@ -273,7 +273,7 @@
<inertial>
<mass value="${arm_elem_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0 0.124"/>
<cuboid_inertia_def length="0.12" width="0.06" height="0.248"
<xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.248"
mass="${arm_elem_link_mass}"/>
</inertial>

Expand Down Expand Up @@ -323,7 +323,7 @@
<inertial>
<mass value="${arm_elem_ball_link_mass}"/>
<origin rpy="0 0 0" xyz="0 0 0.0625"/>
<cuboid_inertia_def length="0.125" width="0.125" height="0.125"
<xacro:cuboid_inertia_def length="0.125" width="0.125" height="0.125"
mass="${arm_elem_ball_link_mass}"/>
</inertial>

Expand Down Expand Up @@ -373,7 +373,7 @@
<inertial>
<mass value="${arm_elem_end_link_mass}"/>
<origin xyz="0 0 0"/>
<cuboid_inertia_def length="1" width="1" height="1"
<xacro:cuboid_inertia_def length="1" width="1" height="1"
mass="${arm_elem_end_link_mass}"/>
</inertial>
<visual>
Expand Down Expand Up @@ -415,7 +415,7 @@
<inertial>
<mass value="${arm_elem_flange_link_mass}"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<cuboid_inertia_def length="1" width="1" height="1"
<xacro:cuboid_inertia_def length="1" width="1" height="1"
mass="${arm_elem_flange_link_mass}"/>
</inertial>

Expand Down

0 comments on commit 6b4635f

Please sign in to comment.