Merge ScalABLE40/feature/simulated-ft-sensor to development

IFL-CAMP · Jun 3, 2019 · aff15f3 · aff15f3
2 parents ab86974 + 8300225
commit aff15f3
Show file tree

Hide file tree

Showing 7 changed files with 48 additions and 14 deletions.
diff --git a/LICENSE b/LICENSE
@@ -1,9 +1,9 @@
 BSD 2-Clause License
 
-Copyright (c) 2016-2017, Salvatore Virga - [email protected], Marco Esposito - [email protected]
-Technische Universität München
-Chair for Computer Aided Medical Procedures and Augmented Reality
-Fakultät für Informatik / I16, Boltzmannstraße 3, 85748 Garching bei München, Germany
+Copyright (c) 2016-2019, Salvatore Virga - [email protected], Marco Esposito - [email protected] 
+Technische Universität München 
+Chair for Computer Aided Medical Procedures and Augmented Reality 
+Fakultät für Informatik / I16, Boltzmannstraße 3, 85748 Garching bei München, Germany 
 http://campar.in.tum.de
 All rights reserved.
 

diff --git a/README.md b/README.md
@@ -1,7 +1,7 @@
 ## IIWA STACK
 ROS Indigo/Kinetic metapackage for the KUKA LBR IIWA R800/R820 (7/14 Kg).
 
-**Current version : v-1.2.0 for Sunrise 1.11**    
+**Current version : v-1.2.0 for Sunrise 1.10 - 1.14**   
 [Using a previous version of Sunrise?](https://github.com/SalvoVirga/iiwa_stack/wiki/FAQ#which-version-of-sunriseossunrise-workbench-is-supported)    
 
 [![Build Status](https://travis-ci.org/SalvoVirga/iiwa_stack.svg?branch=master)](https://travis-ci.org/SalvoVirga/iiwa_stack)
@@ -30,12 +30,15 @@ ___
 
 If you use iiwa_stack for reseach, you could cite the following work. It is the first publication where it was used.
 
-    @article{hennersperger2016towards,
-        title={Towards MRI-based Autonomous Robotic US Acquisitions: A First Feasibility Study},
-        author={Hennersperger, Christoph and Fuerst, Bernhard and Virga, Salvatore and Zettinig, Oliver and Frisch, Benjamin and Neff, Thomas and Navab, Nassir},
-        journal={IEEE transactions on medical imaging},
-        year={2016},
-        publisher={IEEE}
+    @article{hennersperger2017towards,
+      title={Towards MRI-based autonomous robotic US acquisitions: a first feasibility study},
+      author={Hennersperger, Christoph and Fuerst, Bernhard and Virga, Salvatore and Zettinig, Oliver and Frisch, Benjamin and Neff, Thomas and Navab, Nassir},
+      journal={IEEE transactions on medical imaging},
+      volume={36},
+      number={2},
+      pages={538--548},
+      year={2017},
+      publisher={IEEE}
     }
 
 ___

diff --git a/iiwa_description/launch/iiwa7_upload.launch b/iiwa_description/launch/iiwa7_upload.launch
@@ -7,5 +7,5 @@
   <arg name="origin_xyz" default="'0 0 0'"/> <!-- Note the syntax to pass a vector -->
   <arg name="origin_rpy" default="'0 0 0'"/>
 
-  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find iiwa_description)/urdf/iiwa7.urdf.xacro' hardware_interface:=$(arg hardware_interface) robot_name:=$(arg robot_name) origin_xyz:=$(arg origin_xyz) origin_rpy:=$(arg origin_rpy)" />
+  <param name="robot_description" command="$(find xacro)/xacro '$(find iiwa_description)/urdf/iiwa7.urdf.xacro' hardware_interface:=$(arg hardware_interface) robot_name:=$(arg robot_name) origin_xyz:=$(arg origin_xyz) origin_rpy:=$(arg origin_rpy)" />
 </launch> 
diff --git a/iiwa_description/package.xml b/iiwa_description/package.xml
@@ -9,5 +9,6 @@
   <license>BSD</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>force_torque_sensor_controller</run_depend>
 
 </package>
diff --git a/iiwa_description/urdf/iiwa.gazebo.xacro b/iiwa_description/urdf/iiwa.gazebo.xacro
@@ -3,7 +3,21 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:macro name="iiwa_gazebo" params="robot_name">
-
+
+     <!-- Enable the Joint Feedback -->
+    <gazebo reference="${robot_name}_joint_7">
+      <provideFeedback>true</provideFeedback>
+    </gazebo>
+
+    <!-- The ft_sensor plugin -->
+    <gazebo>
+      <plugin name="ft_sensor" filename="libgazebo_ros_ft_sensor.so">
+        <updateRate>50.0</updateRate>
+        <topicName>/${robot_name}/state/CartesianWrench</topicName>
+        <jointName>${robot_name}_joint_7</jointName>
+      </plugin>
+    </gazebo>   
+
     <!-- Load Gazebo lib and set the robot namespace -->
     <gazebo>
       <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

diff --git a/iiwa_gazebo/worlds/iiwa.world b/iiwa_gazebo/worlds/iiwa.world
@@ -13,6 +13,20 @@
     <include>
       <uri>model://sun</uri>
     </include>
+
+    <physics name="default_physics" default="0" type="ode">
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>100</real_time_update_rate>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>50</iters>
+          <sor>1.0</sor> <!-- Important, see issue #2209 -->
+          <use_dynamic_moi_rescaling>false</use_dynamic_moi_rescaling>
+        </solver>
+      </ode>
+    </physics>
 
     <!-- Focus camera -->
     <gui fullscreen='0'>

diff --git a/iiwa_hw/src/iiwa_hw.cpp b/iiwa_hw/src/iiwa_hw.cpp
@@ -153,7 +153,9 @@ void HardwareInterface::registerJointLimits(const std::string& joint_name,
   if (has_soft_limits) {
     const joint_limits_interface::EffortJointSoftLimitsHandle limits_handle(joint_handle, limits, soft_limits);
     ej_limits_interface_.registerHandle(limits_handle);
-  } else {
+  }
+  else
+  {
     const joint_limits_interface::EffortJointSaturationHandle sat_handle(joint_handle, limits);
     ej_sat_interface_.registerHandle(sat_handle);
   }