Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Paul Rouanet
ros2_control_bolt
Commits
508d78fa
Commit
508d78fa
authored
Jul 22, 2021
by
Paul Rouanet
Browse files
Simplification of calibration() : use of RRunCalibration from ODRI
parent
27cc5f00
Changes
1
Hide whitespace changes
Inline
Side-by-side
ros2_control_bolt/src/system_bolt.cpp
View file @
508d78fa
...
...
@@ -423,7 +423,24 @@ SystemBoltHardware::export_command_interfaces()
return_type
SystemBoltHardware
::
calibration
(
const
hardware_interface
::
HardwareInfo
&
info
){
// Calibration part
double
Kp
=
stod
(
info
.
hardware_parameters
.
at
(
"calib_kp"
));
double
Kd
=
stod
(
info
.
hardware_parameters
.
at
(
"calib_kd"
));
double
T
=
stod
(
info
.
hardware_parameters
.
at
(
"calib_T"
));
double
dt
=
stod
(
info
.
hardware_parameters
.
at
(
"calib_dt"
));
std
::
vector
<
CalibrationMethod
>
directions
=
{
AUTO
,
AUTO
,
AUTO
,
AUTO
,
AUTO
,
AUTO
};
auto
calib_ctrl
=
std
::
make_shared
<
JointCalibrator
>
(
joints_
,
directions
,
position_offsets_
,
Kp
,
Kd
,
T
,
dt
);
robot_
->
RunCalibration
(
calib_ctrl
);
/*// Calibration part
double Kp = stod(info.hardware_parameters.at("calib_kp"));
double Kd = stod(info.hardware_parameters.at("calib_kd"));
double T = stod(info.hardware_parameters.at("calib_T"));
...
...
@@ -459,7 +476,7 @@ return_type SystemBoltHardware::calibration(
std::this_thread::yield();
}
}
*/
return
return_type
::
OK
;
}
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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