Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fetch use lower limit #293

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Fetch use lower limit #293

wants to merge 5 commits into from

Conversation

tonypan1995
Copy link
Contributor

The joint limits file was not correctly loaded because moveit does not load it when urdf and srdf is available. Now we check and load the joint limits correctly when a file is provided.

Copy link
Contributor

@ChamzasKonstantinos ChamzasKonstantinos left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why this function is not loading the joint_lii

@@ -82,7 +82,7 @@ bool Robot::initialize(const std::string &urdf_file, const std::string &srdf_fil
return false;
}

initializeInternal();
initializeInternal(true, limits_file);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why the limits file is not correctly loaded here . If there is a problem with this function, Then I think it should be fixed and not add a custom function inside robot.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Because when the underlying moveIt loads the robot, if it has access to the URDF and SRDF files, it skips loading the joint_limits file. Instead it only gets the limits from the URDF. So if we provide all of these three files, hoping to use our custom joint_limits file, moveIt would never read it and it's not used (see line 105 here).

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Inside initializeInternal is where we call the moveIt loadModel API. So the fix of loading the limits_file should follow that (if not in MoveIt).

@ChamzasKonstantinos
Copy link
Contributor

ChamzasKonstantinos commented Jul 31, 2022

Ok i Looked into this and it seems as you correctly pointed out, that the joint_limits file is completely ignored!, so definitely a good catch that needs fixing.

I have the following suggestions:
A) The yaml loading code should be moved to yaml.cpp and use the encode/decode system to load the join_limits file. Basically to read the file you will use this generic templated argument function, which is much cleaner. For this function to automatically work you need to create the decode/encode functions for a JointLimitsMsg, like here.
B) I would create an internal joint_limits_ member variable that is a JointLimitsMsg to eliminate the extra argument in initializeInternal(). Then to use it there are two options:

  1. Directly set it to the param server, similar to here. and then load the (RobotModel)[http://docs.ros.org/en/noetic/api/moveit_ros_planning/html/robot__model__loader_8cpp_source.html]. From my understanding this would work, since line 105 reads from the param server when the urdf exists.
  2. The other solution is to do it manually like now, with setJointVariableBounds but I would still set the param server afterwards just so the loaded params are available and consistent.

I am sorry if I am being annoying, I just think this is much cleaner and will make the code more readable/maintainable. This is definitely an important bug that needs fixing, so very good catch!

@tonypan1995
Copy link
Contributor Author

Ok i Looked into this and it seems as you correctly pointed out, that the joint_limits file is completely ignored!, so definitely a good catch that needs fixing.

I have the following suggestions: A) The yaml loading code should be moved to yaml.cpp and use the encode/decode system to load the join_limits file. Basically to read the file you will use this generic templated argument function, which is much cleaner. For this function to automatically work you need to create the decode/encode functions for a JointLimitsMsg, like here. B) I would create an internal joint_limits_ member variable that is a JointLimitsMsg to eliminate the extra argument in initializeInternal(). Then to use it there are two options:

  1. Directly set it to the param server, similar to here. and then load the (RobotModel)[http://docs.ros.org/en/noetic/api/moveit_ros_planning/html/robot__model__loader_8cpp_source.html]. From my understanding this would work, since line 105 reads from the param server when the urdf exists.
  2. The other solution is to do it manually like now, with setJointVariableBounds but I would still set the param server afterwards just so the loaded params are available and consistent.

I am sorry if I am being annoying, I just think this is much cleaner and will make the code more readable/maintainable. This is definitely an important bug that needs fixing, so very good catch!

Don't worry! I think this is good suggestions and I'll try to address them later when I have time.

One thing to point out about loading the joint limits as params from the param server though, is that robowflex is already loading the joint limits file to the ros server. However, if urdf exists, that line 105 would not load robot description from ros server. So the problem is not that the params in the server are inconsistent, but that the joint limits in the joint_limits.yaml file and the urdf file are different. When URDF exists, MoveIt only reads joint limits from the urdf.

So in general when URDF exists, we need some post-processing after loading model using MoveIt, which loads the joint limits either from server or file (I did it from file as above because I faced with some bugs reading from server - I can look into this because as you suggested reading from the server is more consistent).

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants