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

[talos_roscontrol_sot/talos_roscontrol_sot_talos] First successfull test of...

[talos_roscontrol_sot/talos_roscontrol_sot_talos] First successfull test of current control on the robot.
parent 95236d08
[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
......@@ -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
......
<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>
......
......@@ -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>
......@@ -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);
}
......@@ -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);
......
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
......@@ -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>
......
<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>
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