Nancy euRobin'24 Changes

.gitignore

@@ -0,0 +1,64 @@
+*.npy
+*.csv
+
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Vscode
+.vscode
+/log
+
+# Catkin custom files
+CATKIN_IGNORE
+hardware/simpleGimball/~$handle.SLDPRT
+hardware/simpleGimball/~$hub1.SLDPRT
+hardware/simpleGimball/~$motorgl30.SLDPRT
+hardware/simpleGimball/~$motorMount1.SLDPRT
+hardware/simpleGimball/~$motorMount2.SLDPRT
+hardware/~$armAssembly.SLDASM

CMakeLists.txt

@@ -1,204 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(tiago_dual_cartesio_config)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+find_package(catkin REQUIRED)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  cartesian_interface
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES franka_cartesio_config
-#  CATKIN_DEPENDS cartesian_interface franka_description
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/franka_cartesio_config.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/franka_cartesio_config_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_franka_cartesio_config.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

README.md

@@ -17,4 +17,26 @@ Simply:
 
 It is possible to check the collision model by enabling ```Collision Enabled``` in the ```RobotModel``` display in RVIZ. Notice that different collision models can be used.
 
+How to run on the robot
+-----------------------
+To run the controller on the robot first run the ```ros_control_bridge.py```:
+
+```rosrun tiago_dual_cartesio_config ros_control_bridge.py```
+
+this will forward the solution from the controller to the ```JointTrajectory``` controllers running on the robot. 
+
+Then run the controller using:
+
+```reset && mon launch tiago_dual_cartesio_config/launch/cartesio.launch```
+
+How to move the robot
+-------------------------------
+First, perform the steps in **How to run on the robot**. Then run the launch file:
+
+```reset && mon launch tiago_dual_cartesio_config teleop.launch```
+
+This will start the teleoperation node that allows to control the arms and the gripper for the robot. Then to move the base of the robot run the launch file:
+
+```reset && mon launch tiago_dual_cartesio_config joystick_controller.launch```
+
 

capsules/urdf/tiago_dual_capsules.rviz

@@ -2341,4 +2341,27 @@
       <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
     </inertial>
   </link>
+
+  <link name="camera_right_frame"/>
+	<joint name="camera_right_frame_joint" type="fixed">
+		  <origin rpy="1.57079632679 0 0" xyz="0.072 -0.085 -0.032"/>
+		  <parent link="wrist_right_ft_tool_link"/>
+		  <child link="camera_right_frame"/>
+	</joint>
+
+  <link name="camera_left_frame"/>
+	<joint name="camera_left_frame_joint" type="fixed">
+		  <origin rpy="1.57079632679 0 0" xyz="0.072 -0.085 -0.032"/>
+		  <parent link="wrist_left_ft_tool_link"/>
+		  <child link="camera_left_frame"/>
+	</joint>
+
+
+	<link name="camera_head_frame"/>
+	<joint name="camera_head_frame_joint" type="fixed">
+		  <origin rpy="-3.1415927 0.0 0.0" xyz="0.171591 0.0112369 1.20358"/>
+		  <parent link="base_link"/>
+		  <child link="camera_head_frame"/>
+	</joint>
+
 </robot>

launch/cartesio.launch

@@ -1,6 +1,4 @@
 <launch>
-
-
   <arg name="multi" default=""/>
   <arg name="arm_left"  default="True"/>
   <arg name="arm_right"  default="True"/>
@@ -57,8 +55,5 @@
     <arg name="gui" default="true"/>
     <node if="$(arg gui)" type="rviz" name="rviz" pkg="rviz" output="screen" args="-d $(find tiago_dual_cartesio_config)/launch/viz.rviz"/>
 
-    <!-- RUN POSTURAL GUI -->
-    <node pkg="cartesian_interface" type="postural_gui" name="postural_gui"/>
-
-
+    <node pkg="tf" type="static_transform_publisher" name="ci_world_to_odom" args="0 0 0 0 0 0 /odom /ci/world 500"/>       
 </launch>

launch/joystick_controller.launch

@@ -0,0 +1,16 @@
+<launch>
+    <group>
+        <node pkg="joy" type="joy_node" name="joy_node">
+            <param name="dev" type="string" value="/dev/input/js0" />
+            <param name="deadzone" type="double" value="0.3" />
+            <remap from="joy" to="tiago_dual_cartesio/joy" />
+            <param name="autorepeat_rate" type="double" value="20" />
+        </node>
+    </group>
+
+    <node pkg="stream_deck_controller" type="joystick_controller.py" name="joystick_controller" output="screen">
+        <param name="joy_topic" type="string" value="/streamdeck/joy" />
+        <param name="lin_scale" type="double" value="0.2" />
+        <param name="ang_scale" type="double" value="0.5" />
+    </node>
+</launch>