Commit ee5f5eb6 authored by Olivier Stasse's avatar Olivier Stasse

Removing namespace with talos_ prefix.

parent 1db67142
[submodule "talos_roscontrol_sot/cmake"]
path = talos_roscontrol_sot/cmake
url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
cmake_minimum_required(VERSION 2.8.3)
project(talos_roscontrol_sot_talos)
project(roscontrol_sot_talos)
find_package(PkgConfig REQUIRED)
......@@ -17,7 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
control_msgs
sensor_msgs
realtime_tools
talos_controller_interface
controller_interface
)
include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
......@@ -35,7 +35,7 @@ catkin_package()
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
)
## Add cmake target dependencies of the executable
......
sot_controller:
type: talos_sot_controller/RCSotController
type: sot_controller/RCSotController
joints:
- arm_left_1_joint
- arm_left_2_joint
......
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_effort.yaml"/>
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/pids.yaml" />
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_effort.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/pids.yaml" />
<env name="PYTHONPATH" value="/opt/pal/dubnium/lib/python2.7/dist-packages:/opt/ros/indigo/lib/python2.7/dist-packages:/opt/openrobots/lib/python2.7/dist-packages:/opt/openrobots/lib/python2.7/site-packages" />
<!-- Spawn walking controller -->
......
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo.yaml"/>
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_gazebo.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<!-- Spawn walking controller -->
<node name="sot_controller_spawner"
pkg="talos_controller_manager" type="spawner" output="screen"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
......
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
<rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find talos_hardware_gazebo)/config/pids.yaml"/>
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
<rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find hardware_gazebo)/config/pids.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<!-- Spawn sot controller -->
<node name="sot_controller_spawner"
pkg="talos_controller_manager" type="spawner" output="screen"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
......
<?xml version="1.0"?>
<package>
<name>talos_roscontrol_sot_talos</name>
<version>0.0.7</version>
<name>roscontrol_sot_talos</name>
<version>0.0.8</version>
<description>Wrapping Stack-of-tasks for Talos in ros-control</description>
<!-- Maintainer email -->
......@@ -41,7 +41,8 @@
<build_depend>sensor_msgs</build_depend>
<build_depend>realtime_tools</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>talos_roscontrol_sot</build_depend>
<build_depend>roscontrol_sot</build_depend>
<build_depend>controller_interface</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>control_msgs</run_depend>
......@@ -50,6 +51,7 @@
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>cmake_modules</run_depend>
<run_depend>talos_roscontrol_sot</run_depend>
<run_depend>roscontrol_sot</run_depend>
<run_depend>controller_interface</run_depend>
<!-- The export tag contains other, unspecified, tags -->
</package>
<package>
<name>sot_pyrene_bringup</name>
<version>0.0.7</version>
<version>0.0.8</version>
<description>ROS package for Stack of Tasks on Pyrene</description>
<maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
......
<package>
<name>talos_metapkg_ros_control_sot</name>
<version>0.0.7</version>
<version>0.0.8</version>
<description>A set of packages that include sot-wrapper and talos sot params .</description>
<maintainer email="olivier.stasse@laas.fr">Olivier Stasse</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/stack-of-tasks/roscontrol_sot</url>
<url type="repository">https://github.com/stack-of-tasks/talos_metapkg_roscontrol_sot</url>
<author>Olivier Stasse</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>talos_roscontrol_sot</run_depend>
<run_depend>talos_roscontrol_sot_talos</run_depend>
<run_depend>roscontrol_sot</run_depend>
<run_depend>roscontrol_sot_talos</run_depend>
<run_depend>realtime_tools</run_depend>
<run_depend>control_toolbox</run_depend>
......
# 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" FALSE)
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
)
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>talos_controller_interface</build_depend>
<build_depend>talos_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>talos_controller_interface</run_depend>
<run_depend>talos_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>
<talos_controller_interface plugin="${prefix}/config/gazebo_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);
gettimeofday(timeorigin_tv_,0);
timeorigin_ = (double)timeorigin_tv_.tv_sec +
0.000001 * ((double)timeorigin_tv_.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 - time_origin_tv_.tv_sec)
+ 0.000001 * (double)(current.tv_usec - time_origin_tv_.tv_usec));
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>
#include <sys/time.h>