This PR aims to deal with the control or not of the upper body. #19

MaximilienNaveau · 2023-04-06T13:49:16Z

In this PR there is a fix for the upper body, namely about this issue: #18
For now the upper body is controlled in torques but make the left wrists controlled and the nan measured in the torques set to zero.

The changes provides 2 things:

parametrization of the talos_params roslaunch to activate/deactivate leg/arm/wrists/head/gripper, etc...
the safe evaluation of joint sensor data when they are not provided

One can now create a simple node to start the lf on robot like so:
my_lfc_starting_node.sh:

ssh robot-name:~ 'roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=false default_params:=false activate_right_leg:=true activate_right_leg:=true activate_torso:=true'

This line will allow you to use your own parameters or the default ones and to activate/de-activate the limbs of the robot you are interested in.

…et to zero

ComePerrot

I tested this code in simulation and didn't have any problem.
However, I didn't manage to reproduce the error related to the lack of torque sensors on the joints of the wrist.

ComePerrot · 2023-04-12T15:17:37Z

config/talos_linear_feedback_controller_params.yaml

I think the moving joint names list should be handled by the main demo package (i.e. not the linear_feedback_controller).
The MPC has a similar parameter which means that, for now, it is duplicated between the two packages.

these params are available via ROS params. So the controller can also read them from there.

ComePerrot · 2023-04-12T15:43:46Z

launch/talos_params.launch

I don't know what the best solution is for this file.
Having a custom version in every demo package, which sets up the robot correctly, may be the most straightforward way to go.
On the other hand, having a way to automatically parametrize the controlled joints using the moving_joint_names list could prevent mistakes where only the parameter file is updated and not the launch file.

Should we setup the whole body by default? or the lower body only?
This should be considered as an example roslaunch.

Whole body should be default in my opinion.

ComePerrot · 2023-04-12T15:43:59Z

src/linear_feedback_controller.cpp

This is the most important change of the PR in my opinion.
I am not sure it is necessary to replace the NaNs in the eigen_sensor_msg_ by zeros.
In any case, I think a warning should be added when a value is modified.

Yes I will flag once with an error message when this happens with a comprehensive error.

MaximilienNaveau · 2023-04-18T09:19:10Z

src/linear_feedback_controller.cpp

@@ -483,6 +488,9 @@ void LinearFeedbackController::acquireSensorAndPublish(
    eigen_sensor_msg_.joint_state.effort(i) =
        getJointMeasuredTorque(pin_to_hwi_[i]) -
        in_torque_offsets_[pin_to_hwi_[i]];
+    if (std::isnan(eigen_sensor_msg_.joint_state.effort(i))) {


So here we should keep sending NaNs?

I do not see why we should send NaNs here.

The question I was asking is, should we remove the NaNs from the measures if there are any?
In my opinion we should either send a value that has been measured/estimated or send a NaN (and not just replace a NaN by 0).

My question was mainly to know if sending NaNs could lead to unwanted behavior in the computation of the MPC (in that case we should change them to 0).
There might be some NaNs in the effort measurement when the wrist are controlled because there are no torque sensors on those joints. In my case I know it won't change anything because I don't use the effort measurements at all.

edantec

I did not test the draft but Come did, so I assume the PR is good to go.

edantec · 2023-04-18T09:39:37Z

launch/talos_params.launch

Whole body should be default in my opinion.

edantec · 2023-04-18T09:42:00Z

src/linear_feedback_controller.cpp

@@ -483,6 +488,9 @@ void LinearFeedbackController::acquireSensorAndPublish(
    eigen_sensor_msg_.joint_state.effort(i) =
        getJointMeasuredTorque(pin_to_hwi_[i]) -
        in_torque_offsets_[pin_to_hwi_[i]];
+    if (std::isnan(eigen_sensor_msg_.joint_state.effort(i))) {


I do not see why we should send NaNs here.

MaximilienNaveau · 2023-06-29T14:52:08Z

I think a second pass is welcome here!
I need to test this on the robot but it will be in 2 weeks.
In the mean time you could give a feedback on the API.

edantec

Seems good to me.

edantec · 2023-06-29T15:41:52Z

README.md

@@ -92,7 +92,7 @@ in the same terminal, once the robot is in the default pose, kill (`ctrl+C`) the

 - Terminal 3: Spawn the linear feedback controller:
 ```
-reset && source install/setup.bash && roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=true default_params:=true
+reset && source install/setup.bash && roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=true default_params:=true activate_right_leg:=true activate_right_leg:=true activate_torso:=true


This implies that default_params has arm joints deactivated. It is not quite intuitive, although I don't know how we can do better.

MaximilienNaveau · 2023-06-29T15:51:00Z

Everything is deactivated unless you specify it Le jeu. 29 juin 2023 à 17:48, edantec ***@***.***> a écrit :

…

***@***.**** approved this pull request. Seems good to me. ------------------------------ In README.md <#19 (comment)> : > @@ -92,7 +92,7 @@ in the same terminal, once the robot is in the default pose, kill (`ctrl+C`) the - Terminal 3: Spawn the linear feedback controller: ``` -reset && source install/setup.bash && roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=true default_params:=true +reset && source install/setup.bash && roslaunch linear_feedback_controller talos_linear_feedback_controller.launch simulation:=true default_params:=true activate_right_leg:=true activate_right_leg:=true activate_torso:=true This implies that default_params has arm joints deactivated. It is not quite intuitive, although I don't know how we can do better. — Reply to this email directly, view it on GitHub <#19 (review)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABLYRZ6EJXUBAZMOEYSCRWTXNWPVHANCNFSM6AAAAAAWVOBJAM> . You are receiving this because you authored the thread.Message ID: ***@***.***>

Make the left wrists controlled and the nan measured in the torques s…

13c3483

…et to zero

MaximilienNaveau requested review from edantec, nim65s, olivier-stasse, nmansard, ComePerrot and pFernbach April 6, 2023 13:49

ComePerrot reviewed Apr 12, 2023

View reviewed changes

MaximilienNaveau commented Apr 18, 2023

View reviewed changes

edantec approved these changes Apr 18, 2023

View reviewed changes

MaximilienNaveau added 2 commits June 29, 2023 16:30

update the roslaunch to specify manually the talos limbs controlled

cda6292

add a log message in case we detect some Nans

25535ea

MaximilienNaveau changed the title ~~draft: This PR aims to deal with the control or not of the upper body.~~ This PR aims to deal with the control or not of the upper body. Jun 29, 2023

MaximilienNaveau requested review from ComePerrot and edantec June 29, 2023 14:51

edantec approved these changes Jun 29, 2023

View reviewed changes

fix thecompilation errors

ba5d11a

ComePerrot approved these changes Mar 7, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

This PR aims to deal with the control or not of the upper body. #19

This PR aims to deal with the control or not of the upper body. #19

MaximilienNaveau commented Apr 6, 2023 •

edited

Loading

ComePerrot left a comment

ComePerrot Apr 12, 2023

MaximilienNaveau Apr 18, 2023

ComePerrot Apr 12, 2023

MaximilienNaveau Apr 18, 2023

edantec Apr 18, 2023

ComePerrot Apr 12, 2023

MaximilienNaveau Apr 18, 2023

MaximilienNaveau Apr 18, 2023

edantec Apr 18, 2023

ComePerrot Apr 18, 2023

edantec left a comment

edantec Apr 18, 2023

edantec Apr 18, 2023

MaximilienNaveau commented Jun 29, 2023

edantec left a comment

edantec Jun 29, 2023

MaximilienNaveau commented Jun 29, 2023 via email

This PR aims to deal with the control or not of the upper body. #19

Are you sure you want to change the base?

This PR aims to deal with the control or not of the upper body. #19

Conversation

MaximilienNaveau commented Apr 6, 2023 • edited Loading

ComePerrot left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

edantec left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaximilienNaveau commented Jun 29, 2023

edantec left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaximilienNaveau commented Jun 29, 2023 via email

MaximilienNaveau commented Apr 6, 2023 •

edited

Loading