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
Paul Rouanet
ros2_control_bolt
Commits
24caf540
Commit
24caf540
authored
Jul 21, 2021
by
Paul Rouanet
Browse files
Modif stop function
parent
972e7820
Changes
1
Hide whitespace changes
Inline
Side-by-side
ros2_control_bolt/src/system_bolt.cpp
View file @
24caf540
...
...
@@ -113,16 +113,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
kp_
=
0.0
;
kd_
=
0.0
;
//Definition of Kp and Kd :
/*std::cout << "Enter Kp value (3 in casual use) : ";
std::cin >> kp_;
std::cout << "Kp = " << kp_;
std::cout << "\n" << std::endl;
std::cout << "Enter Kd value (0.05 in casual use) : ";
std::cin >> kd_;
std::cout << "Kd = " << kd_;
std::cout << "\n" << std::endl;*/
/*
for (const hardware_interface::ComponentInfo & joint : info.joints) {
...
...
@@ -513,8 +503,32 @@ return_type SystemBoltHardware::start()
return_type
SystemBoltHardware
::
stop
()
{
// Set 0 values to commands
for
(
const
hardware_interface
::
ComponentInfo
&
joint
:
info_
.
joints
)
{
/*
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
hw_commands_[joint.name] = {0.0, 0.0, 0.0, 0.0, 0.0};
}*/
// Put torques to 0.0
Eigen
::
Vector6d
torques
;
double
t
=
0.0
;
double
dt
=
0.001
;
std
::
chrono
::
time_point
<
std
::
chrono
::
system_clock
>
last
=
std
::
chrono
::
system_clock
::
now
();
while
(
!
robot_
->
IsTimeout
())
{
if
(((
std
::
chrono
::
duration
<
double
>
)(
std
::
chrono
::
system_clock
::
now
()
-
last
)).
count
()
>
0.001
)
{
last
=
std
::
chrono
::
system_clock
::
now
();
// last+dt would be better
t
+=
dt
;
for
(
const
hardware_interface
::
ComponentInfo
&
joint
:
info_
.
joints
)
{
torques
[
joint_name_to_motor_nb_
[
joint
.
name
]]
=
0.0
;
}
robot_
->
joints
->
SetTorques
(
torques
);
}
robot_
->
SendCommand
();
}
...
...
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