Commit 4a26911e authored by Olivier Stasse's avatar Olivier Stasse

Remove talos_roscontrol_sot.

parent 63089b9f
[submodule "talos_roscontrol_sot/cmake"]
path = talos_roscontrol_sot/cmake
url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
# 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()
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.
# 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
```
Subproject commit 38d4efa72f8e3fe087eb5abc72efd51747b2271d
<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>
<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>
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
#exclude_patterns: '*/ARDroneLib/*'
INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc
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: ''
<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>
<?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>
/*
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(&current,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(&current,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(&current,0);
time_start_it_ =
((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
}
void Log::stop_it()
{
struct timeval current;
gettimeofday(&current,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();
}
}
/*
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_ */
This diff is collapsed.
///////////////////////////////////////////////////////////////////////////////
// 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<