Can we have a way to update the root location of RobotModel

[yck011522]

Feature Request

As a developer who have ToolModel with multiple links and joints, I would like to be able to update the location of a that ToolModel object and visualize it.

At the moment, There is no dedicated ToolModelArtist, I user RobotModelArtist to visualize my tool, as such I use RobotModelArtist.update() to update the robot's joint state. However, I cannot update the tool's root's location.

I needed this because I want to visualized my gripper tool at my target object and perform a collision check before attempting IK or Pathplan, therefore I do not have any robot attaching to that tool, and thus not able to use the RobotModelArtist.update_tool() . It would be nice to have a function to set / update the root transformation (as described in the tutorial as Robot coordinate frame RCF) of the robot, perhaps even keeping its state.

I have tried to use the RobotArtistModel._update() which has a parent_transformation input, but the result is not correct. The root of my ToolModel is not transformed, only the links are transformed.

from compas_ghpython.artists import RobotModelArtist
t = Transformation.from_frame(tool_RCF_frame)
artist = RobotModelArtist(tool_model)
artist._update(tool_model, tool_model.current_configuration.joint_dict, parent_transformation = t)
visual = artist.draw_visual()
# The results are not correct

Notice my gripper fingers are in the transformed position, but the gripper body (root) is not.

[yck011522]

Thanks @beverlylytle with the conversation that solved the problem. These lines of code that make the RobotModel root transform too. However, as per conversation, you will decide when to close this issue with a more proper fix.

artist = RobotModelArtist(robot_model)
transformations = robot_model.compute_transformations(robot_model.current_configuration.joint_dict, None, t)
artist._transform_link_geometry(robot_model.root, t, True) # This line transforms the root.
artist._update(robot_model, robot_model.current_configuration.joint_dict, parent_transformation = t)
visual = artist.draw_visual()

[gonzalocasas]

Although not directly related to this request, the implementation should keep in mind the behavior of mobile robots when used in combination with ROS: https://www.ros.org/reps/rep-0105.html

[gonzalocasas]

@yck011522 ok, there's quite a bit underlying this seemingly trivial request:

1. There was a bug in the add_joint method, leading to the incorrect recalculation of the chain. This was fixed and merged yesterday. With that, you can move a robot model's base with model._create(model.root, your_desired_transformation) (however that this is still a private method)
2. However, the "proper" way to do this (according to the URDF gods), is to add a fixed joint between a non-geometry world link (or name it whatever you want), and your gripper_base. As far as I understand, this is the standard way most robots are defined in ROS (mobile robots introduce even more intermediate frames).
3. But if you use MoveIt, things get a bit more complicated. MoveIt does not really allow URDF to change dynamically, it lets you define "virtual joints" at the SRDF level and changes to the position of those need to be manipulated via the TF tree (or some static transform publisher), which we currently don't really support (roslibpy does, but we haven't integrated it in compas/compas_fab).

Based on that, I'm starting to fix this with this pull request which adds RCF setting for the model only (supporting both with or without a fixed frame in the URDF). If you have time to check this out and see if it works for you, it'd be appreciated.

Next, I will add one or two more pull requests to hook up that change into COMPAS FAB and have MoveIt reflect the change accordingly.

[yck011522]

@gonzalocasas Yes

[laurend]

In the meantime while we wait for set_rcf to be implemented can you provide some basic instructions for how to change the the RCF?

[yck011522]

@gonzalocasas gentle ping

[gonzalocasas]

@laurend sure, I can try, but first: do you use your robot together with compas_fab's MoveIt backend? or are you interested in setting it purely on the RobotModel?

[laurend]

@gonzalocasas we are using ROS backends via Docker. Thanks!

[tomvanmele]

robots are no longer in compas core so will close this...