Skip to content

Commit 76bc78d

Browse files
florent-lamirauxflorent-lamiraux
committed
Make package able to handle other types of robots.
1 parent 0723716 commit 76bc78d

21 files changed

+327
-246
lines changed

‎CMakeLists.txt

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -73,14 +73,12 @@ INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
7373
INSTALL(FILES
7474
launch/controller.launch
7575
launch/sot_params.launch
76-
launch/sot_params.launch
7776
DESTINATION share/${PROJECT_NAME}/launch)
7877

7978
# Install configuration files
8079
INSTALL(FILES
81-
config/controller.yaml
82-
config/sot_params.yaml
83-
config/sot_params_simu.yaml
80+
config/sot_params_common.yaml
81+
config/sot_params_ur.yaml
8482
DESTINATION share/${PROJECT_NAME}/config)
8583

8684
# Install default URDF file

‎README.md

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# Control of robot arm via the Stack of Tasks and roscontrol

‎config/controller.yaml

Lines changed: 0 additions & 11 deletions
This file was deleted.

‎config/sot_params_common.yaml

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
sot_controller:
2+
libname: libsot-robot-arm-controller.so
3+
map_rc_to_sot_device:
4+
motor-angles: motor-angles
5+
joint-angles: joint-angles
6+
velocities: velocities
7+
forces: forces
8+
torques: torques
9+
control: control
10+
11+
type:
12+
sot_controller/RCSotController
13+
check_mode:
14+
False
15+
verbosity_level:
16+
25

‎config/sot_params_simu.yaml

Lines changed: 0 additions & 32 deletions
This file was deleted.

‎config/sot_params.yaml renamed to ‎config/sot_params_ur.yaml

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,4 @@
11
sot_controller:
2-
libname: libsot-universal-robot-controller.so
3-
map_rc_to_sot_device:
4-
motor-angles: motor-angles
5-
joint-angles: joint-angles
6-
velocities: velocities
7-
forces: forces
8-
torques: torques
9-
control: control
10-
112
control_mode:
123
shoulder_pan_joint:
134
ros_control_mode: POSITION
@@ -28,5 +19,14 @@ sot_controller:
2819
ros_control_mode: POSITION
2920
sot_control_mode: VELOCITY
3021

31-
dt: 0.002
32-
jitter: 0.004
22+
joint_names:
23+
[shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint,
24+
wrist_2_joint, wrist_3_joint]
25+
joints:
26+
[shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint,
27+
wrist_2_joint, wrist_3_joint]
28+
29+
sot:
30+
state_vector_map:
31+
[shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint,
32+
wrist_2_joint, wrist_3_joint]

‎launch/controller.launch

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,10 @@
11
<launch>
22
<arg name="simulation" default="false"/>
3-
<include file="$(find sot_universal_robot)/launch/sot_params.launch"/>
4-
5-
<!-- Sot Controller configuration -->
6-
<rosparam command="load" unless="$(arg simulation)"
7-
file="$(find sot_universal_robot)/config/sot_params.yaml"/>
8-
<rosparam command="load" if="$(arg simulation)"
9-
file="$(find sot_universal_robot)/config/sot_params_simu.yaml"/>
10-
<rosparam command="load"
11-
file="$(find sot_universal_robot)/config/controller.yaml" />
12-
3+
<arg name="robot" default="" />
4+
<include file="$(find sot_universal_robot)/launch/sot_params.launch">
5+
<arg name="robot" value="$(arg robot)"/>
6+
<arg name="simulation" value="$(arg simulation)"/>
7+
</include>
138
<!-- Spawn sot controller -->
149
<node name="sot_controller_spawner"
1510
pkg="controller_manager" type="spawner" output="screen"

‎launch/sot_params.launch

Lines changed: 16 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,19 @@
11
<launch>
2+
<arg name="simulation" default="false"/>
3+
<arg name="robot" default="" />
4+
<arg name="arm_id" default="panda"/>
25
<!-- Load robot sot params. -->
3-
<group ns="sot">
4-
<rosparam param="state_vector_map">
5-
["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
6-
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
7-
</rosparam>
8-
</group>
9-
<group ns="sot_controller">
10-
<rosparam param="joint_names">
11-
["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
12-
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
13-
</rosparam>
14-
</group>
6+
<group ns="sot_controller" if="$(eval robot == 'ur')">
7+
<rosparam param="dt" if="$(arg simulation)">
8+
0.002
9+
</rosparam>
10+
<rosparam param="dt" unless="$(arg simulation)">
11+
0.01
12+
</rosparam>
13+
</group>
14+
<!-- Sot Controller configuration -->
15+
<rosparam command="load"
16+
file="$(find sot_universal_robot)/config/sot_params_common.yaml"/>
17+
<rosparam command="load" if="$(eval robot == 'ur')"
18+
file="$(find sot_universal_robot)/config/sot_params_ur.yaml"/>
1519
</launch>

‎src/CMakeLists.txt

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525

2626
# Add the library to wrap the device of Tiago.
2727

28-
SET(DEVICE_NAME sot-universal-robot-device)
28+
SET(DEVICE_NAME sot-robot-arm-device)
2929
ADD_LIBRARY(${DEVICE_NAME} SHARED device.cc)
3030

3131
# Link the dynamic library containing the SoT with its dependencies.
@@ -35,25 +35,26 @@ INSTALL(TARGETS ${DEVICE_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
3535

3636
# build python submodule
3737
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${DEVICE_NAME})
38-
DYNAMIC_GRAPH_PYTHON_MODULE("sot/universal_robot/${PYTHON_LIBRARY_NAME}"
39-
${DEVICE_NAME} sot_universal_robot-${PYTHON_LIBRARY_NAME}-wrap
38+
DYNAMIC_GRAPH_PYTHON_MODULE("sot/robot_arm/${PYTHON_LIBRARY_NAME}"
39+
${DEVICE_NAME} sot_robot_arm-${PYTHON_LIBRARY_NAME}-wrap
4040
SOURCE_PYTHON_MODULE ${CMAKE_CURRENT_SOURCE_DIR}/device-python.cc)
4141

4242
# Install Python files.
43-
SET(PYTHON_MODULE dynamic_graph/sot/universal_robot)
43+
SET(PYTHON_MODULE dynamic_graph/sot/robot_arm)
4444
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py")
45-
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "universal_robot.py")
4645
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
46+
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot_arm.py")
47+
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "universal_robot.py")
4748

48-
# Add the library to wrap the controller of Universal-Robot.
49-
SET(CONTROLLER_NAME sot-universal-robot-controller)
49+
# Add the library to wrap the controller of Robot-Arm.
50+
SET(CONTROLLER_NAME sot-robot-arm-controller)
5051
ADD_LIBRARY(${CONTROLLER_NAME} SHARED
5152
controller.cc
5253
controller.hh
5354
)
5455

5556
# Link the dynamic library containing the SoT with its dependencies.
5657
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} sot-core::sot-core dynamic-graph-python::dynamic-graph-python
57-
dynamic_graph_bridge::ros_bridge dynamic_graph_bridge::ros_interpreter sot-universal-robot-device)
58+
dynamic_graph_bridge::ros_bridge dynamic_graph_bridge::ros_interpreter sot-robot-arm-device)
5859

5960
INSTALL(TARGETS ${CONTROLLER_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)

‎src/controller.cc

Lines changed: 32 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,8 @@
3636

3737
#include "device.hh"
3838

39-
const std::string SoTUniversalRobotController::LOG_PYTHON
40-
("/tmp/UniversalRobotController_python.out");
39+
const std::string SoTRobotArmController::LOG_PYTHON
40+
("/tmp/RobotArmController_python.out");
4141

4242
using std::cout;
4343
using std::endl;
@@ -47,7 +47,7 @@ boost::condition_variable cond;
4747
boost::mutex mut;
4848
bool data_ready;
4949

50-
void workThread(SoTUniversalRobotController *controller)
50+
void workThread(SoTRobotArmController *controller)
5151
{
5252

5353
dynamicgraph::Interpreter aLocalInterpreter(dynamicgraph::rosInit(false,true));
@@ -63,88 +63,92 @@ void workThread(SoTUniversalRobotController *controller)
6363
ros::waitForShutdown();
6464
}
6565

66-
SoTUniversalRobotController::SoTUniversalRobotController():
67-
device_(new SoTUniversalRobotDevice("UniversalRobot"))
66+
SoTRobotArmController::SoTRobotArmController():
67+
device_(new SoTRobotArmDevice("RobotArm"))
6868
{
6969
// Create thread and python interpreter
7070
init();
71+
std::string robotType("ur");
7172
ros::NodeHandle nh;
7273
/// Read /sot_controller/dt to know what is the control period
7374
if (nh.hasParam("/sot_controller/dt")) {
7475
double dt;
7576
nh.getParam("/sot_controller/dt", dt);
7677
device_->setTimeStep(dt);
77-
ROS_INFO_STREAM("Set Universal Robot control period to: " << dt);
78+
ROS_INFO_STREAM("Set control period to: " << dt);
79+
if (nh.hasParam("/sot/robot")){
80+
nh.getParam("/sot/robot", robotType);
81+
}
7882
}
79-
startupPython();
83+
startupPython(robotType);
8084
interpreter_->startRosService ();
8185
}
8286

83-
SoTUniversalRobotController::SoTUniversalRobotController(std::string RobotName):
84-
device_(new SoTUniversalRobotDevice (RobotName))
87+
SoTRobotArmController::SoTRobotArmController(std::string RobotName):
88+
device_(new SoTRobotArmDevice (RobotName))
8589
{
8690
// Create thread and python interpreter
8791
init();
8892
}
8993

90-
SoTUniversalRobotController::SoTUniversalRobotController(const char robotName[]):
91-
device_(new SoTUniversalRobotDevice (robotName))
94+
SoTRobotArmController::SoTRobotArmController(const char robotName[]):
95+
device_(new SoTRobotArmDevice (robotName))
9296
{
9397
// Create thread and python interpreter
9498
init();
9599
}
96100

97-
void SoTUniversalRobotController::init()
101+
void SoTRobotArmController::init()
98102
{
99-
cout << "Going through SoTUniversalRobotController." << endl;
103+
cout << "Going through SoTRobotArmController." << endl;
100104
boost::thread thr(workThread,this);
101105
boost::unique_lock<boost::mutex> lock(mut);
102106
cond.wait(lock);
103107
}
104108

105-
SoTUniversalRobotController::~SoTUniversalRobotController()
109+
SoTRobotArmController::~SoTRobotArmController()
106110
{
107111
}
108112

109-
void SoTUniversalRobotController::setupSetSensors
113+
void SoTRobotArmController::setupSetSensors
110114
(map<string,SensorValues> &SensorsIn)
111115
{
112116
device_->setupSetSensors(SensorsIn);
113117
}
114118

115119

116-
void SoTUniversalRobotController::nominalSetSensors
120+
void SoTRobotArmController::nominalSetSensors
117121
(map<string,SensorValues> &SensorsIn)
118122
{
119123
device_->nominalSetSensors(SensorsIn);
120124
}
121125

122-
void SoTUniversalRobotController::cleanupSetSensors
126+
void SoTRobotArmController::cleanupSetSensors
123127
(map<string, SensorValues> &SensorsIn)
124128
{
125129
device_->cleanupSetSensors(SensorsIn);
126130
}
127131

128132

129-
void SoTUniversalRobotController::getControl
133+
void SoTRobotArmController::getControl
130134
(map<string,ControlValues> &controlOut)
131135
{
132136
device_->getControl(controlOut);
133137
}
134138

135-
void SoTUniversalRobotController::
139+
void SoTRobotArmController::
136140
setNoIntegration(void)
137141
{
138142
device_->setNoIntegration();
139143
}
140144

141-
void SoTUniversalRobotController::
145+
void SoTRobotArmController::
142146
setSecondOrderIntegration(void)
143147
{
144148
device_->setSecondOrderIntegration();
145149
}
146150

147-
void SoTUniversalRobotController::
151+
void SoTRobotArmController::
148152
runPython(std::ostream& file,
149153
const std::string& command,
150154
dynamicgraph::Interpreter& interpreter)
@@ -165,7 +169,7 @@ runPython(std::ostream& file,
165169
}
166170
}
167171

168-
void SoTUniversalRobotController::startupPython()
172+
void SoTRobotArmController::startupPython(const std::string& robotType)
169173
{
170174
std::ofstream aof(LOG_PYTHON.c_str());
171175
runPython (aof, "import sys, os", *interpreter_);
@@ -177,10 +181,12 @@ void SoTUniversalRobotController::startupPython()
177181
" path.append(p)", *interpreter_);
178182
runPython (aof, "path.extend(sys.path)", *interpreter_);
179183
runPython (aof, "sys.path = path", *interpreter_);
180-
runPython (aof, "import dynamic_graph.sot.universal_robot.prologue",
181-
*interpreter_);
182-
runPython (aof, "robot = dynamic_graph.sot.universal_robot.prologue.makeRobot ()",
184+
runPython (aof, "import dynamic_graph.sot.robot_arm.prologue",
183185
*interpreter_);
186+
std::ostringstream os;
187+
os << "robot = dynamic_graph.sot.robot_arm.prologue.makeRobot"
188+
" (robotType='" << robotType << "')";
189+
runPython (aof, os.str(), *interpreter_);
184190

185191
// Calling again rosInit here to start the spinner. It will
186192
// deal with topics and services callbacks in a separate, non
@@ -194,7 +200,7 @@ extern "C"
194200
{
195201
AbstractSotExternalInterface * createSotExternalInterface()
196202
{
197-
return new SoTUniversalRobotController ();
203+
return new SoTRobotArmController ();
198204
}
199205
}
200206

0 commit comments

Comments
 (0)