Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
R
roscontrol_sot
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Stack Of Tasks
roscontrol_sot
Commits
48ce14ca
Commit
48ce14ca
authored
6 years ago
by
Olivier Stasse
Browse files
Options
Downloads
Patches
Plain Diff
Adapt automatically to temperature cotnroller if present.
parent
ab3593c0
No related branches found
Branches containing commit
Tags
v0.0.4
Tags containing commit
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
CMakeLists.txt
+8
-1
8 additions, 1 deletion
CMakeLists.txt
src/roscontrol-sot-controller.cpp
+74
-30
74 additions, 30 deletions
src/roscontrol-sot-controller.cpp
src/roscontrol-sot-controller.hh
+10
-1
10 additions, 1 deletion
src/roscontrol-sot-controller.hh
with
92 additions
and
32 deletions
CMakeLists.txt
+
8
−
1
View file @
48ce14ca
...
...
@@ -70,6 +70,7 @@ 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"
)
# This is necessary so that the pc file generated by catking is similar to the on
# done directly by jrl-cmake-modules
...
...
@@ -77,13 +78,19 @@ catkin_package(CATKIN_DEPENDS
roscpp realtime_tools message_runtime dynamic_graph_bridge pal_hardware_interfaces controller_interface
LIBRARIES rcsot_controller
)
# Detect the
prf-ros-control
version to switch code
# Detect the
controller interface
version to switch code
if
(
CONTROLLER_INTERFACE_FOUND
)
if
(
${
CONTROLLER_INTERFACE_VERSION
}
VERSION_GREATER
"0.2.5"
)
add_definitions
(
-DCONTROLLER_INTERFACE_KINETIC
)
endif
(
${
CONTROLLER_INTERFACE_VERSION
}
VERSION_GREATER
"0.2.5"
)
endif
(
CONTROLLER_INTERFACE_FOUND
)
# Detect if temperature sensor controller package is found
# if yes then it is a PARL Robotics Forked code.
if
(
TEMPERATURE_SENSOR_CONTROLLER_FOUND
)
add_definitions
(
-DTEMPERATURE_SENSOR_CONTROLLER
)
endif
(
TEMPERATURE_SENSOR_CONTROLLER_FOUND
)
###########
## Build ##
...
...
This diff is collapsed.
Click to expand it.
src/roscontrol-sot-controller.cpp
+
74
−
30
View file @
48ce14ca
...
...
@@ -7,6 +7,7 @@
#include
<pluginlib/class_list_macros.h>
#include
"roscontrol-sot-controller.hh"
#include
<ros/console.h>
#if DEBUG
#define ODEBUG(x) std::cout << x << std::endl
...
...
@@ -164,55 +165,53 @@ namespace sot_controller
// Get a pointer to the joint position control interface
pos_iface_
=
robot_hw
->
get
<
PositionJointInterface
>
();
if
(
!
pos_iface_
)
if
(
!
pos_iface_
)
{
ROS_
ERROR
(
"This controller
requires
a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class."
,
ROS_
WARN
(
"This controller
did not find
a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class
if it is required
."
,
getHardwareInterfaceType
().
c_str
(),
lns
.
c_str
());
return
false
;
}
// Get a pointer to the joint effort control interface
effort_iface_
=
robot_hw
->
get
<
EffortJointInterface
>
();
if
(
!
effort_iface_
)
{
ROS_ERROR
(
"This controller requires a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class."
,
getHardwareInterfaceType
().
c_str
(),
lns
.
c_str
());
return
false
;
ROS_WARN
(
"This controller did not find a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class if it is required."
,
getHardwareInterfaceType
().
c_str
(),
lns
.
c_str
());
}
// Get a pointer to the force-torque sensor interface
ft_iface_
=
robot_hw
->
get
<
ForceTorqueSensorInterface
>
();
if
(
!
ft_iface_
)
{
ROS_
ERROR
(
"This controller
requires
a hardware interface of type '%s '. "
" Make sure this is registered inthe %s::RobotHW class."
,
ROS_
WARN
(
"This controller
did not find
a hardware interface of type '%s '. "
" Make sure this is registered inthe %s::RobotHW class
if it is required
."
,
internal
::
demangledTypeName
<
ForceTorqueSensorInterface
>
().
c_str
(),
lns
.
c_str
());
return
false
;
}
// Get a pointer to the IMU sensor interface
imu_iface_
=
robot_hw
->
get
<
ImuSensorInterface
>
();
if
(
!
imu_iface_
)
{
ROS_ERROR
(
"This controller requires a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class."
,
internal
::
demangledTypeName
<
ImuSensorInterface
>
().
c_str
(),
lns
.
c_str
());
return
false
;
ROS_WARN
(
"This controller did not find a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class if it is required."
,
internal
::
demangledTypeName
<
ImuSensorInterface
>
().
c_str
(),
lns
.
c_str
());
}
// Temperature sensor not available in simulation mode
if
(
!
simulation_mode_
)
{
#ifdef TEMPERATURE_SENSOR_CONTROLLER_FOUND
// Get a pointer to the actuator temperature sensor interface
act_temp_iface_
=
robot_hw
->
get
<
ActuatorTemperatureSensorInterface
>
();
if
(
!
act_temp_iface_
)
{
ROS_
ERROR
(
"This controller
requires
a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class."
,
ROS_
WARN
(
"This controller
did not find
a hardware interface of type '%s'."
" Make sure this is registered in the %s::RobotHW class
if it is required
."
,
internal
::
demangledTypeName
<
ActuatorTemperatureSensorInterface
>
().
c_str
(),
lns
.
c_str
());
return
false
;
}
#endif
}
...
...
@@ -330,15 +329,17 @@ namespace sot_controller
{
XmlRpc
::
XmlRpcValue
xml_rpc_pci_el
;
ROS_INFO
(
"/sot_controller/position_control_init_pos/ %ld: %d %d %d
\n
"
,
i
,
xml_rpc_pci_pose
[
i
].
getType
(),
XmlRpc
::
XmlRpcValue
::
TypeArray
,
XmlRpc
::
XmlRpcValue
::
TypeStruct
);
ROS_INFO
(
"/sot_controller/position_control_init_pos/ %ld: %ld %d %d
\n
"
,
i
,
xml_rpc_pci_pose
[(
int
)
i
].
getType
(),
XmlRpc
::
XmlRpcValue
::
TypeArray
,
XmlRpc
::
XmlRpcValue
::
TypeStruct
);
if
(
xml_rpc_pci_pose
[
i
].
hasMember
(
"name"
))
if
(
xml_rpc_pci_pose
[
(
int
)
i
].
hasMember
(
"name"
))
{
std
::
string
local_joint_name
=
std
::
string
(
xml_rpc_pci_pose
[
i
][
"name"
]);
if
(
xml_rpc_pci_pose
[
i
].
hasMember
(
"name"
))
std
::
string
local_joint_name
=
std
::
string
(
xml_rpc_pci_pose
[
(
int
)
i
][
"name"
]);
if
(
xml_rpc_pci_pose
[
(
int
)
i
].
hasMember
(
"name"
))
{
double
local_des_pose
=
double
(
xml_rpc_pci_pose
[
i
][
"des_pos"
]);
double
local_des_pose
=
double
(
xml_rpc_pci_pose
[
(
int
)
i
][
"des_pos"
]);
desired_init_pose_
[
local_joint_name
]
=
local_des_pose
;
}
else
...
...
@@ -441,8 +442,26 @@ namespace sot_controller
robot_nh
.
getParam
(
"/sot_controller/joint_names"
,
joints_name_
);
for
(
std
::
vector
<
std
::
string
>::
size_type
i
=
0
;
i
<
joints_name_
.
size
();
i
++
)
{
ROS_INFO_STREAM
(
"joints_name_["
<<
i
<<
"]="
<<
joints_name_
[
i
]);}
{
ROS_INFO_STREAM
(
"joints_name_["
<<
i
<<
"]="
<<
joints_name_
[
i
]);
if
(
modelURDF_
.
use_count
())
{
urdf
::
JointConstSharedPtr
aJCSP
=
modelURDF_
->
getJoint
(
joints_name_
[
i
]);
if
(
aJCSP
.
use_count
()
!=
0
)
ROS_INFO_STREAM
(
joints_name_
[
i
]
+
" found in the robot model"
);
else
{
ROS_ERROR
(
" %s not found in the robot model"
,
joints_name_
[
i
]);
return
false
;
}
}
else
{
ROS_ERROR
(
"No robot model loaded in /robot_description"
);
return
false
;
}
}
}
else
return
false
;
...
...
@@ -510,6 +529,25 @@ namespace sot_controller
ROS_ERROR
(
"You need to define a control period in param /sot_controller/dt"
);
return
false
;
}
bool
RCSotController
::
readUrdf
(
ros
::
NodeHandle
&
robot_nh
)
{
/// Reading the parameter /robot_description which contains the robot
/// description
if
(
!
robot_nh
.
hasParam
(
"/robot_description"
))
{
ROS_ERROR
(
"ROS application does not have robot_description"
);
return
false
;
}
std
::
string
robot_description_str
;
robot_nh
.
getParam
(
"/robot_description"
,
robot_description_str
);
modelURDF_
=
urdf
::
parseURDF
(
robot_description_str
);
ROS_INFO
(
"Loaded /robot_description %d"
,
modelURDF_
.
use_count
());
return
true
;
}
bool
RCSotController
::
readParams
(
ros
::
NodeHandle
&
robot_nh
)
...
...
@@ -525,6 +563,9 @@ namespace sot_controller
if
(
robot_nh
.
hasParam
(
"/sot_controller/simulation_mode"
))
simulation_mode_
=
true
;
/// Read URDF file.
readUrdf
(
robot_nh
);
/// Calls readParamsJointNames
// Reads the list of joints to be controlled.
if
(
!
readParamsJointNames
(
robot_nh
))
...
...
@@ -672,12 +713,15 @@ namespace sot_controller
for
(
unsigned
int
idJoint
=
0
;
idJoint
<
joints_
.
size
();
idJoint
++
)
{
DataOneIter_
.
motor_angle
[
idJoint
]
=
joints_
[
idJoint
].
getPosition
();
//if (!simulation_mode_)
DataOneIter_
.
joint_angle
[
idJoint
]
=
joints_
[
idJoint
].
getAbsolutePosition
();
#ifdef TEMPERATURE_SENSOR_CONTROLLER_FOUND
DataOneIter_
.
joint_angle
[
idJoint
]
=
joints_
[
idJoint
].
getAbsolutePosition
();
#endif
DataOneIter_
.
velocities
[
idJoint
]
=
joints_
[
idJoint
].
getVelocity
();
//if (!simulation_mode_)
DataOneIter_
.
torques
[
idJoint
]
=
joints_
[
idJoint
].
getTorqueSensor
();
#ifdef TEMPERATURE_SENSOR_CONTROLLER_FOUND
DataOneIter_
.
torques
[
idJoint
]
=
joints_
[
idJoint
].
getTorqueSensor
();
#endif
DataOneIter_
.
motor_currents
[
idJoint
]
=
joints_
[
idJoint
].
getEffort
();
}
...
...
This diff is collapsed.
Click to expand it.
src/roscontrol-sot-controller.hh
+
10
−
1
View file @
48ce14ca
...
...
@@ -45,6 +45,10 @@
#include
<ros/ros.h>
#include
<control_toolbox/pid.h>
/** URDF DOM*/
#include
<urdf_parser/urdf_parser.h>
/* Local header */
#include
"log.hh"
namespace
sot_controller
...
...
@@ -165,7 +169,10 @@ namespace sot_controller
/// Jitter for the subsampling.
double
jitter_
;
/// URDF model of the robot.
urdf
::
ModelInterfaceSharedPtr
modelURDF_
;
public
:
RCSotController
();
...
...
@@ -287,6 +294,8 @@ namespace sot_controller
/// One iteration: read sensor, compute the control law, apply control.
void
one_iteration
();
/// Read URDF model from /robot_description parameter.
bool
readUrdf
(
ros
::
NodeHandle
&
robot_nh
);
};
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment