--- /dev/null
+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
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(q2d_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+ effort_controllers
+ joint_state_controller
+)
+
+## 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 run_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 run_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 run_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 you 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 q2d_bringup
+# CATKIN_DEPENDS effort_controllers joint_state_controller
+# 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}/q2d_bringup.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/q2d_bringup_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
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_q2d_bringup.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)
--- /dev/null
+shoulder_controller:
+ type: effort_controllers/JointEffortController
+ joint: shoulder_active_joint
+
+elbow_controller:
+ type: effort_controllers/JointEffortController
+ joint: elbow_active_joint
+
+joint_states_publisher:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
--- /dev/null
+shoulder_controller:
+ type: effort_controllers/JointPositionController
+ joint: shoulder_active_joint
+ pid: {p: 2310, i: 4640, d: 0.299}
+
+elbow_controller:
+ type: effort_controllers/JointPositionController
+ joint: elbow_active_joint
+ pid: {p: 339, i: 851, d: 0.351}
+
+joint_states_publisher:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
--- /dev/null
+ijc.launch
\ No newline at end of file
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+
+ <arg name="controller" default="pid"/>
+ <arg name="config" default="$(find q2d_bringup)/config/$(arg controller).yaml"/>
+
+ <include file="$(find q2d_description)/launch/gazebo.launch" >
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ </include>
+
+ <rosparam file="$(arg config)" command="load"/>
+
+ <include file="$(find q2d_bringup)/launch/$(arg controller).launch" />
+</launch>
--- /dev/null
+<launch>
+ <node name="controller_spawner" pkg="controller_manager"
+ type="spawner" respawn="false" output="screen"
+ args="shoulder_controller elbow_controller joint_states_publisher" />
+</launch>
--- /dev/null
+ijc.launch
\ No newline at end of file
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>q2d_bringup</name>
+ <version>0.0.0</version>
+ <description>The q2d_bringup package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/q2d_bringup</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>effort_controllers</build_depend>
+ <build_depend>joint_state_controller</build_depend>
+ <run_depend>effort_controllers</run_depend>
+ <run_depend>joint_state_controller</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#!/usr/bin/python
+
+import sys
+import time
+
+import rospy
+from std_msgs.msg import Float64
+
+def square():
+ if len(sys.argv) < 5:
+ print 'square.py topic min max period'
+ exit()
+ ref=Float64()
+
+ pub=rospy.Publisher(sys.argv[1],Float64,queue_size=1)
+
+ rospy.init_node('square',anonymous=True)
+
+ while True:
+ ref.data=float(sys.argv[2]);
+ pub.publish(ref)
+ time.sleep(float(sys.argv[4]))
+
+ ref.data=float(sys.argv[3]);
+ pub.publish(ref)
+ time.sleep(float(sys.argv[4]))
+
+if __name__ == '__main__':
+ try:
+ square()
+ except rospy.ROSInterruptException:
+ pass
--- /dev/null
+#!/bin/bash
+
+rostopic pub /shoulder_controller/command std_msgs/Float64 "{data: $1}" -1 &
+rostopic pub /elbow_controller/command std_msgs/Float64 "{data: $2}" -1
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(q2d_description)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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)
+
+## 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 run_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 run_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 run_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 you 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 q2d_description
+# CATKIN_DEPENDS other_catkin_pkg
+# 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}/q2d_description.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/q2d_description_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
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_q2d_description.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)
--- /dev/null
+<launch>
+ <arg name="use_gui" default="false"/>
+ <include file="$(find q2d_description)/launch/q2d.launch" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="q2d_origin_publisher" args="0 0 0 0 0 0 1 map origin_link" />
+ <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" args="_use_gui:=$(arg use_gui)" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find q2d_description)/rviz/display.rviz" required="true" />
+</launch>
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+
+ <include file="$(find gazebo_ros)/launch/empty_world.launch">
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ <arg name="world_name" value="worlds/empty_sky.world" />
+ </include>
+
+ <include file="$(find q2d_description)/launch/q2d.launch" />
+
+ <node name="q2d_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model q2d" respawn="false" output="screen" />
+</launch>
--- /dev/null
+<launch>
+ <param name="robot_description" textfile="$(find q2d_description)/urdf/q2d.urdf" />
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>q2d_description</name>
+ <version>0.0.0</version>
+ <description>The q2d_description package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="gui.a.ritter@gmail.com">Guilherme Alan Ritter</maintainer>
+ <maintainer email="fetter@ee.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/q2d_description</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="gui.a.ritter@gmail.com">Guilherme Alan Ritter</author>
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ Splitter Ratio: 0.5
+ Tree Height: 559
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ 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:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_active_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_passive_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ end_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ origin_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ shoulder_active_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_passive_link:
+ 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
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 1.74931
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0.170924
+ Y: -0.214539
+ Z: 0.105634
+ Name: Current View
+ Near Clip Distance: 0.01
+ Pitch: 0.505398
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 4.70358
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000013c000002bafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000036000002ba000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024700fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 336
+ Y: 94
--- /dev/null
+<robot name="q2d">\r
+\r
+ <link name="origin_link"/>\r
+\r
+ <link name="base_link">\r
+ <inertial>\r
+ <origin xyz="0.074571214 -0.00008502114 0.039600060500000"/>\r
+ <mass value="6.9621272394"/>\r
+ <inertia ixx="0.12983348950453222" ixy="0.00004414079177225"\r
+ ixz="-0.03019200341025967" iyy="0.17857934382693791"\r
+ iyz="0.00000393635720902" izz="0.25812143874621400" />\r
+ </inertial>\r
+\r
+ <visual>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/base.STL"/>\r
+ </geometry>\r
+ <material name="base_material">\r
+ <color rgba="0.55 0.45 0.45 1.0"/>\r
+ </material>\r
+ </visual>\r
+\r
+ <collision>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/base.STL"/>\r
+ </geometry>\r
+ </collision>\r
+ </link>\r
+\r
+ <joint name="origin_joint" type="fixed">\r
+ <parent link="origin_link"/>\r
+ <child link="base_link"/>\r
+ </joint>\r
+\r
+ <link name="shoulder_active_link">\r
+ <inertial>\r
+ <origin xyz="0.0252456823 -0.00000002723 0.06470401873"/>\r
+ <mass value="0.19730261508"/>\r
+ <inertia ixx="0.00038685518702305" ixy="0.00000000055222416"\r
+ ixz="-0.00000031340718614" iyy="0.00010241438913870"\r
+ iyz="-0.00000000015426019" izz="0.00047879093657893" />\r
+ </inertial>\r
+\r
+ <visual>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/shoulder_active.STL"/>\r
+ </geometry>\r
+ <material name="shoulder_active_material">\r
+ <color rgba="0.55 0.55 0.45 1.0"/>\r
+ </material>\r
+ </visual>\r
+\r
+ <collision>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/collision/shoulder_active.STL"/>\r
+ </geometry>\r
+ </collision>\r
+ </link>\r
+\r
+ <joint name="shoulder_active_joint" type="revolute">\r
+ <parent link="base_link"/>\r
+ <child link="shoulder_active_link"/>\r
+ <origin xyz="0 0 0.1477"/>\r
+ <axis xyz="0 0 1"/>\r
+ <limit lower="-1.570796326794897" upper="1.570796326794897"\r
+ velocity="2.27" effort="35.7" /> \r
+ <dynamics damping="17.4775" />\r
+ </joint>\r
+\r
+ <link name="shoulder_passive_link">\r
+ <inertial>\r
+ <mass value="1.26475817816"/>\r
+ <origin xyz="0.16516344805 -0.00048428845 -0.00016382412"/>\r
+ <inertia ixx="0.00346199967740929" ixy="-0.00010902049981923"\r
+ ixz="-0.00401182173261703" iyy="0.03314904030482527"\r
+ iyz="0.00005087359051462" izz="0.03113579694057124" />\r
+ </inertial>\r
+\r
+ <visual>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/shoulder_passive.STL"/>\r
+ </geometry>\r
+ <material name="shoulder_passive_material">\r
+ <color rgba="0.45 0.55 0.45 1.0"/>\r
+ </material>\r
+ </visual>\r
+\r
+ <collision>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/collision/shoulder_passive.STL"/>\r
+ </geometry>\r
+ </collision>\r
+ </link>\r
+\r
+ <joint name="shoulder_passive_joint" type="fixed">\r
+ <parent link="shoulder_active_link"/>\r
+ <child link="shoulder_passive_link"/>\r
+ </joint>\r
+\r
+ <link name="elbow_active_link">\r
+ <inertial>\r
+ <mass value="0.19712951877"/>\r
+ <origin xyz="0.02548273493 -0.00000002263 0.05254513577"/>\r
+ <inertia ixx="0.00038850510800265" ixy="0.00000000052121416"\r
+ ixz="0.00000404728675587" iyy="0.00010146693248154"\r
+ iyz="0.00000000002789435" izz="0.00048091942023028" />\r
+ </inertial>\r
+\r
+ <visual>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/elbow_active.STL"/>\r
+ </geometry>\r
+ <material name="elbow_active_material">\r
+ <color rgba="0.45 0.55 0.55 1.0"/>\r
+ </material>\r
+ </visual>\r
+\r
+ <collision>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/collision/elbow_active.STL"/>\r
+ </geometry>\r
+ </collision>\r
+ </link>\r
+\r
+ <joint name="elbow_active_joint" type="revolute">\r
+ <parent link="shoulder_passive_link"/>\r
+ <child link="elbow_active_link"/>\r
+ <origin xyz="0.343 0 0"/>\r
+ <axis xyz="0 0 1"/>\r
+ <limit lower="-1.570796326794897" upper="1.570796326794897"\r
+ velocity="23.08" effort="3.51" />\r
+ <dynamics damping="2.0503" />\r
+ </joint>\r
+\r
+ <link name="elbow_passive_link">\r
+ <inertial>\r
+ <mass value="0.67529215765"/>\r
+ <origin xyz="0.06204831581 0.00000013809 0.01489882531"/>\r
+ <inertia ixx="0.00132247071698947" ixy="-0.00000000605403474"\r
+ ixz="-0.00090893541574333" iyy="0.00774007102253750"\r
+ iyz="0.00000000624688369" izz="0.00751638349361413" />\r
+ </inertial>\r
+\r
+ <visual>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/elbow_passive.STL"/>\r
+ </geometry>\r
+ <material name="elbow_passive_material">\r
+ <color rgba="0.45 0.45 0.55 1.0"/>\r
+ </material>\r
+ </visual>\r
+\r
+ <collision>\r
+ <geometry>\r
+ <mesh filename="package://q2d_description/meshes/collision/elbow_passive.STL"/>\r
+ </geometry>\r
+ </collision>\r
+ </link>\r
+\r
+ <joint name="elbow_passive_joint" type="fixed">\r
+ <parent link="elbow_active_link"/>\r
+ <child link="elbow_passive_link"/>\r
+ </joint>\r
+\r
+ <link name="tool_link"/>\r
+\r
+ <joint name="end_joint" type="fixed">\r
+ <parent link="elbow_passive_link"/>\r
+ <child link="tool_link"/>\r
+ <origin xyz="0.267 0 0"/>\r
+ </joint>\r
+\r
+ <transmission name="shoulder_active_transmission">\r
+ <type>transmission_interface/SimpleTransmission</type>\r
+ <joint name="shoulder_active_joint">\r
+ <hardwareInterface>EffortJointInterface</hardwareInterface>\r
+ </joint>\r
+ <actuator name="shoulder_motor">\r
+ <mechanicalReduction>1</mechanicalReduction>\r
+ </actuator>\r
+ </transmission>\r
+\r
+ <transmission name="elbow_active_transmission">\r
+ <type>transmission_interface/SimpleTransmission</type>\r
+ <joint name="elbow_active_joint">\r
+ <hardwareInterface>EffortJointInterface</hardwareInterface>\r
+ </joint>\r
+ <actuator name="elbow_motor">\r
+ <mechanicalReduction>1</mechanicalReduction>\r
+ </actuator>\r
+ </transmission>\r
+\r
+ <gazebo>\r
+ <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">\r
+ <!--<robotNamespace>/q2d</robotNamespace>-->\r
+ <!-- Custom plugin -->
+ <robotSimType>gazebo_ros_electrical/DCmotorRobotHWSim</robotSimType>
+ <!-- Default plugin -->
+ <!-- <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType> -->\r
+ <controlPeriod>0.001</controlPeriod>\r
+ </plugin>\r
+ </gazebo>\r
+\r
+ <gazebo reference="base_link">
+ <visual>
+ <material>
+ <ambient>0.55 0.45 0.45 1.0</ambient>
+ <diffuse>0.55 0.45 0.45 1.0</diffuse>
+ <specular>0.55 0.45 0.45 1.0</specular>
+ <emissive>0.0 0.0 0.0 0.0</emissive>
+ </material>
+ </visual>
+ </gazebo>\r
+\r
+ <gazebo reference="shoulder_active_link">
+ <visual>
+ <material>
+ <ambient>0.55 0.55 0.45 1.0</ambient>
+ <diffuse>0.55 0.55 0.45 1.0</diffuse>
+ <specular>0.55 0.55 0.45 1.0</specular>
+ <emissive>0.0 0.0 0.0 0.0</emissive>
+ </material>
+ </visual>
+ </gazebo>\r
+\r
+ <gazebo reference="shoulder_passive_link">
+ <visual>
+ <material>
+ <ambient>0.55 0.45 0.45 1.0</ambient>
+ <diffuse>0.55 0.45 0.45 1.0</diffuse>
+ <specular>0.55 0.45 0.45 1.0</specular>
+ <emissive>0.0 0.0 0.0 0.0</emissive>
+ </material>
+ </visual>
+ </gazebo>
+\r
+ <gazebo reference="elbow_active_link">
+ <visual>
+ <material>
+ <ambient>0.45 0.55 0.55 1.0</ambient>
+ <diffuse>0.45 0.55 0.55 1.0</diffuse>
+ <specular>0.45 0.55 0.55 1.0</specular>
+ <emissive>0.0 0.0 0.0 0.0</emissive>
+ </material>
+ </visual>
+ </gazebo>\r
+\r
+ <gazebo reference="elbow_passive_link">
+ <visual>
+ <material>
+ <ambient>0.45 0.45 0.45 1.0</ambient>
+ <diffuse>0.45 0.45 0.45 1.0</diffuse>
+ <specular>0.45 0.45 0.45 1.0</specular>
+ <emissive>0.0 0.0 0.0 0.0</emissive>
+ </material>
+ </visual>
+ </gazebo>\r
+</robot>\r
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(q2d_teleop)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+ cmake_modules
+ gfxtablet_ros
+ joy
+ kdl_parser
+ orocos_kdl
+ roscpp
+ sensor_msgs
+ geometry_msgs
+ std_msgs
+ urdf
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Eigen REQUIRED)
+
+
+## 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 run_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 run_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
+# sensor_msgs geometry_msgs # std_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 run_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 you 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 q2d_teleop
+# CATKIN_DEPENDS cmake_modules gfxtablet_ros joy kdl_parser orocos_kdl roscpp sensor_msgs goemetry_msgs std_msgs urdf
+# DEPENDS Eigen
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+# TODO: Check names of system library include directories (Eigen)
+ ${Eigen_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/q2d_teleop.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(q2d_teleop_tablet src/q2d_teleop_tablet.cpp)
+add_executable(q2d_teleop_rviz src/q2d_teleop_rviz.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(q2d_teleop_tablet
+ ${catkin_LIBRARIES}
+ ${Eigen_LIBRARIES}
+)
+
+target_link_libraries(q2d_teleop_rviz
+ ${catkin_LIBRARIES}
+ ${Eigen_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
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_q2d_teleop.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)
--- /dev/null
+<launch>
+ <include file="$(find q2d_description)/launch/q2d.launch" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="q2d_origin_publisher" args="0 0 0 0 0 0 1 map origin_link" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find q2d_teleop)/rviz/display.rviz" required="true" />
+</launch>
--- /dev/null
+<launch>
+ <include file="$(find q2d_bringup)/launch/gazebo.launch">
+ <arg name="paused" value="false" />
+ </include>
+
+ <include file="$(find q2d_teleop)/launch/q2d_teleop_tablet.launch" />
+
+ <include file="$(find q2d_teleop)/launch/q2d_teleop_rviz.launch" />
+
+ <include file="$(find q2d_teleop)/launch/display.launch" />
+
+ <include file="$(find gfxtablet_ros)/launch/gfxtablet.launch" />
+</launch>
--- /dev/null
+<launch>
+ <node name="q2d_teleop_rviz" pkg="q2d_teleop" type="q2d_teleop_rviz"
+ respawn="false" output="screen" />
+</launch>
--- /dev/null
+<launch>
+ <node name="q2d_teleop_tablet" pkg="q2d_teleop" type="q2d_teleop_tablet"
+ respawn="false" output="screen" />
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>q2d_teleop</name>
+ <version>0.0.0</version>
+ <description>The q2d_teleop package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/q2d_teleop</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>Eigen</build_depend>
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>gfxtablet_ros</build_depend>
+ <build_depend>joy</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>urdf</build_depend>
+ <run_depend>Eigen</run_depend>
+ <run_depend>cmake_modules</run_depend>
+ <run_depend>gfxtablet_ros</run_depend>
+ <run_depend>joy</run_depend>
+ <run_depend>kdl_parser</run_depend>
+ <run_depend>orocos_kdl</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>urdf</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Grid1/Offset1
+ - /RobotModel1
+ - /RobotModel1/Links1
+ - /RobotModel1/Links1/tool_link1
+ - /RobotModel1/Links1/tool_link1/Position1
+ Splitter Ratio: 0.5
+ Tree Height: 559
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ 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: 0.2
+ 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: 8
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 0
+ Cell Size: 0.001
+ Class: rviz/Grid
+ Color: 59; 59; 59
+ 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: 1220
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_active_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_passive_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ origin_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ shoulder_active_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_passive_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tool_link:
+ Alpha: 1
+ Show Axes: true
+ Show Trail: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Angle: 0
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Name: Current View
+ Near Clip Distance: 0.01
+ Scale: 348.433
+ Target Frame: <Fixed Frame>
+ Value: TopDownOrtho (rviz)
+ X: 0.00473011
+ Y: -0.00390336
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000013c000002bafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000036000002ba000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024700fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 60
+ Y: 60
--- /dev/null
+/*
+ q2d_teleop_rviz: A ROS node to teloperate the Quanser 2DSFJE robot.
+
+ Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*/
+
+#include <ros/ros.h>
+#include <geometry_msgs/PointStamped.h>
+#include <std_msgs/Float64.h>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+
+#define sqr(x) (x*x)
+
+class Q2dTeleop
+{
+ public:
+ Q2dTeleop(ros::NodeHandle node);
+ ~Q2dTeleop(void);
+ void publish(void);
+
+ private:
+ ros::NodeHandle node_;
+
+ ros::Subscriber clickSub_;
+ ros::Publisher shoulderCmdPub_;
+ ros::Publisher elbowCmdPub_;
+
+ KDL::Frame goal_;
+ KDL::ChainIkSolverPos_LMA *ikSolverPos_;
+ KDL::JntArray q_;
+
+ void clickCB(const geometry_msgs::PointStamped &click);
+};
+
+Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
+{
+ node_=node;
+
+ clickSub_=node_.subscribe("/clicked_point",100,&Q2dTeleop::clickCB,this);
+ shoulderCmdPub_=node_.advertise<std_msgs::Float64>("shoulder_controller/command",100);
+ elbowCmdPub_=node_.advertise<std_msgs::Float64>("elbow_controller/command",100);
+
+ std::string robotDescription;
+ if(!node.getParam("robot_description",robotDescription))
+ ROS_ERROR_STREAM("Could not find 'robot_description'.");
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robotDescription,tree))
+ ROS_ERROR_STREAM("Failed to construct KDL tree.");
+
+ KDL::Chain chain;
+ if (!tree.getChain("origin_link","tool_link",chain))
+ ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
+
+ q_.resize(chain.getNrOfJoints());
+
+ goal_.Identity();
+ goal_.p.x(0.61);
+ goal_.p.z(0.1477);
+
+ Eigen::Matrix<double,6,1> L;
+ L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;
+
+ ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain,L);
+ ikSolverPos_->display_information=true;
+
+/*
+ std::cout << "q_=" << q_ << std::endl;
+ std::cout << "goal=" << goal_ << std::endl;
+
+ KDL::JntArray q_out=q_;
+ int error=0;
+ if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
+ ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ << ikSolverPos_->strError(error));
+ q_=q_out;
+ std::cout << "q_=" << q_ << std::endl;
+*/
+}
+
+Q2dTeleop::~Q2dTeleop(void)
+{
+ clickSub_.shutdown();
+ shoulderCmdPub_.shutdown();
+ elbowCmdPub_.shutdown();
+ delete ikSolverPos_;
+}
+
+void Q2dTeleop::clickCB(const geometry_msgs::PointStamped &click)
+{
+ goal_.p.x(click.point.x);
+ goal_.p.y(click.point.y);
+ goal_.p.z(0.1477);
+ goal_.M.RotZ(atan2(click.point.y,click.point.x));
+
+ publish();
+}
+
+void Q2dTeleop::publish(void)
+{
+/*
+ std::cout << "q_=" << q_ << std::endl;
+ std::cout << "goal=" << goal_ << std::endl;
+
+ KDL::JntArray q_out=q_;
+ int error=0;
+ if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
+ ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ << ikSolverPos_->strError(error));
+ q_=q_out;
+ std::cout << "q_=" << q_ << std::endl;
+*/
+
+ const double l1=0.343;
+ const double l2=0.267;
+ double c1=(sqr(goal_.p.x())+sqr(goal_.p.y())-sqr(l1)-sqr(l2))/2/l1/l2;
+ if(c1 >= 0.0 && c1 <= 1.0)
+ {
+ double s1a=sqrt(1-sqr(c1));
+ double s1b=-s1a;
+ double q1a=atan2(s1a,c1);
+ double q1b=atan2(s1b,c1);
+
+ double q0a=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1a,l1+l2*c1);
+ double q0b=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1b,l1+l2*c1);
+
+ if((fabs(q_(0)-q0a) < fabs(q_(0)-q0b)) || (cos(q0a) > 0.0))
+ {
+ q_(0)=q0a;
+ q_(1)=q1a;
+ }
+ else
+ {
+ q_(0)=q0b;
+ q_(1)=q1b;
+ }
+ }
+
+ std_msgs::Float64 shoulderCmd;
+ std_msgs::Float64 elbowCmd;
+
+ shoulderCmd.data=q_(0);
+ elbowCmd.data=q_(1);
+
+ shoulderCmdPub_.publish(shoulderCmd);
+ elbowCmdPub_.publish(elbowCmd);
+}
+
+
+int main(int argc,char* argv[])
+{
+ ros::init(argc,argv,"q2d_teleop_rviz");
+ ros::NodeHandle node;
+
+ Q2dTeleop q2dTeleop(node);
+
+ ros::spin();
+
+ return 0;
+}
--- /dev/null
+/*
+ q2d_teleop_tablet: A ROS node to teloperate the Quanser 2DSFJE robot.
+
+ Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+*/
+
+#include <ros/ros.h>
+#include <sensor_msgs/Joy.h>
+#include <std_msgs/Float64.h>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+
+#define sqr(x) (x*x)
+
+//DEBUG
+#include <kdl/frames_io.hpp>
+#include <kdl//kinfam_io.hpp>
+
+class Q2dTeleop
+{
+ public:
+ Q2dTeleop(ros::NodeHandle node);
+ ~Q2dTeleop(void);
+ void publish(void);
+
+ private:
+ ros::NodeHandle node_;
+
+ ros::Subscriber joySub_;
+ ros::Publisher shoulderCmdPub_;
+ ros::Publisher elbowCmdPub_;
+
+ KDL::Frame goal_;
+ KDL::ChainIkSolverPos_LMA *ikSolverPos_;
+ KDL::JntArray q_;
+
+ void joyCB(const sensor_msgs::Joy &joy);
+};
+
+Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
+{
+ node_=node;
+
+ joySub_=node_.subscribe("joy",100,&Q2dTeleop::joyCB,this);
+ shoulderCmdPub_=node_.advertise<std_msgs::Float64>("shoulder_controller/command",100);
+ elbowCmdPub_=node_.advertise<std_msgs::Float64>("elbow_controller/command",100);
+
+ std::string robotDescription;
+ if(!node.getParam("robot_description",robotDescription))
+ ROS_ERROR_STREAM("Could not find 'robot_description'.");
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robotDescription,tree))
+ ROS_ERROR_STREAM("Failed to construct KDL tree.");
+
+ KDL::Chain chain;
+ if (!tree.getChain("origin_link","tool_link",chain))
+ ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
+
+ q_.resize(chain.getNrOfJoints());
+
+ goal_.Identity();
+ goal_.p.x(0.61);
+ goal_.p.z(0.1477);
+
+ Eigen::Matrix<double,6,1> L;
+ L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;
+
+ ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain,L);
+ ikSolverPos_->display_information=true;
+
+/*
+ std::cout << "q_=" << q_ << std::endl;
+ std::cout << "goal=" << goal_ << std::endl;
+
+ KDL::JntArray q_out=q_;
+ int error=0;
+ if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
+ ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ << ikSolverPos_->strError(error));
+ q_=q_out;
+ std::cout << "q_=" << q_ << std::endl;
+*/
+}
+
+Q2dTeleop::~Q2dTeleop(void)
+{
+ joySub_.shutdown();
+ shoulderCmdPub_.shutdown();
+ elbowCmdPub_.shutdown();
+ delete ikSolverPos_;
+}
+
+void Q2dTeleop::joyCB(const sensor_msgs::Joy &joy)
+{
+// goal_.p.x(joy.axes[0]*0.61/2.0);
+// goal_.p.y(-joy.axes[1]*0.61);
+ goal_.p.x(joy.axes[0]*0.8);
+ goal_.p.y(-joy.axes[1]*0.8); // 20 cm/division in GfxTablet
+ goal_.p.z(0.1477);
+ goal_.M.RotZ(atan2(joy.axes[1],joy.axes[0]));
+
+ publish();
+}
+
+void Q2dTeleop::publish(void)
+{
+/*
+ std::cout << "q_=" << q_ << std::endl;
+ std::cout << "goal=" << goal_ << std::endl;
+
+ KDL::JntArray q_out=q_;
+ int error=0;
+ if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
+ ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ << ikSolverPos_->strError(error));
+ q_=q_out;
+ std::cout << "q_=" << q_ << std::endl;
+*/
+
+ const double l1=0.343;
+ const double l2=0.267;
+ double c1=(sqr(goal_.p.x())+sqr(goal_.p.y())-sqr(l1)-sqr(l2))/2/l1/l2;
+ if(c1 >= 0.0 && c1 <= 1.0)
+ {
+ double s1a=sqrt(1-sqr(c1));
+ double s1b=-s1a;
+ double q1a=atan2(s1a,c1);
+ double q1b=atan2(s1b,c1);
+
+ double q0a=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1a,l1+l2*c1);
+ double q0b=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1b,l1+l2*c1);
+
+ if((fabs(q_(0)-q0a) < fabs(q_(0)-q0b)) || (cos(q0a) > 0.0))
+ {
+ q_(0)=q0a;
+ q_(1)=q1a;
+ }
+ else
+ {
+ q_(0)=q0b;
+ q_(1)=q1b;
+ }
+ }
+
+ std_msgs::Float64 shoulderCmd;
+ std_msgs::Float64 elbowCmd;
+
+ shoulderCmd.data=q_(0);
+ elbowCmd.data=q_(1);
+
+ shoulderCmdPub_.publish(shoulderCmd);
+ elbowCmdPub_.publish(elbowCmd);
+}
+
+
+int main(int argc,char* argv[])
+{
+ ros::init(argc,argv,"q2d_teleop_tablet");
+ ros::NodeHandle node;
+
+ Q2dTeleop q2dTeleop(node);
+
+ ros::spin();
+
+ return 0;
+}
projects / q2d.git / commitdiff