Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
roscontrol_sot
Commits
ec23b4e6
Commit
ec23b4e6
authored
Jul 13, 2019
by
Olivier Stasse
Browse files
Merge branch 'devel' into master
parents
0385fe4e
ea5153ff
Changes
8
Expand all
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
ec23b4e6
...
...
@@ -20,13 +20,13 @@ find_package(PkgConfig REQUIRED)
add_required_dependency
(
bullet
)
add_required_dependency
(
"urdfdom"
)
#set(bullet_FOUND 0)
#pkg_check_modules(bullet REQUIRED bullet)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package
(
catkin REQUIRED COMPONENTS
pal_hardware_interfaces
controller_interface
roscpp
rospy
std_msgs
...
...
@@ -34,8 +34,6 @@ find_package(catkin REQUIRED COMPONENTS
control_msgs
sensor_msgs
realtime_tools
controller_interface
pal_hardware_interfaces
)
## LAAS cmake submodule part
...
...
@@ -57,12 +55,12 @@ set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${LIBRARY_OUTPUT_PATH}")
# Add dependency through jrl-cmakemodules to compile
# this code without catkin_make
add_required_dependency
(
"pal_hardware_interfaces"
)
add_optional_dependency
(
"temperature_sensor_controller"
)
add_required_dependency
(
roscpp
)
add_required_dependency
(
"realtime_tools >= 1.8"
)
add_required_dependency
(
"dynamic_graph_bridge"
)
add_required_dependency
(
"controller_interface"
)
add_required_dependency
(
"pal_hardware_interfaces"
)
add_optional_dependency
(
"temperature_sensor_controller"
)
add_required_dependency
(
"pal_common_msgs"
)
add_required_dependency
(
"dynamic-graph"
)
...
...
@@ -113,8 +111,8 @@ target_link_libraries(rcsot_controller
${
bullet_LIBRARIES
}
)
pkg_config_use_dependency
(
rcsot_controller urdfdom
)
pkg_config_use_dependency
(
rcsot_controller dynamic-graph
)
pkg_config_use_dependency
(
rcsot_controller urdfdom
optional NO_INCLUDE_SYSTEM
)
pkg_config_use_dependency
(
rcsot_controller dynamic-graph
optional NO_INCLUDE_SYSTEM
)
## Mark executables and/or libraries for installation
install
(
TARGETS rcsot_controller DESTINATION lib
)
...
...
cmake
@
f34901f1
Compare
5c8c19f4
...
f34901f1
Subproject commit
5c8c19
f491f
2c6f8488f5f37ff81d711d69dbb3f
Subproject commit f
3
49
0
1f
143d843b48dfdb8d9e904503ed96e2310
package.xml
View file @
ec23b4e6
<?xml version="1.0"?>
<package>
<name>
roscontrol_sot
</name>
<version>
0.0.
4
</version>
<version>
0.0.
8
</version>
<description>
Wrapping the Stack-of-tasks framework in ros-control
</description>
<!-- Maintainer email -->
...
...
src/log.cpp
View file @
ec23b4e6
...
...
@@ -9,7 +9,6 @@
#include
<fstream>
#include
<iomanip>
#include
<ros/console.h>
using
namespace
std
;
using
namespace
rc_sot_system
;
...
...
@@ -31,6 +30,7 @@ void DataToLog::init(ProfileLog &aProfileLog)
gyrometer
.
resize
(
3
*
aProfileLog
.
length
);
force_sensors
.
resize
(
aProfileLog
.
nbForceSensors
*
6
*
aProfileLog
.
length
);
temperatures
.
resize
(
aProfileLog
.
nbDofs
*
aProfileLog
.
length
);
controls
.
resize
(
aProfileLog
.
nbDofs
*
aProfileLog
.
length
);
timestamp
.
resize
(
aProfileLog
.
length
);
duration
.
resize
(
aProfileLog
.
length
);
...
...
@@ -80,13 +80,15 @@ void Log::record(DataToLog &aDataToLog)
StoredData_
.
motor_currents
[
JointID
+
lref_
]
=
aDataToLog
.
motor_currents
[
JointID
];
if
(
aDataToLog
.
temperatures
.
size
()
>
JointID
)
StoredData_
.
temperatures
[
JointID
+
lref_
]
=
aDataToLog
.
temperatures
[
JointID
];
if
(
aDataToLog
.
controls
.
size
()
>
JointID
)
StoredData_
.
controls
[
JointID
+
lref_
]
=
aDataToLog
.
controls
[
JointID
];
}
for
(
unsigned
int
axis
=
0
;
axis
<
3
;
axis
++
)
{
StoredData_
.
accelerometer
[
lrefts_
*
3
+
axis
]
=
aDataToLog
.
accelerometer
[
axis
];
StoredData_
.
gyrometer
[
lrefts_
*
3
+
axis
]
=
aDataToLog
.
gyrometer
[
axis
];
}
unsigned
width_pad
=
6
*
profileLog_
.
nbForceSensors
;
std
::
size_t
width_pad
=
6
*
profileLog_
.
nbForceSensors
;
for
(
unsigned
int
fsID
=
0
;
fsID
<
profileLog_
.
nbForceSensors
;
fsID
++
)
{
...
...
@@ -158,31 +160,34 @@ void Log::save(std::string &fileName)
suffix
=
"-temperatures.log"
;
saveVector
(
fileName
,
suffix
,
StoredData_
.
temperatures
,
profileLog_
.
nbDofs
);
suffix
=
"-controls.log"
;
saveVector
(
fileName
,
suffix
,
StoredData_
.
controls
,
profileLog_
.
nbDofs
);
suffix
=
"-duration.log"
;
saveVector
(
fileName
,
suffix
,
StoredData_
.
duration
,
1
);
}
inline
void
writeHeaderToBinaryBuffer
(
ofstream
&
of
,
const
unsigned
in
t
&
nVector
,
const
unsigned
in
t
&
vectorSize
)
const
std
::
size_
t
&
nVector
,
const
std
::
size_
t
&
vectorSize
)
{
of
.
write
((
char
*
)
&
nVector
,
sizeof
(
unsigned
in
t
));
of
.
write
((
char
*
)
&
vectorSize
,
sizeof
(
unsigned
in
t
));
of
.
write
((
const
char
*
)
(
&
nVector
)
,
sizeof
(
std
::
size_
t
));
of
.
write
((
const
char
*
)
(
&
vectorSize
)
,
sizeof
(
std
::
size_
t
));
}
inline
void
writeToBinaryFile
(
ofstream
&
of
,
const
double
&
t
,
const
double
&
dt
,
const
std
::
vector
<
double
>&
data
,
const
std
::
size_t
&
idx
,
const
std
::
size_t
&
size
)
{
of
.
write
((
char
*
)
&
t
,
sizeof
(
double
));
of
.
write
((
char
*
)
&
dt
,
sizeof
(
double
));
of
.
write
((
char
*
)(
&
data
[
idx
]),
size
*
(
sizeof
(
double
)));
of
.
write
((
const
char
*
)
&
t
,
sizeof
(
double
));
of
.
write
((
const
char
*
)
&
dt
,
sizeof
(
double
));
of
.
write
((
const
char
*
)(
&
data
[
idx
]),
size
*
(
sizeof
(
double
)));
}
void
Log
::
saveVector
(
std
::
string
&
fileName
,
std
::
string
&
suffix
,
const
std
::
vector
<
double
>
&
avector
,
unsigned
in
t
size
)
std
::
size_
t
size
)
{
ostringstream
oss
;
oss
<<
fileName
;
...
...
src/log.hh
View file @
ec23b4e6
...
...
@@ -14,9 +14,9 @@ namespace rc_sot_system {
struct
ProfileLog
{
unsigned
in
t
nbDofs
;
unsigned
in
t
nbForceSensors
;
unsigned
in
t
length
;
std
::
size_
t
nbDofs
;
std
::
size_
t
nbForceSensors
;
std
::
size_
t
length
;
};
struct
DataToLog
...
...
@@ -41,6 +41,8 @@ namespace rc_sot_system {
std
::
vector
<
double
>
motor_currents
;
// Measured temperatures
std
::
vector
<
double
>
temperatures
;
// Control
std
::
vector
<
double
>
controls
;
// Timestamp
std
::
vector
<
double
>
timestamp
;
...
...
@@ -51,9 +53,9 @@ namespace rc_sot_system {
DataToLog
();
void
init
(
ProfileLog
&
aProfileLog
);
unsigned
in
t
nbDofs
()
{
return
profileLog_
.
nbDofs
;}
unsigned
in
t
nbForceSensors
()
{
return
profileLog_
.
nbForceSensors
;}
unsigned
in
t
length
()
{
return
profileLog_
.
length
;}
std
::
size_
t
nbDofs
()
{
return
profileLog_
.
nbDofs
;}
std
::
size_
t
nbForceSensors
()
{
return
profileLog_
.
nbForceSensors
;}
std
::
size_
t
length
()
{
return
profileLog_
.
length
;}
};
...
...
@@ -80,7 +82,7 @@ namespace rc_sot_system {
void
saveVector
(
std
::
string
&
filename
,
std
::
string
&
suffix
,
const
std
::
vector
<
double
>
&
avector
,
unsigned
in
t
);
std
::
size_
t
);
public:
...
...
@@ -95,4 +97,10 @@ namespace rc_sot_system {
};
}
#pragma GCC diagnostic push
#pragma GCC system_header
#include
<ros/console.h>
#pragma GCC diagnostic pop
#endif
/* _RC_SOT_SYSTEM_LOG_H_ */
src/roscontrol-sot-controller.cpp
View file @
ec23b4e6
This diff is collapsed.
Click to expand it.
src/roscontrol-sot-controller.hh
View file @
ec23b4e6
...
...
@@ -8,11 +8,15 @@
#include
<string>
#include
<map>
#pragma GCC diagnostic push
#pragma GCC system_header
#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
<pluginlib/class_list_macros.h>
#pragma GCC diagnostic pop
#include
<dynamic_graph_bridge/sot_loader_basic.hh>
#include
<ros/ros.h>
...
...
@@ -26,7 +30,9 @@
namespace
sot_controller
{
enum
SotControlMode
{
POSITION
,
EFFORT
};
enum
ControlMode
{
POSITION
,
VELOCITY
,
EFFORT
};
namespace
lhi
=
hardware_interface
;
namespace
lci
=
controller_interface
;
class
XmlrpcHelperException
:
public
ros
::
Exception
{
...
...
@@ -36,21 +42,27 @@ namespace sot_controller
};
struct
Effort
ControlPDMotorControlData
struct
ControlPDMotorControlData
{
control_toolbox
::
Pid
pid_controller
;
//double p_gain,d_gain,i_gain;
double
prev
;
double
vel_prev
;
double
des_pos
;
double
integ_err
;
EffortControlPDMotorControlData
();
ControlPDMotorControlData
();
// void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
void
read_from_xmlrpc_value
(
const
std
::
string
&
prefix
);
};
struct
JointSotHandle
{
lhi
::
JointHandle
joint
;
double
desired_init_pose
;
// This should not be handled in roscontrol_sot package. The control type
// should be handled in SoT directly, by externalizing the integration from
// the Device.
//ControlMode sot_control_mode;
ControlMode
ros_control_mode
;
};
typedef
std
::
map
<
std
::
string
,
JointSotHandle
>::
iterator
it_joint_sot_h
;
#ifndef CONTROLLER_INTERFACE_KINETIC
typedef
std
::
set
<
std
::
string
>
ClaimedResources
;
#endif
...
...
@@ -58,9 +70,6 @@ namespace sot_controller
This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
*/
namespace
lhi
=
hardware_interface
;
namespace
lci
=
controller_interface
;
class
RCSotController
:
public
lci
::
ControllerBase
,
SotLoaderBasic
{
...
...
@@ -76,7 +85,7 @@ namespace sot_controller
/// @{ \name Ros-control related fields
/// \brief Vector of joint handles.
std
::
vector
<
lhi
::
JointHandle
>
joints_
;
std
::
map
<
std
::
string
,
Joint
Sot
Handle
>
joints_
;
std
::
vector
<
std
::
string
>
joints_name_
;
/// \brief Vector towards the IMU.
...
...
@@ -94,6 +103,9 @@ namespace sot_controller
/// \brief Interface to the joints controlled in position.
lhi
::
PositionJointInterface
*
pos_iface_
;
/// \brief Interface to the joints controlled in position.
lhi
::
VelocityJointInterface
*
vel_iface_
;
/// \brief Interface to the joints controlled in force.
lhi
::
EffortJointInterface
*
effort_iface_
;
...
...
@@ -118,15 +130,13 @@ namespace sot_controller
/// \brief Adapt the interface to Gazebo simulation
bool
simulation_mode_
;
/// \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
,
Effort
ControlPDMotorControlData
>
effort_mode_pd_motors_
;
std
::
map
<
std
::
string
,
ControlPDMotorControlData
>
effort_mode_pd_motors_
;
/// \brief Give the desired position when the dynamic graph is not on.
std
::
vector
<
double
>
desired_init_pose_
;
/// \brief Implement a PD controller for the robot when the dynamic graph
/// is not on.
std
::
map
<
std
::
string
,
ControlPDMotorControlData
>
velocity_mode_pd_motors_
;
/// \brief Map from ros-control quantities to robot device
/// ros-control quantities are for the sensors:
...
...
@@ -179,8 +189,6 @@ namespace sot_controller
void
starting
(
const
ros
::
Time
&
);
/// \brief Stopping the control
void
stopping
(
const
ros
::
Time
&
);
/// \brief Display the kind of hardware interface that this controller is using.
virtual
std
::
string
getHardwareInterfaceType
()
const
;
protected:
/// Initialize the roscontrol interfaces
...
...
@@ -220,6 +228,9 @@ namespace sot_controller
/// \brief Read the control mode.
bool
readParamsControlMode
(
ros
::
NodeHandle
&
robot_nh
);
/// \brief Read the PID information of the robot in velocity mode.
bool
readParamsVelocityControlPDMotorControlData
(
ros
::
NodeHandle
&
robot_nh
);
/// \brief Read the PID information of the robot in effort mode.
bool
readParamsEffortControlPDMotorControlData
(
ros
::
NodeHandle
&
robot_nh
);
...
...
@@ -259,6 +270,8 @@ namespace sot_controller
///@{ Control the robot while waiting for the SoT
/// Default control in effort.
void
localStandbyEffortControlMode
(
const
ros
::
Duration
&
period
);
/// Default control in velocity.
void
localStandbyVelocityControlMode
(
const
ros
::
Duration
&
period
);
/// Default control in position.
void
localStandbyPositionControlMode
();
...
...
@@ -285,6 +298,13 @@ namespace sot_controller
/// Read URDF model from /robot_description parameter.
bool
readUrdf
(
ros
::
NodeHandle
&
robot_nh
);
/// Returns control mode by reading rosparam.
/// It reads /sot_controller/control_mode/joint_name
/// and check
bool
getJointControlMode
(
std
::
string
&
joint_name
,
JointSotHandle
&
aJointSotHandle
);
};
}
...
...
src/roscontrol-sot-parse-log.cc
View file @
ec23b4e6
#include
<fstream>
#include
<iostream>
#include
<iomanip>
#include
<cassert>
#include
<cstring>
void
usage
(
char
*
bin
)
{
std
::
cerr
<<
"Usage: "
<<
bin
<<
" [--separator sep] binary_file_name
\n
"
;
}
int
main
(
int
argc
,
char
*
argv
[])
{
if
(
argc
!=
2
)
{
std
::
cerr
<<
"Usage: "
<<
argv
[
0
]
<<
" binary_file_name
\n
"
;
if
(
argc
<
2
)
{
usage
(
argv
[
0
])
;
return
1
;
}
int
iarg
=
1
;
std
::
string
sep
(
" "
);
while
(
iarg
+
1
<
argc
-
1
)
{
if
(
strcmp
(
argv
[
iarg
],
"--separator"
)
==
0
)
{
sep
=
argv
[
iarg
+
1
];
iarg
+=
2
;
}
else
{
usage
(
argv
[
0
]);
return
1
;
}
}
assert
(
iarg
==
argc
-
1
);
std
::
ifstream
in
(
argv
[
1
],
std
::
ios
::
binary
);
std
::
ifstream
in
(
argv
[
iarg
],
std
::
ios
::
binary
);
if
(
!
in
.
is_open
()
||
!
in
.
good
())
{
std
::
cerr
<<
"Couldn't open file "
<<
argv
[
1
]
<<
'\n'
;
std
::
cerr
<<
"Couldn't open file "
<<
argv
[
iarg
]
<<
'\n'
;
return
2
;
}
// Read headers
unsigned
in
t
nVector
=
0
,
vectorSize
=
0
;
in
.
read
((
char
*
)
&
nVector
,
sizeof
(
unsigned
in
t
));
in
.
read
((
char
*
)
&
vectorSize
,
sizeof
(
unsigned
in
t
));
std
::
size_
t
nVector
=
0
,
vectorSize
=
0
;
in
.
read
((
char
*
)
&
nVector
,
sizeof
(
std
::
size_
t
));
in
.
read
((
char
*
)
&
vectorSize
,
sizeof
(
std
::
size_
t
));
if
(
!
in
.
good
())
{
std
::
cerr
<<
"Couldn't parse file: "
<<
argv
[
1
]
<<
'\n'
;
std
::
cerr
<<
"Couldn't parse file: "
<<
argv
[
iarg
]
<<
'\n'
;
return
3
;
}
...
...
@@ -33,10 +52,10 @@ int main (int argc, char* argv[])
in
.
read
((
char
*
)
&
v
,
sizeof
(
double
));
if
(
!
in
.
good
())
{
std
::
cerr
<<
"Stopped to parse at ("
<<
i
<<
','
<<
j
<<
") of file: "
<<
argv
[
1
]
<<
'\n'
;
<<
") of file: "
<<
argv
[
iarg
]
<<
'\n'
;
return
4
;
}
std
::
cout
<<
v
<<
' '
;
std
::
cout
<<
v
<<
sep
;
}
std
::
cout
<<
'\n'
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment