search

compas-dev / compas_fab

Robotic fabrication package for the COMPAS Framework.
https://compas.dev/compas_fab/
MIT License
108 stars 32 forks source link

Attached collision mesh visualizer error when attaching to non-active planning groups #372

Open ebruun opened 1 year ago

ebruun commented 1 year ago

Using compas_fab v0.27.0 with GH/Rhino7 (pre-compiled compas_fab version sent over by @gonzalocasas )

I have a three robot work cell (R1, R2, R3) with separate planning groups. When I attach a collision mesh to the r1_tcp with R1 as the active planning group this is visualized fine, but if I then additionally attach a mesh to R2 I get the following error:

Solution exception:Link name r2_tcp does not exist in planning group

I'm sure this issue is with the FK function of the show_acm part of the visualizer component because everything on the ROS end is fine, i.e. the two meshes show up attached to their respective robots when viewed in RVIZ. Path planning also works fine taking into account both meshes.

Presumably this error is due the code not handling the fact that a mesh could be attached to a link in a non-active planning group.

Discussing with @gonzalocasas earlier and he implemented this quick fix (at line 97 of the GH component), but said that this should be addressed in a more robust way in the future:

######GONZALO MAGIC#########
fk_group = group
for g in robot.semantics.groups:
    if frame_id in robot.get_link_names(group=g):
        fk_group = g
        break

frame = robot.forward_kinematics(
    configuration, fk_group, options=dict(link=frame_id)
)
#############################