From cc57de80b97e0f8cb73d98346a4e205f6e6ae894 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 10 Oct 2017 10:41:59 +0200
Subject: [PATCH] [talos_roscontrol_sot/talos_roscontrol_sot_talos] First
successfull test of current control on the robot.
---
.gitmodules | 2 +-
talos_roscontrol_sot/CMakeLists.txt | 5 ++-
.../roscontrol_sot_controller_plugins.xml | 2 +-
talos_roscontrol_sot/package.xml | 6 ++--
.../src/roscontrol-sot-controller.cpp | 31 +++++++++--------
.../src/roscontrol-sot-controller.hh | 34 +++++++++----------
.../config/sot_talos_params_effort.yaml | 12 +++++++
.../launch/sot_talos_controller.launch | 2 +-
.../launch/sot_talos_controller_effort.launch | 13 +++++++
9 files changed, 69 insertions(+), 38 deletions(-)
create mode 100644 talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
create mode 100644 talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch
diff --git a/.gitmodules b/.gitmodules
index d74587e..0443e7a 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 c2f0cb9..a946927 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 9388d0b..3c82281 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 97832a8..cee4046 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 8af32fa..29c65a1 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 addf29e..0280d88 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 0000000..ccdef5f
--- /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 e3a2338..76e60ab 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 0000000..7343f9e
--- /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>
+
--
GitLab