From f473593ec1655445638057ccf6758f5dd1cf3dfd Mon Sep 17 00:00:00 2001
From: "E. Gil Jones" <gjones@willowgarage.com>
Date: Thu, 29 Mar 2012 13:59:50 -0700
Subject: [PATCH] Adding pretty urdf and updating visualization launch to
something that actually works
---
ur5_description/urdf/robot.urdf.pretty.xacro | 289 ++++++++++++++++++
ur5_description/urdf/robot.urdf.pretty.xml | 248 +++++++++++++++
ur5_moveit/config/joint_limits.yaml | 12 +-
ur5_moveit/config/ur5.srdf | 2 -
.../launch/ur5_moveit_visualization.launch | 17 +-
.../src/universal_robot_arm_ikfast_plugin.cpp | 4 +-
6 files changed, 558 insertions(+), 14 deletions(-)
create mode 100644 ur5_description/urdf/robot.urdf.pretty.xacro
create mode 100644 ur5_description/urdf/robot.urdf.pretty.xml
diff --git a/ur5_description/urdf/robot.urdf.pretty.xacro b/ur5_description/urdf/robot.urdf.pretty.xacro
new file mode 100644
index 0000000..019ed7e
--- /dev/null
+++ b/ur5_description/urdf/robot.urdf.pretty.xacro
@@ -0,0 +1,289 @@
+<?xml version="1.0"?>
+<!--
+DH for UR5:
+a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000]
+d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823]
+alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ]
+q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0]
+joint_direction = [-1, -1, 1, 1, 1, 1]
+mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879]
+center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ]
+-->
+<robot name="universal_robot">
+
+ <property name="pi" value="3.14159265" />
+
+<!-- Inertia parameters -->
+ <property name="base_mass" value="4.0" /> <!-- Invented number, only matters for simulator -->
+ <property name="shoulder_mass" value="3.7000" />
+ <property name="upper_arm_mass" value="8.3930" />
+ <property name="forearm_mass" value="2.2750" />
+ <property name="wrist_1_mass" value="1.2190" />
+ <property name="wrist_2_mass" value="1.2190" />
+ <property name="wrist_3_mass" value="0.1879" />
+
+ <property name="shoulder_cog" value="0.0 0.00193 -0.02561" />
+ <property name="upper_arm_cog" value="0.0 -0.024201 0.2125" /> <!-- 0.11336 - 0.089159 = -->
+ <property name="forearm_cog" value="0.0 0.0265 0.11993" /> <!-- 0.119 is not half of 0.39225, is this really correct? -->
+ <property name="wrist_1_cog" value="0.0 0.110949 0.01634" /> <!-- 0.0018 + 0.10915 = 0.110949 -->
+ <property name="wrist_2_cog" value="0.0 0.0018 0.11099" /> <!-- 0.01634 + 0.09465 = 0.11099-->
+ <property name="wrist_3_cog" value="0.0 0.001159 0.0" />
+
+<!-- Kinematic model -->
+ <property name="shoulder_height" value="0.089159" />
+ <property name="shoulder_offset" value="0.13585" /> <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
+ <property name="upper_arm_length" value="0.42500" />
+ <property name="elbow_offset" value="0.1197" /> <!-- CAD measured -->
+ <property name="forearm_length" value="0.39225" />
+ <property name="wrist_1_length" value="0.093" /> <!-- CAD measured -->
+ <property name="wrist_2_length" value="0.09465" /> <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
+ <property name="wrist_3_length" value="0.0823" />
+
+ <property name="shoulder_radius" value="0.060" /> <!-- manually measured -->
+ <property name="upper_arm_radius" value="0.054" /> <!-- manually measured -->
+ <property name="elbow_radius" value="0.060" /> <!-- manually measured -->
+ <property name="forearm_radius" value="0.040" /> <!-- manually measured -->
+ <property name="wrist_radius" value="0.045" /> <!-- manually measured -->
+
+<!-- Collision model -->
+ <property name="base_collision_length" value="0.160" /> <!-- manually measured -->
+ <property name="shoulder_collision_length" value="0.200" /> <!-- manually measured -->
+ <property name="shoulder_collision_offset" value="0.035" /> <!-- manually measured -->
+ <property name="elbow_collision_length" value="0.200" /> <!-- manually measured -->
+ <property name="elbow_collision_offset" value="0.035" /> <!-- manually measured -->
+
+
+
+
+ <link name="base_link" >
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Base.dae" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Base.dae" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
+ </collision>
+ <inertial>
+ <mass value="${base_mass}" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="shoulder_pan_joint" type="revolute">
+ <parent link="base_link" />
+ <child link = "shoulder_link" />
+ <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 0.0 1.0" />
+ <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+
+ <link name="shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Shoulder.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Shoulder.dae" />
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="${shoulder_mass}" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ <origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" />
+ </inertial>
+ </link>
+
+ <joint name="shoulder_lift_joint" type="revolute">
+ <parent link="shoulder_link" />
+ <child link = "upper_arm_link" />
+ <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+
+ <link name="upper_arm_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/UpperArm.dae" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/UpperArm.dae" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ </collision>
+ <inertial>
+ <mass value="${upper_arm_mass}" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ <origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" />
+ </inertial>
+ </link>
+
+ <joint name="elbow_joint" type="revolute">
+ <parent link="upper_arm_link" />
+ <child link = "forearm_link" />
+ <origin xyz="0.0 ${-elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+
+ <link name="forearm_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Forearm.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Forearm.dae" />
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="${forearm_mass}" />
+ <origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="wrist_1_joint" type="revolute">
+ <parent link="forearm_link" />
+ <child link = "wrist_1_link" />
+ <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+
+ <link name="wrist_1_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist1.dae" />
+ </geometry>
+ <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist1.dae" />
+ </geometry>
+ <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
+ </collision>
+ <inertial>
+ <mass value="${wrist_1_mass}" />
+ <origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="wrist_2_joint" type="revolute">
+ <parent link="wrist_1_link" />
+ <child link = "wrist_2_link" />
+ <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 0.0 1.0" />
+ <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+
+ <link name="wrist_2_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist2.dae" />
+ </geometry>
+ <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist2.dae" />
+ </geometry>
+ <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
+ </collision>
+ <inertial>
+ <mass value="${wrist_2_mass}" />
+ <origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="wrist_3_joint" type="revolute">
+ <parent link="wrist_2_link" />
+ <child link = "wrist_3_link" />
+ <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+
+ <link name="wrist_3_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist3.dae" />
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist3.dae" />
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="${wrist_3_mass}" />
+ <origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="ee_fixed_joint" type="fixed">
+ <parent link="wrist_3_link" />
+ <child link = "ee_link" />
+ <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
+ </joint>
+
+ <link name="ee_link" />
+
+
+
+ <!-- Extra links for collision model -->
+ <joint name="shoulder_collision_joint" type="fixed">
+ <parent link="upper_arm_link" />
+ <child link = "shoulder_collision_link" />
+ </joint>
+ <link name="shoulder_collision_link">
+ <collision>
+ <geometry>
+ <cylinder length="${shoulder_collision_length}" radius="${shoulder_radius}"/>
+ </geometry>
+ <origin xyz="0.0 ${-shoulder_collision_offset} 0.0" rpy="${-pi / 2.0} 0.0 0.0" />
+ </collision>
+ </link>
+
+ <joint name="elbow_collision_joint" type="fixed">
+ <parent link="upper_arm_link" />
+ <child link = "elbow_collision_link" />
+ </joint>
+ <link name="elbow_collision_link">
+ <collision>
+ <geometry>
+ <cylinder length="${elbow_collision_length}" radius="${elbow_radius}"/>
+ </geometry>
+ <origin xyz="0.0 ${-elbow_collision_offset} ${upper_arm_length}" rpy="${pi / 2.0} 0.0 0.0" />
+ </collision>
+ </link>
+
+ <gazebo reference="universal_robot">
+ <material>Gazebo/Blue</material>
+ </gazebo>
+
+</robot>
+
+
+
diff --git a/ur5_description/urdf/robot.urdf.pretty.xml b/ur5_description/urdf/robot.urdf.pretty.xml
new file mode 100644
index 0000000..44f307c
--- /dev/null
+++ b/ur5_description/urdf/robot.urdf.pretty.xml
@@ -0,0 +1,248 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from robot.urdf.pretty.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<!--
+DH for UR5:
+a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000]
+d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823]
+alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ]
+q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0]
+joint_direction = [-1, -1, 1, 1, 1, 1]
+mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879]
+center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ]
+-->
+<robot name="universal_robot">
+ <!-- Inertia parameters -->
+ <!-- Invented number, only matters for simulator -->
+ <!-- 0.11336 - 0.089159 = -->
+ <!-- 0.119 is not half of 0.39225, is this really correct? -->
+ <!-- 0.0018 + 0.10915 = 0.110949 -->
+ <!-- 0.01634 + 0.09465 = 0.11099-->
+ <!-- Kinematic model -->
+ <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
+ <!-- CAD measured -->
+ <!-- CAD measured -->
+ <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- Collision model -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <!-- manually measured -->
+ <link name="base_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Base.dae"/>
+ </geometry>
+ <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Base.dae"/>
+ </geometry>
+ <origin rpy="0.0 0.0 2.3561944875" xyz="0.0 0.0 0.003"/>
+ </collision>
+ <inertial>
+ <mass value="4.0"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="shoulder_pan_joint" type="revolute">
+ <parent link="base_link"/>
+ <child link="shoulder_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
+ <axis xyz="0.0 0.0 1.0"/>
+ <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+ <link name="shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Shoulder.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Shoulder.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="3.7"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.00193 -0.02561"/>
+ </inertial>
+ </link>
+ <joint name="shoulder_lift_joint" type="revolute">
+ <parent link="shoulder_link"/>
+ <child link="upper_arm_link"/>
+ <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
+ <axis xyz="0.0 1.0 0.0"/>
+ <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+ <link name="upper_arm_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/UpperArm.dae"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/UpperArm.dae"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+ </collision>
+ <inertial>
+ <mass value="8.393"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.024201 0.2125"/>
+ </inertial>
+ </link>
+ <joint name="elbow_joint" type="revolute">
+ <parent link="upper_arm_link"/>
+ <child link="forearm_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
+ <axis xyz="0.0 1.0 0.0"/>
+ <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+ <link name="forearm_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Forearm.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Forearm.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="2.275"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0265 0.11993"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="wrist_1_joint" type="revolute">
+ <parent link="forearm_link"/>
+ <child link="wrist_1_link"/>
+ <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/>
+ <axis xyz="0.0 1.0 0.0"/>
+ <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+ <link name="wrist_1_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist1.dae"/>
+ </geometry>
+ <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist1.dae"/>
+ </geometry>
+ <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
+ </collision>
+ <inertial>
+ <mass value="1.219"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.110949 0.01634"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="wrist_2_joint" type="revolute">
+ <parent link="wrist_1_link"/>
+ <child link="wrist_2_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
+ <axis xyz="0.0 0.0 1.0"/>
+ <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+ <link name="wrist_2_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist2.dae"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist2.dae"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
+ </collision>
+ <inertial>
+ <mass value="1.219"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0018 0.11099"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="wrist_3_joint" type="revolute">
+ <parent link="wrist_2_link"/>
+ <child link="wrist_3_link"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
+ <axis xyz="0.0 1.0 0.0"/>
+ <limit effort="10.0" lower="-6.2831853" upper="6.2831853" velocity="3.14159265"/>
+ <dynamics damping="0.1" friction="0.1"/>
+ </joint>
+ <link name="wrist_3_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist3.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://ur5_description/meshes/Wrist3.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.1879"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.001159 0.0"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="ee_fixed_joint" type="fixed">
+ <parent link="wrist_3_link"/>
+ <child link="ee_link"/>
+ <origin rpy="0.0 0.0 1.570796325" xyz="0.0 0.0823 0.0"/>
+ </joint>
+ <link name="ee_link"/>
+ <!-- Extra links for collision model -->
+ <joint name="shoulder_collision_joint" type="fixed">
+ <parent link="upper_arm_link"/>
+ <child link="shoulder_collision_link"/>
+ </joint>
+ <link name="shoulder_collision_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.2" radius="0.06"/>
+ </geometry>
+ <origin rpy="-1.570796325 0.0 0.0" xyz="0.0 -0.035 0.0"/>
+ </collision>
+ </link>
+ <joint name="elbow_collision_joint" type="fixed">
+ <parent link="upper_arm_link"/>
+ <child link="elbow_collision_link"/>
+ </joint>
+ <link name="elbow_collision_link">
+ <collision>
+ <geometry>
+ <cylinder length="0.2" radius="0.06"/>
+ </geometry>
+ <origin rpy="1.570796325 0.0 0.0" xyz="0.0 -0.035 0.425"/>
+ </collision>
+ </link>
+ <gazebo reference="universal_robot">
+ <material>Gazebo/Blue</material>
+ </gazebo>
+</robot>
+
diff --git a/ur5_moveit/config/joint_limits.yaml b/ur5_moveit/config/joint_limits.yaml
index d79d499..97f1242 100644
--- a/ur5_moveit/config/joint_limits.yaml
+++ b/ur5_moveit/config/joint_limits.yaml
@@ -4,40 +4,40 @@ joint_limits:
has_velocity_limits: true
max_velocity: 3.14159265
has_acceleration_limits: true
- max_acceleration: 1
+ max_acceleration: 3
angle_wraparound: false
shoulder_lift_joint:
has_position_limits: true
has_velocity_limits: true
max_velocity: 3.14159265
has_acceleration_limits: true
- max_acceleration: 1
+ max_acceleration: 3
angle_wraparound: false
elbow_joint:
has_position_limits: true
has_velocity_limits: true
max_velocity: 3.14159265
has_acceleration_limits: true
- max_acceleration: 1
+ max_acceleration: 3
angle_wraparound: false
wrist_1_joint:
has_position_limits: true
has_velocity_limits: true
max_velocity: 3.14159265
has_acceleration_limits: true
- max_acceleration: 1
+ max_acceleration: 3
angle_wraparound: false
wrist_2_joint:
has_position_limits: true
has_velocity_limits: true
max_velocity: 3.14159265
has_acceleration_limits: true
- max_acceleration: 1
+ max_acceleration: 3
angle_wraparound: false
wrist_3_joint:
has_position_limits: true
has_velocity_limits: true
max_velocity: 3.14159265
has_acceleration_limits: true
- max_acceleration: 1
+ max_acceleration: 3
angle_wraparound: false
\ No newline at end of file
diff --git a/ur5_moveit/config/ur5.srdf b/ur5_moveit/config/ur5.srdf
index d7e5611..cdbe1fb 100644
--- a/ur5_moveit/config/ur5.srdf
+++ b/ur5_moveit/config/ur5.srdf
@@ -2,8 +2,6 @@
<robot name="universal_robot">
- <virtual_joint name="world_joint" type="floating" parent_frame="odom_combined" child_link="base_link"/>
-
<group name="arm">
<chain base_link="base_link" tip_link="wrist_3_link"/>
</group>
diff --git a/ur5_moveit/launch/ur5_moveit_visualization.launch b/ur5_moveit/launch/ur5_moveit_visualization.launch
index 2b20088..4a9244a 100644
--- a/ur5_moveit/launch/ur5_moveit_visualization.launch
+++ b/ur5_moveit/launch/ur5_moveit_visualization.launch
@@ -1,19 +1,28 @@
<launch>
- <param name="robot_description" textfile="$(find ur5_description)/urdf/robot.urdf.xml" />
+ <arg name="monitor_robot_state" default="false"/>
+ <arg name="allow_trajectory_execution" default="false"/>
+
+ <param name="robot_description" textfile="$(find ur5_description)/urdf/robot.urdf.pretty.xml" />
<!--<param name="robot_description" textfile="$(find ur5_description)/urdf/universal_or.dae" /-->
<param name="robot_description_semantic" textfile="$(find ur5_moveit)/config/ur5.srdf" />
+ <node pkg="robot_state_publisher" type="state_publisher" name="rob_st_pub" />
+ <node unless="$(arg monitor_robot_state)" pkg="moveit_visualization_ros" name="wall_clock_server" type="fake_time.py" />
+
+ <rosparam if="$(arg allow_trajectory_execution)" command="load" file="$(find ur5_moveit)/config/controllers.yaml" />
+
<group ns="robot_description_planning">
<rosparam command="load" file="$(find ur5_moveit)/config/collision_matrix.yaml"/>
<rosparam command="load" file="$(find ur5_moveit)/config/joint_limits.yaml"/>
</group>
- <node pkg="robot_state_publisher" type="state_publisher" name="rob_st_pub" />
- <node pkg="moveit_visualization_ros" name="wall_clock_server" type="fake_time.py" />
-
<node pkg="moveit_visualization_ros" type="moveit_visualizer" name="moveit_visualizer" output="screen">
<rosparam command="load" file="$(find ur5_moveit)/config/ompl_planning.yaml"/>
<rosparam command="load" file="$(find ur5_moveit)/config/kinematics.yaml"/>
+ <rosparam if="$(arg allow_trajectory_execution)" file="$(find ur5_moveit)/config/controllers.yaml"/>
+ <param name="monitor_robot_state" value="$(arg monitor_robot_state)"/>
+ <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
+ <param if="$(arg allow_trajectory_execution)" name="manage_controllers" value="false"/>
</node>
</launch>
diff --git a/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp b/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp
index d18972d..25d705e 100644
--- a/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp
+++ b/ur5_moveit/src/universal_robot_arm_ikfast_plugin.cpp
@@ -191,7 +191,7 @@ public:
std::vector<double> sol;
ik_solver_->getSolution(s,sol);
bool obeys_limits = true;
- ROS_INFO_STREAM("Got " << numsol << " solutions");
+ ROS_DEBUG_STREAM("Got " << numsol << " solutions");
for(unsigned int i = 0; i < sol.size(); i++) {
if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
obeys_limits = false;
@@ -206,7 +206,7 @@ public:
}
}
} else {
- ROS_INFO_STREAM("No ik solution");
+ ROS_DEBUG_STREAM("No ik solution");
}
error_code.val = error_code.NO_IK_SOLUTION;
--
GitLab