...
 
Commits (11)
# Copyright 2016, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
# Copyright 2016, 2020, Thomas Moulard, Olivier Stasse, Guilhem Saurel, JRL, CNRS/AIST, LAAS-CNRS
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
SET(PROJECT_NAMESPACE stack-of-tasks)
# Project properties
SET(PROJECT_ORG stack-of-tasks)
SET(PROJECT_NAME sot-tiago)
SET(PROJECT_DESCRIPTION "dynamic-graph package for Tiago robot")
SET(PROJECT_URL "http://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
# Project options
OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
# Project configuration
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(CXX_DISABLE_WERROR TRUE)
# JRL-cmakemodule setup
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
......@@ -14,59 +24,33 @@ INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/ros.cmake)
INCLUDE(cmake/python.cmake)
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(CXX_DISABLE_WERROR True)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
plugindirname
plugindir
)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
# Search for dependencies.
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
ADD_REQUIRED_DEPENDENCY("pinocchio")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-dynamic-pinocchio >= 3.1")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge_msgs")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge >= 3.0")
ADD_OPTIONAL_DEPENDENCY("tiago_data")
# Search for dependencies.
# Boost
# Project dependencies
SET(BOOST_COMPONENTS filesystem system thread)
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
ADD_PROJECT_DEPENDENCY(sot-dynamic-pinocchio REQUIRED PKG_CONFIG_REQUIRES sot-dynamic-pinocchio)
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED PKG_CONFIG_REQUIRES dynamic-graph-python)
ADD_PROJECT_DEPENDENCY(dynamic_graph_bridge REQUIRED PKG_CONFIG_REQUIRES dynamic_graph_bridge)
ADD_PROJECT_DEPENDENCY(tiago_data)
ADD_REQUIRED_DEPENDENCY(dynamic_graph_bridge_msgs)
FINDPYTHON()
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_PATH})
SEARCH_FOR_BOOST()
# Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion
SET(CMAKE_INSTALL_LIBDIR lib)
SET(CMAKE_SKIP_BUILD_RPATH FALSE)
SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
# Main Library
ADD_LIBRARY(${PROJECT_NAME} INTERFACE)
INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
# Add subdirectories.
ADD_SUBDIRECTORY(src)
IF(TIAGO_DATA_FOUND)
IF(tiago_data_FOUND)
INSTALL(FILES
${PROJECT_SOURCE_DIR}/tests/test.py
${PROJECT_SOURCE_DIR}/tests/appli.py
${PROJECT_SOURCE_DIR}/tests/stop-controllers.py
DESTINATION
${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/tests
)
ENDIF(TIAGO_DATA_FOUND)
tests/test.py
tests/appli.py
tests/stop-controllers.py
DESTINATION share/${PROJECT_NAME}/tests)
ENDIF(tiago_data_FOUND)
Subproject commit 52fd089803657a9deab13c9af66581a2da5724f0
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
# Copyright 2016, R. Budhiraja, Olivier Stasse, CNRS
FUNCTION(COMPILE_PLUGIN NAME)
ADD_LIBRARY(${NAME} SHARED ${SOURCES})
SET_TARGET_PROPERTIES(${lib} PROPERTIES
PREFIX ""
SOVERSION ${PROJECT_VERSION})
PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${NAME} pinocchio)
INSTALL(TARGETS ${NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${NAME})
SET(NEW_ENTITY_CLASS ${ENTITIES})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/tiago/${PYTHON_LIBRARY_NAME}"
${NAME}
sot-tiago-${PYTHON_LIBRARY_NAME}-wrap
)
ENDFUNCTION()
SET(ENTITIES DiffDriveController HolonomicProjection)
SET(NAME diff-drive-controller)
SET(SOURCES
diff_drive_controller.cpp
speed_limiter.cpp
odometry.cpp
holonomic-projection.cc)
COMPILE_PLUGIN(diff-drive-controller)
TARGET_INCLUDE_DIRECTORIES(diff-drive-controller
PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
# Install Python files.
SET(PYTHON_MODULE_DIR
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/tiago)
SET(PYTHON_MODULE_BUILD_DIR
${CMAKE_CURRENT_BINARY_DIR}/dynamic_graph/sot/tiago)
ADD_LIBRARY(${NAME} SHARED ${SOURCES})
IF(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
ENDIF(SUFFIX_SO_VERSION)
SET(PYTHON_MODULE dynamic_graph/sot/tiago)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "tiago.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" )
SET(FILES __init__.py robot.py)
TARGET_INCLUDE_DIRECTORIES(${NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
TARGET_LINK_LIBRARIES(${NAME} sot-core::sot-core)
# Install dynamic_graph.sot.tiago.steel
SET(PYTHON_MODULE dynamic_graph/sot/tiago/steel)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
INSTALL(TARGETS ${NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${NAME})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/tiago/${PYTHON_LIBRARY_NAME}"
${NAME} sot-tiago-${PYTHON_LIBRARY_NAME}-wrap)
# Add the library to wrap the device of Tiago.
MACRO(build_tiago_device)
# Install Python files.
SET(PYTHON_MODULE dynamic_graph/sot/tiago)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "tiago.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py")
SET(DEVICE_NAME sot-tiago-device)
ADD_LIBRARY(${DEVICE_NAME}
SHARED
sot-tiago-device.cpp
)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}/steel" "__init__.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}/steel" "robot.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}/steel" "prologue.py")
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "dynamic-graph")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "dynamic-graph-python")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "sot-core")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "pinocchio")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "dynamic_graph_bridge")
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${DEVICE_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
# Add the library to wrap the device of Tiago.
SET(DEVICE_NAME sot-tiago-device)
ADD_LIBRARY(${DEVICE_NAME} SHARED sot-tiago-device.cpp)
INSTALL(TARGETS ${DEVICE_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
# Link the dynamic library containing the SoT with its dependencies.
TARGET_LINK_LIBRARIES(${DEVICE_NAME} sot-core::sot-core)
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${DEVICE_NAME})
SET(NEW_ENTITY_CLASS ${ENTITIES})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/tiago/${PYTHON_LIBRARY_NAME}"
${DEVICE_NAME}
sot-tiago-${PYTHON_LIBRARY_NAME}-wrap
)
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${DEVICE_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
ENDMACRO()
INSTALL(TARGETS ${DEVICE_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
build_tiago_device()
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${DEVICE_NAME})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/tiago/${PYTHON_LIBRARY_NAME}"
${DEVICE_NAME} sot-tiago-${PYTHON_LIBRARY_NAME}-wrap)
# Add the library to wrap the controller of Tiago.
MACRO(build_tiago_controller CONTROLLER_NAME WITH_WHEELS)
ADD_LIBRARY(${CONTROLLER_NAME}
SHARED
sot-tiago-controller.cpp
sot-tiago-steel-controller.cpp
# Add the library to wrap the controller of Tiago.
SET(CONTROLLER_NAME sot-tiago-steel-controller)
ADD_LIBRARY(${CONTROLLER_NAME} SHARED
sot-tiago-controller.cpp
sot-tiago-steel-controller.cpp
)
IF(WITH_WHEELS)
TARGET_COMPILE_DEFINITIONS(${CONTROLLER_NAME}
PRIVATE TIAGO_STEEL_WITH_WHEELS=1)
ELSE(WITH_WHEELS)
TARGET_COMPILE_DEFINITIONS(${CONTROLLER_NAME}
PRIVATE TIAGO_STEEL_WITH_WHEELS=0)
ENDIF(WITH_WHEELS)
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph")
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph-python")
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "sot-core")
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic_graph_bridge")
ADD_DEPENDENCIES(${CONTROLLER_NAME} "sot-tiago-device")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "sot-tiago-device")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "ros_bridge")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "ros_interpreter")
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
# Link the dynamic library containing the SoT with its dependencies.
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} sot-core::sot-core dynamic-graph-python::dynamic-graph-python
sot-tiago-device "ros_bridge" "ros_interpreter")
ENDMACRO()
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
build_tiago_controller(sot-tiago-steel-controller TRUE)
build_tiago_controller(sot-tiago-steel-without-wheels-controller FALSE)
INSTALL(TARGETS ${CONTROLLER_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
......@@ -344,12 +344,12 @@ class Robot(AbstractRobot):
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
# quaternions...
self.device.setPositionBounds(pinocchio_model.lowerPositionLimit.T.tolist()[0][1:],
pinocchio_model.upperPositionLimit.T.tolist()[0][1:])
self.device.setVelocityBounds((-pinocchio_model.velocityLimit).T.tolist()[0],
pinocchio_model.velocityLimit.T.tolist()[0])
self.device.setTorqueBounds((-pinocchio_model.effortLimit).T.tolist()[0],
pinocchio_model.effortLimit.T.tolist()[0])
self.device.setPositionBounds(pinocchio_model.lowerPositionLimit.tolist()[1:],
pinocchio_model.upperPositionLimit.tolist()[1:])
self.device.setVelocityBounds((-pinocchio_model.velocityLimit).tolist(),
pinocchio_model.velocityLimit.tolist())
self.device.setTorqueBounds((-pinocchio_model.effortLimit).tolist(),
pinocchio_model.effortLimit.tolist())
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
......
# -*- coding: utf-8 -*-
# Copyright 2018, Joseph Mirabel,
from dynamic_graph.entity import PyEntityFactoryClass
from dynamic_graph.sot.tiago.steel.robot import TiagoSteel as Robot
......
......@@ -42,6 +42,8 @@
#ifndef ODOMETRY_H_
#define ODOMETRY_H_
#include "pinocchio/fwd.hpp"
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
......
......@@ -130,6 +130,16 @@ SoTTiagoDevice::SoTTiagoDevice(std::string RobotName):
sotDEBUGOUT(25);
}
void SoTTiagoDevice::setLeftWheelIndex(int idx)
{
leftWheelIdx_ = idx;
}
void SoTTiagoDevice::setRightWheelIndex(int idx)
{
rightWheelIdx_ = idx;
}
SoTTiagoDevice::~SoTTiagoDevice()
{ }
......
......@@ -60,6 +60,12 @@ dgsot::Device
void setRightWheelIndex (int idx);
/// \todo this should go into the parent class, in sot-core package
void setTimeStep (double dt)
{
timestep_ = dt;
}
protected:
void setClosedLoop (const bool& closedLoop)
......
......@@ -10,10 +10,6 @@
*
*/
#ifndef TIAGO_STEEL_WITH_WHEELS
# error "You must define TIAGO_STEEL_WITH_WHEELS to 0 or 1"
#endif
#include <pinocchio/fwd.hpp>
#include <sot/core/debug.hh>
......@@ -24,15 +20,26 @@
const std::string SoTTiagoSteelController::LOG_PYTHON_TIAGOSTEEL="/tmp/TiagoSteelController_python.out";
SoTTiagoSteelController::SoTTiagoSteelController(bool withWheels):
SoTTiagoSteelController::SoTTiagoSteelController():
SoTTiagoController(ROBOTNAME),
withWheels_ (withWheels)
withWheels_ (false)
{
ros::NodeHandle nh;
nh.getParam("/sot_controller/use_mobile_base", withWheels_);
ROS_INFO_STREAM("Loading SoT Tiago steel controller with"
<< (withWheels_ ? "" : "out") << " wheel");
if (withWheels_) {
// Control wheels in velocity.
// 6 and 7 correspond to left and right wheel joints.
device_->setLeftWheelIndex (6);
device_->setLeftWheelIndex (7);
device_->setRightWheelIndex (7);
}
/// Read /sot_controller/dt to know what is the control period
if (nh.hasParam("/sot_controller/dt")) {
double dt;
nh.getParam("/sot_controller/dt", dt);
device_->setTimeStep(dt);
ROS_INFO_STREAM("Set Tiago control period to: " << dt);
}
startupPython();
interpreter_->startRosService ();
......@@ -60,11 +67,7 @@ extern "C"
{
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
#if TIAGO_STEEL_WITH_WHEELS
return new SoTTiagoSteelController (true );
#else
return new SoTTiagoSteelController (false);
#endif
return new SoTTiagoSteelController ();
}
}
......
......@@ -22,7 +22,7 @@ class SoTTiagoSteelController: public SoTTiagoController
public:
static const std::string LOG_PYTHON_TIAGOSTEEL;
SoTTiagoSteelController(bool withWheels);
SoTTiagoSteelController();
virtual ~SoTTiagoSteelController() {};
......
File mode changed from 100755 to 100644
File mode changed from 100755 to 100644