Commit 8ea3f6a1 authored by Felix Mauch's avatar Felix Mauch
Browse files

Merge remote-tracking branch 'origin/melodic-devel' into calibration_devel

parents 6143f6cd c8c27c15
sudo: required
dist: trusty
language: generic
compiler:
- gcc
services:
- docker
notifications:
email:
on_success: always
on_failure: always
env:
matrix:
- USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
- USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
- USE_DEB=true ROS_DISTRO="kinetic" PRERELEASE=true
- USE_DEB=true ROS_DISTRO="melodic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
- USE_DEB=true ROS_DISTRO="melodic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
- USE_DEB=true ROS_DISTRO="melodic" PRERELEASE=true
- ROS_DISTRO="kinetic" ROS_REPO=ros
- ROS_DISTRO="kinetic" ROS_REPO=ros-shadow-fixed
- ROS_DISTRO="kinetic" PRERELEASE=true
- ROS_DISTRO="melodic" ROS_REPO=ros
- ROS_DISTRO="melodic" ROS_REPO=ros-shadow-fixed
- ROS_DISTRO="melodic" PRERELEASE=true
matrix:
allow_failures:
- env: USE_DEB=true ROS_DISTRO="melodic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
- env: USE_DEB=true ROS_DISTRO="melodic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
- env: USE_DEB=true ROS_DISTRO="melodic" PRERELEASE=true
- env: ROS_DISTRO="melodic" ROS_REPO=ros
- env: ROS_DISTRO="melodic" ROS_REPO=ros-shadow-fixed
- env: ROS_DISTRO="melodic" PRERELEASE=true
install:
- git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
- git clone --quiet --depth=1 https://github.com/ros-industrial/industrial_ci.git .ci_config
script:
- source .ci_config/travis.sh
# Universal Robot
[![Build Status](http://build.ros.org/job/Kdev__universal_robot__ubuntu_xenial_amd64/badge/icon)](http://build.ros.org/job/Kdev__universal_robot__ubuntu_xenial_amd64)
[![license - apache 2.0](https://img.shields.io/:license-Apache%202.0-yellowgreen.svg)](https://opensource.org/licenses/Apache-2.0)
[![License](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause)
[![support level: community](https://img.shields.io/badge/support%20level-community-lightgray.png)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform)
......
<package>
<package format="2">
<name>universal_robot</name>
<version>1.2.5</version>
<description>
......@@ -20,7 +20,7 @@
<author email="fxm@ipa.fhg.de">Felix Messmer</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>universal_robots</run_depend>
<exec_depend>universal_robots</exec_depend>
<export>
<metapackage/>
......
<package>
<?xml version="1.0"?>
<package format="2">
<name>ur10_e_moveit_config</name>
<version>1.2.5</version>
......@@ -16,16 +17,15 @@
<url type="bugtracker">https://github.com/ros-planning/moveit_setup_assistant/issues</url>
<url type="repository">https://github.com/ros-planning/moveit_setup_assistant</url>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<build_depend>ur_description</build_depend>
<run_depend>ur_description</run_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>moveit_fake_controller_manager</exec_depend>
<exec_depend>moveit_simple_controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<depend>ur_description</depend>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
......
<package>
<?xml version="1.0"?>
<package format="2">
<name>ur10_moveit_config</name>
<version>1.2.5</version>
......@@ -16,16 +17,15 @@
<url type="bugtracker">https://github.com/ros-planning/moveit_setup_assistant/issues</url>
<url type="repository">https://github.com/ros-planning/moveit_setup_assistant</url>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<build_depend>ur_description</build_depend>
<run_depend>ur_description</run_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>moveit_fake_controller_manager</exec_depend>
<exec_depend>moveit_simple_controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<depend>ur_description</depend>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
......
<package>
<?xml version="1.0"?>
<package format="2">
<name>ur3_e_moveit_config</name>
<version>1.2.5</version>
......@@ -16,16 +17,15 @@
<url type="bugtracker">https://github.com/ros-planning/moveit_setup_assistant/issues</url>
<url type="repository">https://github.com/ros-planning/moveit_setup_assistant</url>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<build_depend>ur_description</build_depend>
<run_depend>ur_description</run_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>moveit_fake_controller_manager</exec_depend>
<exec_depend>moveit_simple_controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<depend>ur_description</depend>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
......
<package>
<?xml version="1.0"?>
<package format="2">
<name>ur3_moveit_config</name>
<version>1.2.5</version>
......@@ -16,16 +17,15 @@
<url type="bugtracker">https://github.com/ros-planning/moveit_setup_assistant/issues</url>
<url type="repository">https://github.com/ros-planning/moveit_setup_assistant</url>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<build_depend>ur_description</build_depend>
<run_depend>ur_description</run_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>moveit_fake_controller_manager</exec_depend>
<exec_depend>moveit_simple_controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<depend>ur_description</depend>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
......
<package>
<?xml version="1.0"?>
<package format="2">
<name>ur5_e_moveit_config</name>
<version>1.2.5</version>
......@@ -16,16 +17,15 @@
<url type="bugtracker">https://github.com/ros-planning/moveit_setup_assistant/issues</url>
<url type="repository">https://github.com/ros-planning/moveit_setup_assistant</url>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<build_depend>ur_description</build_depend>
<run_depend>ur_description</run_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>moveit_fake_controller_manager</exec_depend>
<exec_depend>moveit_simple_controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<depend>ur_description</depend>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
......
<package>
<?xml version="1.0"?>
<package format="2">
<name>ur5_moveit_config</name>
<version>1.2.5</version>
......@@ -16,16 +17,15 @@
<url type="bugtracker">https://github.com/ros-planning/moveit_setup_assistant/issues</url>
<url type="repository">https://github.com/ros-planning/moveit_setup_assistant</url>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<build_depend>ur_description</build_depend>
<run_depend>ur_description</run_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners_ompl</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>moveit_fake_controller_manager</exec_depend>
<exec_depend>moveit_simple_controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<depend>ur_description</depend>
<test_depend>roslaunch</test_depend>
<buildtool_depend>catkin</buildtool_depend>
......
<?xml version="1.0"?>
<package>
<package format="2">
<name>ur_bringup</name>
<version>1.2.5</version>
<description>The ur_bringup package</description>
......@@ -16,10 +16,10 @@
<buildtool_depend>catkin</buildtool_depend>
<test_depend>roslaunch</test_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>ur_description</run_depend>
<run_depend>ur_driver</run_depend>
<run_depend>robot_state_publisher</run_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>ur_description</exec_depend>
<exec_depend>ur_driver</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<export>
</export>
......
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- /TF1
Splitter Ratio: 0.5
Tree Height: 787
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
link_6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
/base_link:
Value: true
/link_1:
Value: true
/link_2:
Value: true
/link_3:
Value: true
/link_4:
Value: true
/link_5:
Value: true
/link_6:
Value: true
/tool0:
Value: true
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
/base_link:
/link_1:
/link_2:
/link_3:
/link_4:
/link_5:
/link_6:
/tool0:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: /base_link
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 10
Focal Point:
X: 0
Y: 0
Z: 0
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.115398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.930399
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1000
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000013c000003a2fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003a2000000dd00ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003a2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f700fffffffb0000000800540069006d00650100000000000004500000000000000000000002a9000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1280
X: 60
Y: 60
<?xml version="1.0"?>
<launch>
<include file="$(find ur_description)/launch/ur10_upload.launch"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
<param name="use_gui" value="true"/>
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_description)/cfg/view_robot.rviz" required="true" />
</launch>
<?xml version="1.0"?>
<launch>
<include file="$(find ur_description)/launch/ur3_upload.launch"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
<param name="use_gui" value="true"/>
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_description)/cfg/view_robot.rviz" required="true" />
</launch>
<?xml version="1.0"?>
<launch>
<include file="$(find ur_description)/launch/ur5_upload.launch"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
<param name="use_gui" value="true"/>
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur_description)/cfg/view_robot.rviz" required="true" />
</launch>
<?xml version="1.0"?>
<package>
<package format="2">
<name>ur_description</name>
<version>1.2.5</version>
<description>
......@@ -19,8 +19,11 @@
<url type="website">http://ros.org/wiki/ur_description</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>urdf</run_depend>
<run_depend>xacro</run_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
<export>
</export>
......
......@@ -27,6 +27,8 @@
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
kinematics_file">
<xacro:property name="__kinematics" value="${kinematics_file['kinematics']}"/>
......@@ -87,9 +89,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>
......@@ -122,9 +130,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_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>
......@@ -157,9 +171,15 @@
<axis xyz="0 0 1" />
<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>
......@@ -192,9 +212,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_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>
......@@ -227,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="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>
......@@ -262,9 +294,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_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="54.0" velocity="3.2"/>
<xacro:if value="${safety_limits}">