Skip to content

Commit

Permalink
Merge pull request #38 from K4R-IAI/master
Browse files Browse the repository at this point in the history
Fix weird bugs when update ubuntu
  • Loading branch information
ichumuh authored Nov 2, 2021
2 parents 185c176 + 66d2c54 commit 3c9f98e
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 39 deletions.
26 changes: 13 additions & 13 deletions iai_ur_description/urdf/ur10.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
safety_k_position:=20
kinematics_file">

<xacro:property name="__kinematics" value="${kinematics_file['kinematics']}"/>
<xacro:property name="kinematics" value="${kinematics_file['kinematics']}"/>

<!-- Inertia parameters -->
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
Expand Down Expand Up @@ -85,7 +85,7 @@
<joint name="${prefix}shoulder_pan_joint" type="revolute">
<parent link="${prefix}base_link" />
<child link = "${prefix}shoulder_link" />
<origin xyz="${__kinematics['shoulder']['x']} ${__kinematics['shoulder']['y']} ${__kinematics['shoulder']['z']}" rpy="${__kinematics['shoulder']['roll']} ${__kinematics['shoulder']['pitch']} ${__kinematics['shoulder']['yaw'] + base_correction}" />
<origin xyz="${kinematics['shoulder']['x']} ${kinematics['shoulder']['y']} ${kinematics['shoulder']['z']}" rpy="${kinematics['shoulder']['roll']} ${kinematics['shoulder']['pitch']} ${kinematics['shoulder']['yaw'] + base_correction}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
Expand Down Expand Up @@ -126,7 +126,7 @@
<joint name="${prefix}shoulder_lift_joint" type="revolute">
<parent link="${prefix}shoulder_link" />
<child link = "${prefix}upper_arm_link" />
<origin xyz="${__kinematics['upper_arm']['x']} ${__kinematics['upper_arm']['y']} ${__kinematics['upper_arm']['z']}" rpy="${__kinematics['upper_arm']['roll']} ${__kinematics['upper_arm']['pitch']} ${__kinematics['upper_arm']['yaw']}" />
<origin xyz="${kinematics['upper_arm']['x']} ${kinematics['upper_arm']['y']} ${kinematics['upper_arm']['z']}" rpy="${kinematics['upper_arm']['roll']} ${kinematics['upper_arm']['pitch']} ${kinematics['upper_arm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
Expand Down Expand Up @@ -159,15 +159,15 @@
<mesh filename="package://iai_ur_description/meshes/ur10/collision/upperarm.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['forearm']['x']}" mass="${upper_arm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*kinematics['forearm']['x']}" mass="${upper_arm_mass}">
<origin xyz="${0.5*kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}elbow_joint" type="revolute">
<parent link="${prefix}upper_arm_link" />
<child link = "${prefix}forearm_link" />
<origin xyz="${__kinematics['forearm']['x']} ${__kinematics['forearm']['y']} ${__kinematics['forearm']['z']}" rpy="${__kinematics['forearm']['roll']} ${__kinematics['forearm']['pitch']} ${__kinematics['forearm']['yaw']}" />
<origin xyz="${kinematics['forearm']['x']} ${kinematics['forearm']['y']} ${kinematics['forearm']['z']}" rpy="${kinematics['forearm']['roll']} ${kinematics['forearm']['pitch']} ${kinematics['forearm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
Expand Down Expand Up @@ -200,15 +200,15 @@
<mesh filename="package://iai_ur_description/meshes/ur10/collision/forearm.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_1']['x']}" mass="${forearm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*kinematics['wrist_1']['x']}" mass="${forearm_mass}">
<origin xyz="${0.5*kinematics['forearm']['x']} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}wrist_1_joint" type="revolute">
<parent link="${prefix}forearm_link" />
<child link = "${prefix}wrist_1_link" />
<origin xyz="${__kinematics['wrist_1']['x']} ${__kinematics['wrist_1']['y']} ${__kinematics['wrist_1']['z']}" rpy="${__kinematics['wrist_1']['roll']} ${__kinematics['wrist_1']['pitch']} ${__kinematics['wrist_1']['yaw']}" />
<origin xyz="${kinematics['wrist_1']['x']} ${kinematics['wrist_1']['y']} ${kinematics['wrist_1']['z']}" rpy="${kinematics['wrist_1']['roll']} ${kinematics['wrist_1']['pitch']} ${kinematics['wrist_1']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
Expand Down Expand Up @@ -241,15 +241,15 @@
<mesh filename="package://iai_ur_description/meshes/ur10/collision/wrist1.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_2']['y']}" mass="${wrist_1_mass}">
<xacro:cylinder_inertial radius="0.075" length="${-1*kinematics['wrist_2']['y']}" mass="${wrist_1_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}wrist_2_joint" type="revolute">
<parent link="${prefix}wrist_1_link" />
<child link = "${prefix}wrist_2_link" />
<origin xyz="${__kinematics['wrist_2']['x']} ${__kinematics['wrist_2']['y']} ${__kinematics['wrist_2']['z']}" rpy="${__kinematics['wrist_2']['roll']} ${__kinematics['wrist_2']['pitch']} ${__kinematics['wrist_2']['yaw']}" />
<origin xyz="${kinematics['wrist_2']['x']} ${kinematics['wrist_2']['y']} ${kinematics['wrist_2']['z']}" rpy="${kinematics['wrist_2']['roll']} ${kinematics['wrist_2']['pitch']} ${kinematics['wrist_2']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
Expand Down Expand Up @@ -282,15 +282,15 @@
<mesh filename="package://iai_ur_description/meshes/ur10/collision/wrist2.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${__kinematics['wrist_3']['y']}" mass="${wrist_2_mass}">
<xacro:cylinder_inertial radius="0.075" length="${kinematics['wrist_3']['y']}" mass="${wrist_2_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}wrist_3_joint" type="revolute">
<parent link="${prefix}wrist_2_link" />
<child link = "${prefix}wrist_3_link" />
<origin xyz="${__kinematics['wrist_3']['x']} ${__kinematics['wrist_3']['y']} ${__kinematics['wrist_3']['z']}" rpy="${__kinematics['wrist_3']['roll']} ${__kinematics['wrist_3']['pitch']} ${__kinematics['wrist_3']['yaw']}" />
<origin xyz="${kinematics['wrist_3']['x']} ${kinematics['wrist_3']['y']} ${kinematics['wrist_3']['z']}" rpy="${kinematics['wrist_3']['roll']} ${kinematics['wrist_3']['pitch']} ${kinematics['wrist_3']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
Expand Down
26 changes: 13 additions & 13 deletions iai_ur_description/urdf/ur3.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
safety_k_position:=20
kinematics_file">

<xacro:property name="__kinematics" value="${kinematics_file['kinematics']}"/>
<xacro:property name="kinematics" value="${kinematics_file['kinematics']}"/>

<!-- Inertia parameters -->
<xacro:property name="base_mass" value="2.0" /> <!-- This mass might be incorrect -->
Expand Down Expand Up @@ -84,7 +84,7 @@
<joint name="${prefix}shoulder_pan_joint" type="revolute">
<parent link="${prefix}base_link" />
<child link = "${prefix}shoulder_link" />
<origin xyz="${__kinematics['shoulder']['x']} ${__kinematics['shoulder']['y']} ${__kinematics['shoulder']['z']}" rpy="${__kinematics['shoulder']['roll']} ${__kinematics['shoulder']['pitch']} ${__kinematics['shoulder']['yaw'] + base_correction}" />
<origin xyz="${kinematics['shoulder']['x']} ${kinematics['shoulder']['y']} ${kinematics['shoulder']['z']}" rpy="${kinematics['shoulder']['roll']} ${kinematics['shoulder']['pitch']} ${kinematics['shoulder']['yaw'] + base_correction}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
Expand Down Expand Up @@ -125,7 +125,7 @@
<joint name="${prefix}shoulder_lift_joint" type="revolute">
<parent link="${prefix}shoulder_link" />
<child link = "${prefix}upper_arm_link" />
<origin xyz="${__kinematics['upper_arm']['x']} ${__kinematics['upper_arm']['y']} ${__kinematics['upper_arm']['z']}" rpy="${__kinematics['upper_arm']['roll']} ${__kinematics['upper_arm']['pitch']} ${__kinematics['upper_arm']['yaw']}" />
<origin xyz="${kinematics['upper_arm']['x']} ${kinematics['upper_arm']['y']} ${kinematics['upper_arm']['z']}" rpy="${kinematics['upper_arm']['roll']} ${kinematics['upper_arm']['pitch']} ${kinematics['upper_arm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
Expand Down Expand Up @@ -158,15 +158,15 @@
<mesh filename="package://iai_ur_description/meshes/ur3/collision/upperarm.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['forearm']['x']}" mass="${upper_arm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*kinematics['forearm']['x']}" mass="${upper_arm_mass}">
<origin xyz="${0.5*kinematics['forearm']['x']} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}elbow_joint" type="revolute">
<parent link="${prefix}upper_arm_link" />
<child link = "${prefix}forearm_link" />
<origin xyz="${__kinematics['forearm']['x']} ${__kinematics['forearm']['y']} ${__kinematics['forearm']['z']}" rpy="${__kinematics['forearm']['roll']} ${__kinematics['forearm']['pitch']} ${__kinematics['forearm']['yaw']}" />
<origin xyz="${kinematics['forearm']['x']} ${kinematics['forearm']['y']} ${kinematics['forearm']['z']}" rpy="${kinematics['forearm']['roll']} ${kinematics['forearm']['pitch']} ${kinematics['forearm']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
Expand Down Expand Up @@ -199,15 +199,15 @@
<mesh filename="package://iai_ur_description/meshes/ur3/collision/forearm.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_1']['x']}" mass="${forearm_mass}">
<origin xyz="${0.5*__kinematics['forearm']['x']} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
<xacro:cylinder_inertial radius="0.075" length="${-1*kinematics['wrist_1']['x']}" mass="${forearm_mass}">
<origin xyz="${0.5*kinematics['forearm']['x']} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}wrist_1_joint" type="revolute">
<parent link="${prefix}forearm_link" />
<child link = "${prefix}wrist_1_link" />
<origin xyz="${__kinematics['wrist_1']['x']} ${__kinematics['wrist_1']['y']} ${__kinematics['wrist_1']['z']}" rpy="${__kinematics['wrist_1']['roll']} ${__kinematics['wrist_1']['pitch']} ${__kinematics['wrist_1']['yaw']}" />
<origin xyz="${kinematics['wrist_1']['x']} ${kinematics['wrist_1']['y']} ${kinematics['wrist_1']['z']}" rpy="${kinematics['wrist_1']['roll']} ${kinematics['wrist_1']['pitch']} ${kinematics['wrist_1']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
Expand Down Expand Up @@ -240,15 +240,15 @@
<mesh filename="package://iai_ur_description/meshes/ur3/collision/wrist1.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${-1*__kinematics['wrist_2']['y']}" mass="${wrist_1_mass}">
<xacro:cylinder_inertial radius="0.075" length="${-1*kinematics['wrist_2']['y']}" mass="${wrist_1_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}wrist_2_joint" type="revolute">
<parent link="${prefix}wrist_1_link" />
<child link = "${prefix}wrist_2_link" />
<origin xyz="${__kinematics['wrist_2']['x']} ${__kinematics['wrist_2']['y']} ${__kinematics['wrist_2']['z']}" rpy="${__kinematics['wrist_2']['roll']} ${__kinematics['wrist_2']['pitch']} ${__kinematics['wrist_2']['yaw']}" />
<origin xyz="${kinematics['wrist_2']['x']} ${kinematics['wrist_2']['y']} ${kinematics['wrist_2']['z']}" rpy="${kinematics['wrist_2']['roll']} ${kinematics['wrist_2']['pitch']} ${kinematics['wrist_2']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
Expand Down Expand Up @@ -281,15 +281,15 @@
<mesh filename="package://iai_ur_description/meshes/ur3/collision/wrist2.stl"/>
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.075" length="${__kinematics['wrist_3']['y']}" mass="${wrist_2_mass}">
<xacro:cylinder_inertial radius="0.075" length="${kinematics['wrist_3']['y']}" mass="${wrist_2_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>

<joint name="${prefix}wrist_3_joint" type="revolute">
<parent link="${prefix}wrist_2_link" />
<child link = "${prefix}wrist_3_link" />
<origin xyz="${__kinematics['wrist_3']['x']} ${__kinematics['wrist_3']['y']} ${__kinematics['wrist_3']['z']}" rpy="${__kinematics['wrist_3']['roll']} ${__kinematics['wrist_3']['pitch']} ${__kinematics['wrist_3']['yaw']}" />
<origin xyz="${kinematics['wrist_3']['x']} ${kinematics['wrist_3']['y']} ${kinematics['wrist_3']['z']}" rpy="${kinematics['wrist_3']['roll']} ${kinematics['wrist_3']['pitch']} ${kinematics['wrist_3']['yaw']}" />
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
Expand Down
Loading

0 comments on commit 3c9f98e

Please sign in to comment.