Commit cc3f764b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Initial commit

parents
# Pre-requisites about cmake itself
CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# The project name and the type of project
PROJECT(arm-control)
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
ENDIF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
ADD_SUBDIRECTORY(src)
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
IF (IRI_DOC_DIR)
ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
ELSE (IRI_DOC_DIR)
ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
ENDIF (IRI_DOC_DIR)
ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
IF (UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "distribution clean"
COMMAND make ARGS clean
COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
TARGET distclean
)
ELSE(UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "distclean only implemented in unix"
TARGET distclean
)
ENDIF(UNIX)
ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
IF (UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "uninstall package"
COMMAND xargs ARGS rm < install_manifest.txt
TARGET uninstall
)
ELSE(UNIX)
ADD_CUSTOM_COMMAND(
COMMENT "uninstall only implemented in unix"
TARGET uninstall
)
ENDIF(UNIX)
#edit the following line to add the librarie's header files
FIND_PATH(arm_control_INCLUDE_DIR arm_control.h /usr/include/iridrivers /usr/local/include/iridrivers)
FIND_LIBRARY(arm_control_LIBRARY
NAMES arm_control
PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
IF (arm_control_INCLUDE_DIR AND arm_control_LIBRARY)
SET(arm_control_FOUND TRUE)
ENDIF (arm_control_INCLUDE_DIR AND arm_control_LIBRARY)
IF (arm_control_FOUND)
IF (NOT arm_control_FIND_QUIETLY)
MESSAGE(STATUS "Found arm_control: ${arm_control_LIBRARY}")
ENDIF (NOT arm_control_FIND_QUIETLY)
ELSE (arm_control_FOUND)
IF (arm_control_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find arm_control")
ENDIF (arm_control_FIND_REQUIRED)
ENDIF (arm_control_FOUND)
arm_control {#mainpage}
============
## Description
ARM control driver
## Dependencies
This package requires of the following system libraries and packages
* [cmake](https://www.cmake.org "CMake's Homepage"), a cross-platform build system.
* [doxygen](http://www.doxygen.org "Doxygen's Homepage") and [graphviz](http://www.graphviz.org "Graphviz's Homepage") to generate the documentation.
* stdc++ and pthread libraries.
Under linux all of these utilities are available in ready-to-use packages.
Under MacOS most of the packages are available via [fink](http://www.finkproject.org/ "Fink's Homepage")
This package also requires of the following IRI libraries:
## Compilation and installation
Download this repository and create a build folder inside:
``` mkdir build ```
Inside the build folder execute the following commands:
``` cmake .. ```
The default build mode is DEBUG. That is, objects and executables include debug information.
The RELEASE build mode optimizes for speed. To build in this mode execute instead
``` cmake .. -DCMAKE_BUILD_TYPE=RELEASE ```
The release mode will be kept until next time cmake is executed.
``` make ```
In case no errors are reported, the generated libraries (if any) will be located at the
_lib_ folder and the executables (if any) will be located at the _bin_ folder.
In order to be able to use the library, it it necessary to copy it into the system.
To do that, execute
``` make install ```
as root and the shared libraries will be copied to */usr/local/lib/iridrivers* directory
and the header files will be copied to */usr/local/include/iridrivers* dierctory. At
this point, the library may be used by any user.
To remove the library from the system, exceute
``` make uninstall ```
as root, and all the associated files will be removed from the system.
To generate the documentation execute the following command:
``` make doc ```
## How to use it
To use this library in an other library or application, in the CMakeLists.txt file, first it is necessary to locate if the library has been installed or not using the following command
``` FIND_PACKAGE(arm_control) ```
In the case that the package is present, it is necessary to add the header files directory to the include directory path by using
``` INCLUDE_DIRECTORIES(${arm_control_INCLUDE_DIR}) ```
and it is also necessary to link with the desired libraries by using the following command
``` TARGET_LINK_LIBRARIES(<executable name> ${arm_control_LIBRARY}) ```
## Disclaimer
Copyright (C) 2009-2018 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
Mantainer IRI labrobotics (labrobotica@iri.upc.edu)
This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>
# Doxyfile 1.5.5
@INCLUDE_PATH = ../doc/
@INCLUDE = doxygen_project_name.conf
#---------------------------------------------------------------------------
# Project related configuration options
#---------------------------------------------------------------------------
DOXYFILE_ENCODING = UTF-8
PROJECT_NUMBER =
OUTPUT_DIRECTORY = ../doc
CREATE_SUBDIRS = NO
OUTPUT_LANGUAGE = English
BRIEF_MEMBER_DESC = YES
REPEAT_BRIEF = NO
ABBREVIATE_BRIEF =
ALWAYS_DETAILED_SEC = NO
INLINE_INHERITED_MEMB = NO
FULL_PATH_NAMES = YES
STRIP_FROM_PATH =
STRIP_FROM_INC_PATH =
SHORT_NAMES = NO
JAVADOC_AUTOBRIEF = NO
QT_AUTOBRIEF = NO
MULTILINE_CPP_IS_BRIEF = NO
DETAILS_AT_TOP = NO
INHERIT_DOCS = YES
SEPARATE_MEMBER_PAGES = NO
TAB_SIZE = 8
ALIASES =
OPTIMIZE_OUTPUT_FOR_C = YES
OPTIMIZE_OUTPUT_JAVA = NO
OPTIMIZE_FOR_FORTRAN = NO
OPTIMIZE_OUTPUT_VHDL = NO
BUILTIN_STL_SUPPORT = NO
CPP_CLI_SUPPORT = NO
SIP_SUPPORT = NO
DISTRIBUTE_GROUP_DOC = NO
SUBGROUPING = YES
TYPEDEF_HIDES_STRUCT = NO
#---------------------------------------------------------------------------
# Build related configuration options
#---------------------------------------------------------------------------
EXTRACT_ALL = NO
EXTRACT_PRIVATE = NO
EXTRACT_STATIC = NO
EXTRACT_LOCAL_CLASSES = YES
EXTRACT_LOCAL_METHODS = NO
EXTRACT_ANON_NSPACES = NO
HIDE_UNDOC_MEMBERS = NO
HIDE_UNDOC_CLASSES = NO
HIDE_FRIEND_COMPOUNDS = NO
HIDE_IN_BODY_DOCS = NO
INTERNAL_DOCS = NO
CASE_SENSE_NAMES = YES
HIDE_SCOPE_NAMES = NO
SHOW_INCLUDE_FILES = YES
INLINE_INFO = YES
SORT_MEMBER_DOCS = NO
SORT_BRIEF_DOCS = NO
SORT_GROUP_NAMES = NO
SORT_BY_SCOPE_NAME = NO
GENERATE_TODOLIST = YES
GENERATE_TESTLIST = YES
GENERATE_BUGLIST = YES
GENERATE_DEPRECATEDLIST= YES
ENABLED_SECTIONS =
MAX_INITIALIZER_LINES = 30
SHOW_USED_FILES = YES
SHOW_DIRECTORIES = YES
FILE_VERSION_FILTER =
#---------------------------------------------------------------------------
# configuration options related to warning and progress messages
#---------------------------------------------------------------------------
QUIET = YES
WARNINGS = YES
WARN_IF_UNDOCUMENTED = YES
WARN_IF_DOC_ERROR = YES
WARN_NO_PARAMDOC = NO
WARN_FORMAT = "$file:$line: $text"
WARN_LOGFILE =
#---------------------------------------------------------------------------
# configuration options related to the input files
#---------------------------------------------------------------------------
INPUT = ../src \
../include \
../ReadMe.md
INPUT_ENCODING = UTF-8
FILE_PATTERNS = *.c \
*.h \
*.cpp
RECURSIVE = YES
EXCLUDE =
EXCLUDE_SYMLINKS = NO
EXCLUDE_PATTERNS = *.tab.c \
*.tab.h \
lex* \
*glr.h \
*llr.h \
*glr.c \
*llr.c \
*general.h
EXCLUDE_SYMBOLS =
EXAMPLE_PATH = ../src/examples
EXAMPLE_PATTERNS =
EXAMPLE_RECURSIVE = NO
IMAGE_PATH = ../doc/images
INPUT_FILTER =
FILTER_PATTERNS =
FILTER_SOURCE_FILES = NO
#---------------------------------------------------------------------------
# configuration options related to source browsing
#---------------------------------------------------------------------------
SOURCE_BROWSER = YES
INLINE_SOURCES = NO
STRIP_CODE_COMMENTS = YES
REFERENCED_BY_RELATION = YES
REFERENCES_RELATION = YES
REFERENCES_LINK_SOURCE = YES
USE_HTAGS = NO
VERBATIM_HEADERS = YES
#---------------------------------------------------------------------------
# configuration options related to the alphabetical class index
#---------------------------------------------------------------------------
ALPHABETICAL_INDEX = YES
COLS_IN_ALPHA_INDEX = 5
IGNORE_PREFIX =
#---------------------------------------------------------------------------
# configuration options related to the HTML output
#---------------------------------------------------------------------------
GENERATE_HTML = YES
HTML_OUTPUT = html
HTML_FILE_EXTENSION = .html
HTML_HEADER =
HTML_FOOTER =
HTML_STYLESHEET =
HTML_ALIGN_MEMBERS = YES
GENERATE_HTMLHELP = NO
GENERATE_DOCSET = NO
DOCSET_FEEDNAME = "Doxygen generated docs"
DOCSET_BUNDLE_ID = org.doxygen.Project
HTML_DYNAMIC_SECTIONS = NO
CHM_FILE =
HHC_LOCATION =
GENERATE_CHI = NO
BINARY_TOC = NO
TOC_EXPAND = NO
DISABLE_INDEX = NO
ENUM_VALUES_PER_LINE = 4
GENERATE_TREEVIEW = NO
TREEVIEW_WIDTH = 250
#---------------------------------------------------------------------------
# configuration options related to the LaTeX output
#---------------------------------------------------------------------------
GENERATE_LATEX = NO
LATEX_OUTPUT = latex
LATEX_CMD_NAME = latex
MAKEINDEX_CMD_NAME = makeindex
COMPACT_LATEX = NO
PAPER_TYPE = a4
EXTRA_PACKAGES =
LATEX_HEADER =
PDF_HYPERLINKS = YES
USE_PDFLATEX = NO
LATEX_BATCHMODE = NO
LATEX_HIDE_INDICES = NO
#---------------------------------------------------------------------------
# configuration options related to the RTF output
#---------------------------------------------------------------------------
GENERATE_RTF = NO
RTF_OUTPUT = rtf
COMPACT_RTF = NO
RTF_HYPERLINKS = NO
RTF_STYLESHEET_FILE =
RTF_EXTENSIONS_FILE =
#---------------------------------------------------------------------------
# configuration options related to the man page output
#---------------------------------------------------------------------------
GENERATE_MAN = NO
MAN_OUTPUT = man
MAN_EXTENSION = .3
MAN_LINKS = NO
#---------------------------------------------------------------------------
# configuration options related to the XML output
#---------------------------------------------------------------------------
GENERATE_XML = NO
XML_OUTPUT = xml
XML_SCHEMA =
XML_DTD =
XML_PROGRAMLISTING = YES
#---------------------------------------------------------------------------
# configuration options for the AutoGen Definitions output
#---------------------------------------------------------------------------
GENERATE_AUTOGEN_DEF = NO
#---------------------------------------------------------------------------
# configuration options related to the Perl module output
#---------------------------------------------------------------------------
GENERATE_PERLMOD = NO
PERLMOD_LATEX = NO
PERLMOD_PRETTY = YES
PERLMOD_MAKEVAR_PREFIX =
#---------------------------------------------------------------------------
# Configuration options related to the preprocessor
#---------------------------------------------------------------------------
ENABLE_PREPROCESSING = YES
MACRO_EXPANSION = NO
EXPAND_ONLY_PREDEF = NO
SEARCH_INCLUDES = YES
INCLUDE_PATH =
INCLUDE_FILE_PATTERNS =
PREDEFINED = _USE_MPI=1
EXPAND_AS_DEFINED =
SKIP_FUNCTION_MACROS = YES
#---------------------------------------------------------------------------
# Configuration::additions related to external references
#---------------------------------------------------------------------------
TAGFILES =
GENERATE_TAGFILE =
ALLEXTERNALS = NO
EXTERNAL_GROUPS = YES
PERL_PATH = /usr/bin/perl
#---------------------------------------------------------------------------
# Configuration options related to the dot tool
#---------------------------------------------------------------------------
CLASS_DIAGRAMS = YES
MSCGEN_PATH =
HIDE_UNDOC_RELATIONS = YES
HAVE_DOT = YES
CLASS_GRAPH = YES
COLLABORATION_GRAPH = YES
GROUP_GRAPHS = YES
UML_LOOK = NO
TEMPLATE_RELATIONS = NO
INCLUDE_GRAPH = NO
INCLUDED_BY_GRAPH = NO
CALL_GRAPH = YES
CALLER_GRAPH = YES
GRAPHICAL_HIERARCHY = YES
DIRECTORY_GRAPH = NO
DOT_IMAGE_FORMAT = png
DOT_PATH =
DOTFILE_DIRS =
DOT_GRAPH_MAX_NODES = 50
MAX_DOT_GRAPH_DEPTH = 2
DOT_TRANSPARENT = YES
DOT_MULTI_TARGETS = NO
GENERATE_LEGEND = YES
DOT_CLEANUP = YES
#---------------------------------------------------------------------------
# Configuration::additions related to the search engine
#---------------------------------------------------------------------------
SEARCHENGINE = NO
PROJECT_NAME = "arm_control"
#ifndef _ARM_CONTROL_H
#define _ARM_CONTROL_H
#include "dynamixelserver_ftdi.h"
#include "dynamixel_motor.h"
#include "threadserver.h"
#include "eventserver.h"
#include "joystick.h"
typedef enum {PAN=0,TILT=1,ROLL=2} motor_id_t;
typedef struct
{
double pan;
double tilt;
double roll;
}TJoints;
class CArmControl
{
private:
bool exit_flag;
CThreadServer *thread_server;
std::string position_monitor_thread_id;
CEventServer *event_server;
std::string finish_thread_event_id;
std::vector<CDynamixelMotor *> motors;
CDynamixelServerFTDI *dyn_server;
CJoystick joy;
std::vector<bool> current_button_state;
double max_speed;
TJoints max_angles;
TJoints min_angles;
TJoints max_torques;
TJoints target_torques;
std::vector<TJoints> stored_poses;
std::string poses_file;
protected:
static void *position_monitor_thread(void *param);
public:
CArmControl(const std::string &joy_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id);
bool get_button_state(unsigned int button_id);
bool exit(void);
// servo functionis
void set_max_angles(unsigned int axis_id,double max,double min);
void set_max_torque(unsigned int axis_id,double max);
void enable_torque(bool enable);
void move(unsigned int axis_id,double torque);
void stop(void);
// stored poses
void set_poses_max_speed(double max);
void load_poses(const std::string &filename);
void save_poses(const std::string &filename);
void update_pose(unsigned int pose_id);
void execute_pose(unsigned int pose_id);
~CArmControl();
};
#endif
ADD_SUBDIRECTORY(xml)
# driver source files
SET(sources arm_control.cpp)
# application header files
SET(headers ../include/arm_control.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(joystick REQUIRED)
FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
FIND_PACKAGE(dynamixel_motor_cont REQUIRED)
# add the necessary include directories
INCLUDE_DIRECTORIES(../include)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${joystick_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_motor_cont_INCLUDE_DIR})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
# create the shared library
ADD_LIBRARY(arm_control SHARED ${sources} ${XSD_SOURCES})
TARGET_LINK_LIBRARIES(arm_control ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${joystick_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${dynamixel_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${dynamixel_motor_cont_LIBRARY})
TARGET_LINK_LIBRARIES(arm_control ${XSD_LIBRARY})
ADD_DEPENDENCIES(arm_control xsd_files_gen)
# link necessary libraries
INSTALL(TARGETS arm_control
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers)
INSTALL(FILES ${headers} DESTINATION include/iridrivers)
INSTALL(FILES ../Findarm_control.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
ADD_SUBDIRECTORY(examples)
#include "arm_control.h"
#include "exceptions.h"
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <sys/select.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <linux/joystick.h>
#include "xml/arm_motions_cfg_file.hxx"
void button_callback(void *param,unsigned int button_id,bool level)
{
CArmControl *arm=(CArmControl *)param;
try{
switch(button_id)
{
case 8: if(level)
arm->enable_torque(false);
else
arm->enable_torque(true);
break;
case 10: if(!level)
{
// button rear left 1 released -> keep position
arm->stop();
}
break;
case 12: if(level)
{
if(arm->get_button_state(11))
arm->update_pose(0);// update circle pose
else
arm->execute_pose(0);// execute circle pose
}
break;
case 13: if(level)
{
if(arm->get_button_state(11))
arm->update_pose(1);// update circle pose
else
arm->execute_pose(1);// execute circle pose
}
break;
case 14: if(level)
{
if(arm->get_button_state(11))
arm->update_pose(2);// update circle pose
else
arm->execute_pose(2);// execute circle pose
}
break;
case 15: if(level)
{
if(arm->get_button_state(11))
arm->update_pose(3);// update circle pose
else
arm->execute_pose(3);// execute circle pose
}
break;
}
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
void axis_callback(void *param,unsigned int axis_id, float value)
{
CArmControl *arm=(CArmControl *)param;
try{
switch(axis_id)
{
case 0: if(arm->get_button_state(10))
arm->move(PAN,value);
break;
case 1: if(arm->get_button_state(10))
arm->move(TILT,value);
break;
case 2: if(arm->get_button_state(10))
arm->move(ROLL,value);
break;
}
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
CArmControl::CArmControl(const std::string &joy_device,std::string &dyn_serial, unsigned int baudrate,unsigned char pan_id, unsigned char tilt_id, unsigned char roll_id) : joy("arm_control_jpy")
{
std::string name;