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

Merge branch 'devel' into master

parents 0385fe4e ea5153ff
...@@ -20,13 +20,13 @@ find_package(PkgConfig REQUIRED) ...@@ -20,13 +20,13 @@ find_package(PkgConfig REQUIRED)
add_required_dependency(bullet) add_required_dependency(bullet)
add_required_dependency("urdfdom") add_required_dependency("urdfdom")
#set(bullet_FOUND 0)
#pkg_check_modules(bullet REQUIRED bullet)
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
pal_hardware_interfaces
controller_interface
roscpp roscpp
rospy rospy
std_msgs std_msgs
...@@ -34,8 +34,6 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -34,8 +34,6 @@ find_package(catkin REQUIRED COMPONENTS
control_msgs control_msgs
sensor_msgs sensor_msgs
realtime_tools realtime_tools
controller_interface
pal_hardware_interfaces
) )
## LAAS cmake submodule part ## LAAS cmake submodule part
...@@ -57,12 +55,12 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}") ...@@ -57,12 +55,12 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
# Add dependency through jrl-cmakemodules to compile # Add dependency through jrl-cmakemodules to compile
# this code without catkin_make # this code without catkin_make
add_required_dependency("pal_hardware_interfaces")
add_optional_dependency("temperature_sensor_controller")
add_required_dependency(roscpp) add_required_dependency(roscpp)
add_required_dependency("realtime_tools >= 1.8") add_required_dependency("realtime_tools >= 1.8")
add_required_dependency("dynamic_graph_bridge") add_required_dependency("dynamic_graph_bridge")
add_required_dependency("controller_interface") 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("pal_common_msgs")
add_required_dependency("dynamic-graph") add_required_dependency("dynamic-graph")
...@@ -113,8 +111,8 @@ target_link_libraries(rcsot_controller ...@@ -113,8 +111,8 @@ target_link_libraries(rcsot_controller
${bullet_LIBRARIES} ${bullet_LIBRARIES}
) )
pkg_config_use_dependency(rcsot_controller urdfdom) pkg_config_use_dependency(rcsot_controller urdfdom optional NO_INCLUDE_SYSTEM)
pkg_config_use_dependency(rcsot_controller dynamic-graph) pkg_config_use_dependency(rcsot_controller dynamic-graph optional NO_INCLUDE_SYSTEM)
## Mark executables and/or libraries for installation ## Mark executables and/or libraries for installation
install(TARGETS rcsot_controller DESTINATION lib ) install(TARGETS rcsot_controller DESTINATION lib )
......
Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f Subproject commit f34901f143d843b48dfdb8d9e904503ed96e2310
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package>
<name>roscontrol_sot</name> <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> <description>Wrapping the Stack-of-tasks framework in ros-control</description>
<!-- Maintainer email --> <!-- Maintainer email -->
......
...@@ -9,7 +9,6 @@ ...@@ -9,7 +9,6 @@
#include <fstream> #include <fstream>
#include <iomanip> #include <iomanip>
#include<ros/console.h>
using namespace std; using namespace std;
using namespace rc_sot_system; using namespace rc_sot_system;
...@@ -31,6 +30,7 @@ void DataToLog::init(ProfileLog &aProfileLog) ...@@ -31,6 +30,7 @@ void DataToLog::init(ProfileLog &aProfileLog)
gyrometer.resize(3*aProfileLog.length); gyrometer.resize(3*aProfileLog.length);
force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length); force_sensors.resize(aProfileLog.nbForceSensors*6*aProfileLog.length);
temperatures.resize(aProfileLog.nbDofs*aProfileLog.length); temperatures.resize(aProfileLog.nbDofs*aProfileLog.length);
controls.resize(aProfileLog.nbDofs*aProfileLog.length);
timestamp.resize(aProfileLog.length); timestamp.resize(aProfileLog.length);
duration.resize(aProfileLog.length); duration.resize(aProfileLog.length);
...@@ -80,13 +80,15 @@ void Log::record(DataToLog &aDataToLog) ...@@ -80,13 +80,15 @@ void Log::record(DataToLog &aDataToLog)
StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID]; StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
if (aDataToLog.temperatures.size()>JointID) if (aDataToLog.temperatures.size()>JointID)
StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[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++) for(unsigned int axis=0;axis<3;axis++)
{ {
StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis]; StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];
StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[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++) for(unsigned int fsID=0;fsID<profileLog_.nbForceSensors;fsID++)
{ {
...@@ -158,31 +160,34 @@ void Log::save(std::string &fileName) ...@@ -158,31 +160,34 @@ void Log::save(std::string &fileName)
suffix = "-temperatures.log"; suffix = "-temperatures.log";
saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs); saveVector(fileName,suffix,StoredData_.temperatures, profileLog_.nbDofs);
suffix = "-controls.log";
saveVector(fileName,suffix,StoredData_.controls, profileLog_.nbDofs);
suffix = "-duration.log"; suffix = "-duration.log";
saveVector(fileName,suffix,StoredData_.duration, 1); saveVector(fileName,suffix,StoredData_.duration, 1);
} }
inline void writeHeaderToBinaryBuffer (ofstream& of, inline void writeHeaderToBinaryBuffer (ofstream& of,
const unsigned int& nVector, const std::size_t& nVector,
const unsigned int& vectorSize) const std::size_t& vectorSize)
{ {
of.write ((char*)&nVector , sizeof(unsigned int)); of.write ((const char*)(&nVector) , sizeof(std::size_t));
of.write ((char*)&vectorSize, sizeof(unsigned int)); of.write ((const char*)(&vectorSize), sizeof(std::size_t));
} }
inline void writeToBinaryFile (ofstream& of, inline void writeToBinaryFile (ofstream& of,
const double& t, const double& dt, const double& t, const double& dt,
const std::vector<double>& data, const std::size_t& idx, const std::size_t& size) const std::vector<double>& data, const std::size_t& idx, const std::size_t& size)
{ {
of.write ((char*)&t , sizeof(double)); of.write ((const char*)&t , sizeof(double));
of.write ((char*)&dt , sizeof(double)); of.write ((const char*)&dt , sizeof(double));
of.write ((char*)(&data[idx]), size*(sizeof(double))); of.write ((const char*)(&data[idx]), size*(sizeof(double)));
} }
void Log::saveVector(std::string &fileName,std::string &suffix, void Log::saveVector(std::string &fileName,std::string &suffix,
const std::vector<double> &avector, const std::vector<double> &avector,
unsigned int size) std::size_t size)
{ {
ostringstream oss; ostringstream oss;
oss << fileName; oss << fileName;
......
...@@ -14,9 +14,9 @@ namespace rc_sot_system { ...@@ -14,9 +14,9 @@ namespace rc_sot_system {
struct ProfileLog struct ProfileLog
{ {
unsigned int nbDofs; std::size_t nbDofs;
unsigned int nbForceSensors; std::size_t nbForceSensors;
unsigned int length; std::size_t length;
}; };
struct DataToLog struct DataToLog
...@@ -41,6 +41,8 @@ namespace rc_sot_system { ...@@ -41,6 +41,8 @@ namespace rc_sot_system {
std::vector<double> motor_currents; std::vector<double> motor_currents;
// Measured temperatures // Measured temperatures
std::vector<double> temperatures; std::vector<double> temperatures;
// Control
std::vector<double> controls;
// Timestamp // Timestamp
std::vector<double> timestamp; std::vector<double> timestamp;
...@@ -51,9 +53,9 @@ namespace rc_sot_system { ...@@ -51,9 +53,9 @@ namespace rc_sot_system {
DataToLog(); DataToLog();
void init(ProfileLog &aProfileLog); void init(ProfileLog &aProfileLog);
unsigned int nbDofs() { return profileLog_.nbDofs;} std::size_t nbDofs() { return profileLog_.nbDofs;}
unsigned int nbForceSensors() { return profileLog_.nbForceSensors;} std::size_t nbForceSensors() { return profileLog_.nbForceSensors;}
unsigned int length() { return profileLog_.length;} std::size_t length() { return profileLog_.length;}
}; };
...@@ -80,7 +82,7 @@ namespace rc_sot_system { ...@@ -80,7 +82,7 @@ namespace rc_sot_system {
void saveVector(std::string &filename, void saveVector(std::string &filename,
std::string &suffix, std::string &suffix,
const std::vector<double> &avector, const std::vector<double> &avector,
unsigned int); std::size_t);
public: public:
...@@ -95,4 +97,10 @@ namespace rc_sot_system { ...@@ -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_ */ #endif /* _RC_SOT_SYSTEM_LOG_H_ */
This diff is collapsed.
...@@ -8,11 +8,15 @@ ...@@ -8,11 +8,15 @@
#include <string> #include <string>
#include <map> #include <map>
#pragma GCC diagnostic push
#pragma GCC system_header
#include <controller_interface/controller.h> #include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h> #include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/imu_sensor_interface.h> #include <hardware_interface/imu_sensor_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h> #include <hardware_interface/force_torque_sensor_interface.h>
#include <pal_hardware_interfaces/actuator_temperature_interface.h> #include <pal_hardware_interfaces/actuator_temperature_interface.h>
#include <pluginlib/class_list_macros.h>
#pragma GCC diagnostic pop
#include <dynamic_graph_bridge/sot_loader_basic.hh> #include <dynamic_graph_bridge/sot_loader_basic.hh>
#include <ros/ros.h> #include <ros/ros.h>
...@@ -26,7 +30,9 @@ ...@@ -26,7 +30,9 @@
namespace sot_controller namespace sot_controller
{ {
enum SotControlMode { POSITION, EFFORT}; enum ControlMode { POSITION, VELOCITY, EFFORT};
namespace lhi = hardware_interface;
namespace lci = controller_interface;
class XmlrpcHelperException : public ros::Exception class XmlrpcHelperException : public ros::Exception
{ {
...@@ -36,21 +42,27 @@ namespace sot_controller ...@@ -36,21 +42,27 @@ namespace sot_controller
}; };
struct EffortControlPDMotorControlData struct ControlPDMotorControlData
{ {
control_toolbox::Pid pid_controller; control_toolbox::Pid pid_controller;
//double p_gain,d_gain,i_gain; ControlPDMotorControlData();
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(XmlRpc::XmlRpcValue &aXRV);
void read_from_xmlrpc_value(const std::string &prefix); void read_from_xmlrpc_value(const std::string &prefix);
}; };
struct JointSotHandle
{
lhi::JointHandle joint;
double desired_init_pose;
// This should not be handled in roscontrol_sot package. The control type
// should be handled in SoT directly, by externalizing the integration from
// the Device.
//ControlMode sot_control_mode;
ControlMode ros_control_mode;
};
typedef std::map<std::string,JointSotHandle>::iterator it_joint_sot_h;
#ifndef CONTROLLER_INTERFACE_KINETIC #ifndef CONTROLLER_INTERFACE_KINETIC
typedef std::set<std::string> ClaimedResources; typedef std::set<std::string> ClaimedResources;
#endif #endif
...@@ -58,9 +70,6 @@ namespace sot_controller ...@@ -58,9 +70,6 @@ namespace sot_controller
This class encapsulates the Stack of Tasks inside the ros-control infra-structure. This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
*/ */
namespace lhi = hardware_interface;
namespace lci = controller_interface;
class RCSotController : public lci::ControllerBase, class RCSotController : public lci::ControllerBase,
SotLoaderBasic SotLoaderBasic
{ {
...@@ -76,7 +85,7 @@ namespace sot_controller ...@@ -76,7 +85,7 @@ namespace sot_controller
/// @{ \name Ros-control related fields /// @{ \name Ros-control related fields
/// \brief Vector of joint handles. /// \brief Vector of joint handles.
std::vector<lhi::JointHandle> joints_; std::map<std::string,JointSotHandle> joints_;
std::vector<std::string> joints_name_; std::vector<std::string> joints_name_;
/// \brief Vector towards the IMU. /// \brief Vector towards the IMU.
...@@ -94,6 +103,9 @@ namespace sot_controller ...@@ -94,6 +103,9 @@ namespace sot_controller
/// \brief Interface to the joints controlled in position. /// \brief Interface to the joints controlled in position.
lhi::PositionJointInterface * pos_iface_; lhi::PositionJointInterface * pos_iface_;
/// \brief Interface to the joints controlled in position.
lhi::VelocityJointInterface * vel_iface_;
/// \brief Interface to the joints controlled in force. /// \brief Interface to the joints controlled in force.
lhi::EffortJointInterface * effort_iface_; lhi::EffortJointInterface * effort_iface_;
...@@ -118,15 +130,13 @@ namespace sot_controller ...@@ -118,15 +130,13 @@ namespace sot_controller
/// \brief Adapt the interface to Gazebo simulation /// \brief Adapt the interface to Gazebo simulation
bool simulation_mode_; 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 /// \brief Implement a PD controller for the robot when the dynamic graph
/// is not on. /// is not on.
std::map<std::string, EffortControlPDMotorControlData> effort_mode_pd_motors_; std::map<std::string, ControlPDMotorControlData> effort_mode_pd_motors_;
/// \brief Give the desired position when the dynamic graph is not on. /// \brief Implement a PD controller for the robot when the dynamic graph
std::vector<double> desired_init_pose_; /// is not on.
std::map<std::string, ControlPDMotorControlData> velocity_mode_pd_motors_;
/// \brief Map from ros-control quantities to robot device /// \brief Map from ros-control quantities to robot device
/// ros-control quantities are for the sensors: /// ros-control quantities are for the sensors:
...@@ -179,8 +189,6 @@ namespace sot_controller ...@@ -179,8 +189,6 @@ namespace sot_controller
void starting(const ros::Time&); void starting(const ros::Time&);
/// \brief Stopping the control /// \brief Stopping the control
void stopping(const ros::Time&); void stopping(const ros::Time&);
/// \brief Display the kind of hardware interface that this controller is using.
virtual std::string getHardwareInterfaceType() const;
protected: protected:
/// Initialize the roscontrol interfaces /// Initialize the roscontrol interfaces
...@@ -220,6 +228,9 @@ namespace sot_controller ...@@ -220,6 +228,9 @@ namespace sot_controller
/// \brief Read the control mode. /// \brief Read the control mode.
bool readParamsControlMode(ros::NodeHandle & robot_nh); bool readParamsControlMode(ros::NodeHandle & robot_nh);
/// \brief Read the PID information of the robot in velocity mode.
bool readParamsVelocityControlPDMotorControlData(ros::NodeHandle &robot_nh);
/// \brief Read the PID information of the robot in effort mode. /// \brief Read the PID information of the robot in effort mode.
bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh); bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
...@@ -259,6 +270,8 @@ namespace sot_controller ...@@ -259,6 +270,8 @@ namespace sot_controller
///@{ Control the robot while waiting for the SoT ///@{ Control the robot while waiting for the SoT
/// Default control in effort. /// Default control in effort.
void localStandbyEffortControlMode(const ros::Duration& period); void localStandbyEffortControlMode(const ros::Duration& period);
/// Default control in velocity.
void localStandbyVelocityControlMode(const ros::Duration& period);
/// Default control in position. /// Default control in position.
void localStandbyPositionControlMode(); void localStandbyPositionControlMode();
...@@ -285,6 +298,13 @@ namespace sot_controller ...@@ -285,6 +298,13 @@ namespace sot_controller
/// Read URDF model from /robot_description parameter. /// Read URDF model from /robot_description parameter.
bool readUrdf(ros::NodeHandle &robot_nh); bool readUrdf(ros::NodeHandle &robot_nh);
/// Returns control mode by reading rosparam.
/// It reads /sot_controller/control_mode/joint_name
/// and check
bool
getJointControlMode(std::string &joint_name,
JointSotHandle &aJointSotHandle);
}; };
} }
......
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
#include <iomanip> #include <iomanip>
#include <cassert>
#include <cstring>
void usage (char* bin)
{
std::cerr << "Usage: " << bin << " [--separator sep] binary_file_name\n";
}
int main (int argc, char* argv[]) int main (int argc, char* argv[])
{ {
if (argc != 2) { if (argc < 2) {
std::cerr << "Usage: " << argv[0] << " binary_file_name\n"; usage(argv[0]);
return 1; return 1;
} }
int iarg = 1;
std::string sep (" ");
while (iarg + 1 < argc - 1) {
if (strcmp(argv[iarg], "--separator") == 0) {
sep = argv[iarg+1];
iarg += 2;
} else {
usage(argv[0]);
return 1;
}
}
assert (iarg == argc-1);
std::ifstream in (argv[1], std::ios::binary); std::ifstream in (argv[iarg], std::ios::binary);
if (!in.is_open() || !in.good()) { if (!in.is_open() || !in.good()) {
std::cerr << "Couldn't open file " << argv[1] << '\n'; std::cerr << "Couldn't open file " << argv[iarg] << '\n';
return 2; return 2;
} }
// Read headers // Read headers
unsigned int nVector = 0, vectorSize = 0; std::size_t nVector = 0, vectorSize = 0;
in.read ((char*)&nVector , sizeof(unsigned int)); in.read ((char*)&nVector , sizeof(std::size_t));
in.read ((char*)&vectorSize, sizeof(unsigned int)); in.read ((char*)&vectorSize, sizeof(std::size_t));
if (!in.good()) { if (!in.good()) {
std::cerr << "Couldn't parse file: " << argv[1] << '\n'; std::cerr << "Couldn't parse file: " << argv[iarg] << '\n';
return 3; return 3;
} }
...@@ -33,10 +52,10 @@ int main (int argc, char* argv[]) ...@@ -33,10 +52,10 @@ int main (int argc, char* argv[])
in.read ((char*)&v, sizeof(double)); in.read ((char*)&v, sizeof(double));
if (!in.good()) { if (!in.good()) {
std::cerr << "Stopped to parse at (" << i << ',' << j std::cerr << "Stopped to parse at (" << i << ',' << j
<< ") of file: " << argv[1] << '\n'; << ") of file: " << argv[iarg] << '\n';
return 4; return 4;
} }
std::cout << v << ' '; std::cout << v << sep;
} }
std::cout << '\n'; std::cout << '\n';
} }
......
Markdown is supported
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