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
457c2c2c
Commit
457c2c2c
authored
Jul 23, 2021
by
Paul Rouanet
Browse files
Modif in write() fct
parent
214fa7e0
Changes
1
Show whitespace changes
Inline
Side-by-side
ros2_control_bolt/src/system_bolt.cpp
View file @
457c2c2c
...
@@ -528,7 +528,20 @@ SystemBoltHardware::write()
...
@@ -528,7 +528,20 @@ SystemBoltHardware::write()
Eigen
::
Vector6d
gain_KP
;
Eigen
::
Vector6d
gain_KP
;
Eigen
::
Vector6d
gain_KD
;
Eigen
::
Vector6d
gain_KD
;
for
(
const
hardware_interface
::
ComponentInfo
&
joint
:
info_
.
joints
)
{
positions
[
joint_name_to_motor_nb_
[
joint
.
name
]]
=
hw_commands_
[
joint
.
name
].
position
;
velocities
[
joint_name_to_motor_nb_
[
joint
.
name
]]
=
hw_commands_
[
joint
.
name
].
velocity
;
torques
[
joint_name_to_motor_nb_
[
joint
.
name
]]
=
hw_commands_
[
joint
.
name
].
effort
;
gain_KP
[
joint_name_to_motor_nb_
[
joint
.
name
]]
=
hw_commands_
[
joint
.
name
].
Kp
;
gain_KD
[
joint_name_to_motor_nb_
[
joint
.
name
]]
=
hw_commands_
[
joint
.
name
].
Kd
;
}
robot_
->
joints
->
SetDesiredPositions
(
positions
);
robot_
->
joints
->
SetDesiredVelocities
(
velocities
);
robot_
->
joints
->
SetTorques
(
torques
);
robot_
->
joints
->
SetPositionGains
(
gain_KP
);
robot_
->
joints
->
SetVelocityGains
(
gain_KD
);
/*
std::chrono::time_point<std::chrono::system_clock> last = std::chrono::system_clock::now();
std::chrono::time_point<std::chrono::system_clock> last = std::chrono::system_clock::now();
while (!robot_->IsTimeout()) {
while (!robot_->IsTimeout()) {
...
@@ -539,8 +552,8 @@ SystemBoltHardware::write()
...
@@ -539,8 +552,8 @@ SystemBoltHardware::write()
if (robot_->IsReady()) {
if (robot_->IsReady()) {
// Control law
// Control law
/*
positions = ...;
positions = ...;
torques = ...;
*/
torques = ...;
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
positions[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].position;
positions[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].position;
...
@@ -584,7 +597,7 @@ SystemBoltHardware::write()
...
@@ -584,7 +597,7 @@ SystemBoltHardware::write()
// }
// }
// RCLCPP_INFO(
// RCLCPP_INFO(
// rclcpp::get_logger("SystemBoltHardware"),
// rclcpp::get_logger("SystemBoltHardware"),
// "Joints sucessfully written!");
// "Joints sucessfully written!");
*/
return
return_type
::
OK
;
return
return_type
::
OK
;
}
}
...
...
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