Skip to content

EDC muscle hand. How to define a new controller

Jump to bottom Edit New page
toliver edited this page Sep 3, 2013 · 24 revisions

Create new controller type C++ files

The easyest way to create a new controller is to copy an existing one and use it as a base for your custom controller. SrhMuscleJointPositionController will be used as a base here.

In the package sr_mechanism_controllers make a copy of the files:

srh_muscle_joint_position_controller.cpp
srh_muscle_joint_position_controller.hpp

and rename them conveniently, e.g:

srh_muscle_joint_pressure_position_controller.cpp
or
srh_muscle_joint_tactile_position_controller.cpp

Then open the new controller files (cpp and hpp) to edit and rename the controller class conveniently everywhere, e.g:

class SrhMuscleJointPressurePositionController : public SrController
  {

Add the new controller to CMakeLists.txt

roscd sr_mechanism_controllers
gedit CMakeLists.txt

Add the new file to rosbuild_add_library so that the compiler will take it into account. E.g.:

src/srh_muscle_joint_pressure_position_controller.cpp

Create the parameters file for the new type of controller

roscd sr_ethercat_hand_config/controls/host

Copy sr_edc_muscle_joint_position_controllers.yaml to e.g.:

sr_edc_muscle_joint_pressure_position_controllers.yaml

Edit the file changing all the parameters to match the new name and type, e.g.:

sh_ffj0_muscle_position_controller:
  joint: FFJ0
  pid:
    d: 0
    deadband: 0
    friction_deadband: 2000
    i: 0
    i_clamp: 1
    max_force: 0.0
    p: 2
    position_deadband: 0.002
  type: sr_mechanism_controllers/SrhMuscleJointPositionController

would become

sh_ffj0_muscle_pressure_position_controller:
  joint: FFJ0
  pid:
    d: 0
    deadband: 0
    friction_deadband: 2000
    i: 0
    i_clamp: 1
    max_force: 0.0
    p: 2
    position_deadband: 0.002
  type: sr_mechanism_controllers/SrhMusclePressureJointPositionController

Add the new params file to the default file

roscd roscd sr_ethercat_hand_config/controls
gedit sr_edc_default_controllers.launch

Whenever you see sr_edc_muscle_joint_position_controllers.yaml in the file, copy the entire line, paste it just below, and replace the name of the yaml file for the correct one for your new controller. e.g.:

<rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/host/$(arg config_dir)sr_edc_muscle_joint_pressure_position_controllers.yaml" />

Access to tactile sensor data inside the controller

The data from all the tactile sensors can be accessed from inside the controller for every joint. So the user should consider what is relevant for the control algorithm for that joint.

Add this to the include section of the .hpp file of the new controller.

#include <sr_hardware_interface/sr_actuator.hpp>

Add this to the private section of the .hpp file of the new controller.

sr_actuator::SrMuscleActuator* joint_actuator_;

Add this to the first init() function of the .cpp file of the new controller.

joint_actuator_ = static_cast<sr_actuator::SrMuscleActuator*>( robot->model_->hw_->getActuator(joint_name_) );

Then you can have access to the tactile information in the update() function of the .cpp file of the new controller. e.g.

int biotac_pressure = joint_actuator_->state_.tactiles_->at(0).biotac.pdc;

Here we access the pressure DC data of the biotac sensor 0. Valid indexes are 0-4.

Access to muscle pressure sensor data inside the controller

The following two variables available in the function update() contain the pressure data (in millibars) for the muscles 0 (flexor) and 1 (extensor) of the joint being controlled.

uint16_t pressure_0;
uint16_t pressure_1;

Use them only after the point in the function code where they are defined and assigned their value.

Define a command topic type and a status topic type that suit your controller

A controller is listening to a command topic and publishing information to a status topic.

The SrhMuscleJointPositionController, redefines only the status message type (and therefore the status publisher), but the command type remains the same as in the parent class SrController (it is a float64 representing in this case the angle in radians).

The SrhJointMuscleValveController, on the other hand redefines not only the status, but also the command type.

In the package sr_robot_msgs/msg create a file for each of the new messages. E.g. in the case of SrhJointMuscleValveController what we have done is:

JointMuscleValveControllerState.msg
JointMuscleValveControllerCommand.msg

Then define the message. E.g., for JointMuscleValveControllerState.msg:

Header header
int8 set_valve_muscle_0
int8 set_valve_muscle_1
uint64 set_duration_muscle_0
uint64 set_duration_muscle_1
int8 current_valve_muscle_0
int8 current_valve_muscle_1
uint64 current_duration_muscle_0
uint64 current_duration_muscle_1
float64 packed_valve
uint16 muscle_pressure_0
uint16 muscle_pressure_1
float64 time_step

In srh_joint_muscle_valve_controller.hpp:

#include <sr_robot_msgs/JointMuscleValveControllerState.h>
#include <sr_robot_msgs/JointMuscleValveControllerCommand.h>
  private:
    //publish our joint controller state
    boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::JointMuscleValveControllerState> > controller_state_publisher_;

    ros::Subscriber sub_command_;
    void setCommandCB(const sr_robot_msgs::JointMuscleValveControllerCommandConstPtr& msg);

In srh_joint_muscle_valve_controller.cpp:

  bool SrhJointMuscleValveController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
  {

    ...

    //We do not call this function, as we want to listen to a different topic type
    //after_init();
    sub_command_ = node_.subscribe<sr_robot_msgs::JointMuscleValveControllerCommand>("command", 1, &SrhJointMuscleValveController::setCommandCB, this);
    return true;
  }

Then inside the update() function you assign the corresponding value to every field in controller_state_publisher_->msg_ . E.g:

controller_state_publisher_->msg_.muscle_pressure_0 = pressure_0;

Load and start the controllers

Via command line

Check which controllers are loaded, and which of them are running:

rosservice call /pr2_controller_manager/list_controllers

Load your controllers if they are not loaded yet:

rosservice call /pr2_controller_manager/load_controller "name: 'sh_ffj0_muscle_pressure_position_controller'"
rosservice call /pr2_controller_manager/load_controller "name: 'sh_ffj3_muscle_pressure_position_controller'"
...

Start your controllers and stop any other running controllers. E.g.:

rosservice call /pr2_controller_manager/switch_controller "{start_controllers: ['sh_ffj0_muscle_pressure_position_controller', 'sh_ffj3_muscle_pressure_position_controllerr'], stop_controllers: ['sh_ffj3_muscle_position_controller'], strictness: 0}"

Modify GUI plugin change_muscle_controllers

Open sr_gui_change_muscle_controllers/uis/SrChangeControllers.ui with the QtDesigner application, and add a new button for your new controller.

In sr_gui_change_muscle_controllers/src/sr_gui_change_muscle_controllers edit change_controllers.py.

The controllers definition looks like this:

controllers = {"valve": ["sh_ffj0_muscle_valve_controller", "sh_ffj3_muscle_valve_controller", "sh_ffj4_muscle_valve_controller", "sh_mfj0_muscle_valve_controller", "sh_mfj3_muscle_valve_controller", "sh_mfj4_muscle_valve_controller", "sh_rfj0_muscle_valve_controller", "sh_rfj3_muscle_valve_controller", "sh_rfj4_muscle_valve_controller", "sh_lfj0_muscle_valve_controller", "sh_lfj3_muscle_valve_controller", "sh_lfj4_muscle_valve_controller", "sh_lfj5_muscle_valve_controller", "sh_thj1_muscle_valve_controller", "sh_thj2_muscle_valve_controller", "sh_thj3_muscle_valve_controller", "sh_thj4_muscle_valve_controller", "sh_thj5_muscle_valve_controller", "sh_wrj1_muscle_valve_controller", "sh_wrj2_muscle_valve_controller"],
                   "position": ["sh_ffj0_muscle_position_controller", "sh_ffj3_muscle_position_controller", "sh_ffj4_muscle_position_controller", "sh_mfj0_muscle_position_controller", "sh_mfj3_muscle_position_controller", "sh_mfj4_muscle_position_controller", "sh_rfj0_muscle_position_controller", "sh_rfj3_muscle_position_controller", "sh_rfj4_muscle_position_controller", "sh_lfj0_muscle_position_controller", "sh_lfj3_muscle_position_controller", "sh_lfj4_muscle_position_controller", "sh_lfj5_muscle_position_controller", "sh_thj1_muscle_position_controller", "sh_thj2_muscle_position_controller", "sh_thj3_muscle_position_controller", "sh_thj4_muscle_position_controller", "sh_thj5_muscle_position_controller", "sh_wrj1_muscle_position_controller", "sh_wrj2_muscle_position_controller"],
                   "stop": []}

Add your new controllers to the list, e.g.:

controllers = {"valve": ["sh_ffj0_muscle_valve_controller", "sh_ffj3_muscle_valve_controller", "sh_ffj4_muscle_valve_controller", "sh_mfj0_muscle_valve_controller", "sh_mfj3_muscle_valve_controller", "sh_mfj4_muscle_valve_controller", "sh_rfj0_muscle_valve_controller", "sh_rfj3_muscle_valve_controller", "sh_rfj4_muscle_valve_controller", "sh_lfj0_muscle_valve_controller", "sh_lfj3_muscle_valve_controller", "sh_lfj4_muscle_valve_controller", "sh_lfj5_muscle_valve_controller", "sh_thj1_muscle_valve_controller", "sh_thj2_muscle_valve_controller", "sh_thj3_muscle_valve_controller", "sh_thj4_muscle_valve_controller", "sh_thj5_muscle_valve_controller", "sh_wrj1_muscle_valve_controller", "sh_wrj2_muscle_valve_controller"],
                   "position": ["sh_ffj0_muscle_position_controller", "sh_ffj3_muscle_position_controller", "sh_ffj4_muscle_position_controller", "sh_mfj0_muscle_position_controller", "sh_mfj3_muscle_position_controller", "sh_mfj4_muscle_position_controller", "sh_rfj0_muscle_position_controller", "sh_rfj3_muscle_position_controller", "sh_rfj4_muscle_position_controller", "sh_lfj0_muscle_position_controller", "sh_lfj3_muscle_position_controller", "sh_lfj4_muscle_position_controller", "sh_lfj5_muscle_position_controller", "sh_thj1_muscle_position_controller", "sh_thj2_muscle_position_controller", "sh_thj3_muscle_position_controller", "sh_thj4_muscle_position_controller", "sh_thj5_muscle_position_controller", "sh_wrj1_muscle_position_controller", "sh_wrj2_muscle_position_controller"],
                   "pressure_position": ["sh_ffj0_muscle_pressure_position_controller", "sh_ffj3_muscle_pressure_position_controller", "sh_ffj4_muscle_pressure_position_controller", "sh_mfj0_muscle_pressure_position_controller", "sh_mfj3_muscle_pressure_position_controller", "sh_mfj4_muscle_pressure_position_controller", "sh_rfj0_muscle_pressure_position_controller", "sh_rfj3_muscle_pressure_position_controller", "sh_rfj4_muscle_pressure_position_controller", "sh_lfj0_muscle_pressure_position_controller", "sh_lfj3_muscle_pressure_position_controller", "sh_lfj4_muscle_pressure_position_controller", "sh_lfj5_muscle_pressure_position_controller", "sh_thj1_muscle_pressure_position_controller", "sh_thj2_muscle_pressure_position_controller", "sh_thj3_muscle_pressure_position_controller", "sh_thj4_muscle_pressure_position_controller", "sh_thj5_muscle_pressure_position_controller", "sh_wrj1_muscle_pressure_position_controller", "sh_wrj2_muscle_pressure_position_controller"],
                   "stop": []}

Then create a new function copying the ones for the other controllers, and link it to the new button the same way the others are.

Add a custom footer
Add a custom sidebar
Clone this wiki locally