diff --git a/.gitmodules b/.gitmodules index d74587ef4b413b9847c4e65c3e959201fe38798c..0443e7ac583310312462bce206b0e957a19fe114 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "talos_roscontrol_sot/cmake"] path = talos_roscontrol_sot/cmake - url = git@github.com:jrl-umi3218/jrl-cmakemodules.git + url = https://github.com/jrl-umi3218/jrl-cmakemodules.git diff --git a/talos_roscontrol_sot/CMakeLists.txt b/talos_roscontrol_sot/CMakeLists.txt index c2f0cb9719f03ac45ccb550a671726cd7e8db41c..a9469278a3fed5c22c5f1d0302960718a09f2f47 100644 --- a/talos_roscontrol_sot/CMakeLists.txt +++ b/talos_roscontrol_sot/CMakeLists.txt @@ -16,6 +16,9 @@ # 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) @@ -41,7 +44,7 @@ find_package(catkin REQUIRED COMPONENTS control_msgs sensor_msgs realtime_tools - talos_controller_interface + controller_interface ) ## LAAS cmake submodule part diff --git a/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml b/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml index 9388d0b06db1ad6b6646c3fd1ef03e1bf7852458..3c822810d02363fc7b640b1002b4f7345518c89f 100644 --- a/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml +++ b/talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml @@ -1,6 +1,6 @@ <library path="lib/libtalos_rcsot_controller"> - <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="talos_controller_interface::ControllerBase"> + <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> diff --git a/talos_roscontrol_sot/package.xml b/talos_roscontrol_sot/package.xml index 97832a8eb283f892bf226a2f34c72cf3a424d5df..cee404645552d90b8d234f13c7caa6540e2d9e91 100644 --- a/talos_roscontrol_sot/package.xml +++ b/talos_roscontrol_sot/package.xml @@ -30,7 +30,7 @@ <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>controller_interface</build_depend> <build_depend>cmake_modules</build_depend> <build_depend>dynamic_graph_bridge</build_depend> @@ -40,12 +40,12 @@ <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>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"/> + <controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/> </export> </package> diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp index 8af32fa9424af0b42ced0e960b86f715f95d6267..29c65a13cb844e3ff1ef496153fe2d15ffde5c3f 100644 --- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp +++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp @@ -35,7 +35,7 @@ #define ODEBUG4FULL(x) #define ODEBUG4(x) -using namespace talos_hardware_interface; +using namespace hardware_interface; using namespace rc_sot_system; namespace talos_sot_controller @@ -68,7 +68,7 @@ namespace talos_sot_controller } bool RCSotController:: - initRequest (talos_hardware_interface::RobotHW * robot_hw, + initRequest (hardware_interface::RobotHW * robot_hw, ros::NodeHandle &robot_nh, ros::NodeHandle &controller_nh, std::set<std::string> & claimed_resources) @@ -91,7 +91,7 @@ namespace talos_sot_controller } bool RCSotController:: - initInterfaces(talos_hardware_interface::RobotHW * robot_hw, + initInterfaces(hardware_interface::RobotHW * robot_hw, ros::NodeHandle &, ros::NodeHandle &, std::set<std::string> & claimed_resources) @@ -107,7 +107,7 @@ namespace talos_sot_controller 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.", + " Make sure this is registered in the hardware_interface::RobotHW class.", getHardwareInterfaceType().c_str()); return false ; } @@ -117,7 +117,7 @@ namespace talos_sot_controller 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.", + " Make sure this is registered in the hardware_interface::RobotHW class.", getHardwareInterfaceType().c_str()); return false ; } @@ -127,7 +127,7 @@ namespace talos_sot_controller 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.", + " Make sure this is registered inthe hardware_interface::RobotHW class.", internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str()); return false ; } @@ -136,7 +136,7 @@ namespace talos_sot_controller 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.", + " Make sure this is registered in the thardware_interface::RobotHW class.", internal :: demangledTypeName<ImuSensorInterface>().c_str()); return false ; } @@ -149,7 +149,7 @@ namespace talos_sot_controller 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.", + " Make sure this is registered in the hardware_interface::RobotHW class.", internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str()); return false ; } @@ -491,6 +491,8 @@ namespace talos_sot_controller fillSensorsIn(ltitle,DataOneIter_.velocities); ltitle = "torques"; fillSensorsIn(ltitle,DataOneIter_.torques); + ltitle = "currents"; + fillSensorsIn(ltitle,DataOneIter_.motor_currents); } @@ -623,7 +625,6 @@ namespace talos_sot_controller void RCSotController::one_iteration() { - /// Update the sensors. fillSensors(); @@ -667,14 +668,16 @@ namespace talos_sot_controller { //return type_name_; if (control_mode_==POSITION) - return talos_hardware_interface::internal:: - demangledTypeName<talos_hardware_interface::PositionJointInterface>(); + return hardware_interface::internal:: + demangledTypeName<hardware_interface::PositionJointInterface>(); else if (control_mode_==EFFORT) - return talos_hardware_interface::internal:: - demangledTypeName<talos_hardware_interface::EffortJointInterface>(); + return hardware_interface::internal:: + demangledTypeName<hardware_interface::EffortJointInterface>(); + std::string voidstring(""); + return voidstring; } PLUGINLIB_EXPORT_CLASS(talos_sot_controller::RCSotController, - talos_controller_interface::ControllerBase); + controller_interface::ControllerBase); } diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh index addf29eceaf00fa270cd9047e1ad21d1109ffdf4..0280d88184a34fae88bc81b92f9ce03fcec58962 100644 --- a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh +++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh @@ -35,11 +35,11 @@ #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 <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> #include <dynamic_graph_bridge/sot_loader_basic.hh> @@ -53,7 +53,7 @@ namespace talos_sot_controller This class encapsulates the Stack of Tasks inside the ros-control infra-structure. */ - class RCSotController : public talos_controller_interface::ControllerBase, + class RCSotController : public controller_interface::ControllerBase, SotLoaderBasic { @@ -68,33 +68,33 @@ namespace talos_sot_controller /// @{ \name Ros-control related fields /// \brief Vector of joint handles. - std::vector<talos_hardware_interface::JointHandle> joints_; + std::vector<hardware_interface::JointHandle> joints_; std::vector<std::string> joints_name_; /// \brief Vector towards the IMU. - std::vector<talos_hardware_interface::ImuSensorHandle> imu_sensor_; + std::vector<hardware_interface::ImuSensorHandle> imu_sensor_; /// \brief Vector of 6D force sensor. - std::vector<talos_hardware_interface::ForceTorqueSensorHandle> ft_sensors_; + std::vector<hardware_interface::ForceTorqueSensorHandle> ft_sensors_; /// \brief Vector of temperature sensors for the actuators. - std::vector<talos_hardware_interface::ActuatorTemperatureSensorHandle> + std::vector<hardware_interface::ActuatorTemperatureSensorHandle> act_temp_sensors_; /// \brief Interface to the joints controlled in position. - talos_hardware_interface::PositionJointInterface * pos_iface_; + hardware_interface::PositionJointInterface * pos_iface_; /// \brief Interface to the joints controlled in force. - talos_hardware_interface::EffortJointInterface * effort_iface_; + hardware_interface::EffortJointInterface * effort_iface_; /// \brief Interface to the sensors (IMU). - talos_hardware_interface::ImuSensorInterface* imu_iface_; + hardware_interface::ImuSensorInterface* imu_iface_; /// \brief Interface to the sensors (Force). - talos_hardware_interface::ForceTorqueSensorInterface* ft_iface_; + hardware_interface::ForceTorqueSensorInterface* ft_iface_; /// \brief Interface to the actuator temperature sensor. - talos_hardware_interface::ActuatorTemperatureSensorInterface * act_temp_iface_; + hardware_interface::ActuatorTemperatureSensorInterface * act_temp_iface_; /// @} @@ -127,7 +127,7 @@ namespace talos_sot_controller /// \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, + bool initRequest (hardware_interface::RobotHW * robot_hw, ros::NodeHandle &robot_nh, ros::NodeHandle &controller_nh, std::set<std::string> & claimed_resources); @@ -150,7 +150,7 @@ namespace talos_sot_controller protected: /// Initialize the roscontrol interfaces - bool initInterfaces(talos_hardware_interface::RobotHW * robot_hw, + bool initInterfaces(hardware_interface::RobotHW * robot_hw, ros::NodeHandle &, ros::NodeHandle &, std::set<std::string> & claimed_resources); diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ccdef5fc426b4015c440a31905df2a00d24471dc --- /dev/null +++ b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml @@ -0,0 +1,12 @@ +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, + 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, + head_1_joint, head_2_joint ] + map_rc_to_sot_device: { motor-angles: motor-angles , + joint-angles: joint-angles, velocities: velocities, + torques: torques, cmd-joints: control, cmd-effort: control } + control_mode: EFFORT diff --git a/talos_roscontrol_sot_talos/launch/sot_talos_controller.launch b/talos_roscontrol_sot_talos/launch/sot_talos_controller.launch index e3a23389aa04d3d4ba0b6c7adfe2e4f99f52788a..76e60ab35ed661a88d1e4dce744115502f9bca66 100644 --- a/talos_roscontrol_sot_talos/launch/sot_talos_controller.launch +++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller.launch @@ -6,7 +6,7 @@ <!-- Spawn walking controller --> <node name="sot_controller_spawner" - pkg="talos_controller_manager" type="spawner" output="screen" + pkg="controller_manager" type="spawner" output="screen" args="sot_controller" /> </launch> diff --git a/talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch b/talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch new file mode 100644 index 0000000000000000000000000000000000000000..7343f9ea6243df2a877167781d26edf917161134 --- /dev/null +++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch @@ -0,0 +1,13 @@ +<launch> + + <!-- Sot Controller configuration --> + <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_effort.yaml"/> + <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" /> + + <!-- Spawn walking controller --> + <node name="sot_controller_spawner" + pkg="controller_manager" type="spawner" output="screen" + args="sot_controller" /> + +</launch> +