Skip to content

GitLab

Commit ec23b4e6 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge branch 'devel' into master

parents 0385fe4e ea5153ff
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ ec23b4e6
......@@ -20,13 +20,13 @@ find_package(PkgConfig REQUIRED)
add_required_dependency(bullet)
add_required_dependency("urdfdom")
#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
pal_hardware_interfaces
controller_interface
roscpp
rospy
std_msgs
......@@ -34,8 +34,6 @@ find_package(catkin REQUIRED COMPONENTS
control_msgs
sensor_msgs
realtime_tools
controller_interface
pal_hardware_interfaces
)
## LAAS cmake submodule part
......@@ -57,12 +55,12 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
# Add dependency through jrl-cmakemodules to compile
# this code without catkin_make
add_required_dependency("pal_hardware_interfaces")
add_optional_dependency("temperature_sensor_controller")
add_required_dependency(roscpp)
add_required_dependency("realtime_tools >= 1.8")
add_required_dependency("dynamic_graph_bridge")
add_required_dependency("controller_interface")
add_required_dependency("pal_hardware_interfaces")
add_optional_dependency("temperature_sensor_controller")
add_required_dependency("pal_common_msgs")
add_required_dependency("dynamic-graph")
......@@ -113,8 +111,8 @@ target_link_libraries(rcsot_controller
${bullet_LIBRARIES}
)
pkg_config_use_dependency(rcsot_controller urdfdom)
pkg_config_use_dependency(rcsot_controller dynamic-graph)
pkg_config_use_dependency(rcsot_controller urdfdom optional NO_INCLUDE_SYSTEM)
pkg_config_use_dependency(rcsot_controller dynamic-graph optional NO_INCLUDE_SYSTEM)
## Mark executables and/or libraries for installation
install(TARGETS rcsot_controller DESTINATION lib )
......
cmake @ f34901f1
Compare 5c8c19f4...f34901f1
Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
Subproject commit f34901f143d843b48dfdb8d9e904503ed96e2310
package.xml
View file @ ec23b4e6
<?xml version="1.0"?>
<package>
<name>roscontrol_sot</name>
<version>0.0.4</version>
<version>0.0.8</version>
<description>Wrapping the Stack-of-tasks framework in ros-control</description>
<!-- Maintainer email -->
......
src/log.cpp
View file @ ec23b4e6
......@@ -9,7 +9,6 @@
#include <fstream>
#include <iomanip>
#include<ros/console.h>
using namespace std;
using namespace rc_sot_system;
......@@ -31,6 +30,7 @@ void DataToLog::init(ProfileLog &aProfileLog)
gyrometer.resize(3*aProfileLog.length);
force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
temperatures.resize(aProfileLog.nbDofs*aProfileLog.length);
controls.resize(aProfileLog.nbDofs*aProfileLog.length);
timestamp.resize(aProfileLog.length);
duration.resize(aProfileLog.length);
......@@ -80,13 +80,15 @@ void Log::record(DataToLog &aDataToLog)
StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
if (aDataToLog.temperatures.size()>JointID)
StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];
if (aDataToLog.controls.size()>JointID)
StoredData_.controls[JointID+lref_]= aDataToLog.controls[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];
}
unsigned width_pad= 6 * profileLog_.nbForceSensors;
std::size_t width_pad= 6 * profileLog_.nbForceSensors;
for(unsigned int fsID=0;fsID<profileLog_.nbForceSensors;fsID++)
{
......@@ -158,31 +160,34 @@ void Log::save(std::string &fileName)
suffix = "-temperatures.log";
saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs);
suffix = "-controls.log";
saveVector(fileName,suffix,StoredData_.controls, profileLog_.nbDofs);
suffix = "-duration.log";
saveVector(fileName,suffix,StoredData_.duration, 1);
}
inline void writeHeaderToBinaryBuffer (ofstream& of,
const unsigned int& nVector,
const unsigned int& vectorSize)
const std::size_t& nVector,
const std::size_t& vectorSize)
{
of.write ((char*)&nVector , sizeof(unsigned int));
of.write ((char*)&vectorSize, sizeof(unsigned int));
of.write ((const char*)(&nVector) , sizeof(std::size_t));
of.write ((const char*)(&vectorSize), sizeof(std::size_t));
}
inline void writeToBinaryFile (ofstream& of,
const double& t, const double& dt,
const std::vector<double>& data, const std::size_t& idx, const std::size_t& size)
{
of.write ((char*)&t , sizeof(double));
of.write ((char*)&dt , sizeof(double));
of.write ((char*)(&data[idx]), size*(sizeof(double)));
of.write ((const char*)&t , sizeof(double));
of.write ((const char*)&dt , sizeof(double));
of.write ((const char*)(&data[idx]), size*(sizeof(double)));
}
void Log::saveVector(std::string &fileName,std::string &suffix,
const std::vector<double> &avector,
unsigned int size)
std::size_t size)
{
ostringstream oss;
oss << fileName;
......
src/log.hh
View file @ ec23b4e6
......@@ -14,9 +14,9 @@ namespace rc_sot_system {
struct ProfileLog
{
unsigned int nbDofs;
unsigned int nbForceSensors;
unsigned int length;
std::size_t nbDofs;
std::size_t nbForceSensors;
std::size_t length;
};
struct DataToLog
......@@ -41,6 +41,8 @@ namespace rc_sot_system {
std::vector<double> motor_currents;
// Measured temperatures
std::vector<double> temperatures;
// Control
std::vector<double> controls;
// Timestamp
std::vector<double> timestamp;
......@@ -51,9 +53,9 @@ namespace rc_sot_system {
DataToLog();
void init(ProfileLog &aProfileLog);
unsigned int nbDofs() { return profileLog_.nbDofs;}
unsigned int nbForceSensors() { return profileLog_.nbForceSensors;}
unsigned int length() { return profileLog_.length;}
std::size_t nbDofs() { return profileLog_.nbDofs;}
std::size_t nbForceSensors() { return profileLog_.nbForceSensors;}
std::size_t length() { return profileLog_.length;}
};
......@@ -80,7 +82,7 @@ namespace rc_sot_system {
void saveVector(std::string &filename,
std::string &suffix,
const std::vector<double> &avector,
unsigned int);
std::size_t);
public:
......@@ -95,4 +97,10 @@ namespace rc_sot_system {
};
}
#pragma GCC diagnostic push
#pragma GCC system_header
#include<ros/console.h>
#pragma GCC diagnostic pop
#endif /* _RC_SOT_SYSTEM_LOG_H_ */
src/roscontrol-sot-controller.cpp
View file @ ec23b4e6
......@@ -3,7 +3,7 @@
#include <dlfcn.h>
#include <sstream>
#include <pluginlib/class_list_macros.h>
#include "roscontrol-sot-controller.hh"
#include<ros/console.h>
......@@ -57,14 +57,13 @@ using namespace rc_sot_system;
namespace sot_controller
{
typedef std::map<std::string,std::string>::iterator it_map_rt_to_sot;
EffortControlPDMotorControlData::EffortControlPDMotorControlData()
typedef std::map<std::string,std::string>::iterator it_control_mode;
ControlPDMotorControlData::ControlPDMotorControlData()
{
prev = 0.0; vel_prev = 0.0; des_pos=0.0;
integ_err=0.0;
}
void EffortControlPDMotorControlData::read_from_xmlrpc_value
void ControlPDMotorControlData::read_from_xmlrpc_value
(const std::string &prefix)
{
pid_controller.initParam(prefix);
......@@ -76,7 +75,6 @@ namespace sot_controller
// -> 124 Mo of data.
type_name_("RCSotController"),
simulation_mode_(false),
control_mode_(POSITION),
accumulated_time_(0.0),
jitter_(0.0),
verbosity_level_(0)
......@@ -127,7 +125,7 @@ namespace sot_controller
/// Initialize the size of the data to store.
/// Set temporary profileLog to one
/// because DataOneIter is just for one iteration.
unsigned tmp_length = profileLog_.length;
size_t tmp_length = profileLog_.length;
profileLog_.length = 1;
DataOneIter_.init(profileLog_);
......@@ -159,23 +157,18 @@ namespace sot_controller
/// Create SoT
SotLoaderBasic::Initialization();
/// If we are in effort mode then the device should not do any integration.
if (control_mode_==EFFORT)
/// Fill desired position during the phase where the robot is waiting.
for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
{
sotController_->setNoIntegration();
/// Fill desired position during the phase where the robot is waiting.
for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
std::string joint_name = joints_name_[idJoint];
std::map<std::string,ControlPDMotorControlData>::iterator
search_ecpd = effort_mode_pd_motors_.find(joint_name);
if (search_ecpd!=effort_mode_pd_motors_.end())
{
std::string joint_name = joints_name_[idJoint];
std::map<std::string,EffortControlPDMotorControlData>::iterator
search_ecpd = effort_mode_pd_motors_.find(joint_name);
if (search_ecpd!=effort_mode_pd_motors_.end())
{
EffortControlPDMotorControlData & ecpdcdata =
search_ecpd->second;
ecpdcdata.des_pos = joints_[idJoint].getPosition();
}
/// If we are in effort mode then the device should not do any integration.
sotController_->setNoIntegration();
}
}
return true;
......@@ -200,18 +193,27 @@ namespace sot_controller
pos_iface_ = robot_hw->get<PositionJointInterface>();
if (!pos_iface_)
{
ROS_WARN("This controller did not find a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class if it is required.",
getHardwareInterfaceType().c_str(), lns.c_str());
ROS_WARN("This controller did not find a hardware interface of type PositionJointInterface."
" Make sure this is registered in the %s::RobotHW class if it is required."
, lns.c_str());
}
// Get a pointer to the joint velocity control interface
vel_iface_ = robot_hw->get<VelocityJointInterface>();
if (!vel_iface_)
{
ROS_WARN("This controller did not find a hardware interface of type VelocityJointInterface."
" Make sure this is registered in the %s::RobotHW class if it is required."
, lns.c_str());
}
// Get a pointer to the joint effort control interface
effort_iface_ = robot_hw->get<EffortJointInterface>();
if (! effort_iface_)
{
ROS_WARN("This controller did not find a hardware interface of type '%s'."
ROS_WARN("This controller did not find a hardware interface of type EffortJointInterface."
" Make sure this is registered in the %s::RobotHW class if it is required.",
getHardwareInterfaceType().c_str(),lns.c_str());
lns.c_str());
}
// Get a pointer to the force-torque sensor interface
......@@ -263,7 +265,9 @@ namespace sot_controller
#ifdef CONTROLLER_INTERFACE_KINETIC
hardware_interface::InterfaceResources iface_res;
iface_res.hardware_interface = hardware_interface::internal::demangledTypeName<PositionJointInterface>();
iface_res.hardware_interface =
hardware_interface::internal::
demangledTypeName<PositionJointInterface>();
iface_res.resources = pos_iface_->getClaims();
claimed_resources.push_back(iface_res);
......@@ -272,7 +276,9 @@ namespace sot_controller
displayClaimedResources(claimed_resources);
pos_iface_->clearClaims();
iface_res.hardware_interface = hardware_interface::internal::demangledTypeName<EffortJointInterface>();
iface_res.hardware_interface =
hardware_interface::internal::
demangledTypeName<EffortJointInterface>();
iface_res.resources = effort_iface_->getClaims();
claimed_resources.push_back(iface_res);
if (verbosity_level_>0)
......@@ -304,12 +310,15 @@ namespace sot_controller
{
if (!initJoints())
return false;
if (!initIMU())
return false;
if (!initForceSensors())
return false;
if (!initTemperatureSensors())
return false;
if (!initIMU()) {
ROS_WARN("could not initialize IMU sensor(s).");
}
if (!initForceSensors()) {
ROS_WARN("could not initialize force sensor(s).");
}
if (!initTemperatureSensors()) {
ROS_WARN("could not initialize temperature sensor(s).");
}
// Initialize ros node.
int argc=1;
......@@ -389,25 +398,37 @@ namespace sot_controller
/// Display gain during transition control.
if (verbosity_level_>0)
ROS_INFO("/sot_controller/effort_control_pd_motor_init/gains: %d %d %d\n",
xml_rpc_ecpd_init.getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
xml_rpc_ecpd_init.getType(),
XmlRpc::XmlRpcValue::TypeArray,
XmlRpc::XmlRpcValue::TypeStruct);
effort_mode_pd_motors_.clear();
for (size_t i=0;i<joints_name_.size();i++)
{
if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
{
std::string prefix= "/sot_controller/effort_control_pd_motor_init/gains/" + joints_name_[i];
effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(prefix);
}
else
{
/// TODO: EFFORT or POSITION control actuator by actuator to make sure
/// that is the actuator is effort control, the violation of this part is
/// trigerring an error.
ROS_INFO("joint %s not in /sot_controller/effort_control_pd_motor_init/gains\n",
joints_name_[i].c_str());
}
// Check if the joint should be in ROS EFFORT mode
std::map<std::string,JointSotHandle>::iterator
search_joint_sot_handle = joints_.find(joints_name_[i]);
if (search_joint_sot_handle!=joints_.end())
{
JointSotHandle aJointSotHandle = search_joint_sot_handle->second;
if (aJointSotHandle.ros_control_mode==EFFORT)
{
// Test if PID data is present
if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
{
std::string prefix=
"/sot_controller/effort_control_pd_motor_init/gains/"
+ joints_name_[i];
effort_mode_pd_motors_[joints_name_[i]].
read_from_xmlrpc_value(prefix);
}
else
ROS_ERROR("No PID data for effort controlled joint %s in /sot_controller/effort_control_pd_motor_init/gains/",
joints_name_[i].c_str());
}
}
}
return true;
}
......@@ -415,6 +436,58 @@ namespace sot_controller
ROS_ERROR("No parameter /sot_controller/effort_controler_pd_motor_init");
return false;
}
bool RCSotController::
readParamsVelocityControlPDMotorControlData(ros::NodeHandle &robot_nh)
{
// Read libname
if (robot_nh.hasParam("/sot_controller/velocity_control_pd_motor_init/gains"))
{
XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
robot_nh.getParamCached("/sot_controller/velocity_control_pd_motor_init/gains",
xml_rpc_ecpd_init);
/// Display gain during transition control.
if (verbosity_level_>0)
ROS_INFO("/sot_controller/velocity_control_pd_motor_init/gains: %d %d %d\n",
xml_rpc_ecpd_init.getType(),
XmlRpc::XmlRpcValue::TypeArray,
XmlRpc::XmlRpcValue::TypeStruct);
velocity_mode_pd_motors_.clear();
for (size_t i=0;i<joints_name_.size();i++)
{
// Check if the joint should be in ROS VELOCITY mode
std::map<std::string,JointSotHandle>::iterator
search_joint_sot_handle = joints_.find(joints_name_[i]);
if (search_joint_sot_handle!=joints_.end())
{
JointSotHandle aJointSotHandle = search_joint_sot_handle->second;
if (aJointSotHandle.ros_control_mode==VELOCITY)
{
// Test if PID data is present
if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
{
std::string prefix=
"/sot_controller/velocity_control_pd_motor_init/gains/"
+ joints_name_[i];
velocity_mode_pd_motors_[joints_name_[i]].
read_from_xmlrpc_value(prefix);
}
else
ROS_ERROR("No PID data for velocity controlled joint %s in /sot_controller/velocity_control_pd_motor_init/gains/",
joints_name_[i].c_str());
}
}
}
return true;
}
ROS_ERROR("No parameter /sot_controller/velocity_controler_pd_motor_init");
return false;
}
bool RCSotController::
readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh)
......@@ -442,7 +515,7 @@ namespace sot_controller
}
else
{
ROS_ERROR_STREAM("Param /sot_controller/map_rc_to_sot_device does not exists !");
ROS_INFO_STREAM("Param /sot_controller/map_rc_to_sot_device does not exists !");
return false;
}
return true;
......@@ -493,35 +566,72 @@ namespace sot_controller
return true;
}
bool RCSotController::
getJointControlMode(std::string &joint_name,
JointSotHandle &aJointSotHandle)
{
std::string scontrol_mode;
static const std::string seffort("EFFORT"),svelocity("VELOCITY"),sposition("POSITION");
static const std::string ros_control_mode = "ros_control_mode";
/// Read the list of control_mode
ros::NodeHandle rnh_ns("/sot_controller/control_mode/"+joint_name);
ControlMode joint_control_mode;
if (!rnh_ns.getParam(ros_control_mode,scontrol_mode))
{
ROS_ERROR("No %s for %s - We found %s",
ros_control_mode.c_str(),
joint_name.c_str(),
scontrol_mode.c_str());
return false;
}
if (scontrol_mode==sposition)
joint_control_mode=POSITION;
else if (scontrol_mode==svelocity)
joint_control_mode=VELOCITY;
else if (scontrol_mode==seffort)
joint_control_mode=EFFORT;
else {
ROS_ERROR("%s for %s not understood. Expected %s, %s or %s. Got %s",
ros_control_mode.c_str(),
joint_name.c_str(),
sposition.c_str(),
svelocity.c_str(),
seffort.c_str(),
scontrol_mode.c_str());
return false;
}
aJointSotHandle.ros_control_mode = joint_control_mode;
//aJointSotHandle.sot_control_mode = joint_control_mode;
return true;
}
bool RCSotController::
readParamsControlMode(ros::NodeHandle &robot_nh)
{
// Read param for the list of joint names.
std::map<std::string,std::string> mapControlMode;
// Read param from control_mode.
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);
if (verbosity_level_>0)
ROS_INFO_STREAM("control mode read from param :|" << scontrol_mode<<"|");
if (scontrol_mode==seffort)
control_mode_ = EFFORT;
else if (scontrol_mode==sposition)
control_mode_ = POSITION;
else
/// For each listed joint
for(unsigned int idJoint=0;idJoint<joints_name_.size();idJoint++)
{
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;
std::string joint_name = joints_name_[idJoint];
ROS_INFO("joint_name[%d]=%s",idJoint,joint_name.c_str());
JointSotHandle &aJoint = joints_[joint_name];
if (!getJointControlMode(joint_name,aJoint))
return false;
}
}
else
ROS_INFO_STREAM("Default control mode : position");
else
{
ROS_INFO_STREAM("Default control mode : position");
}
/// Always return true;
return true;
}
......@@ -597,20 +707,20 @@ namespace sot_controller
/// Calls readParamsControlMode.
// Defines if the control mode is position or effort
readParamsControlMode(robot_nh);
if (!readParamsControlMode(robot_nh))
return false;
/// Calls readParamsFromRCToSotDevice
// Mapping from ros-controll to sot device
readParamsFromRCToSotDevice(robot_nh);
/// Get control perioud
/// Get control period
if (!readParamsdt(robot_nh))
return false;
if (control_mode_==EFFORT)
readParamsEffortControlPDMotorControlData(robot_nh);
else if (control_mode_==POSITION)
readParamsPositionControlData(robot_nh);
readParamsEffortControlPDMotorControlData(robot_nh);
readParamsVelocityControlPDMotorControlData(robot_nh);
readParamsPositionControlData(robot_nh);
return true;
}
......@@ -619,54 +729,52 @@ namespace sot_controller
initJoints()
{
// Init Joint Names.
joints_.resize(joints_name_.size());
desired_init_pose_.resize (joints_.size());
// joints_.resize(joints_name_.size());
for (unsigned int i=0;i<nbDofs_;i++)
{
bool notok=true;
SotControlMode lcontrol_mode = control_mode_;
bool failure=false;
while (notok)
{
std::string &joint_name = joints_name_[i];
try
{
if (lcontrol_mode==POSITION)
{
joints_[i] = pos_iface_->getHandle(joints_name_[i]);
JointSotHandle &aJointSotHandle = joints_[joint_name];
switch (aJointSotHandle.ros_control_mode)
{
case POSITION:
aJointSotHandle.joint = pos_iface_->getHandle(joint_name);
if (verbosity_level_>0)
ROS_INFO_STREAM("Found joint " << joints_name_[i] << " in position "
<< i << " " << joints_[i].getName());
}