Skip to content
Snippets Groups Projects
Commit 63eb7ea1 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a driver for the new version of the darwin robot firmware.

parents
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1434 additions and 0 deletions
CMakeLists.txt 0 → 100644
+ 68
− 0
View file @ 63eb7ea1
# 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(darwin-robot)
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)
FindKDL.cmake 0 → 100755
+ 56
− 0
View file @ 63eb7ea1
# Locate KDL install directory
# This module defines
# KDL_INSTALL where to find include, lib, bin, etc.
# KDL_FOUND, is set to true
#INCLUDE (${PROJECT_SOURCE_DIR}/config/FindPkgConfig.cmake)
IF ( CMAKE_PKGCONFIG_EXECUTABLE )
MESSAGE( STATUS "Detecting KDL" )
PKGCONFIG( "orocos-kdl" KDL_FOUND KDL_INCLUDE_DIRS KDL_DEFINES KDL_LINK_DIRS KDL_LIBS )
IF( KDL_FOUND )
MESSAGE(" Includes in: ${KDL_INCLUDE_DIRS}")
MESSAGE(" Libraries in: ${KDL_LINK_DIRS}")
MESSAGE(" Libraries: ${KDL_LIBS}")
MESSAGE(" Defines: ${KDL_DEFINES}")
INCLUDE_DIRECTORIES( ${KDL_INCLUDE_DIRS} )
LINK_DIRECTORIES( ${KDL_LINK_DIRS})
#OROCOS_PKGCONFIG_INCPATH("${KDLTK_INCLUDE_DIRS}")
#OROCOS_PKGCONFIG_LIBS("${KDL_LIBS}")
#OROCOS_PKGCONFIG_LIBPATH("${KDL_LINK_DIRS}")
SET(ENV{PKG_CONFIG_PATH} "${KDL_INSTALL}/lib/pkgconfig/:${OROCOS_INSTALL}/lib/pkgconfig")
MESSAGE( "Looking for KDL Toolkit in: ${PKG_CONFIG_PATH}")
PKGCONFIG( "orocos-kdltk-${OROCOS_TARGET} >= 0.99" KDLTK_FOUND KDLTK_INCLUDE_DIRS KDLTK_DEFINES KDLTK_LINK_DIRS KDLTK_LIBS )
IF(KDLTK_FOUND)
INCLUDE_DIRECTORIES( ${KDLTK_INCLUDE_DIRS} )
LINK_DIRECTORIES( ${KDLTK_LINK_DIRS})
OROCOS_PKGCONFIG_INCPATH("${KDLTK_INCLUDE_DIRS}")
OROCOS_PKGCONFIG_LIBPATH("${KDLTK_LINK_DIRS}")
OROCOS_PKGCONFIG_LIBS("${KDLTK_LIBS}")
IF(CORBA_ENABLED)
SET(ENV{PKG_CONFIG_PATH} "${KDL_INSTALL}/lib/pkgconfig/:${OROCOS_INSTALL}/lib/pkgconfig")
MESSAGE("Looking for KDL Toolkit CORBA extension in ${PKG_CONFIG_PATH}")
PKGCONFIG( "orocos-kdltk-corba-${OROCOS_TARGET} >= 0.99" KDLTKCORBA_FOUND KDLTKCORBA_INCLUDE_DIRS KDLTKCORBA_DEFINES KDLTKCORBA_LINK_DIRS KDLTKCORBA_LIBS )
IF(KDLTKCORBA_FOUND)
INCLUDE_DIRECTORIES( ${KDLTKCORBA_INCLUDE_DIRS} )
LINK_DIRECTORIES( ${KDLTKCORBA_LINK_DIRS})
OROCOS_PKGCONFIG_INCPATH("${KDLTKCORBA_INCLUDE_DIRS}")
OROCOS_PKGCONFIG_LIBPATH("${KDLTKCORBA_LINK_DIRS}")
OROCOS_PKGCONFIG_LIBS("${KDLTKCORBA_LIBS}")
ENDIF ( KDLTKCORBA_FOUND )
ENDIF(CORBA_ENABLED)
ENDIF ( KDLTK_FOUND )
ENDIF ( KDL_FOUND )
ELSE ( CMAKE_PKGCONFIG_EXECUTABLE )
# Can't find pkg-config -- have to search manually
MESSAGE( FATAL_ERROR "Can't find KDL without pkgconfig !")
ENDIF ( CMAKE_PKGCONFIG_EXECUTABLE )
FindPkgConfig.cmake 0 → 100755
+ 133
− 0
View file @ 63eb7ea1
## FindPkgConfig.cmake
## by Albert Strasheim <http://students . ee . sun . ac . za/~albert/>
## and Alex Brooks (a.brooks at acfr . usyd . edu . au)
##
## This module finds packages using pkg-config, which retrieves
## information about packages from special metadata files.
##
## See http://www . freedesktop . org/Software/pkgconfig/
##
## -------------------------------------------------------------------
##
## Usage:
##
## INCLUDE( ${CMAKE_ROOT}/Modules/FindPkgConfig.cmake)
##
## IF ( CMAKE_PKGCONFIG_EXECUTABLE )
##
## # Find all the librtk stuff with pkg-config
## PKGCONFIG( "librtk >= 2.0" HAVE_RTK RTK_INCLUDE_DIRS RTK_DEFINES RTK_LINK_DIRS RTK_LIBS )
##
## ELSE ( CMAKE_PKGCONFIG_EXECUTABLE )
##
## # Can't find pkg-config -- have to find librtk somehow else
##
## ENDIF ( CMAKE_PKGCONFIG_EXECUTABLE )
##
##
## Notes:
##
## You can set the PKG_CONFIG_PATH environment variable to tell
## pkg-config where to search for .pc files. See pkg-config(1) for
## more information.
##
#
# FIXME: IF(WIN32) pkg-config --msvc-syntax ENDIF(WIN32) ???
#
# FIXME: Parsing of pkg-config output is specific to gnu-style flags
#
FIND_PROGRAM(CMAKE_PKGCONFIG_EXECUTABLE pkg-config)
MARK_AS_ADVANCED(CMAKE_PKGCONFIG_EXECUTABLE)
########################################
MACRO(PKGCONFIG_PARSE_FLAGS FLAGS INCLUDES DEFINES)
#MESSAGE("DEBUG: FLAGS: ${FLAGS}")
STRING(REGEX MATCHALL " -I[^ ]*" ${INCLUDES} "${FLAGS}")
STRING(REGEX REPLACE " -I" "" ${INCLUDES} "${${INCLUDES}}")
#MESSAGE("DEBUG: INCLUDES: ${${INCLUDES}}")
STRING(REGEX REPLACE " -I[^ ]*" "" ${DEFINES} "${FLAGS}")
#MESSAGE("DEBUG: DEFINES: ${${DEFINES}}")
ENDMACRO(PKGCONFIG_PARSE_FLAGS)
########################################
MACRO(PKGCONFIG_PARSE_LIBS LIBS LINKDIRS LINKLIBS)
#MESSAGE("DEBUG: LIBS: ${LIBS}")
STRING(REGEX MATCHALL " -L[^ ]*" ${LINKDIRS} "${LIBS}")
STRING(REGEX REPLACE " -L" "" ${LINKDIRS} "${${LINKDIRS}}")
#MESSAGE("DEBUG: LINKDIRS: ${${LINKDIRS}}")
STRING(REGEX MATCHALL " -l[^ ]*" ${LINKLIBS} "${LIBS}")
STRING(REGEX REPLACE " -l" "" ${LINKLIBS} "${${LINKLIBS}}")
#MESSAGE("DEBUG: LINKLIBS: ${${LINKLIBS}}")
ENDMACRO(PKGCONFIG_PARSE_LIBS)
########################################
MACRO(PKGCONFIG LIBRARY FOUND INCLUDE_DIRS DEFINES LINKDIRS LINKLIBS)
SET(${FOUND} 0)
# alexm: why print it twice? once here, and once when it's found/not found
# MESSAGE("-- Looking for ${LIBRARY}")
IF(CMAKE_PKGCONFIG_EXECUTABLE)
# MESSAGE("DEBUG: pkg-config executable found")
EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE}
ARGS "'${LIBRARY}'"
OUTPUT_VARIABLE PKGCONFIG_OUTPUT
RETURN_VALUE PKGCONFIG_RETURN)
IF(NOT PKGCONFIG_RETURN)
# set C_FLAGS and CXX_FLAGS
EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE}
ARGS "--cflags '${LIBRARY}'"
OUTPUT_VARIABLE CMAKE_PKGCONFIG_C_FLAGS)
#SET(CMAKE_PKGCONFIG_CXX_FLAGS "${CMAKE_PKGCONFIG_C_FLAGS}")
PKGCONFIG_PARSE_FLAGS(" ${CMAKE_PKGCONFIG_C_FLAGS}" ${INCLUDE_DIRS} ${DEFINES} )
# set LIBRARIES
EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE}
ARGS "--libs '${LIBRARY}'"
OUTPUT_VARIABLE CMAKE_PKGCONFIG_LIBRARIES)
PKGCONFIG_PARSE_LIBS (" ${CMAKE_PKGCONFIG_LIBRARIES}" ${LINKDIRS} ${LINKLIBS} )
SET(${FOUND} 1)
MESSAGE("-- Looking for ${LIBRARY} -- found")
ELSE(NOT PKGCONFIG_RETURN)
MESSAGE("-- Looking for ${LIBRARY} -- not found")
SET(CMAKE_PKGCONFIG_C_FLAGS "")
SET(CMAKE_PKGCONFIG_CXX_FLAGS "")
SET(CMAKE_PKGCONFIG_LIBRARIES "")
SET(${INCLUDE_DIRS} "")
SET(${DEFINES} "")
SET(${LINKDIRS} "")
SET(${LINKLIBS} "")
ENDIF(NOT PKGCONFIG_RETURN)
ELSE(CMAKE_PKGCONFIG_EXECUTABLE)
MESSAGE("-- pkg-config executable NOT FOUND")
ENDIF(CMAKE_PKGCONFIG_EXECUTABLE)
#MESSAGE("Have ${LIBRARY} : ${${FOUND}}")
#MESSAGE("${LIBRARY} include dirs: ${${INCLUDE_DIRS}}")
#MESSAGE("${LIBRARY} defines : ${${DEFINES}}")
#MESSAGE("${LIBRARY} link dirs : ${${LINKDIRS}}")
#MESSAGE("${LIBRARY} link libs : ${${LINKLIBS}}")
ENDMACRO(PKGCONFIG)
Finddarwin_arm_kinematics.cmake 0 → 100755
+ 21
− 0
View file @ 63eb7ea1
#edit the following line to add the librarie's header files
FIND_PATH(darwin_arm_kinematics_INCLUDE_DIR darwin_arm_kinematics.h /usr/include/iridrivers /usr/local/include/iridrivers)
FIND_LIBRARY(darwin_arm_kinematics_LIBRARY
NAMES darwin_arm_kinematics
PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
IF (darwin_arm_kinematics_INCLUDE_DIR AND darwin_arm_kinematics_LIBRARY)
SET(darwin_arm_kinematics_FOUND TRUE)
ENDIF (darwin_arm_kinematics_INCLUDE_DIR AND darwin_arm_kinematics_LIBRARY)
IF (darwin_arm_kinematics_FOUND)
IF (NOT darwin_arm_kinematics_FIND_QUIETLY)
MESSAGE(STATUS "Found darwin_arm_kinematics: ${darwin_arm_kinematics_LIBRARY}")
ENDIF (NOT darwin_arm_kinematics_FIND_QUIETLY)
ELSE (darwin_arm_kinematics_FOUND)
IF (darwin_arm_kinematics_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find darwin_arm_kinematics")
ENDIF (darwin_arm_kinematics_FIND_REQUIRED)
ENDIF (darwin_arm_kinematics_FOUND)
Finddarwin_robot.cmake 0 → 100644
+ 21
− 0
View file @ 63eb7ea1
#edit the following line to add the librarie's header files
FIND_PATH(darwin_robot_INCLUDE_DIR darwin_robot.h /usr/include/iridrivers /usr/local/include/iridrivers)
FIND_LIBRARY(darwin_robot_LIBRARY
NAMES darwin_robot
PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
IF (darwin_robot_INCLUDE_DIR AND darwin_robot_LIBRARY)
SET(darwin_robot_FOUND TRUE)
ENDIF (darwin_robot_INCLUDE_DIR AND darwin_robot_LIBRARY)
IF (darwin_robot_FOUND)
IF (NOT darwin_robot_FIND_QUIETLY)
MESSAGE(STATUS "Found darwin_robot: ${darwin_robot_LIBRARY}")
ENDIF (NOT darwin_robot_FIND_QUIETLY)
ELSE (darwin_robot_FOUND)
IF (darwin_robot_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find darwin_robot")
ENDIF (darwin_robot_FIND_REQUIRED)
ENDIF (darwin_robot_FOUND)
ReadMe.txt 0 → 100644
+ 19
− 0
View file @ 63eb7ea1
Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
Author sergi (sergi@iri.upc.edu)
All rights reserved.
This file is part of darwin_robot library
darwin_robot library is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 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 Lesser General Public License for more details.
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/>
doc/doxygen.conf 0 → 100755
+ 251
− 0
View file @ 63eb7ea1
# 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 \
../doc/main.dox
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
doc/doxygen_project_name.conf 0 → 100644
+ 1
− 0
View file @ 63eb7ea1
PROJECT_NAME = "darwin_robot"
doc/main.dox 0 → 100644
+ 107
− 0
View file @ 63eb7ea1
/*! \mainpage darwin_robot
\section Introduction
\subsection Pre-Requisites
This package requires of the following libraries and packages
- <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
- <A href="http://www.doxygen.org">doxygen</a> and
<A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
- stdc++,
.
Under linux all of these utilities are available in ready-to-use packages.
Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
\subsection Compilation
Just download this package, uncompress it, and execute
- cd build
- cmake ..
.
to generate the makefile and then
- make
.
to obtain the shared library (in this case called <em>iriutils.so</em>) and
also all the example programs.
The <em>cmake</em> only need to be executed once (make will automatically call
<em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
To generate this documentation type
- make doc
.
The files in the <em>build</em> directory are genetated by <em>cmake</em>
and <em>make</em> and can be safely removed.
After doing so you will need to call cmake manually again.
\subsection Configuration
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
- cmake .. -DCMAKE_BUILD_TYPE=RELEASE
.
The release mode will be kept until next time cmake is executed.
\subsection Installation
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 <em>/usr/local/lib/iriutils</em> directory
and the header files will be copied to <em>/usr/local/include/iriutils</em> 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.
\section Customization
To build a new application using these library, first it is necessary to locate if the library
has been installed or not using the following command
- FIND_PACKAGE(darwin_robot REQUIRED)
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(${darwin_robot_INCLUDE_DIR})
Finally, it is also nevessary to link with the desired libraries by using the following command
- TARGET_LINK_LIBRARIES(<executable name> ${darwin_robot_LIBRARY})
.
\section License
This package is licensed under a
<a href="http://www.gnu.org/licenses/lgpl.html">
LGPL 3.0 License</a>.
\section Disclaimer
This is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 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 Lesser General Public License for more details.
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/>.
*/
src/CMakeLists.txt 0 → 100644
+ 55
− 0
View file @ 63eb7ea1
ADD_SUBDIRECTORY(xml)
IF(HAVE_XSD)
ADD_DEFINITIONS(-D_HAVE_XSD)
ENDIF(HAVE_XSD)
# locate the necessary dependencies
INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
## Find KDL
SET(KDL_INSTALL /opt/ros/hydro)
INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
# driver source files
SET(robot_sources darwin_robot.cpp darwin_robot_exceptions.cpp)
# application header files
SET(robot_headers darwin_robot.h darwin_robot_exceptions.h darwin_registers.h)
# driver source files
SET(kin_sources darwin_arm_kinematics.cpp darwin_robot_exceptions.cpp)
# application header files
SET(kin_headers darwin_arm_kinematics.h darwin_robot_exceptions.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
# add the necessary include directories
INCLUDE_DIRECTORIES(.)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
# create the shared library
ADD_LIBRARY(darwin_robot SHARED ${robot_sources} ${XSD_SOURCES})
ADD_DEPENDENCIES(darwin_robot xsd_files_gen)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_robot ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(darwin_robot ${dynamixel_LIBRARY})
TARGET_LINK_LIBRARIES(darwin_robot ${XSD_LIBRARY})
ADD_LIBRARY(darwin_arm_kinematics SHARED ${kin_sources} ${XSD_SOURCES})
ADD_DEPENDENCIES(darwin_arm_kinematics xsd_files_gen)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_arm_kinematics ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(darwin_arm_kinematics ${XSD_LIBRARY})
TARGET_LINK_LIBRARIES(darwin_arm_kinematics ${KDL_LIBS})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
INSTALL(TARGETS darwin_robot darwin_arm_kinematics
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers)
INSTALL(FILES ${robot_headers} ${kin_headers} DESTINATION include/iridrivers)
INSTALL(FILES ../Finddarwin_robot.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES ../Finddarwin_arm_kinematics.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
ADD_SUBDIRECTORY(examples)
src/darwin_arm_kinematics.cpp 0 → 100644
+ 173
− 0
View file @ 63eb7ea1
#include "darwin_arm_kinematics.h"
#include "darwin_robot_exceptions.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
#ifdef _HAVE_XSD
#include "xml/darwin_arm_kin.hxx"
#endif
CDarwinArmKinematics::CDarwinArmKinematics()
{
srand(time(NULL));
this->fksolver=NULL;
this->iksolver_vel=NULL;
this->iksolver_pos=NULL;
this->chain=NULL;
this->num_dof=0;
}
double CDarwinArmKinematics::angle_norm(double angle)
{
while(fabs(angle)>1.5707)
{
if(angle>0) angle-=1.5707;
if(angle<0) angle+=1.5707;
}
return angle;
}
void CDarwinArmKinematics::load_chain(std::string &filename)
{
joints_t::joint_iterator iterator;
struct stat buffer;
unsigned int i=0,extra;
if(stat(filename.c_str(),&buffer)==0)
{
try{
std::auto_ptr<joints_t> chain(darwin_arm_chain(filename.c_str(), xml_schema::flags::dont_validate));
extra=6-chain->joint().size();
if(extra<0) extra=0;
this->q_max.resize(6);
this->q_min.resize(6);
if(this->chain!=NULL)
{
delete this->chain;
this->num_dof=0;
}
this->chain=new KDL::Chain;
for(iterator=chain->joint().begin(),i=0;iterator!=chain->joint().end();iterator++,i++)
{
this->q_max(i)=iterator->max_angle();
this->q_min(i)=iterator->min_angle();
this->chain->addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH(iterator->params().a(),
iterator->params().alpha(),iterator->params().d(),iterator->params().theta())));
this->num_dof++;
}
/* add extra joints to get to 6 dof */
for(;i<6;i++)
{
this->q_max(i)=1.5707;
this->q_min(i)=-1.5707;
if(i==4) this->chain->addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotY)));
else this->chain->addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ)));
}
/* create the solvers */
if(this->fksolver!=NULL)
delete this->fksolver;
this->fksolver=new KDL::ChainFkSolverPos_recursive(*this->chain);
if(this->iksolver_vel!=NULL)
delete this->iksolver_vel;
this->iksolver_vel=new KDL::ChainIkSolverVel_pinv(*this->chain);
if(this->iksolver_pos!=NULL)
delete this->iksolver_pos;
this->iksolver_pos=new KDL::ChainIkSolverPos_NR_JL(*this->chain,this->q_min,this->q_max,*this->fksolver,*this->iksolver_vel,100,1e-3);
}catch(const xml_schema::exception& e){
std::ostringstream os;
os << e;
throw CDarwinRobotException(_HERE_,os.str());
}
}
else
throw CDarwinRobotException(_HERE_,"Arm chain definition file does not exist");
}
void CDarwinArmKinematics::get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot)
{
double yaw=0.0,pitch=0.0,roll=0.0;
KDL::JntArray joint_positions;
int kinematics_status;
KDL::Frame cartpos;
unsigned int i=0;
if(angles.size()!=this->num_dof)
throw CDarwinRobotException(_HERE_,"Invalid angle vector size");
else
{
joint_positions.resize(this->chain->getNrOfJoints());
for(i=0;i<this->num_dof;i++)
joint_positions(i)=angles[i];
for(;i<6;i++)
joint_positions(i)=0;
kinematics_status=this->fksolver->JntToCart(joint_positions,cartpos);
if(kinematics_status<0)
throw CDarwinRobotException(_HERE_,"Impossible to find solution");
else
{
pos.resize(3);
pos[0]=cartpos.p(0);
pos[1]=cartpos.p(1);
pos[2]=cartpos.p(2);
cartpos.M.GetRPY(roll,pitch,yaw);
rot.resize(3);
rot[0]=roll;
rot[1]=pitch;
rot[2]=yaw;
}
}
}
void CDarwinArmKinematics::get_inverse_kinematics(std::vector<double> &pos,std::vector<double> &rot,std::vector<double> &angles)
{
int count=0,kinematics_status;
KDL::JntArray q_init,joints;
KDL::Frame cartpos_sol;
KDL::Vector error;
unsigned int i=0;
if(pos.size()!=3 || rot.size()!=3)
throw CDarwinRobotException(_HERE_,"Invalid position and/or rotation vector sizes");
KDL::Frame desired_frame(KDL::Rotation::RPY(rot[0],rot[1],rot[2]),KDL::Vector(pos[0],pos[1],pos[2]));
q_init.resize(this->chain->getNrOfJoints());
joints.resize(this->chain->getNrOfJoints());
do{
count++;
for(i=0;i<this->num_dof;i++)
q_init(i)=(rand()*3.14159/RAND_MAX)-1.5707;
for(;i<6;i++)
q_init(i)=0;
kinematics_status=this->iksolver_pos->CartToJnt(q_init,desired_frame,joints);
kinematics_status=this->fksolver->JntToCart(joints,cartpos_sol);
if(kinematics_status<0)
throw CDarwinRobotException(_HERE_,"Impossible to find solution");
else
error=desired_frame.p-cartpos_sol.p;
}while((fabs(error(0))>0.001 || fabs(error(1))>0.001 || fabs(error(2))>0.001) && count<50);
if(count<50)
{
angles.resize(this->num_dof);
for(i=0;i<this->num_dof;i++)
angles[i]=joints(i);
}
else
throw CDarwinRobotException(_HERE_,"Impossible to find solution");
}
CDarwinArmKinematics::~CDarwinArmKinematics()
{
if(this->fksolver!=NULL)
delete this->fksolver;
if(this->iksolver_vel!=NULL)
delete this->iksolver_vel;
if(this->iksolver_pos!=NULL)
delete this->iksolver_pos;
if(this->chain!=NULL)
delete this->chain;
}
src/darwin_arm_kinematics.h 0 → 100644
+ 31
− 0
View file @ 63eb7ea1
#ifndef _DARWIN_ARM_KINEMATICS_H
#define _DARWIN_ARM_KINEMATICS_H
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/frames_io.hpp>
class CDarwinArmKinematics
{
private:
KDL::Chain *chain;
KDL::JntArray q_max;
KDL::JntArray q_min;
KDL::ChainFkSolverPos_recursive *fksolver;
KDL::ChainIkSolverVel_pinv *iksolver_vel;
KDL::ChainIkSolverPos_NR_JL *iksolver_pos;
unsigned int num_dof;
protected:
double angle_norm(double angle);
public:
CDarwinArmKinematics();
void load_chain(std::string &filename);
void get_forward_kinematics(std::vector<double> &angles,std::vector<double> &pos,std::vector<double> &rot);
void get_inverse_kinematics(std::vector<double> &pos,std::vector<double> &rot,std::vector<double> &angles);
~CDarwinArmKinematics();
};
#endif
src/darwin_registers.h 0 → 100644
+ 250
− 0
View file @ 63eb7ea1
#ifndef _DARWIN_REGISTERS_
#define _DARWIN_REGISTERS_
#define MAX_NUM_SERVOS 27
typedef enum {
DARWIN_MODEL_NUMBER_L = 0x00,
DARWIN_MODEL_NUMBER_H = 0x01,
DARWIN_VERSION = 0x02,
DARWIN_ID = 0x03,
DARWIN_BAUD_RATE = 0x04,
DARWIN_RETURN_DELAY_TIME = 0x05,
DARWIN_MM_PERIOD_LOW = 0x06,
DARWIN_MM_PERIOD_HIGH = 0x07,
DARWIN_RETURN_LEVEL = 0x10,
DARWIN_DXL_POWER = 0x18,
DARWIN_LED_PANNEL = 0x19,
DARWIN_LED_5_L = 0x1A,
DARWIN_LED_5_H = 0x1B,
DARWIN_LED_6_L = 0x1C,
DARWIN_LED_6_H = 0x1D,
DARWIN_PUSHBUTTON = 0x1E,
DARWIN_MM_NUM_SERVOS = 0x1F,
DARWIN_MM_CONTROL = 0x20,
DARWIN_MM_STATUS = 0x21,
DARWIN_WALK_CONTROL = 0x22,
DARWIN_WALK_STATUS = 0x23,
DARWIN_IMU_CONTROL = 0x24,
DARWIN_IMU_STATUS = 0x25,
DARWIN_IMU_ACCEL_X_L = 0x26,
DARWIN_IMU_ACCEL_X_H = 0x27,
DARWIN_IMU_ACCEL_Y_L = 0x28,
DARWIN_IMU_ACCEL_Y_H = 0x29,
DARWIN_IMU_ACCEL_Z_L = 0x2A,
DARWIN_IMU_ACCEL_Z_H = 0x2B,
DARWIN_IMU_GYRO_X_L = 0x2C,
DARWIN_IMU_GYRO_X_H = 0x2D,
DARWIN_IMU_GYRO_Y_L = 0x2E,
DARWIN_IMU_GYRO_Y_H = 0x2F,
DARWIN_IMU_GYRO_Z_L = 0x30,
DARWIN_IMU_GYRO_Z_H = 0x31,
DARWIN_VOLTAGE = 0x32,
DARWIN_MIC1_L = 0x33,
DARWIN_MIC1_H = 0x34,
DARWIN_ADC2_L = 0x35,
DARWIN_ADC2_H = 0x36,
DARWIN_ADC3_L = 0x37,
DARWIN_ADC3_H = 0x38,
DARWIN_ADC4_L = 0x39,
DARWIN_ADC4_H = 0x3A,
DARWIN_ADC5_L = 0x3B,
DARWIN_ADC5_H = 0x3C,
DARWIN_ADC6_L = 0x3D,
DARWIN_ADC6_H = 0x3E,
DARWIN_ADC7_L = 0x3F,
DARWIN_ADC7_H = 0x40,
DARWIN_ADC8_L = 0x41,
DARWIN_ADC8_H = 0x42,
DARWIN_MIC2_L = 0x43,
DARWIN_MIC2_H = 0x44,
DARWIN_ADC10_L = 0x45,
DARWIN_ADC10_H = 0x46,
DARWIN_ADC11_L = 0x47,
DARWIN_ADC11_H = 0x48,
DARWIN_ADC12_L = 0x49,
DARWIN_ADC12_H = 0x4A,
DARWIN_ADC13_L = 0x4B,
DARWIN_ADC13_H = 0x4C,
DARWIN_ADC14_L = 0x4D,
DARWIN_ADC14_H = 0x4E,
DARWIN_ADC15_L = 0x4F,
DARWIN_ADC15_H = 0x50,
DARWIN_MM_MODULE_EN0 = 0x51,
DARWIN_MM_MODULE_EN1 = 0x52,
DARWIN_MM_MODULE_EN2 = 0x53,
DARWIN_MM_MODULE_EN3 = 0x54,
DARWIN_MM_MODULE_EN4 = 0x55,
DARWIN_MM_MODULE_EN5 = 0x56,
DARWIN_MM_MODULE_EN6 = 0x57,
DARWIN_MM_MODULE_EN7 = 0x58,
DARWIN_MM_MODULE_EN8 = 0x59,
DARWIN_MM_MODULE_EN9 = 0x5A,
DARWIN_MM_MODULE_EN10 = 0x5B,
DARWIN_MM_MODULE_EN11 = 0x5C,
DARWIN_MM_MODULE_EN12 = 0x5D,
DARWIN_MM_MODULE_EN13 = 0x5E,
DARWIN_MM_MODULE_EN14 = 0x5F,
DARWIN_MM_MODULE_EN15 = 0x60,
DARWIN_MM_SERVO0_CUR_POS_L = 0x61,
DARWIN_MM_SERVO0_CUR_POS_H = 0x62,
DARWIN_MM_SERVO1_CUR_POS_L = 0x63,
DARWIN_MM_SERVO1_CUR_POS_H = 0x64,
DARWIN_MM_SERVO2_CUR_POS_L = 0x65,
DARWIN_MM_SERVO2_CUR_POS_H = 0x66,
DARWIN_MM_SERVO3_CUR_POS_L = 0x67,
DARWIN_MM_SERVO3_CUR_POS_H = 0x68,
DARWIN_MM_SERVO4_CUR_POS_L = 0x69,
DARWIN_MM_SERVO4_CUR_POS_H = 0x6A,
DARWIN_MM_SERVO5_CUR_POS_L = 0x6B,
DARWIN_MM_SERVO5_CUR_POS_H = 0x6C,
DARWIN_MM_SERVO6_CUR_POS_L = 0x6D,
DARWIN_MM_SERVO6_CUR_POS_H = 0x6E,
DARWIN_MM_SERVO7_CUR_POS_L = 0x6F,
DARWIN_MM_SERVO7_CUR_POS_H = 0x70,
DARWIN_MM_SERVO8_CUR_POS_L = 0x71,
DARWIN_MM_SERVO8_CUR_POS_H = 0x72,
DARWIN_MM_SERVO9_CUR_POS_L = 0x73,
DARWIN_MM_SERVO9_CUR_POS_H = 0x74,
DARWIN_MM_SERVO10_CUR_POS_L = 0x75,
DARWIN_MM_SERVO10_CUR_POS_H = 0x76,
DARWIN_MM_SERVO11_CUR_POS_L = 0x77,
DARWIN_MM_SERVO11_CUR_POS_H = 0x78,
DARWIN_MM_SERVO12_CUR_POS_L = 0x79,
DARWIN_MM_SERVO12_CUR_POS_H = 0x7A,
DARWIN_MM_SERVO13_CUR_POS_L = 0x7B,
DARWIN_MM_SERVO13_CUR_POS_H = 0x7C,
DARWIN_MM_SERVO14_CUR_POS_L = 0x7D,
DARWIN_MM_SERVO14_CUR_POS_H = 0x7E,
DARWIN_MM_SERVO15_CUR_POS_L = 0x7F,
DARWIN_MM_SERVO15_CUR_POS_H = 0x80,
DARWIN_MM_SERVO16_CUR_POS_L = 0x81,
DARWIN_MM_SERVO16_CUR_POS_H = 0x82,
DARWIN_MM_SERVO17_CUR_POS_L = 0x83,
DARWIN_MM_SERVO17_CUR_POS_H = 0x84,
DARWIN_MM_SERVO18_CUR_POS_L = 0x85,
DARWIN_MM_SERVO18_CUR_POS_H = 0x86,
DARWIN_MM_SERVO19_CUR_POS_L = 0x87,
DARWIN_MM_SERVO19_CUR_POS_H = 0x88,
DARWIN_MM_SERVO20_CUR_POS_L = 0x89,
DARWIN_MM_SERVO20_CUR_POS_H = 0x8A,
DARWIN_MM_SERVO21_CUR_POS_L = 0x8B,
DARWIN_MM_SERVO21_CUR_POS_H = 0x8C,
DARWIN_MM_SERVO22_CUR_POS_L = 0x8D,
DARWIN_MM_SERVO22_CUR_POS_H = 0x8E,
DARWIN_MM_SERVO23_CUR_POS_L = 0x8F,
DARWIN_MM_SERVO23_CUR_POS_H = 0x90,
DARWIN_MM_SERVO24_CUR_POS_L = 0x91,
DARWIN_MM_SERVO24_CUR_POS_H = 0x92,
DARWIN_MM_SERVO25_CUR_POS_L = 0x93,
DARWIN_MM_SERVO25_CUR_POS_H = 0x94,
DARWIN_MM_SERVO26_CUR_POS_L = 0x95,
DARWIN_MM_SERVO26_CUR_POS_H = 0x96,
DARWIN_ACTION_PAGE = 0x97,
DARWIN_ACTION_CONTROL = 0x98,
DARWIN_ACTION_STATUS = 0x99,
DARWIN_X_OFFSET = 0x9A,
DARWIN_Y_OFFSET = 0x9B,
DARWIN_Z_OFFSET = 0x9C,
DARWIN_ROLL = 0x9D,
DARWIN_PITCH = 0x9E,
DARWIN_YAW = 0x9F,
DARWIN_HIP_PITCH_OFF_L = 0xA0,
DARWIN_HIP_PITCH_OFF_H = 0xA1,
DARWIN_PERIOD_TIME_L = 0xA2,
DARWIN_PERIOD_TIME_H = 0xA3,
DARWIN_DSP_RATIO = 0xA4,
DARWIN_STEP_FW_BW_RATIO = 0xA5,
DARWIN_STEP_FW_BW = 0xA6,
DARWIN_STEP_LEFT_RIGHT = 0xA7,
DARWIN_STEP_DIRECTION = 0xA8,
DARWIN_FOOT_HEIGHT = 0xA9,
DARWIN_SWING_RIGHT_LEFT = 0xAA,
DARWIN_SWING_TOP_DOWN = 0xAB,
DARWIN_PELVIS_OFFSET = 0xAC,
DARWIN_ARM_SWING_GAIN = 0xAD,
DARWIN_P_GAIN = 0xAE,
DARWIN_D_GAIN = 0xAF,
DARWIN_I_GAIN = 0xB0,
DARWIN_MAX_ACC = 0xB1,
DARWIN_MAX_ROT_ACC = 0xB2,
DARWIN_MM_KNEE_GAIN = 0xB3,
DARWIN_MM_ANKLE_PITCH_GAIN = 0xB4,
DARWIN_MM_HIP_ROLL_GAIN = 0xB5,
DARWIN_MM_ANKLE_ROLL_GAIN = 0xB6,
DARWIN_SERVO_0_SPEED = 0xB7,
DARWIN_SERVO_1_SPEED = 0xB8,
DARWIN_SERVO_2_SPEED = 0xB9,
DARWIN_SERVO_3_SPEED = 0xBA,
DARWIN_SERVO_4_SPEED = 0xBB,
DARWIN_SERVO_5_SPEED = 0xBC,
DARWIN_SERVO_6_SPEED = 0xBD,
DARWIN_SERVO_7_SPEED = 0xBE,
DARWIN_SERVO_8_SPEED = 0xBF,
DARWIN_SERVO_9_SPEED = 0xC0,
DARWIN_SERVO_10_SPEED = 0xC1,
DARWIN_SERVO_11_SPEED = 0xC2,
DARWIN_SERVO_12_SPEED = 0xC3,
DARWIN_SERVO_13_SPEED = 0xC4,
DARWIN_SERVO_14_SPEED = 0xC5,
DARWIN_SERVO_15_SPEED = 0xC6,
DARWIN_SERVO_16_SPEED = 0xC7,
DARWIN_SERVO_17_SPEED = 0xC8,
DARWIN_SERVO_18_SPEED = 0xC9,
DARWIN_SERVO_19_SPEED = 0xCA,
DARWIN_SERVO_20_SPEED = 0xCB,
DARWIN_SERVO_21_SPEED = 0xCC,
DARWIN_SERVO_22_SPEED = 0xCD,
DARWIN_SERVO_23_SPEED = 0xCE,
DARWIN_SERVO_24_SPEED = 0xCF,
DARWIN_SERVO_25_SPEED = 0xD0,
DARWIN_SERVO_26_SPEED = 0xD1,
DARWIN_SERVO_0_ACCEL = 0xD2,
DARWIN_SERVO_1_ACCEL = 0xD3,
DARWIN_SERVO_2_ACCEL = 0xD4,
DARWIN_SERVO_3_ACCEL = 0xD5,
DARWIN_SERVO_4_ACCEL = 0xD6,
DARWIN_SERVO_5_ACCEL = 0xD7,
DARWIN_SERVO_6_ACCEL = 0xD8,
DARWIN_SERVO_7_ACCEL = 0xD9,
DARWIN_SERVO_8_ACCEL = 0xDA,
DARWIN_SERVO_9_ACCEL = 0xDB,
DARWIN_SERVO_10_ACCEL = 0xDC,
DARWIN_SERVO_11_ACCEL = 0xDD,
DARWIN_SERVO_12_ACCEL = 0xDE,
DARWIN_SERVO_13_ACCEL = 0xDF,
DARWIN_SERVO_14_ACCEL = 0xE0,
DARWIN_SERVO_15_ACCEL = 0xE1,
DARWIN_SERVO_16_ACCEL = 0xE2,
DARWIN_SERVO_17_ACCEL = 0xE3,
DARWIN_SERVO_18_ACCEL = 0xE4,
DARWIN_SERVO_19_ACCEL = 0xE5,
DARWIN_SERVO_20_ACCEL = 0xE6,
DARWIN_SERVO_21_ACCEL = 0xE7,
DARWIN_SERVO_22_ACCEL = 0xE8,
DARWIN_SERVO_23_ACCEL = 0xE9,
DARWIN_SERVO_24_ACCEL = 0xEA,
DARWIN_SERVO_25_ACCEL = 0xEB,
DARWIN_SERVO_26_ACCEL = 0xEC,
DARWIN_JOINTS_CONTROL = 0xED,
DARWIN_JOINTS_STATUS = 0xEE,
DARWIN_HEAD_PAN_L = 0xEF,
DARWIN_HEAD_PAN_H = 0xF0,
DARWIN_HEAD_TILT_L = 0xF1,
DARWIN_HEAD_TILT_H = 0xF2,
DARWIN_HEAD_PAN_P = 0xF3,
DARWIN_HEAD_PAN_I = 0xF4,
DARWIN_HEAD_PAN_D = 0xF5,
DARWIN_HEAD_TILT_P = 0xF6,
DARWIN_HEAD_TILT_I = 0xF7,
DARWIN_HEAD_TILT_D = 0xF8,
DARWIN_HEAD_CONTROL = 0xF9,
DARWIN_HEAD_STATUS = 0xFA,
DARWIN_MM_CUR_PERIOD = 0xFB,
DARWIN_GRIPPER_CONTROL = 0xFC,
DARWIN_GRIPPER_STATUS = 0xFD,
DARWIN_GRIPPER_FORCE_LEFT = 0xFE,
DARWIN_GRIPPER_FORCE_RIGHT = 0xFF
} darwin_registers;
#endif
src/darwin_robot.cpp 0 → 100644
+ 0
− 0
View file @ 63eb7ea1
This diff is collapsed.
src/darwin_robot.h 0 → 100644
+ 106
− 0
View file @ 63eb7ea1
#ifndef _DARWIN_ROBOT_H
#define _DARWIN_ROBOT_H
#include "dynamixel.h"
#include "darwin_registers.h"
extern const std::string servo_names[MAX_NUM_SERVOS];
typedef enum {DARWIN_MM_NONE=0x00,DARWIN_MM_ACTION=0x01,DARWIN_MM_WALKING=0x02,DARWIN_MM_JOINTS=0x03,DARWIN_MM_HEAD=0x04,DARWIN_MM_GRIPPER=0x05} mm_mode_t;
typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t;
class CDarwin_Robot
{
private:
CDynamixelServer *dyn_server;
CDynamixel *robot_device;
std::string robot_name;
unsigned char robot_id;
// GPIO variables
unsigned char current_led_value;
unsigned char current_pushbutton_value;
// motion manager variables
unsigned char num_servos;
public:
CDarwin_Robot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
void set_TxD_LED(void);
void clear_TxD_LED(void);
void set_RxD_LED(void);
void clear_RxD_LED(void);
void set_LED2(void);
void clear_LED2(void);
void set_LED3(void);
void clear_LED3(void);
void set_LED4(void);
void clear_LED4(void);
bool is_mode_button_pressed(void);
bool is_start_button_pressed(void);
// motion manager interface
void mm_set_period(double period_ms);
double mm_get_period(void);
double mm_get_current_period(void);
void mm_get_num_servos(int *v1_servos,int *v2_servos);
void mm_start(void);
void mm_stop(void);
void mm_enable_power(void);
void mm_disable_power(void);
bool mm_is_power_enabled(void);
void mm_enable_servo(unsigned char servo_id);
void mm_disable_servo(unsigned char servo_id);
bool mm_is_servo_enabled(unsigned char servo_id);
void mm_enable_balance(void);
void mm_disable_balance(void);
bool mm_is_balance_enabled(void);
void mm_assign_module(unsigned char servo_id, mm_mode_t mode);
void mm_assign_module(std::string &servo,std::string &module);
mm_mode_t mm_get_module(unsigned char servo_id);
void mm_load_config(std::string &filename);
std::vector<double> mm_get_servo_angles(void);
double mm_get_servo_angle(unsigned char servo_id);
void mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll);
void mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll);
// motion action interface
void action_load_page(unsigned char page_id);
unsigned char action_get_current_page(void);
void action_start(void);
void action_stop(void);
bool action_is_page_running(void);
// imu interface
void imu_start(void);
void imu_stop(void);
void imu_start_gyro_cal(void);
bool imu_is_gyro_cal_done(void);
bool imu_is_accel_detected(void);
bool imu_is_gyro_detected(void);
void imu_get_accel_data(short int *x,short int *y,short int *z);
void imu_get_gyro_data(short int *x,short int *y,short int *z);
// adc interface
void adc_get_data(void);
// walking interface
void walking_set_speeds(double fw_step,double side_step,double turn);
void walking_start(void);
void walking_stop(void);
bool is_walking(void);
void walking_set_param(int param_id,double value);
double walking_get_param(int param_id);
// joint motion interface
void joints_load(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
void joints_start(void);
void joints_stop(void);
bool are_joints_moving(void);
// head tracking interface
void head_tracking_set_target(double pan,double tilt);
void head_tracking_set_pan_pid(double p,double i,double d);
void head_tracking_set_tilt_pid(double p,double i,double d);
void head_tracking_start(void);
void head_tracking_stop(void);
bool is_head_tracking(void);
// gripper interface
void gripper_open(grippers_t gripper);
void gripper_close(grippers_t gripper);
~CDarwin_Robot();
};
#endif
src/darwin_robot_exceptions.cpp 0 → 100644
+ 12
− 0
View file @ 63eb7ea1
#include "darwin_robot_exceptions.h"
#include <sstream>
#include <string.h>
#include <stdio.h>
const std::string darwin_robot_error_message="CDarwinRobot error - ";
CDarwinRobotException::CDarwinRobotException(const std::string& where,const std::string &error_msg):CException(where,darwin_robot_error_message)
{
this->error_msg+=error_msg;
}
src/darwin_robot_exceptions.h 0 → 100644
+ 20
− 0
View file @ 63eb7ea1
#ifndef _DARWIN_ROBOT_EXCEPTIONS
#define _DARWIN_ROBOT_EXCEPTIONS
#include "exceptions.h"
/**
* \brief
*
*/
class CDarwinRobotException : public CException
{
public:
/**
* \brief
*
*/
CDarwinRobotException(const std::string& where,const std::string &error_msg);
};
#endif
src/examples/CMakeLists.txt 0 → 100644
+ 39
− 0
View file @ 63eb7ea1
# create an example application
ADD_EXECUTABLE(darwin_robot_test darwin_robot_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_robot_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
# create an example application
ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_kin darwin_arm_kinematics)
# create an example application
ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)
src/examples/darwin_action_test.cpp 0 → 100644
+ 35
− 0
View file @ 63eb7ea1
#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"
#include <iostream>
std::string robot_device="A603LOBS";
int main(int argc, char *argv[])
{
int i=0,num_v1_servos,num_v2_servos;
try{
CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
// enable all servos and assign them to the action module
darwin.mm_enable_power();
for(i=1;i<=num_v1_servos;i++)
{
darwin.mm_enable_servo(i);
darwin.mm_assign_module(i,DARWIN_MM_ACTION);
}
darwin.mm_start();
// execute an action
darwin.action_load_page(9);
darwin.action_start();
while(darwin.action_is_page_running())
usleep(100000);
darwin.mm_stop();
darwin.mm_disable_power();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
src/examples/darwin_gripper_test.cpp 0 → 100644
+ 36
− 0
View file @ 63eb7ea1
#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"
#include <iostream>
std::string robot_device="A603LOBS";
int main(int argc, char *argv[])
{
int i=0,num_v1_servos,num_v2_servos;
std::vector<double> angles;
try{
CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
darwin.mm_enable_power();
darwin.mm_enable_servo(24);
darwin.mm_assign_module(24,DARWIN_MM_GRIPPER);
darwin.mm_enable_servo(21);
darwin.mm_assign_module(21,DARWIN_MM_GRIPPER);
darwin.mm_start();
darwin.gripper_open(LEFT_GRIPPER);
sleep(2);
darwin.gripper_close(LEFT_GRIPPER);
sleep(2);
darwin.gripper_open(RIGHT_GRIPPER);
sleep(2);
darwin.gripper_close(RIGHT_GRIPPER);
sleep(2);
darwin.mm_stop();
darwin.mm_disable_power();
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment