Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
G
gz_gep_tools
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
Package Registry
Container Registry
Model registry
Operate
Environments
Terraform modules
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
Olivier Stasse
gz_gep_tools
Commits
202faa5e
Commit
202faa5e
authored
2 weeks ago
by
Olivier Stasse
Browse files
Options
Downloads
Patches
Plain Diff
Adding ControlJointValue to object joint_state_interface.
parent
247b1893
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/gz_gep_tools/joint_state_interface.hh
+65
-27
65 additions, 27 deletions
include/gz_gep_tools/joint_state_interface.hh
src/joint_state_interface.cc
+121
-41
121 additions, 41 deletions
src/joint_state_interface.cc
with
186 additions
and
68 deletions
include/gz_gep_tools/joint_state_interface.hh
+
65
−
27
View file @
202faa5e
...
...
@@ -9,35 +9,79 @@
#include
<gz/transport.hh>
namespace
gz_transport_hw_tools
{
/// This class is to be used by the user to provide:
/// @param Kp: Gain to reach the desired position pos_des
/// @param Kd: Gain to reach the desired velocity vel_des
/// @param Ki; Gain for the integral error
/// @param i_clamp: Clamping the integral action
/// @param pos_des: Desired position (used for the initailization phase, i.e. at
/// start up)
/// @param vel_des: Velocity position (used for the initailization phase, i.e.
/// at start up)
/// Each joint has such a structure which can initialized in a container here a map.
class
ControlJointValue
{
public:
ControlJointValue
(
double
Kp
,
double
Kd
,
double
Ki
,
double
i_clamp
,
double
pos_des
,
double
vel_des
);
ControlJointValue
(
const
ControlJointValue
&
other
);
ControlJointValue
(
const
ControlJointValue
&&
other
)
noexcept
;
double
Cmd
()
const
{
return
cmd_
;}
double
PosMes
()
const
{
return
pos_mes_
;}
double
VelMes
()
const
{
return
vel_mes_
;}
double
PosDes
()
const
{
return
pos_des_
;}
double
VelDes
()
const
{
return
vel_des_
;}
void
SetPosMes
(
double
&
pos_mes
)
{
pos_mes_
=
pos_mes
;}
void
SetVelMes
(
double
&
vel_mes
)
{
vel_mes_
=
vel_mes
;}
void
ComputeCmd
();
private
:
/// Desired quantities
double
pos_des
,
vel_des
;
double
pos_des
_
,
vel_des
_
;
// Control gains
double
Kp
,
Kd
;
double
Kp
_
,
Kd
_
,
Ki_
;
// Clamping i value to i_clamp
double
i_clamp_
;
// Measured quantities
double
pos_mes
,
vel_mes
;
double
pos_mes_
,
vel_mes_
;
/// Command
double
cmd_
;
/// Cumulative error
double
cumul_err_
;
double
cmd
;
void
compute_cmd
()
{
cmd
=
Kp
*
(
pos_des
-
pos_mes
)
+
Kd
*
(
vel_des
-
vel_mes
);
}
};
/// Definition of the type handling a map of controlled joint
typedef
std
::
map
<
std
::
string
,
ControlJointValue
>
RobotCtrlJointInfos
;
bool
SaveCmd
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
);
const
std
::
string
&
afilename
);
bool
SavePos
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
);
class
JointValues
{
const
std
::
string
&
afilename
);
bool
SavePosDes
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
);
bool
SaveListOfNamedJoints
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
);
/// Intermediate structure used to read information from Gazebo "joint_name/joint_state" topic.
///
class
GZJointValues
{
public:
/// Measured quantities
double
pos_mes
;
...
...
@@ -50,18 +94,18 @@ class JointValues {
};
class
RobotJoints
{
class
GZ
RobotJoints
{
public:
/// Time
int64_t
time_sec_
;
int64_t
time_nsec_
;
/// Map of joints values
std
::
map
<
std
::
string
,
JointValues
>
dict_joint_values
;
std
::
map
<
std
::
string
,
GZ
JointValues
>
dict_joint_values
;
std
::
mutex
lock_state_access_
;
RobotJoints
();
GZ
RobotJoints
();
};
...
...
@@ -96,20 +140,14 @@ class JointStateInterface {
/// Prefix world
std
::
string
prefix_world_
;
/// List of joints
std
::
vector
<
std
::
string
>
list_of_joints_
;
/// Map joints to index in list
std
::
map
<
std
::
string
,
std
::
size_t
>
map_name_2_indx_
;
/// Topic name joint state
std
::
string
joint_state_topic_
;
/// GZ node
gz
::
transport
::
Node
node_
;
/// Last state of the robot
RobotJoints
robot_joints_
;
/// Last state of the robot
from Gazebo
GZ
RobotJoints
gz_
robot_joints_
;
bool
debug_level_
;
};
...
...
This diff is collapsed.
Click to expand it.
src/joint_state_interface.cc
+
121
−
41
View file @
202faa5e
...
...
@@ -7,33 +7,104 @@
namespace
gz_transport_hw_tools
{
ControlJointValue
::
ControlJointValue
(
double
Kp
,
double
Kd
,
double
Ki
,
double
i_clamp
,
double
pos_des
,
double
vel_des
)
:
pos_des_
(
pos_des
),
vel_des_
(
vel_des
),
Kp_
(
Kp
),
Kd_
(
Kd
),
Ki_
(
Ki
),
i_clamp_
(
i_clamp
),
pos_mes_
(
0.0
),
vel_mes_
(
0.0
),
cmd_
(
0.0
),
cumul_err_
(
0.0
)
{
}
ControlJointValue
::
ControlJointValue
(
const
ControlJointValue
&
other
)
:
ControlJointValue
(
other
.
Kp_
,
other
.
Kd_
,
other
.
Ki_
,
other
.
i_clamp_
,
other
.
pos_des_
,
other
.
vel_des_
)
{
pos_mes_
=
other
.
pos_mes_
;
vel_mes_
=
other
.
vel_mes_
;
cmd_
=
other
.
cmd_
;
cumul_err_
=
other
.
cumul_err_
;
}
ControlJointValue
::
ControlJointValue
(
const
ControlJointValue
&&
other
)
noexcept
:
ControlJointValue
(
other
.
Kp_
,
other
.
Kd_
,
other
.
Ki_
,
other
.
i_clamp_
,
other
.
pos_des_
,
other
.
vel_des_
)
{
pos_mes_
=
other
.
pos_mes_
;
vel_mes_
=
other
.
vel_mes_
;
cmd_
=
other
.
cmd_
;
cumul_err_
=
other
.
cumul_err_
;
}
void
ControlJointValue
::
ComputeCmd
()
{
double
err_pos
=
pos_des_
-
pos_mes_
;
double
i_term
=
Ki_
*
cumul_err_
;
if
(
i_term
>
i_clamp_
)
i_term
=
i_clamp_
;
else
if
(
i_term
<
-
i_clamp_
)
i_term
=
-
i_clamp_
;
cmd_
=
Kp_
*
err_pos
+
Kd_
*
(
vel_des_
-
vel_mes_
)
+
i_term
;
cumul_err_
+=
err_pos
;
}
bool
SavePos
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
)
const
std
::
string
&
afilename
)
{
std
::
ofstream
ofs_position_robot
(
afilename
.
c_str
(),
std
::
ios
::
app
);
for
(
auto
it_joint_info
=
a_rbt_ctrl_joint_infos
.
begin
();
it_joint_info
!=
a_rbt_ctrl_joint_infos
.
end
();
it_joint_info
++
)
ofs_position_robot
<<
it_joint_info
->
second
.
PosMes
()
<<
" "
;
ofs_position_robot
<<
std
::
endl
;
ofs_position_robot
.
close
();
return
true
;
}
bool
SavePosDes
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
)
{
std
::
ofstream
ofs_position_robot
(
afilename
.
c_str
(),
std
::
ios
::
app
);
for
(
auto
it_joint_info
=
a_rbt_ctrl_joint_infos
.
begin
();
it_joint_info
!=
a_rbt_ctrl_joint_infos
.
end
();
it_joint_info
++
)
ofs_position_robot
<<
it_joint_info
->
second
.
p
os
_mes
<<
" "
;
ofs_position_robot
<<
it_joint_info
->
second
.
P
os
Des
()
<<
" "
;
ofs_position_robot
<<
std
::
endl
;
ofs_position_robot
.
close
();
return
true
;
}
bool
SaveCmd
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
)
const
std
::
string
&
afilename
)
{
std
::
ofstream
ofs_position_robot
(
afilename
.
c_str
(),
std
::
ios
::
app
);
for
(
auto
it_joint_info
=
a_rbt_ctrl_joint_infos
.
begin
();
it_joint_info
!=
a_rbt_ctrl_joint_infos
.
end
();
it_joint_info
++
)
ofs_position_robot
<<
it_joint_info
->
second
.
c
md
<<
" "
;
ofs_position_robot
<<
it_joint_info
->
second
.
C
md
()
<<
" "
;
ofs_position_robot
<<
std
::
endl
;
ofs_position_robot
.
close
();
return
true
;
}
RobotJoints
::
RobotJoints
()
:
bool
SaveListOfNamedJoints
(
const
RobotCtrlJointInfos
&
a_rbt_ctrl_joint_infos
,
const
std
::
string
&
afilename
)
{
std
::
ofstream
ofs_position_robot
(
afilename
.
c_str
(),
std
::
ios
::
app
);
for
(
auto
it_joint_info
=
a_rbt_ctrl_joint_infos
.
begin
();
it_joint_info
!=
a_rbt_ctrl_joint_infos
.
end
();
it_joint_info
++
)
ofs_position_robot
<<
it_joint_info
->
first
<<
" "
;
ofs_position_robot
<<
std
::
endl
;
ofs_position_robot
.
close
();
return
true
;
}
GZRobotJoints
::
GZRobotJoints
()
:
time_sec_
(
0
),
time_nsec_
(
0
)
{}
...
...
@@ -57,7 +128,7 @@ JointStateInterface::~JointStateInterface() {}
void
JointStateInterface
::
SetListOfJoints
(
const
RobotCtrlJointInfos
&
rbt_ctrl_joint_infos
)
{
robot_joints_
.
dict_joint_values
.
clear
();
gz_
robot_joints_
.
dict_joint_values
.
clear
();
for
(
auto
jointItr
=
rbt_ctrl_joint_infos
.
begin
();
jointItr
!=
rbt_ctrl_joint_infos
.
end
();
...
...
@@ -65,13 +136,13 @@ void JointStateInterface::SetListOfJoints(
/// Build a new string
std
::
string
a_new_cmd_force
=
prefix_model_root_
+
std
::
string
(
"joint/"
)
+
jointItr
->
first
+
std
::
string
(
"/cmd_force"
);
/// Build advertise
robot_joints_
.
dict_joint_values
[
jointItr
->
first
].
gz_pub_cmd_force
=
gz_
robot_joints_
.
dict_joint_values
[
jointItr
->
first
].
gz_pub_cmd_force
=
node_
.
Advertise
<
gz
::
msgs
::
Double
>
(
a_new_cmd_force
);
robot_joints_
.
dict_joint_values
[
jointItr
->
first
].
cmd_force_topic
=
a_new_cmd_force
;
gz_
robot_joints_
.
dict_joint_values
[
jointItr
->
first
].
cmd_force_topic
=
a_new_cmd_force
;
}
}
...
...
@@ -80,7 +151,7 @@ void JointStateInterface::CallbackJointState(
const
gz
::
msgs
::
Model
&
a_gz_model_msg
)
{
std
::
lock_guard
(
robot_joints_
.
lock_state_access_
);
std
::
lock_guard
(
gz_
robot_joints_
.
lock_state_access_
);
for
(
auto
jointItr
=
a_gz_model_msg
.
joint
().
begin
();
jointItr
!=
a_gz_model_msg
.
joint
().
end
();
...
...
@@ -88,61 +159,61 @@ void JointStateInterface::CallbackJointState(
const
::
gz
::
msgs
::
Axis
&
axis1
=
jointItr
->
axis1
();
robot_joints_
.
dict_joint_values
[
jointItr
->
name
()].
pos_mes
=
axis1
.
position
();
robot_joints_
.
dict_joint_values
[
jointItr
->
name
()].
vel_mes
=
axis1
.
velocity
();
gz_
robot_joints_
.
dict_joint_values
[
jointItr
->
name
()].
pos_mes
=
axis1
.
position
();
gz_
robot_joints_
.
dict_joint_values
[
jointItr
->
name
()].
vel_mes
=
axis1
.
velocity
();
}
if
(
debug_level_
)
{
std
::
cerr
<<
"JointStateInterface::CallbackJointState: time: "
<<
" "
<<
robot_joints_
.
time_sec_
<<
" "
<<
robot_joints_
.
time_nsec_
;
if
(
robot_joints_
.
dict_joint_values
[
"arm_left_1_joint"
].
pos_mes
)
std
::
cerr
<<
" "
<<
robot_joints_
.
dict_joint_values
[
"arm_left_1_joint"
].
pos_mes
;
<<
" "
<<
gz_
robot_joints_
.
time_sec_
<<
" "
<<
gz_
robot_joints_
.
time_nsec_
;
if
(
gz_
robot_joints_
.
dict_joint_values
[
"arm_left_1_joint"
].
pos_mes
)
std
::
cerr
<<
" "
<<
gz_
robot_joints_
.
dict_joint_values
[
"arm_left_1_joint"
].
pos_mes
;
std
::
cerr
<<
std
::
endl
;
}
robot_joints_
.
time_sec_
=
a_gz_model_msg
.
header
().
stamp
().
sec
();
robot_joints_
.
time_nsec_
=
a_gz_model_msg
.
header
().
stamp
().
nsec
();
gz_
robot_joints_
.
time_sec_
=
a_gz_model_msg
.
header
().
stamp
().
sec
();
gz_
robot_joints_
.
time_nsec_
=
a_gz_model_msg
.
header
().
stamp
().
nsec
();
}
bool
JointStateInterface
::
SetCmd
(
const
RobotCtrlJointInfos
&
rbt_ctrl_joint_infos
)
{
if
(
rbt_ctrl_joint_infos
.
size
()
>
robot_joints_
.
dict_joint_values
.
size
())
if
(
rbt_ctrl_joint_infos
.
size
()
>
gz_
robot_joints_
.
dict_joint_values
.
size
())
{
std
::
cerr
<<
"rbt_ctrl_joint_infos.size(): "
<<
rbt_ctrl_joint_infos
.
size
()
<<
" robot_joints_.dict_joint_values.size()"
<<
robot_joints_
.
dict_joint_values
.
size
()
<<
"
gz_
robot_joints_.dict_joint_values.size()"
<<
gz_
robot_joints_
.
dict_joint_values
.
size
()
<<
std
::
endl
;
return
false
;
}
// Iterate over the cmd map.
for
(
auto
cmd_it
=
rbt_ctrl_joint_infos
.
begin
();
cmd_it
!=
rbt_ctrl_joint_infos
.
end
();
cmd_it
++
)
cmd_it
++
)
{
gz
::
msgs
::
Double
msg
;
msg
.
set_data
(
cmd_it
->
second
.
c
md
);
msg
.
set_data
(
cmd_it
->
second
.
C
md
()
);
auto
it_robot_joint_value
=
robot_joints_
.
dict_joint_values
.
find
(
cmd_it
->
first
);
auto
it_robot_joint_value
=
gz_
robot_joints_
.
dict_joint_values
.
find
(
cmd_it
->
first
);
// If the joint is not found go to the next iteration.
if
(
it_robot_joint_value
==
robot_joints_
.
dict_joint_values
.
end
())
if
(
it_robot_joint_value
==
gz_
robot_joints_
.
dict_joint_values
.
end
())
continue
;
if
(
!
it_robot_joint_value
->
second
.
gz_pub_cmd_force
.
Publish
(
msg
)){
std
::
cerr
<<
"Unable to publish on "
<<
it_robot_joint_value
->
second
.
cmd_force_topic
<<
std
::
endl
;
continue
;
}
}
if
(
debug_level_
)
{
std
::
cout
<<
" Publish "
<<
cmd_it
->
second
.
c
md
<<
" on "
<<
robot_joints_
.
dict_joint_values
[
cmd_it
->
first
].
cmd_force_topic
std
::
cout
<<
" Publish "
<<
cmd_it
->
second
.
C
md
()
<<
" on "
<<
gz_
robot_joints_
.
dict_joint_values
[
cmd_it
->
first
].
cmd_force_topic
<<
std
::
endl
;
}
}
...
...
@@ -152,28 +223,37 @@ bool JointStateInterface::SetCmd( const RobotCtrlJointInfos &rbt_ctrl_joint_info
bool
JointStateInterface
::
GetPosVel
(
RobotCtrlJointInfos
&
rbt_ctrl_joint_infos
,
double
&
time
)
{
if
(
rbt_ctrl_joint_infos
.
size
()
>
robot_joints_
.
dict_joint_values
.
size
())
if
(
rbt_ctrl_joint_infos
.
size
()
>
gz_
robot_joints_
.
dict_joint_values
.
size
())
{
std
::
cerr
<<
"rbt_ctrl_joint_infos.size(): "
<<
rbt_ctrl_joint_infos
.
size
()
<<
" robot_joints_.dict_joint_values.size():"
<<
robot_joints_
.
dict_joint_values
.
size
()
<<
"
gz_
robot_joints_.dict_joint_values.size():"
<<
gz_
robot_joints_
.
dict_joint_values
.
size
()
<<
std
::
endl
;
return
false
;
}
std
::
lock_guard
(
robot_joints_
.
lock_state_access_
);
time
=
(
double
)
robot_joints_
.
time_sec_
+
1e-9
*
(
double
)
robot_joints_
.
time_nsec_
;
for
(
auto
joint_it
=
rbt_ctrl_joint_infos
.
begin
();
joint_it
!=
rbt_ctrl_joint_infos
.
end
();
joint_it
++
)
std
::
lock_guard
(
gz_
robot_joints_
.
lock_state_access_
);
time
=
(
double
)
gz_
robot_joints_
.
time_sec_
+
1e-9
*
(
double
)
gz_
robot_joints_
.
time_nsec_
;
for
(
auto
ctrl_
joint_it
=
rbt_ctrl_joint_infos
.
begin
();
ctrl_
joint_it
!=
rbt_ctrl_joint_infos
.
end
();
ctrl_
joint_it
++
)
{
joint_it
->
second
.
pos_mes
=
robot_joints_
.
dict_joint_values
[
joint_it
->
first
].
pos_mes
;
joint_it
->
second
.
vel_des
=
robot_joints_
.
dict_joint_values
[
joint_it
->
first
].
vel_mes
;
ctrl_
joint_it
->
second
.
SetPosMes
(
gz_
robot_joints_
.
dict_joint_values
[
ctrl_
joint_it
->
first
].
pos_mes
)
;
ctrl_
joint_it
->
second
.
SetVelMes
(
gz_
robot_joints_
.
dict_joint_values
[
ctrl_
joint_it
->
first
].
vel_mes
)
;
}
if
(
debug_level_
)
{
std
::
cerr
<<
"JointStateInterface::GetPosVel: time: "
<<
time
<<
" "
<<
rbt_ctrl_joint_infos
[
"arm_left_1_joint"
].
pos_mes
<<
" "
<<
std
::
endl
;
for
(
auto
ctrl_joint_it
=
rbt_ctrl_joint_infos
.
begin
();
ctrl_joint_it
!=
rbt_ctrl_joint_infos
.
end
();
ctrl_joint_it
++
)
{
std
::
cerr
<<
ctrl_joint_it
->
second
.
PosMes
()
<<
" "
;
}
std
::
cerr
<<
std
::
endl
;
}
return
true
;
}
...
...
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