diff --git a/ur_description/urdf/ur10.urdf.xacro b/ur_description/urdf/ur10.urdf.xacro
index d1f6f8f2de3d5e4c98d5d2bc0da426ebec777cf0..f8c86c499cc3ffe58555686f3ddb21a6f89eb5b0 100644
--- a/ur_description/urdf/ur10.urdf.xacro
+++ b/ur_description/urdf/ur10.urdf.xacro
@@ -26,7 +26,9 @@
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
- transmission_hw_interface:=hardware_interface/PositionJointInterface">
+ transmission_hw_interface:=hardware_interface/PositionJointInterface
+ safety_limits:=false safety_pos_margin:=0.15
+ safety_k_position:=20">
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
@@ -96,9 +98,15 @@
<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"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="2.16"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -129,9 +137,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="2.16"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -162,9 +176,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -195,9 +215,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -228,9 +254,15 @@
<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"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -261,9 +293,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
diff --git a/ur_description/urdf/ur3.urdf.xacro b/ur_description/urdf/ur3.urdf.xacro
index d1116d979c32b48db3cee1da67e216e5f30a9fd9..839cc06acc32d6ba35e5e591c5e24ac02c42debc 100644
--- a/ur_description/urdf/ur3.urdf.xacro
+++ b/ur_description/urdf/ur3.urdf.xacro
@@ -25,7 +25,9 @@
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
- transmission_hw_interface:=hardware_interface/PositionJointInterface">
+ transmission_hw_interface:=hardware_interface/PositionJointInterface
+ safety_limits:=false safety_pos_margin:=0.15
+ safety_k_position:=20">
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="2.0" /> <!-- This mass might be incorrect -->
@@ -94,9 +96,15 @@
<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"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="2.16"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -127,9 +135,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="2.16"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -160,9 +174,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -193,9 +213,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -226,9 +252,15 @@
<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"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -259,9 +291,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
diff --git a/ur_description/urdf/ur5.urdf.xacro b/ur_description/urdf/ur5.urdf.xacro
index 5e9e54727ed7af6487c8897ac16e133265801b5f..909f923624d145dfadd1ae8aaed1d890af7f1d26 100644
--- a/ur_description/urdf/ur5.urdf.xacro
+++ b/ur_description/urdf/ur5.urdf.xacro
@@ -21,7 +21,9 @@
wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
- transmission_hw_interface:=hardware_interface/PositionJointInterface">
+ transmission_hw_interface:=hardware_interface/PositionJointInterface
+ safety_limits:=false safety_pos_margin:=0.15
+ safety_k_position:=20">
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
@@ -111,9 +113,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -144,9 +152,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -177,9 +191,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -210,9 +230,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="28.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -243,9 +269,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="28.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -276,9 +308,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="28.0" velocity="3.2"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
diff --git a/ur_e_description/urdf/ur10e.urdf.xacro b/ur_e_description/urdf/ur10e.urdf.xacro
index e40d7ac1c77c916796b9a5568cf89350e6ecbf4b..9cd24e78639996e66f46dae0a3e75fa766f5ba4f 100644
--- a/ur_e_description/urdf/ur10e.urdf.xacro
+++ b/ur_e_description/urdf/ur10e.urdf.xacro
@@ -20,12 +20,15 @@
</xacro:macro>
<xacro:macro name="ur10e_robot" params="prefix joint_limited
- shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
- shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
- elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
- wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
- wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
- wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}">
+ shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
+ shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
+ elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
+ wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
+ wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
+ wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
+ transmission_hw_interface:=hardware_interface/PositionJointInterface
+ safety_limits:=false safety_pos_margin:=0.15
+ safety_k_position:=20">
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
@@ -94,9 +97,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -127,9 +136,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -160,9 +175,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -193,9 +214,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -226,9 +253,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -259,9 +292,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
diff --git a/ur_e_description/urdf/ur3e.urdf.xacro b/ur_e_description/urdf/ur3e.urdf.xacro
index 1dd80f97e375bb4ff63d25fe90ad20f6190c2212..fc8fe1da14c41fc7982060d1b8d8328fb2bdd845 100644
--- a/ur_e_description/urdf/ur3e.urdf.xacro
+++ b/ur_e_description/urdf/ur3e.urdf.xacro
@@ -19,12 +19,15 @@
</xacro:macro>
<xacro:macro name="ur3e_robot" params="prefix joint_limited
- shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
- shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
- elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
- wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
- wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
- wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}">
+ shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
+ shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
+ elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
+ wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
+ wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
+ wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
+ transmission_hw_interface:=hardware_interface/PositionJointInterface
+ safety_limits:=false safety_pos_margin:=0.15
+ safety_k_position:=20">
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="2.0" /> <!-- This mass might be incorrect -->
@@ -93,9 +96,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -126,9 +135,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="330.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -159,9 +174,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -192,9 +213,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -225,9 +252,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -258,9 +291,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
diff --git a/ur_e_description/urdf/ur5e.urdf.xacro b/ur_e_description/urdf/ur5e.urdf.xacro
index fe6bada461e1a9e640362162231135636ef9ac1d..460ba61a52e6c86e86ec651f701fc33145f1babb 100644
--- a/ur_e_description/urdf/ur5e.urdf.xacro
+++ b/ur_e_description/urdf/ur5e.urdf.xacro
@@ -15,12 +15,15 @@
</xacro:macro>
<xacro:macro name="ur5e_robot" params="prefix joint_limited
- shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
- shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
- elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
- wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
- wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
- wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}">
+ shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi}
+ shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi}
+ elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi}
+ wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi}
+ wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi}
+ wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi}
+ transmission_hw_interface:=hardware_interface/PositionJointInterface
+ safety_limits:=false safety_pos_margin:=0.15
+ safety_k_position:=20">
<!-- Inertia parameters -->
<xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect -->
@@ -110,9 +113,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -143,9 +152,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -176,9 +191,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.14"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -209,9 +230,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="28.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -242,9 +269,15 @@
<axis xyz="0 0 1" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="28.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>
@@ -275,9 +308,15 @@
<axis xyz="0 1 0" />
<xacro:unless value="${joint_limited}">
<limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:unless>
<xacro:if value="${joint_limited}">
<limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="28.0" velocity="6.28"/>
+ <xacro:if value="${safety_limits}">
+ <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+ </xacro:if>
</xacro:if>
<dynamics damping="0.0" friction="0.0"/>
</joint>