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
d5f786a7
Commit
d5f786a7
authored
Aug 02, 2019
by
Olivier Stasse
Browse files
Merge remote-tracking branch 'remotes/origin/devel' into origin-master-2019-07-13
parents
ec23b4e6
d3168e6e
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/roscontrol-sot-controller.cpp
View file @
d5f786a7
...
...
@@ -252,6 +252,7 @@ namespace sot_controller
// Return which resources are claimed by this controller
pos_iface_
->
clearClaims
();
vel_iface_
->
clearClaims
();
effort_iface_
->
clearClaims
();
if
(
!
init
())
...
...
@@ -270,20 +271,25 @@ namespace sot_controller
demangledTypeName
<
PositionJointInterface
>
();
iface_res
.
resources
=
pos_iface_
->
getClaims
();
claimed_resources
.
push_back
(
iface_res
);
/// Display claimed ressources
if
(
verbosity_level_
>
0
)
displayClaimedResources
(
claimed_resources
);
pos_iface_
->
clearClaims
();
iface_res
.
hardware_interface
=
hardware_interface
::
internal
::
demangledTypeName
<
VelocityJointInterface
>
();
iface_res
.
resources
=
vel_iface_
->
getClaims
();
claimed_resources
.
push_back
(
iface_res
);
iface_res
.
hardware_interface
=
hardware_interface
::
internal
::
demangledTypeName
<
EffortJointInterface
>
();
iface_res
.
resources
=
effort_iface_
->
getClaims
();
claimed_resources
.
push_back
(
iface_res
);
/// Display claimed ressources
if
(
verbosity_level_
>
0
)
displayClaimedResources
(
claimed_resources
);
pos_iface_
->
clearClaims
();
vel_iface_
->
clearClaims
();
effort_iface_
->
clearClaims
();
#else
claimed_resources
=
pos_iface_
->
getClaims
();
...
...
@@ -292,6 +298,12 @@ namespace sot_controller
displayClaimedResources
(
claimed_resources
);
pos_iface_
->
clearClaims
();
claimed_resources
=
vel_iface_
->
getClaims
();
/// Display claimed ressources
if
(
verbosity_level_
>
0
)
displayClaimedResources
(
claimed_resources
);
vel_iface_
->
clearClaims
();
claimed_resources
=
effort_iface_
->
getClaims
();
if
(
verbosity_level_
>
0
)
displayClaimedResources
(
claimed_resources
);
...
...
@@ -622,10 +634,11 @@ namespace sot_controller
for
(
unsigned
int
idJoint
=
0
;
idJoint
<
joints_name_
.
size
();
idJoint
++
)
{
std
::
string
joint_name
=
joints_name_
[
idJoint
];
ROS_INFO
(
"joint_name[%d]=%s"
,
idJoint
,
joint_name
.
c_str
());
JointSotHandle
&
aJoint
=
joints_
[
joint_name
];
if
(
!
getJointControlMode
(
joint_name
,
aJoint
))
return
false
;
ROS_INFO
(
"joint_name[%d]=%s, control_mode=%d"
,
idJoint
,
joint_name
.
c_str
(),
aJoint
.
ros_control_mode
);
}
}
else
...
...
@@ -1189,6 +1202,16 @@ namespace sot_controller
}
else
{
/// Update the sensors.
fillSensors
();
/// Generate a control law.
try
{
sotController_
->
nominalSetSensors
(
sensorsIn_
);
}
catch
(
std
::
exception
&
e
)
{
throw
e
;}
// But in effort mode it means that we are sending 0
// Therefore implements a default PD controller on the system.
// Applying both to handle mixed system.
...
...
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