From 4a26911e7d482dfa14b9aa0e32fd1554b71c8be2 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Mon, 7 May 2018 10:53:13 +0200
Subject: [PATCH] Remove talos_roscontrol_sot.
---
.gitmodules | 3 -
talos_roscontrol_sot/CMakeLists.txt | 125 ---
talos_roscontrol_sot/LICENSE | 25 -
talos_roscontrol_sot/README.md | 70 --
talos_roscontrol_sot/cmake | 1 -
...zebo_roscontrol_sot_controller_plugins.xml | 9 -
.../roscontrol_sot_controller_plugins.xml | 9 -
talos_roscontrol_sot/config/rosdoc.yaml | 5 -
talos_roscontrol_sot/doc/Doxyfile.extra.in | 3 -
talos_roscontrol_sot/doc/manifest.yaml | 22 -
.../launch/sot_controller_gazebo.launch | 13 -
talos_roscontrol_sot/package.xml | 53 --
talos_roscontrol_sot/src/log.cpp | 193 ----
talos_roscontrol_sot/src/log.hh | 89 --
.../src/roscontrol-sot-controller.cpp | 856 ------------------
.../src/roscontrol-sot-controller.hh | 288 ------
16 files changed, 1764 deletions(-)
delete mode 100644 talos_roscontrol_sot/CMakeLists.txt
delete mode 100644 talos_roscontrol_sot/LICENSE
delete mode 100644 talos_roscontrol_sot/README.md
delete mode 160000 talos_roscontrol_sot/cmake
delete mode 100644 talos_roscontrol_sot/config/gazebo_roscontrol_sot_controller_plugins.xml
delete mode 100644 talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml
delete mode 100644 talos_roscontrol_sot/config/rosdoc.yaml
delete mode 100644 talos_roscontrol_sot/doc/Doxyfile.extra.in
delete mode 100644 talos_roscontrol_sot/doc/manifest.yaml
delete mode 100644 talos_roscontrol_sot/launch/sot_controller_gazebo.launch
delete mode 100644 talos_roscontrol_sot/package.xml
delete mode 100644 talos_roscontrol_sot/src/log.cpp
delete mode 100644 talos_roscontrol_sot/src/log.hh
delete mode 100644 talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
delete mode 100644 talos_roscontrol_sot/src/roscontrol-sot-controller.hh
diff --git a/.gitmodules b/.gitmodules
index 0443e7a..e69de29 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +0,0 @@
-[submodule "talos_roscontrol_sot/cmake"]
- path = talos_roscontrol_sot/cmake
- url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
diff --git a/talos_roscontrol_sot/CMakeLists.txt b/talos_roscontrol_sot/CMakeLists.txt
deleted file mode 100644
index b7c21ed..0000000
--- a/talos_roscontrol_sot/CMakeLists.txt
+++ /dev/null
@@ -1,125 +0,0 @@
-# Copyright (C) 2017 LAAS-CNRS
-#
-# Author: Olivier Stasse
-#
-# 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 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with this program. If not, see <http://www.gnu.org/licenses/>.
-cmake_minimum_required(VERSION 2.8.3)
-
-# Authorize warning error.
-SET(CXX_DISABLE_WERROR)
-
-include(cmake/base.cmake)
-include(cmake/ros.cmake)
-include(cmake/GNUInstallDirs.cmake)
-include(cmake/python.cmake)
-
-#project(talos_roscontrol_sot)
-
-OPTION(REAL_ROBOT "Compiling this package for the real robot" TRUE)
-
-find_package(PkgConfig REQUIRED)
-
-add_required_dependency(bullet)
-
-#set(bullet_FOUND 0)
-#pkg_check_modules(bullet REQUIRED bullet)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-if(${REAL_ROBOT})
- set(additional_catkin_required_components
- controller_interface
- pal_hardware_interfaces
- )
-
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DREAL_ROBOT")
-
-else(${REAL_ROBOT})
- set(additional_catkin_required_components
- talos_controller_interface
- talos_pal_hardware_interfaces
- )
-endif(${REAL_ROBOT})
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- rospy
- std_msgs
- dynamic_graph_bridge
- control_msgs
- sensor_msgs
- realtime_tools
- ${additional_catkin_required_components}
-)
-
-## LAAS cmake submodule part
-set(PROJECT_DESCRIPTION "Integration of the Stack of Tasks in roscontrol")
-set(PROJECT_NAME talos_roscontrol_sot)
-set(PROJECT_URL "ssh://git@redmine.laas.fr/laas/users/ostasse/pyrene-talos/metapkg_talos_roscontrol_sot.git")
-
-set(CXX_DISABLE_WERROR False)
-
-include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
-
-link_directories(${bullet_LIBRARY_DIRS})
-
-SETUP_PROJECT()
-
-# This is necessary so that the pc file generated by catking is similar to the on
-# done directly by jrl-cmake-modules
-catkin_package(CATKIN_DEPENDS
-roscpp realtime_tools message_runtime dynamic_graph_bridge
-LIBRARIES rcsot_controller)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
-include_directories(
- ${catkin_INCLUDE_DIRS}
- )
-
-## Declare a C++ library
-add_library(talos_rcsot_controller
-src/roscontrol-sot-controller.cpp
-src/log.cpp
-)
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(roscontrol_sot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(talos_rcsot_controller
- ${catkin_LIBRARIES}
- ${bullet_LIBRARIES}
- )
-
-
-## Mark executables and/or libraries for installation
-install(TARGETS talos_rcsot_controller DESTINATION lib )
-
-
-foreach(dir config launch)
- install(DIRECTORY ${dir}
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
- )
-endforeach()
-
-SETUP_PROJECT_FINALIZE()
-
-
diff --git a/talos_roscontrol_sot/LICENSE b/talos_roscontrol_sot/LICENSE
deleted file mode 100644
index b609833..0000000
--- a/talos_roscontrol_sot/LICENSE
+++ /dev/null
@@ -1,25 +0,0 @@
-BSD 2-Clause License
-
-Copyright (c) 2017, Stack Of Tasks development team
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
-* Redistributions of source code must retain the above copyright notice, this
- list of conditions and the following disclaimer.
-
-* Redistributions in binary form must reproduce the above copyright notice,
- this list of conditions and the following disclaimer in the documentation
- and/or other materials provided with the distribution.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/talos_roscontrol_sot/README.md b/talos_roscontrol_sot/README.md
deleted file mode 100644
index 764a703..0000000
--- a/talos_roscontrol_sot/README.md
+++ /dev/null
@@ -1,70 +0,0 @@
-# Introduction
-This package encapsulates a SoT graph to control a robot in the ros-control framework.
-The intent is to make it generic and adapted to any robot through rosparam.
-As the Stack-Of-Taks is a whole-body controller it tries to connect to all the available
-resources of the robot.
-
-# rosparam
-
-## Namespace
-All the parameters regarding the SoT inside ros-control are in the namespace
-```
-/sot_controller
-```
-
-## Setting the SoT dynamic library which contains the robot device.
-
-Let us assume that you want to control a TALOS robot.
-You should then write a YAML file than can be named:
-```
-sot_talos_param.yaml
-```
-
-Its SoT device entity is located inside the following dynamic library:
-```
-/opt/openrobots/lib/libsot-pyrene-controller.so
-```
-Then inside the file sot_talos_param.yaml
-```
- libname: libsot-pyrene-controller.so
-```
-
-## Specifying the actuated state vector
-To map the joints from the URDF model to the SoT actuated state vector, it is simply done by giving the ordered list of the joints name in the URDF model.
-For instance:
-```
- joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
- leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
- torso_1_joint,torso_2_joint,head_1_joint, head_2_joint,
- arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
- arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint
- ]
-```
-
-## Specifying the map between the ros control data and the sot device entity.
-
-This ros-control plugin connect to the robot (or simulated robot) by requesting the available ressources such as:
-1. motor-angles: Reading of the motor position
-2. joint-angles: Reading of the joint position
-3. velocities: Reading/estimation of the joint velocities
-4. torques: Reading/estimation of the joint torques
-5. cmd-position: Command for the joint positions
-6. cmd-effort: Command for the joint torques
-
-For instance the map for a robot which maps __cmd-position__ to __joints__ and __cmd-effort__ to __effort__ in its device will hae
-have the following lines in its param file:
-```
-map_rc_to_sot_device: { motor-angles: motor-angles ,
- joint-angles: joint-angles, velocities: velocities,
- torques: torques, cmd-joints: joints, cmd-effort: effort }
-```
-## Specifying the control mode
-Robots with __ros-control__ can be controlled either in position (POSITION) or in torque (EFFORT)
-using the __control_mode__ variable:
-
-```
- control_mode: EFFORT
-```
-
-
-
diff --git a/talos_roscontrol_sot/cmake b/talos_roscontrol_sot/cmake
deleted file mode 160000
index 38d4efa..0000000
--- a/talos_roscontrol_sot/cmake
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 38d4efa72f8e3fe087eb5abc72efd51747b2271d
diff --git a/talos_roscontrol_sot/config/gazebo_roscontrol_sot_controller_plugins.xml b/talos_roscontrol_sot/config/gazebo_roscontrol_sot_controller_plugins.xml
deleted file mode 100644
index 9388d0b..0000000
--- a/talos_roscontrol_sot/config/gazebo_roscontrol_sot_controller_plugins.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<library path="lib/libtalos_rcsot_controller">
-
- <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="talos_controller_interface::ControllerBase">
- <description>
- The roscontrol SotController generates whole body motions for your robot (following the talos interface).
- </description>
- </class>
-</library>
-
diff --git a/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml b/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml
deleted file mode 100644
index 3c82281..0000000
--- a/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<library path="lib/libtalos_rcsot_controller">
-
- <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="controller_interface::ControllerBase">
- <description>
- The roscontrol SotController generates whole body motions for your robot (following the talos interface).
- </description>
- </class>
-</library>
-
diff --git a/talos_roscontrol_sot/config/rosdoc.yaml b/talos_roscontrol_sot/config/rosdoc.yaml
deleted file mode 100644
index 8720895..0000000
--- a/talos_roscontrol_sot/config/rosdoc.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-- builder: doxygen
- name: C++ API
- output_dir: c++
- file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
- #exclude_patterns: '*/ARDroneLib/*'
diff --git a/talos_roscontrol_sot/doc/Doxyfile.extra.in b/talos_roscontrol_sot/doc/Doxyfile.extra.in
deleted file mode 100644
index 707e217..0000000
--- a/talos_roscontrol_sot/doc/Doxyfile.extra.in
+++ /dev/null
@@ -1,3 +0,0 @@
-INPUT = @PROJECT_SOURCE_DIR@/include \
- @PROJECT_SOURCE_DIR@/doc
-
diff --git a/talos_roscontrol_sot/doc/manifest.yaml b/talos_roscontrol_sot/doc/manifest.yaml
deleted file mode 100644
index 2ac3fbb..0000000
--- a/talos_roscontrol_sot/doc/manifest.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-actions: []
-authors: Olivier Stasse <ostasse@laas.fr>
-brief: ''
-bugtracker: ''
-depends:
-- controller_interface
-- roscpp
-- std_msgs
-- sensor_msgs
-- realtime_tools
-- catkin
-- cmake_modules
-- rospy
-- control_msgs
-description: Wrapping the Stack-of-tasks framework in ros-control
-license: BSD
-maintainers: Olivier Stasse <ostasse@laas.fr>
-msgs: []
-package_type: package
-repo_url: ''
-srvs: []
-url: ''
diff --git a/talos_roscontrol_sot/launch/sot_controller_gazebo.launch b/talos_roscontrol_sot/launch/sot_controller_gazebo.launch
deleted file mode 100644
index 08fcab8..0000000
--- a/talos_roscontrol_sot/launch/sot_controller_gazebo.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<launch>
-
- <!-- Sot Controller configuration -->
- <!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_params_gazebo.yaml"/> -->
- <!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_controller.yaml" /> -->
-
- <!-- Spawn walking controller -->
- <node name="sot_controller_spawner"
- pkg="talos_controller_manager" type="spawner" output="screen"
- args="sot_controller" />
-
-</launch>
-
diff --git a/talos_roscontrol_sot/package.xml b/talos_roscontrol_sot/package.xml
deleted file mode 100644
index 836e1c9..0000000
--- a/talos_roscontrol_sot/package.xml
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0"?>
-<package>
- <name>talos_roscontrol_sot</name>
- <version>0.0.7</version>
- <description>Wrapping the Stack-of-tasks framework in ros-control for talos</description>
-
- <!-- Maintainer email -->
- <maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <license>BSD</license>
-
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/roscontrol_sot</url> -->
-
-
- <!-- Author tag -->
- <author email="ostasse@laas.fr">Olivier Stasse</author>
-
-
- <!-- The *_depend tags are used to specify dependencies -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>rospy</build_depend>
- <build_depend>std_msgs</build_depend>
- <build_depend>control_msgs</build_depend>
- <build_depend>sensor_msgs</build_depend>
- <build_depend>realtime_tools</build_depend>
- <build_depend>controller_interface</build_depend>
- <build_depend>pal_hardware_interfaces</build_depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>dynamic_graph_bridge</build_depend>
-
- <run_depend>roscpp</run_depend>
- <run_depend>control_msgs</run_depend>
- <run_depend>sensor_msgs</run_depend>
- <run_depend>realtime_tools</run_depend>
- <run_depend>rospy</run_depend>
- <run_depend>std_msgs</run_depend>
- <run_depend>controller_interface</run_depend>
- <run_depend>pal_hardware_interfaces</run_depend>
- <run_depend>cmake_modules</run_depend>
- <run_depend>message_runtime</run_depend>
- <run_depend>dynamic_graph_bridge</run_depend>
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/>
- </export>
-</package>
diff --git a/talos_roscontrol_sot/src/log.cpp b/talos_roscontrol_sot/src/log.cpp
deleted file mode 100644
index a30431b..0000000
--- a/talos_roscontrol_sot/src/log.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/*
- Olivier Stasse CNRS,
- 31/07/2012
- Object to log the low-level informations of a robot.
-*/
-#include "log.hh"
-#include <sys/time.h>
-#include <sstream>
-#include <fstream>
-#include <iostream>
-#include <iomanip>
-
-using namespace std;
-using namespace rc_sot_system;
-
-DataToLog::DataToLog()
-{
-}
-
-void DataToLog::init(unsigned int nbDofs,long int length)
-{
- motor_angle.resize(nbDofs*length);
- joint_angle.resize(nbDofs*length);
- velocities.resize(nbDofs*length);
- torques.resize(nbDofs*length);
- motor_currents.resize(nbDofs*length);
- orientation.resize(4*length);
- accelerometer.resize(3*length);
- gyrometer.resize(3*length);
- force_sensors.resize(4*6*length);
- temperatures.resize(nbDofs*length);
- timestamp.resize(length);
- duration.resize(length);
-
- for(unsigned int i=0;i<nbDofs*length;i++)
- { motor_angle[i] = joint_angle[i] =
- velocities[i] = 0.0; }
-
-}
-
-
-Log::Log():
- lref_(0),
- lrefts_(0)
-{
-}
-
-void Log::init(unsigned int nbDofs, unsigned int length)
-{
- lref_ =0;
- lrefts_=0;
- nbDofs_=nbDofs;
- length_=length;
- StoredData_.init(nbDofs,length);
- struct timeval current;
- gettimeofday(¤t,0);
-
- timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec);
-
-}
-
-void Log::record(DataToLog &aDataToLog)
-{
- if ( (aDataToLog.motor_angle.size()!=nbDofs_) ||
- (aDataToLog.velocities.size()!=nbDofs_))
- return;
-
- for(unsigned int JointID=0;JointID<nbDofs_;JointID++)
- {
- if (aDataToLog.motor_angle.size()>JointID)
- StoredData_.motor_angle[JointID+lref_]= aDataToLog.motor_angle[JointID];
- if (aDataToLog.joint_angle.size()>JointID)
- StoredData_.joint_angle[JointID+lref_]= aDataToLog.joint_angle[JointID];
- if (aDataToLog.velocities.size()>JointID)
- StoredData_.velocities[JointID+lref_]= aDataToLog.velocities[JointID];
- if (aDataToLog.torques.size()>JointID)
- StoredData_.torques[JointID+lref_]= aDataToLog.torques[JointID];
- if (aDataToLog.motor_currents.size()>JointID)
- StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
- if (aDataToLog.temperatures.size()>JointID)
- StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];
- }
- for(unsigned int axis=0;axis<3;axis++)
- {
- StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];
- StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];
- }
- for(unsigned int fsID=0;fsID<4;fsID++)
- {
- for(unsigned int axis=0;axis<6;axis++)
- {
- StoredData_.force_sensors[lrefts_*24+fsID*6+axis] =
- aDataToLog.force_sensors[fsID*6+axis];
- }
- }
- struct timeval current;
- gettimeofday(¤t,0);
-
- StoredData_.timestamp[lrefts_] =
- ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
-
- StoredData_.duration[lrefts_] = time_stop_it_ - time_start_it_;
-
- lref_ += nbDofs_;
- lrefts_ ++;
- if (lref_>=nbDofs_*length_)
- {
- lref_=0;
- lrefts_=0;
- }
-}
-
-void Log::start_it()
-{
- struct timeval current;
- gettimeofday(¤t,0);
-
- time_start_it_ =
- ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
-
-}
-
-void Log::stop_it()
-{
- struct timeval current;
- gettimeofday(¤t,0);
-
- time_stop_it_ =
- ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
-}
-
-void Log::save(std::string &fileName)
-{
- std::string suffix("-mastate.log");
- saveVector(fileName,suffix,StoredData_.motor_angle, nbDofs_);
- suffix="-jastate.log";
- saveVector(fileName,suffix,StoredData_.joint_angle, nbDofs_);
- suffix = "-vstate.log";
- saveVector(fileName,suffix,StoredData_.velocities, nbDofs_);
- suffix = "-torques.log";
- saveVector(fileName,suffix,StoredData_.torques, nbDofs_);
- suffix = "-motor-currents.log";
- saveVector(fileName,suffix,StoredData_.motor_currents, nbDofs_);
- suffix = "-accelero.log";
- saveVector(fileName,suffix,StoredData_.accelerometer, 3);
- suffix = "-gyro.log";
- saveVector(fileName,suffix,StoredData_.gyrometer, 3);
-
- ostringstream oss;
- oss << "-forceSensors.log";
- suffix = oss.str();
- saveVector(fileName,suffix,StoredData_.force_sensors, 6);
-
- suffix = "-temperatures.log";
- saveVector(fileName,suffix,StoredData_.temperatures, nbDofs_);
-
- suffix = "-duration.log";
- saveVector(fileName,suffix,StoredData_.duration, 1);
-
-}
-
-void Log::saveVector(std::string &fileName,std::string &suffix,
- std::vector<double> &avector,
- unsigned int size)
-{
- ostringstream oss;
- oss << fileName;
- oss << suffix.c_str();
- std::string actualFileName= oss.str();
- ofstream aof(actualFileName.c_str());
- aof << std::setprecision(12) << std::setw(12) << std::setfill('0');
- if (aof.is_open())
- {
- for(unsigned long int i=0;i<length_;i++)
- {
- // Save timestamp
- aof << StoredData_.timestamp[i] << " " ;
-
- // Compute and save dt
- if (i==0)
- aof << StoredData_.timestamp[i] - StoredData_.timestamp[length_-1] << " ";
- else
- aof << StoredData_.timestamp[i] - StoredData_.timestamp[i-1] << " ";
-
- // Save all data
- for(unsigned long int datumID=0;datumID<size;datumID++)
- aof << avector[i*size+datumID] << " " ;
-
- aof << std::endl;
- }
- aof.close();
- }
-}
diff --git a/talos_roscontrol_sot/src/log.hh b/talos_roscontrol_sot/src/log.hh
deleted file mode 100644
index 66fc44d..0000000
--- a/talos_roscontrol_sot/src/log.hh
+++ /dev/null
@@ -1,89 +0,0 @@
-/*
- Olivier Stasse CNRS,
- 19/12/2016
- Object to control the low-level part of TALOS.
-*/
-
-#ifndef _RC_SOT_SYSTEM_LOG_H_
-#define _RC_SOT_SYSTEM_LOG_H_
-
-#include <vector>
-#include <string>
-
-namespace rc_sot_system {
-
- struct DataToLog
- {
- // Measured angle values at the motor side.
- std::vector<double> motor_angle;
- // Measured angle at the joint side.
- std::vector<double> joint_angle;
- // Measured or computed velocities.
- std::vector<double> velocities;
- // Measured torques.
- std::vector<double> torques;
- // Reconstructed orientation (from internal IMU).
- std::vector<double> orientation;
- // Measured linear acceleration
- std::vector<double> accelerometer;
- // Measured angular velocities
- std::vector<double> gyrometer;
- // Measured force sensors
- std::vector<double> force_sensors;
- // Measured motor currents
- std::vector<double> motor_currents;
- // Measured temperatures
- std::vector<double> temperatures;
-
- // Timestamp
- std::vector<double> timestamp;
- // Duration
- std::vector<double> duration;
-
- DataToLog();
- void init(unsigned int nbDofs, long int length);
-
-
- };
-
- class Log
- {
- private:
- // Actuated informations logged.
- unsigned int nbDofs_;
- // Number of iterations to be logged.
- unsigned int length_;
-
- // Current position in the circular buffer for angles.
- long unsigned int lref_;
- // Current position int the circular buffer for timestamp
- // lref_ = lrefts_ * nbDofs_
- long unsigned int lrefts_;
-
- // Circular buffer for all the data.
- DataToLog StoredData_;
-
- double timeorigin_;
- double time_start_it_;
- double time_stop_it_;
-
- // Save one vector of information.
- void saveVector(std::string &filename,
- std::string &suffix,
- std::vector<double> &avector,
- unsigned int);
-
- public:
-
- Log();
-
- void init(unsigned int nbDofs, unsigned int length);
- void record(DataToLog &aDataToLog);
-
- void save(std::string &fileName);
- void start_it();
- void stop_it();
-
- };
-}
-#endif /* _RC_SOT_SYSTEM_LOG_H_ */
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
deleted file mode 100644
index 72600af..0000000
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
+++ /dev/null
@@ -1,856 +0,0 @@
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <dlfcn.h>
-#include <sstream>
-
-#include <pluginlib/class_list_macros.h>
-#include "roscontrol-sot-controller.hh"
-
-
-#if DEBUG
-#define ODEBUG(x) std::cout << x << std::endl
-#else
-#define ODEBUG(x)
-#endif
-#define ODEBUG3(x) std::cout << x << std::endl
-
-#define DBGFILE "/tmp/rcoh2sot.dat"
-
-#define RESETDEBUG5() { std::ofstream DebugFile; \
- DebugFile.open(DBGFILE,std::ofstream::out); \
- DebugFile.close();}
-#define ODEBUG5FULL(x) { std::ofstream DebugFile; \
- DebugFile.open(DBGFILE,std::ofstream::app); \
- DebugFile << __FILE__ << ":" \
- << __FUNCTION__ << "(#" \
- << __LINE__ << "):" << x << std::endl; \
- DebugFile.close();}
-#define ODEBUG5(x) { std::ofstream DebugFile; \
- DebugFile.open(DBGFILE,std::ofstream::app); \
- DebugFile << x << std::endl; \
- DebugFile.close();}
-
-#define RESETDEBUG4()
-#define ODEBUG4FULL(x)
-#define ODEBUG4(x)
-
-/// lhi: nickname for local_hardware_interface
-/// Depends if we are on the real robot or not.
-
-#ifdef REAL_ROBOT
-namespace lhi=hardware_interface;
-#else
-namespace lhi=talos_hardware_interface;
-#endif
-using namespace lhi;
-
-using namespace rc_sot_system;
-
-namespace talos_sot_controller
-{
- typedef std::map<std::string,std::string>::iterator it_map_rt_to_sot;
-
- EffortControlPDMotorControlData::EffortControlPDMotorControlData()
- {
- prev = 0.0; vel_prev = 0.0; des_pos=0.0;
- integ_err=0.0;
- }
-
- void EffortControlPDMotorControlData::read_from_xmlrpc_value
- (const std::string &prefix)
- {
- pid_controller.initParam(prefix);
- }
-
- RCSotController::
- RCSotController():
- // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
- // -> 124 Mo of data.
- type_name_("RCSotController"),
- simulation_mode_(false),
- control_mode_(POSITION),
- accumulated_time_(0.0)
- {
- RESETDEBUG4();
- }
-
- void RCSotController::
- displayClaimedResources(std::set<std::string> & claimed_resources)
- {
- std::set<std::string >::iterator it_claim;
- ROS_INFO_STREAM("Size of claimed resources: "<< claimed_resources.size());
- for (it_claim = claimed_resources.begin();
- it_claim != claimed_resources.end();
- ++it_claim)
- {
- std::string aclaim = *it_claim;
- ROS_INFO_STREAM("Claimed by RCSotController: " << aclaim);
- }
- }
-
- bool RCSotController::
- initRequest (lhi::RobotHW * robot_hw,
- ros::NodeHandle &robot_nh,
- ros::NodeHandle &controller_nh,
- std::set<std::string> & claimed_resources)
- {
- /// Read the parameter server
- if (!readParams(robot_nh))
- return false;
-
- /// Create ros control interfaces to hardware
- if (!initInterfaces(robot_hw,robot_nh,controller_nh,claimed_resources))
- return false;
-
- /// Create SoT
- SotLoaderBasic::Initialization();
-
- /// If we are in effort mode then the device should not do any integration.
- if (control_mode_==EFFORT)
- {
- sotController_->setNoIntegration();
- /// Fill desired position during the phase where the robot is waiting.
- for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
- {
- std::string joint_name = joints_name_[idJoint];
- std::map<std::string,EffortControlPDMotorControlData>::iterator
- search_ecpd = effort_mode_pd_motors_.find(joint_name);
-
- if (search_ecpd!=effort_mode_pd_motors_.end())
- {
- EffortControlPDMotorControlData & ecpdcdata =
- search_ecpd->second;
- ecpdcdata.des_pos = joints_[idJoint].getPosition();
- }
- }
- }
- return true;
- }
-
- bool RCSotController::
- initInterfaces(lhi::RobotHW * robot_hw,
- ros::NodeHandle &,
- ros::NodeHandle &,
- std::set<std::string> & claimed_resources)
- {
- std::string lns;
- #ifdef REAL_ROBOT
- lns="hardware_interface";
- #else
- lns="talos_hardware_interface";
- #endif
- // Check if construction finished cleanly
- if (state_!=CONSTRUCTED)
- {
- ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
- }
-
- // Get a pointer to the joint position control interface
- pos_iface_ = robot_hw->get<PositionJointInterface>();
- if (! pos_iface_)
- {
- ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered in the %s::RobotHW class.",
- getHardwareInterfaceType().c_str(), lns.c_str());
- return false ;
- }
-
- // Get a pointer to the joint effort control interface
- effort_iface_ = robot_hw->get<EffortJointInterface>();
- if (! effort_iface_)
- {
- ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered in the %s::RobotHW class.",
- getHardwareInterfaceType().c_str(),lns.c_str());
- return false ;
- }
-
- // Get a pointer to the force-torque sensor interface
- ft_iface_ = robot_hw->get<ForceTorqueSensorInterface>();
- if (! ft_iface_ )
- {
- ROS_ERROR("This controller requires a hardware interface of type '%s '. "
- " Make sure this is registered inthe %s::RobotHW class.",
- internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str(),lns.c_str());
- return false ;
- }
- // Get a pointer to the IMU sensor interface
- imu_iface_ = robot_hw->get<ImuSensorInterface>();
- if (! imu_iface_)
- {
- ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered in the %s::RobotHW class.",
- internal :: demangledTypeName<ImuSensorInterface>().c_str(),lns.c_str());
- return false ;
- }
-
- // Temperature sensor not available in simulation mode
- if (!simulation_mode_)
- {
- // Get a pointer to the actuator temperature sensor interface
- act_temp_iface_ = robot_hw->get<ActuatorTemperatureSensorInterface>();
- if (!act_temp_iface_)
- {
- ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered in the %s::RobotHW class.",
- internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str(),lns.c_str());
- return false ;
- }
- }
-
-
- // Return which resources are claimed by this controller
- pos_iface_->clearClaims();
- effort_iface_->clearClaims();
-
- if (! init ())
- {
- ROS_ERROR("Failed to initialize sot-controller" );
- std :: cerr << "FAILED LOADING SOT CONTROLLER" << std::endl;
- return false ;
- }
- ROS_INFO_STREAM("Initialization of interfaces for sot-controller Ok !");
- claimed_resources = pos_iface_->getClaims();
- displayClaimedResources(claimed_resources);
- pos_iface_->clearClaims();
-
- claimed_resources = effort_iface_->getClaims();
- displayClaimedResources(claimed_resources);
- effort_iface_->clearClaims();
-
- ROS_INFO_STREAM("Initialization of sot-controller Ok !");
- // success
- state_ = INITIALIZED;
-
- return true;
- }
-
- bool RCSotController::
- init()
- {
- if (!initJoints())
- return false;
- if (!initIMU())
- return false;
- if (!initForceSensors())
- return false;
- if (!initTemperatureSensors())
- return false;
-
- // Initialize ros node.
- int argc=1;
- char *argv[1];
- argv[0] = new char[10];
- strcpy(argv[0],"libsot");
- SotLoaderBasic::initializeRosNode(argc,argv);
-
- return true;
- }
-
- bool RCSotController::
- readParamsSotLibName(ros::NodeHandle &robot_nh)
- {
- // Read param to find the library to load
- std::string dynamic_library_name;
-
- // Read libname
- if (!robot_nh.getParam("/sot_controller/libname",dynamic_library_name))
- {
- ROS_ERROR_STREAM("Could not read param /sot_controller/libname");
- if (robot_nh.hasParam("/sot_controller/libname"))
- {
- ROS_ERROR_STREAM("Param /sot_controller/libname exists !");
- }
- else
- {
- ROS_ERROR_STREAM("Param /sot_controller/libname does not exists !");
- return false;
- }
- }
- else
- {
- ROS_INFO_STREAM("Loading library name: " << dynamic_library_name);
- }
- /// SotLoaderBasic related method calls.
- // Initialize the dynamic_library_name for the sotLoader
- setDynamicLibraryName(dynamic_library_name);
- return true;
- }
-
- bool RCSotController::
- readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh)
- {
- // Read libname
- if (robot_nh.hasParam("/sot_controller/effort_control_pd_motor_init/gains"))
- {
- XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
- robot_nh.getParamCached("/sot_controller/effort_control_pd_motor_init/gains",
- xml_rpc_ecpd_init);
-
- ROS_INFO("/sot_controller/effort_control_pd_motor_init/gains: %d %d %d\n",
- xml_rpc_ecpd_init.getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
- effort_mode_pd_motors_.clear();
-
- for (int i=0;i<joints_name_.size();i++)
- {
- if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
- {
- /*
- XmlRpc::XmlRpcValue &aDataSet= xml_rpc_ecpd_init[joints_name_[i]];
- ROS_INFO("/sot_controller/effort_control_pd_motor_init %s type: %d\n",joints_name_[i],aDataSet.getType());
- if (aDataSet.getType()!=XmlRpc::XmlRpcValue::TypeStruct)
- {
- ROS_ERROR("In /sot_controller/effort_control_pd_motor_init/gains/%s not a struct"
- ,joints_name_[i]);
- throw XmlrpcHelperException("Pb in readParamsEffortControlPDMotorControlData");
- }
- */
- std::string prefix= "/sot_controller/effort_control_pd_motor_init/gains/" + joints_name_[i];
- effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(prefix);
- }
- else
- ROS_INFO("joint %s not in /sot_controller/effort_control_pd_motor_init/gains\n",
- joints_name_[i].c_str());
- }
- return true;
- }
-
- ROS_ERROR("No parameter /sot_controller/effort_controler_pd_motor_init");
- return false;
- }
-
- bool RCSotController::
- readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh)
- {
- // Read libname
- if (robot_nh.hasParam("/sot_controller/map_rc_to_sot_device"))
- {
- if (robot_nh.getParam("/sot_controller/map_rc_to_sot_device",
- mapFromRCToSotDevice_))
- {
- /// TODO: Check if the mapping is complete wrt to the interface and the mapping.
- ROS_INFO_STREAM("Loading map rc to sot device: ");
- for (it_map_rt_to_sot it = mapFromRCToSotDevice_.begin();
- it != mapFromRCToSotDevice_.end(); ++it)
- ROS_INFO_STREAM( it->first << ", " << it->second);
- }
- else
- {
- ROS_ERROR_STREAM("Could not read param /sot_controller/map_rc_to_sot_device");
- return false;
- }
- }
- else
- {
- ROS_ERROR_STREAM("Param /sot_controller/map_rc_to_sot_device does not exists !");
- return false;
- }
- return true;
- }
-
- bool RCSotController::
- readParamsJointNames(ros::NodeHandle &robot_nh)
- {
- /// Check if the /sot_controller/joint_names parameter exists.
- if (robot_nh.hasParam("/sot_controller/joint_names"))
- {
- /// Read the joint_names list from this parameter
- robot_nh.getParam("/sot_controller/joint_names",
- joints_name_);
- for(std::vector<std::string>::size_type i=0;i<joints_name_.size();i++)
- {ROS_INFO_STREAM("joints_name_[" << i << "]=" << joints_name_[i]);}
-
- }
- else
- return false;
-
- /// Deduce from this the degree of freedom number.
- nbDofs_ = joints_name_.size();
- /// Initialize the size of the data to store.
- DataOneIter_.init(nbDofs_,1);
- /// Initialize the data logger for 300s.
- RcSotLog.init(nbDofs_,300000);
-
- return true;
- }
-
- bool RCSotController::
- readParamsControlMode(ros::NodeHandle &robot_nh)
- {
- // Read param for the list of joint names.
- if (robot_nh.hasParam("/sot_controller/control_mode"))
- {
- std::string scontrol_mode,seffort("EFFORT"),sposition("POSITION");
-
- /// Read the joint_names list
- robot_nh.getParam("/sot_controller/control_mode",scontrol_mode);
- ROS_INFO_STREAM("control mode read from param file:|" << scontrol_mode<<"|");
-
- if (scontrol_mode==seffort)
- control_mode_ = EFFORT;
- else if (scontrol_mode==sposition)
- control_mode_ = POSITION;
- else
- {
- ROS_INFO_STREAM("Error in specifying control mode-> falls back to default position. Wrong control is:" << scontrol_mode);
- std::string::size_type n;
- n = scontrol_mode.find("EFFORT");
- ROS_INFO_STREAM("n: " << n << " size: " << scontrol_mode.size() << " "<< sposition.size() << " " << seffort.size());
- control_mode_ = POSITION;
- }
- }
- else
- ROS_INFO_STREAM("Default control mode : position");
-
- /// Always return true;
- return true;
- }
-
- bool RCSotController::
- readParamsdt(ros::NodeHandle &robot_nh)
- {
- /// Read /sot_controller/dt to know what is the control period
- if (robot_nh.hasParam("/sot_controller/dt"))
- {
- robot_nh.getParam("/sot_controller/dt",dt_);
- ROS_INFO_STREAM("dt: " << dt_);
- return true;
- }
- ROS_ERROR("You need to define a control period in param /sot_controller/dt");
- return false;
- }
-
- bool RCSotController::
- readParams(ros::NodeHandle &robot_nh)
- {
-
- /// Calls readParamsSotLibName
- // Reads the SoT dynamic library.
- if (!readParamsSotLibName(robot_nh))
- return false;
-
- /// Read /sot_controller/simulation_mode to know if we are in simulation mode
- // Defines if we are in simulation node.
- if (robot_nh.hasParam("/sot_controller/simulation_mode"))
- simulation_mode_ = true;
-
- /// Calls readParamsJointNames
- // Reads the list of joints to be controlled.
- if (!readParamsJointNames(robot_nh))
- return false;
-
- /// Calls readParamsControlMode.
- // Defines if the control mode is position or effort
- readParamsControlMode(robot_nh);
-
- /// Calls readParamsFromRCToSotDevice
- // Mapping from ros-controll to sot device
- readParamsFromRCToSotDevice(robot_nh);
-
- /// Get control perioud
- if (!readParamsdt(robot_nh))
- return false;
-
- if (control_mode_==EFFORT)
- readParamsEffortControlPDMotorControlData(robot_nh);
-
- return true;
- }
-
-
- bool RCSotController::
- initJoints()
- {
- // Init Joint Names.
- joints_.resize(joints_name_.size());
-
- for (unsigned int i=0;i<nbDofs_;i++)
- {
- bool notok=true;
- SotControlMode lcontrol_mode = control_mode_;
-
- while (notok)
- {
- try
- {
- if (lcontrol_mode==POSITION)
- {
- joints_[i] = pos_iface_->getHandle(joints_name_[i]);
- ROS_INFO_STREAM("Found joint " << joints_name_[i] << " in position.");
- }
- else if (lcontrol_mode==EFFORT)
- {
- joints_[i] = effort_iface_->getHandle(joints_name_[i]);
- ROS_INFO_STREAM("Found joint " << joints_name_[i] << " in effort.");
- }
-
- // throws on failure
- notok=false;
- }
- catch (...)
- {
- ROS_ERROR_STREAM("Could not find joint "
- << joints_name_[i]);
- if (lcontrol_mode==POSITION)
- ROS_ERROR_STREAM(" in POSITION");
- else
- ROS_ERROR_STREAM(" in EFFORT");
-
- if (lcontrol_mode==POSITION)
- return false ;
- else if (lcontrol_mode==EFFORT)
- lcontrol_mode = POSITION;
- }
- }
- }
-
- return true ;
-
- }
-
- bool RCSotController::
- initIMU()
- {
- // get all imu sensor names
- const std :: vector<std :: string >& imu_iface_names = imu_iface_->getNames();
- for (unsigned i=0; i <imu_iface_names.size(); i++)
- ROS_INFO("Got sensor %s", imu_iface_names[i].c_str());
- for (unsigned i=0; i <imu_iface_names.size(); i++){
- // sensor handle on imu
- imu_sensor_.push_back(imu_iface_->getHandle(imu_iface_names[i]));
- }
-
- return true ;
- }
-
- bool RCSotController::
- initForceSensors()
- {
- // get force torque sensors names package.
- const std::vector<std::string>& ft_iface_names = ft_iface_->getNames();
- for (unsigned i=0; i <ft_iface_names.size(); i++)
- ROS_INFO("Got sensor %s", ft_iface_names[i].c_str());
- for (unsigned i=0; i <ft_iface_names.size(); i++){
- // sensor handle on torque forces
- ft_sensors_.push_back(ft_iface_->getHandle(ft_iface_names[i]));
- }
- return true;
- }
-
- bool RCSotController::
- initTemperatureSensors()
- {
- if (!simulation_mode_)
- {
- // get temperature sensors names
- const std::vector<std::string>& act_temp_iface_names = act_temp_iface_->getNames();
- ROS_INFO("Actuator temperature sensors: %ld",act_temp_iface_names.size() );
-
- for (unsigned i=0; i <act_temp_iface_names.size(); i++)
- ROS_INFO("Got sensor %s", act_temp_iface_names[i].c_str());
- for (unsigned i=0; i <act_temp_iface_names.size(); i++){
- // sensor handle on actuator temperature
- act_temp_sensors_.push_back(act_temp_iface_->getHandle(act_temp_iface_names[i]));
- }
- }
-
- return true;
- }
-
- void RCSotController::
- fillSensorsIn(std::string &title, std::vector<double> & data)
- {
- /// Tries to find the mapping from the local validation
- /// to the SoT device.
- it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(title);
- /// If the mapping is found
- if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
- {
- /// Expose the data to the SoT device.
- std::string lmapRC2Sot = it_mapRC2Sot->second;
- sensorsIn_[lmapRC2Sot].setName(lmapRC2Sot);
- sensorsIn_[lmapRC2Sot].setValues(data);
- }
- }
-
- void RCSotController::
- fillJoints()
- {
- /// Fill positions, velocities and torques.
- for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
- {
- DataOneIter_.motor_angle[idJoint] = joints_[idJoint].getPosition();
- if (!simulation_mode_)
- DataOneIter_.joint_angle[idJoint] = joints_[idJoint].getAbsolutePosition();
-
- DataOneIter_.velocities[idJoint] = joints_[idJoint].getVelocity();
- if (!simulation_mode_)
- DataOneIter_.torques[idJoint] = joints_[idJoint].getTorqueSensor();
- DataOneIter_.motor_currents[idJoint] = joints_[idJoint].getEffort();
- }
-
- /// Update SoT internal values
- std::string ltitle("motor-angles");
- fillSensorsIn(ltitle,DataOneIter_.motor_angle);
- ltitle = "joint-angles";
- fillSensorsIn(ltitle,DataOneIter_.joint_angle);
- ltitle = "velocities";
- fillSensorsIn(ltitle,DataOneIter_.velocities);
- ltitle = "torques";
- fillSensorsIn(ltitle,DataOneIter_.torques);
- ltitle = "currents";
- fillSensorsIn(ltitle,DataOneIter_.motor_currents);
-
- }
-
- void RCSotController::setSensorsImu(std::string &name,
- int IMUnb,
- std::vector<double> & data)
- {
- std::ostringstream labelOss;
- labelOss << name << IMUnb;
- std::string label_s = labelOss.str();
- fillSensorsIn(label_s,data);
- }
-
- void RCSotController::
- fillImu()
- {
- for(unsigned int idIMU=0;idIMU<imu_sensor_.size();idIMU++)
- {
- /// Fill orientations, gyrometer and acceleration from IMU.
- if (imu_sensor_[idIMU].getOrientation())
- {
- for(unsigned int idquat = 0;idquat<4;idquat++)
- {
- DataOneIter_.orientation[idquat] = imu_sensor_[idIMU].getOrientation ()[idquat];
- }
- }
- if (imu_sensor_[idIMU].getAngularVelocity())
- {
- for(unsigned int idgyrometer = 0;idgyrometer<3;
- idgyrometer++)
- {
- DataOneIter_.gyrometer[idgyrometer] =
- imu_sensor_[idIMU].getAngularVelocity()[idgyrometer];
- }
- }
- if (imu_sensor_[idIMU].getLinearAcceleration())
- {
- for(unsigned int idlinacc = 0;idlinacc<3;
- idlinacc++)
- {
- DataOneIter_.accelerometer[idlinacc] =
- imu_sensor_[idIMU].getLinearAcceleration()[idlinacc];
- }
- }
-
- std::string orientation_s("orientation_");
- setSensorsImu(orientation_s, idIMU, DataOneIter_.orientation);
-
- std::string gyrometer_s("gyrometer_");
- setSensorsImu(gyrometer_s, idIMU, DataOneIter_.gyrometer);
-
- std::string accelerometer_s("accelerometer_");
- setSensorsImu(accelerometer_s, idIMU, DataOneIter_.accelerometer);
- }
- }
-
- void RCSotController::
- fillForceSensors()
- {
- for(unsigned int idFS=0;idFS<ft_sensors_.size();
- idFS++)
- {
- for(unsigned int idForce=0;idForce<3;idForce++)
- DataOneIter_.force_sensors[idFS*6+idForce]=
- ft_sensors_[idFS].getForce()[idForce];
- for(unsigned int idTorque=0;idTorque<3;idTorque++)
- DataOneIter_.force_sensors[idFS*6+3+idTorque]=
- ft_sensors_[idFS].getTorque()[idTorque];
- }
-
-
- std::string alabel("forces");
- fillSensorsIn(alabel,DataOneIter_.force_sensors);
- }
-
- void RCSotController::
- fillTempSensors()
- {
- if (!simulation_mode_)
- {
- for(unsigned int idFS=0;idFS<act_temp_sensors_.size();idFS++)
- {
- DataOneIter_.temperatures[idFS]= act_temp_sensors_[idFS].getValue();
- }
- }
- else
- {
- for(unsigned int idFS=0;idFS<nbDofs_;idFS++)
- DataOneIter_.temperatures[idFS]= 0.0;
- }
-
- std::string alabel("act-temp");
- fillSensorsIn(alabel,DataOneIter_.temperatures);
- }
-
- void RCSotController::
- fillSensors()
- {
- fillJoints();
- fillImu();
- fillForceSensors();
- fillTempSensors();
- }
-
- void RCSotController::
- readControl(std::map<std::string,dgs::ControlValues> &controlValues)
- {
- ODEBUG4("joints_.size() = " << joints_.size());
-
- std::string cmdTitle;
- if (control_mode_==POSITION)
- cmdTitle="cmd-joints";
- else if (control_mode_==EFFORT)
- cmdTitle="cmd-effort";
-
- it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(cmdTitle);
- if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
- {
- std::string lmapRC2Sot = it_mapRC2Sot->second;
- command_ = controlValues[lmapRC2Sot].getValues();
- ODEBUG4("angleControl_.size() = " << command_.size());
- for(unsigned int i=0;
- i<command_.size();++i)
- {
- joints_[i].setCommand(command_[i]);
- }
- }
- }
-
- void RCSotController::one_iteration()
- {
- // Chrono start
- RcSotLog.start_it();
-
- /// Update the sensors.
- fillSensors();
-
- /// Generate a control law.
- try
- {
- sotController_->nominalSetSensors(sensorsIn_);
- sotController_->getControl(controlValues_);
- }
- catch(std::exception &e) { throw e;}
-
- /// Read the control values
- readControl(controlValues_);
-
- // Chrono stop.
- RcSotLog.stop_it();
-
- /// Store everything in Log.
- RcSotLog.record(DataOneIter_);
- }
-
- void RCSotController::
- update(const ros::Time&, const ros::Duration& period)
- {
- // Do not send any control if the dynamic graph is not started
- if (!isDynamicGraphStopped())
- {
- try
- {
- double periodInSec = period.toSec();
- if (periodInSec+accumulated_time_>dt_)
- {
- one_iteration();
- accumulated_time_ = 0.0;
- }
- else
- accumulated_time_ += periodInSec;
- }
- catch (std::exception const &exc)
- {
- std::cerr << "Failure happened during one_iteration evaluation: std_exception" << std::endl;
- std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
- std::cerr << __FILE__ << " " << __LINE__ << std::endl;
- throw exc;
- }
- catch (...)
- {
- std::cerr << "Failure happened during one_iteration evaluation: unknown exception" << std::endl;
- std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
- std::cerr << __FILE__ << " " << __LINE__ << std::endl;
- }
- }
- else
- // But in effort mode it means that we are sending 0
- // Therefore implements a default PD controller on the system.
- if (control_mode_==EFFORT)
- {
- // ROS_INFO("Compute command for effort mode: %d %d",joints_.size(),effort_mode_pd_motors_.size());
- for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
- {
- std::string joint_name = joints_name_[idJoint];
- std::map<std::string,EffortControlPDMotorControlData>::iterator
- search_ecpd = effort_mode_pd_motors_.find(joint_name);
-
- if (search_ecpd!=effort_mode_pd_motors_.end())
- {
- EffortControlPDMotorControlData & ecpdcdata =
- search_ecpd->second;
- double vel_err = 0 - joints_[idJoint].getVelocity();
- double err = ecpdcdata.des_pos - joints_[idJoint].getPosition();
-
- ecpdcdata.integ_err +=err;
-
- double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
- // Apply command
- control_toolbox::Pid::Gains gains = ecpdcdata.pid_controller.getGains();
- //ROS_INFO("command: %d %s %f %f (%f %f %f)",idJoint, joints_name_[idJoint].c_str(),
- //local_command, DataOneIter_.motor_angle[idJoint],
- //gains.p_gain_,gains.d_gain_, gains.i_gain_);
- joints_[idJoint].setCommand(local_command);
-
- // Update previous value.
- ecpdcdata.prev = DataOneIter_.motor_angle[idJoint];
- }
- }
- }
- }
-
- void RCSotController::
- starting(const ros::Time &)
- {
- fillSensors();
- }
-
- void RCSotController::
- stopping(const ros::Time &)
- {
- std::string afilename("/tmp/sot.log");
- RcSotLog.save(afilename);
- }
-
- std::string RCSotController::
- getHardwareInterfaceType() const
- {
- //return type_name_;
- if (control_mode_==POSITION)
- return lhi::internal::
- demangledTypeName<lhi::PositionJointInterface>();
- else if (control_mode_==EFFORT)
- return lhi::internal::
- demangledTypeName<lhi::EffortJointInterface>();
- std::string voidstring("");
- return voidstring;
- }
-
-
- PLUGINLIB_EXPORT_CLASS(talos_sot_controller::RCSotController,
- lci::ControllerBase);
-}
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
deleted file mode 100644
index 96cc4b0..0000000
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
+++ /dev/null
@@ -1,288 +0,0 @@
-///////////////////////////////////////////////////////////////////////////////
-// Copyright (C) 2016, CNRS
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright
-// notice, this list of conditions and the following disclaimer in the
-// documentation and/or other materials provided with the distribution.
-// * Neither the name of hiDOF, Inc. nor the names of its
-// contributors may be used to endorse or promote products derived from
-// this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//////////////////////////////////////////////////////////////////////////////
-
-/*
- * Author: Olivier STASSE
- */
-
-#ifndef RC_SOT_CONTROLLER_H
-#define RC_SOT_CONTROLLER_H
-
-#include <string>
-#include <map>
-
-#ifdef REAL_ROBOT
-#include <controller_interface/controller.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <hardware_interface/imu_sensor_interface.h>
-#include <hardware_interface/force_torque_sensor_interface.h>
-#include <pal_hardware_interfaces/actuator_temperature_interface.h>
-#else
-#include <talos_controller_interface/controller.h>
-#include <talos_hardware_interface/joint_command_interface.h>
-#include <talos_hardware_interface/imu_sensor_interface.h>
-#include <talos_hardware_interface/force_torque_sensor_interface.h>
-#include <talos_pal_hardware_interfaces/actuator_temperature_interface.h>
-#endif
-
-#include <dynamic_graph_bridge/sot_loader_basic.hh>
-#include <ros/ros.h>
-#include <control_toolbox/pid.h>
-
-#include "log.hh"
-
-namespace talos_sot_controller
-{
- enum SotControlMode { POSITION, EFFORT};
-
- class XmlrpcHelperException : public ros::Exception
- {
- public:
- XmlrpcHelperException(const std::string& what)
- : ros::Exception(what) {}
- };
-
-
- struct EffortControlPDMotorControlData
- {
- control_toolbox::Pid pid_controller;
-
- //double p_gain,d_gain,i_gain;
- double prev;
- double vel_prev;
- double des_pos;
- double integ_err;
-
- EffortControlPDMotorControlData();
- // void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
- void read_from_xmlrpc_value(const std::string &prefix);
- };
-
- /**
- This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
-
- */
-#ifdef REAL_ROBOT
- namespace lhi = hardware_interface;
- namespace lci = controller_interface;
-#else
- namespace lhi = talos_hardware_interface;
- namespace lci = talos_controller_interface;
-#endif
-
- class RCSotController : public lci::ControllerBase,
- SotLoaderBasic
- {
-
- protected:
- /// Robot nb dofs.
- size_t nbDofs_;
-
- /// Data log.
- rc_sot_system::DataToLog DataOneIter_;
- private:
-
- /// @{ \name Ros-control related fields
-
- /// \brief Vector of joint handles.
- std::vector<lhi::JointHandle> joints_;
- std::vector<std::string> joints_name_;
-
- /// \brief Vector towards the IMU.
- std::vector<lhi::ImuSensorHandle> imu_sensor_;
-
- /// \brief Vector of 6D force sensor.
- std::vector<lhi::ForceTorqueSensorHandle> ft_sensors_;
-
- /// \brief Vector of temperature sensors for the actuators.
- std::vector<lhi::ActuatorTemperatureSensorHandle>
- act_temp_sensors_;
-
- /// \brief Interface to the joints controlled in position.
- lhi::PositionJointInterface * pos_iface_;
-
- /// \brief Interface to the joints controlled in force.
- lhi::EffortJointInterface * effort_iface_;
-
- /// \brief Interface to the sensors (IMU).
- lhi::ImuSensorInterface* imu_iface_;
-
- /// \brief Interface to the sensors (Force).
- lhi::ForceTorqueSensorInterface* ft_iface_;
-
- /// \brief Interface to the actuator temperature sensor.
- lhi::ActuatorTemperatureSensorInterface * act_temp_iface_;
-
- /// @}
-
- /// \brief Log
- rc_sot_system::Log RcSotLog;
- /// @}
-
- const std::string type_name_;
-
- /// \brief Adapt the interface to Gazebo simulation
- bool simulation_mode_;
-
- /// \brief The robot can controlled in effort or position mode (default).
- SotControlMode control_mode_;
-
-
- /// \brief Implement a PD controller for the robot when the dynamic graph
- /// is not on.
- std::map<std::string,EffortControlPDMotorControlData> effort_mode_pd_motors_;
-
- /// \brief Map from ros-control quantities to robot device
- /// ros-control quantities are for the sensors:
- /// * motor-angles
- /// * joint-angles
- /// * velocities
- /// * torques
- /// ros-control quantities for control are:
- /// * joints
- /// * torques
- std::map<std::string,std::string> mapFromRCToSotDevice_;
-
- /// To be able to subsample control period.
- double accumulated_time_;
-
- public :
-
- RCSotController ();
-
- /// \brief Read the configuration files,
- /// claims the request to the robot and initialize the Stack-Of-Tasks.
- bool initRequest (lhi::RobotHW * robot_hw,
- ros::NodeHandle &robot_nh,
- ros::NodeHandle &controller_nh,
- std::set<std::string> & claimed_resources);
-
- /// \brief Display claimed resources
- void displayClaimedResources(std::set<std::string> & claimed_resources);
-
- /// \brief Claims
- bool init();
-
- /// \brief Read the sensor values, calls the control graph, and apply the control.
- ///
- void update(const ros::Time&, const ros::Duration& );
- /// \brief Starting by filling the sensors.
- void starting(const ros::Time&);
- /// \brief Stopping the control
- void stopping(const ros::Time&);
- /// \brief Display the kind of hardware interface that this controller is using.
- virtual std::string getHardwareInterfaceType() const;
-
- protected:
- /// Initialize the roscontrol interfaces
- bool initInterfaces(lhi::RobotHW * robot_hw,
- ros::NodeHandle &,
- ros::NodeHandle &,
- std::set<std::string> & claimed_resources);
-
- /// Initialize the hardware interface using the joints.
- bool initJoints();
- /// Initialize the hardware interface accessing the IMU.
- bool initIMU();
- /// Initialize the hardware interface accessing the force sensors.
- bool initForceSensors();
- /// Initialize the hardware interface accessing the temperature sensors.
- bool initTemperatureSensors();
-
- ///@{ \name Read the parameter server
- /// \brief Entry point
- bool readParams(ros::NodeHandle &robot_nh);
-
- /// \brief Creates the list of joint names.
- bool readParamsJointNames(ros::NodeHandle &robot_nh);
-
- /// \brief Set the SoT library name.
- bool readParamsSotLibName(ros::NodeHandle &robot_nh);
-
- /// \Brief Set the mapping between ros-control and the robot device
- /// For instance the yaml file should have a line with map_rc_to_sot_device:
- /// map_rc_to_sot_device: [ ]
- bool readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh);
-
- /// \brief Read the control mode.
- bool readParamsControlMode(ros::NodeHandle & robot_nh);
-
- /// \brief Read the PID information of the robot in effort mode.
- bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
-
- /// \brief Read the control period.
- bool readParamsdt(ros::NodeHandle & robot_nh);
- ///@}
-
- /// \brief Fill the SoT map structures
- void fillSensorsIn(std::string &title, std::vector<double> & data);
-
- /// \brief Get the information from the low level and calls fillSensorsIn.
- void fillJoints();
-
- /// In the map sensorsIn_ creates the key "name_IMUNb"
- /// and associate to this key the vector data.
- void setSensorsImu(std::string &name,
- int IMUNb,
- std::vector<double> &data);
-
- /// @{ \name Fill the sensors
- /// Read the imus and set the interface to the SoT.
- void fillImu();
- /// Read the force sensors
- void fillForceSensors();
- /// Read the temperature sensors
- void fillTempSensors();
- /// Entry point for reading all the sensors .
- void fillSensors();
- ///@}
-
-
- /// Extract control values to send to the simulator.
- void readControl(std::map<std::string,dgs::ControlValues> &controlValues);
-
- /// Map of sensor readings
- std::map <std::string,dgs::SensorValues> sensorsIn_;
-
- /// Map of control values
- std::map<std::string,dgs::ControlValues> controlValues_;
-
- /// Control period
- double dt_;
-
- /// \brief Command send to motors
- /// Depending on control_mode it can be either
- /// position control or torque control.
- std::vector<double> command_;
-
- /// One iteration: read sensor, compute the control law, apply control.
- void one_iteration();
-
- };
-}
-
-#endif /* RC_SOT_CONTROLLER_H */
--
GitLab