Skip to content
Snippets Groups Projects
Commit 4a26911e authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Remove talos_roscontrol_sot.

parent 63089b9f
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 1764 deletions
.gitmodules
+ 0
− 3
View file @ 4a26911e
[submodule "talos_roscontrol_sot/cmake"]
path = talos_roscontrol_sot/cmake
url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
talos_roscontrol_sot/CMakeLists.txt deleted 100644 → 0
+ 0
− 125
View file @ 63089b9f
# 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()
talos_roscontrol_sot/LICENSE deleted 100644 → 0
+ 0
− 25
View file @ 63089b9f
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.
talos_roscontrol_sot/README.md deleted 100644 → 0
+ 0
− 70
View file @ 63089b9f
# 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
```
cmake @ 38d4efa7
Compare 38d4efa7...38d4efa7
Subproject commit 38d4efa72f8e3fe087eb5abc72efd51747b2271d
talos_roscontrol_sot/config/gazebo_roscontrol_sot_controller_plugins.xml deleted 100644 → 0
+ 0
− 9
View file @ 63089b9f
<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>
talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml deleted 100644 → 0
+ 0
− 9
View file @ 63089b9f
<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>
talos_roscontrol_sot/config/rosdoc.yaml deleted 100644 → 0
+ 0
− 5
View file @ 63089b9f
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
#exclude_patterns: '*/ARDroneLib/*'
talos_roscontrol_sot/doc/Doxyfile.extra.in deleted 100644 → 0
+ 0
− 3
View file @ 63089b9f
INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc
talos_roscontrol_sot/doc/manifest.yaml deleted 100644 → 0
+ 0
− 22
View file @ 63089b9f
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: ''
talos_roscontrol_sot/launch/sot_controller_gazebo.launch deleted 100644 → 0
+ 0
− 13
View file @ 63089b9f
<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>
talos_roscontrol_sot/package.xml deleted 100644 → 0
+ 0
− 53
View file @ 63089b9f
<?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>
talos_roscontrol_sot/src/log.cpp deleted 100644 → 0
+ 0
− 193
View file @ 63089b9f
/*
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();
}
}
talos_roscontrol_sot/src/log.hh deleted 100644 → 0
+ 0
− 89
View file @ 63089b9f
/*
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_ */
talos_roscontrol_sot/src/roscontrol-sot-controller.cpp deleted 100644 → 0
+ 0
− 856
View file @ 63089b9f
This diff is collapsed.
talos_roscontrol_sot/src/roscontrol-sot-controller.hh deleted 100644 → 0
+ 0
− 288
View file @ 63089b9f
///////////////////////////////////////////////////////////////////////////////
// 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 */
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment