Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/talos-metapkg-ros-control-sot
  • stack-of-tasks/talos-metapkg-ros-control-sot
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 1334 deletions
talos_roscontrol_sot/doc/Doxyfile.extra.in deleted 100644 → 0
View file @ 0eed1085
INPUT = @PROJECT_SOURCE_DIR@/include \
@PROJECT_SOURCE_DIR@/doc
talos_roscontrol_sot/doc/manifest.yaml deleted 100644 → 0
View file @ 0eed1085
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
View file @ 0eed1085
<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
View file @ 0eed1085
<?xml version="1.0"?>
<package>
<name>talos_roscontrol_sot</name>
<version>0.0.2</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>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>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/roscontrol_sot_controller_plugins.xml"/>
</export>
</package>
talos_roscontrol_sot/src/log.cpp deleted 100644 → 0
View file @ 0eed1085
/*
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>
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);
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_;
lref_ += nbDofs_;
lrefts_ ++;
if (lref_>=nbDofs_*length_)
{
lref_=0;
lrefts_=0;
}
}
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_);
}
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());
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
View file @ 0eed1085
/*
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;
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_;
// 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);
};
}
#endif /* _RC_SOT_SYSTEM_LOG_H_ */
talos_roscontrol_sot/src/roscontrol-sot-controller.cpp deleted 100644 → 0
View file @ 0eed1085
#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 RESERDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
using namespace talos_hardware_interface;
using namespace rc_sot_system;
namespace talos_sot_controller
{
typedef std::map<std::string,std::string>::iterator it_map_rt_to_sot;
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)
{
}
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 (talos_hardware_interface::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();
return true;
}
bool RCSotController::
initInterfaces(talos_hardware_interface::RobotHW * robot_hw,
ros::NodeHandle &,
ros::NodeHandle &,
std::set<std::string> & claimed_resources)
{
// 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 talos_hardware_interface::RobotHW class.",
getHardwareInterfaceType().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 talos_hardware_interface::RobotHW class.",
getHardwareInterfaceType().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 talos_hardware_interface::RobotHW class.",
internal :: demangledTypeName<ForceTorqueSensorInterface>().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 talos_hardware_interface::RobotHW class.",
internal :: demangledTypeName<ImuSensorInterface>().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 talos_hardware_interface::RobotHW class.",
internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().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::
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::
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);
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);
}
void RCSotController::setSensorsImu(std::string &name,
int IMUnb,
std::vector<double> & data)
{
std::ostringstream labelOss(name);
labelOss << 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
cmdTitle="cmd-torques";
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()
{
/// 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_);
/// Store everything in Log.
RcSotLog.record(DataOneIter_);
}
void RCSotController::
update(const ros::Time&, const ros::Duration& )
{
if (!isDynamicGraphStopped())
one_iteration();
}
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_;
return talos_hardware_interface::internal::demangledTypeName<talos_hardware_interface::PositionJointInterface>();
}
PLUGINLIB_EXPORT_CLASS(talos_sot_controller::RCSotController,
talos_controller_interface::ControllerBase);
}
talos_roscontrol_sot/src/roscontrol-sot-controller.hh deleted 100644 → 0
View file @ 0eed1085
///////////////////////////////////////////////////////////////////////////////
// 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>
#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>
#include <dynamic_graph_bridge/sot_loader_basic.hh>
#include "log.hh"
namespace talos_sot_controller
{
enum SotControlMode { POSITION, EFFORT};
/**
This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
*/
class RCSotController : public talos_controller_interface::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<talos_hardware_interface::JointHandle> joints_;
std::vector<std::string> joints_name_;
/// \brief Vector towards the IMU.
std::vector<talos_hardware_interface::ImuSensorHandle> imu_sensor_;
/// \brief Vector of 6D force sensor.
std::vector<talos_hardware_interface::ForceTorqueSensorHandle> ft_sensors_;
/// \brief Vector of temperature sensors for the actuators.
std::vector<talos_hardware_interface::ActuatorTemperatureSensorHandle>
act_temp_sensors_;
/// \brief Interface to the joints controlled in position.
talos_hardware_interface::PositionJointInterface * pos_iface_;
/// \brief Interface to the joints controlled in force.
talos_hardware_interface::EffortJointInterface * effort_iface_;
/// \brief Interface to the sensors (IMU).
talos_hardware_interface::ImuSensorInterface* imu_iface_;
/// \brief Interface to the sensors (Force).
talos_hardware_interface::ForceTorqueSensorInterface* ft_iface_;
/// \brief Interface to the actuator temperature sensor.
talos_hardware_interface::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 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;
public :
RCSotController ();
/// \brief Read the configuration files,
/// claims the request to the robot and initialize the Stack-Of-Tasks.
bool initRequest (talos_hardware_interface::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(talos_hardware_interface::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 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_;
/// \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 */
talos_roscontrol_sot_talos/CMakeLists.txt deleted 100644 → 0
View file @ 0eed1085
cmake_minimum_required(VERSION 2.8.3)
project(talos_roscontrol_sot_talos)
find_package(PkgConfig REQUIRED)
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
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
dynamic_graph_bridge
control_msgs
sensor_msgs
realtime_tools
controller_interface
)
include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
link_directories(${bullet_LIBRARY_DIRS})
catkin_package()
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(roscontrol_sot_talos_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
foreach(dir config launch)
install(DIRECTORY ${dir}
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
endforeach()
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_roscontrol_sot_talos.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
talos_roscontrol_sot_talos/config/sot_talos_params.yaml deleted 100644 → 0
View file @ 0eed1085
sot_controller:
libname: libsot-pyrene-controller.so
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
]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities,
torques: torques, cmd-positions: joints, cmd-effort: effort }
control_mode: EFFORT