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

Add possibility to set a base from the build_from_model_description method #302

Open
CarlottaSartore opened this issue Nov 29, 2024 · 2 comments
Assignees

Comments

@CarlottaSartore
Copy link
Contributor

I have seen that it is not possible to set the base from the method build_from_model_description although it is the suggested function for loading a model. It would be useful to allow the user to define any base.

@CarlottaSartore CarlottaSartore self-assigned this Nov 29, 2024
@CarlottaSartore
Copy link
Contributor Author

CarlottaSartore commented Nov 29, 2024

I did a first implementation of this in https://github.com/CarlottaSartore/jaxsim/tree/add_base_parameter and I tested it with ergocub, i.e. this model https://github.com/icub-tech-iit/ergocub-software/blob/master/urdf/ergoCub/robots/ergoCubSN001/model.urdf and definite the head as the base link

But it does not work, for instance, if I define head as base, the only links that get parsed are the head and the real sense

the problem seems that the children of the head link contain only the real sense while they should contain other links such as the head imu

the problem seems here

def breadth_first_search(
root: LinkDescription,
sort_children: Callable[[Any], Any] | None = lambda link: link.name,
) -> Iterable[LinkDescription]:
"""
Perform a breadth-first search (BFS) traversal of the kinematic graph.
Args:
root: The root link for BFS.
sort_children: A function to sort children of a node.
Yields:
The links in the kinematic graph in BFS order.
"""
# Initialize the queue with the root node.
queue = [root]
# We assume that nodes have unique names and mark a link as visited using
# its name. This speeds up considerably object comparison.
visited = []
visited.append(root.name)
yield root
while len(queue) > 0:
# Extract the first element of the queue.
l = queue.pop(0)
# Note: sorting the links with their name so that the order of children
# insertion does not matter when assigning the link index.
for child in sorted(l.children, key=sort_children):
if child.name in visited:
continue
visited.append(child.name)
queue.append(child)
yield child

for reference this is the idyntree function that performs the same action :

https://github.com/robotology/idyntree/blob/13cf9e9f0befc6dd08a1b90d21424a043dbf0e48/src/model/src/Model.cpp#L828-L878

@flferretti
Copy link
Collaborator

flferretti commented Nov 29, 2024

Hi @CarlottaSartore, thanks for working on this! The head and the realsense are the only links parsed because realsense is the only child of the head link. This logic should be changed where the link children are set:

# Reset the connections of the root link.
for link in links_dict.values():
link.children = tuple()
# Couple links and joints creating the kinematic graph.
for joint in joints:
# Get the parent and child links of the joint.
parent_link = links_dict[joint.parent.name]
child_link = links_dict[joint.child.name]
assert child_link.name == joint.child.name
assert parent_link.name == joint.parent.name
# Assign link's parent.
child_link.parent = parent_link
# Assign link's children and make sure they are unique.
if child_link.name not in {l.name for l in parent_link.children}:
with parent_link.mutable_context(Mutability.MUTABLE_NO_VALIDATION):
parent_link.children = (*parent_link.children, child_link)

In brief, the full ordering of the traversal should be changed, as I guess it's done in iDynTree:

https://github.com/robotology/idyntree/blob/13cf9e9f0befc6dd08a1b90d21424a043dbf0e48/src/model/src/Traversal.cpp#L36-L44

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

No branches or pull requests

2 participants