From bda1e849ea142e15fab00b03ac9695d8f84af206 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Thu, 4 Jan 2018 11:52:04 +0100
Subject: [PATCH] Make sure that the robot is not falling when switching to
effort control mode.
---
talos_roscontrol_sot/CMakeLists.txt | 2 +-
.../src/roscontrol-sot-controller.cpp | 111 +++++++++++++++--
.../src/roscontrol-sot-controller.hh | 35 +++++-
.../config/sot_talos_params_effort.yaml | 113 ++++++++++++++++++
.../sot_talos_controller_gazebo_effort.launch | 4 +-
5 files changed, 252 insertions(+), 13 deletions(-)
diff --git a/talos_roscontrol_sot/CMakeLists.txt b/talos_roscontrol_sot/CMakeLists.txt
index 2e8f55e..6e96b51 100644
--- a/talos_roscontrol_sot/CMakeLists.txt
+++ b/talos_roscontrol_sot/CMakeLists.txt
@@ -24,7 +24,7 @@ include(cmake/ros.cmake)
include(cmake/GNUInstallDirs.cmake)
include(cmake/python.cmake)
-project(talos_roscontrol_sot)
+#project(talos_roscontrol_sot)
find_package(PkgConfig REQUIRED)
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
index f6f8faa..7f00db9 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
@@ -41,6 +41,18 @@ using namespace rc_sot_system;
namespace talos_sot_controller
{
typedef std::map<std::string,std::string>::iterator it_map_rt_to_sot;
+
+ EffortControlPDMotorControlData::EffortControlPDMotorControlData()
+ {
+ prev = 0.0; vel_prev = 0.0; des_pos=0.0;
+ integ_err=0.0;
+ }
+
+ void EffortControlPDMotorControlData::read_from_xmlrpc_value
+ (const std::string &prefix)
+ {
+ pid_controller.initParam(prefix);
+ }
RCSotController::
RCSotController():
@@ -234,19 +246,61 @@ namespace talos_sot_controller
return true;
}
-
+ bool RCSotController::
+ readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh)
+ {
+ // Read libname
+ if (robot_nh.hasParam("/sot_controller/effort_control_pd_motor_init/gains"))
+ {
+ XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
+ robot_nh.getParamCached("/sot_controller/effort_control_pd_motor_init/gains",
+ xml_rpc_ecpd_init);
+
+ 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);
+ effort_mode_pd_motors_.clear();
+
+ for (int i=0;i<joints_name_.size();i++)
+ {
+ if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
+ {
+ /*
+ XmlRpc::XmlRpcValue &aDataSet= xml_rpc_ecpd_init[joints_name_[i]];
+ ROS_INFO("/sot_controller/effort_control_pd_motor_init %s type: %d\n",joints_name_[i],aDataSet.getType());
+ if (aDataSet.getType()!=XmlRpc::XmlRpcValue::TypeStruct)
+ {
+ ROS_ERROR("In /sot_controller/effort_control_pd_motor_init/gains/%s not a struct"
+ ,joints_name_[i]);
+ throw XmlrpcHelperException("Pb in readParamsEffortControlPDMotorControlData");
+ }
+ */
+ 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_INFO("joint %s not in /sot_controller/effort_control_pd_motor_init/gains\n",
+ joints_name_[i]);
+ }
+ return true;
+ }
+
+ ROS_ERROR("No parameter /sot_controller/effort_controler_pd_motor_init");
+ return false;
+ }
+
bool RCSotController::
readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh)
{
// Read libname
if (robot_nh.hasParam("/sot_controller/map_rc_to_sot_device"))
{
- if (robot_nh.getParam("/sot_controller/map_rc_to_sot_device",mapFromRCToSotDevice))
+ if (robot_nh.getParam("/sot_controller/map_rc_to_sot_device",
+ mapFromRCToSotDevice_))
{
/// TODO: Check if the mapping is complete wrt to the interface and the mapping.
ROS_INFO_STREAM("Loading map rc to sot device: ");
- for (it_map_rt_to_sot it = mapFromRCToSotDevice.begin();
- it != mapFromRCToSotDevice.end(); ++it)
+ for (it_map_rt_to_sot it = mapFromRCToSotDevice_.begin();
+ it != mapFromRCToSotDevice_.end(); ++it)
ROS_INFO_STREAM( it->first << ", " << it->second);
}
else
@@ -347,6 +401,9 @@ namespace talos_sot_controller
/// Calls readParamsFromRCToSotDevice
// Mapping from ros-controll to sot device
readParamsFromRCToSotDevice(robot_nh);
+
+ if (control_mode_==EFFORT)
+ readParamsEffortControlPDMotorControlData(robot_nh);
return true;
}
@@ -455,9 +512,9 @@ namespace talos_sot_controller
{
/// Tries to find the mapping from the local validation
/// to the SoT device.
- it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice.find(title);
+ it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(title);
/// If the mapping is found
- if (it_mapRC2Sot!=mapFromRCToSotDevice.end())
+ if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
{
/// Expose the data to the SoT device.
std::string lmapRC2Sot = it_mapRC2Sot->second;
@@ -608,8 +665,8 @@ namespace talos_sot_controller
else if (control_mode_==EFFORT)
cmdTitle="cmd-effort";
- it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice.find(cmdTitle);
- if (it_mapRC2Sot!=mapFromRCToSotDevice.end())
+ it_map_rt_to_sot it_mapRC2Sot= mapFromRCToSotDevice_.find(cmdTitle);
+ if (it_mapRC2Sot!=mapFromRCToSotDevice_.end())
{
std::string lmapRC2Sot = it_mapRC2Sot->second;
command_ = controlValues[lmapRC2Sot].getValues();
@@ -643,10 +700,46 @@ namespace talos_sot_controller
}
void RCSotController::
- update(const ros::Time&, const ros::Duration& )
+ update(const ros::Time&, const ros::Duration& period)
{
+ // Do not send any control if the dynamic graph is not started
if (!isDynamicGraphStopped())
one_iteration();
+ else
+ // But in effort mode it means that we are sending 0
+ // Therefore implements a default PD controller on the system.
+ if (control_mode_==EFFORT)
+ {
+ // ROS_INFO("Compute command for effort mode: %d %d",joints_.size(),effort_mode_pd_motors_.size());
+ for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
+ {
+ 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;
+
+ double vel_err = 0 - joints_[idJoint].getVelocity();
+ double err = ecpdcdata.des_pos - joints_[idJoint].getPosition();
+
+ ecpdcdata.integ_err +=err;
+
+ double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
+ // Apply command
+ control_toolbox::Pid::Gains gains = ecpdcdata.pid_controller.getGains();
+ //ROS_INFO("command: %d %s %f %f (%f %f %f)",idJoint, joints_name_[idJoint].c_str(),
+ //local_command, DataOneIter_.motor_angle[idJoint],
+ //gains.p_gain_,gains.d_gain_, gains.i_gain_);
+ joints_[idJoint].setCommand(local_command);
+
+ // Update previous value.
+ ecpdcdata.prev = DataOneIter_.motor_angle[idJoint];
+ }
+ }
+ }
}
void RCSotController::
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
index addf29e..e45c20a 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
@@ -44,11 +44,35 @@
#include <dynamic_graph_bridge/sot_loader_basic.hh>
#include "log.hh"
+#include <ros/ros.h>
+#include <control_toolbox/pid.h>
namespace talos_sot_controller
{
enum SotControlMode { POSITION, EFFORT};
+ class XmlrpcHelperException : public ros::Exception
+ {
+ public:
+ XmlrpcHelperException(const std::string& what)
+ : ros::Exception(what) {}
+ };
+
+
+ struct EffortControlPDMotorControlData
+ {
+ control_toolbox::Pid pid_controller;
+
+ //double p_gain,d_gain,i_gain;
+ 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(const std::string &prefix);
+ };
/**
This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
@@ -109,7 +133,11 @@ namespace talos_sot_controller
/// \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
+ /// is not on.
+ std::map<std::string,EffortControlPDMotorControlData> effort_mode_pd_motors_;
+
/// \brief Map from ros-control quantities to robot device
/// ros-control quantities are for the sensors:
/// * motor-angles
@@ -119,7 +147,7 @@ namespace talos_sot_controller
/// ros-control quantities for control are:
/// * joints
/// * torques
- std::map<std::string,std::string> mapFromRCToSotDevice;
+ std::map<std::string,std::string> mapFromRCToSotDevice_;
public :
@@ -182,6 +210,9 @@ namespace talos_sot_controller
/// \brief Read the control mode.
bool readParamsControlMode(ros::NodeHandle & robot_nh);
+ /// \brief Read the PID information of the robot in effort mode.
+ bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
+
///@}
/// \brief Fill the SoT map structures
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
index 9bd89c7..a0396e0 100644
--- a/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
@@ -9,4 +9,117 @@ sot_controller:
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
+ effort_control_pd_motor_init:
+ - name: leg_left_1_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_left_2_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_left_3_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_left_4_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_left_5_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_left_6_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_right_1_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_right_2_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_right_3_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_right_4_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_right_5_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: leg_right_6_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: torso_1_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: torso_2_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_1_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_2_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_3_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_4_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_5_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_6_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_left_7_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_1_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_2_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_3_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_4_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_5_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_6_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
+ - name: arm_right_7_joint
+ p_gain: 1000.0
+ d_gain: 1000.0
+ des_pos: 0.0
control_mode: EFFORT
diff --git a/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch b/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
index 9f5f1f5..bb3cc97 100644
--- a/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
+++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
@@ -2,10 +2,12 @@
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
+ <rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find talos_hardware_gazebo)/config/pids.yaml"/>
<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<!-- Spawn walking controller -->
- <node name="sot_controller_spawner"
+ <node launch-prefix="xterm -e python -m pdb"
+ name="sot_controller_spawner"
pkg="talos_controller_manager" type="spawner" output="screen"
args="sot_controller" />
--
GitLab