Skip to content

Fix ABB/RFL inertias and preserve PyBullet base-link placement #488

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.

Sign up for GitHub

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

Open
gonzalocasas wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix ABB/RFL inertias and preserve PyBullet base-link placement #488

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions AUTHORS.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,3 +17,4 @@
- Edvard Bruun <<ebruun@princeton.edu>> [@ebruun](https://github.com/ebruun)
- Victor Pok Yin Leung <<leung@arch.ethz.ch>> [@yck011522](https://github.com/yck011522)
- Begüm Saral <<begum.saral@tum.de>> [@begums](https://github.com/begums)
- OmniLink [@omnilink-tech](https://github.com/omnilink-tech)
2 changes: 2 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@ Requires `compas_robots >= 1.1`.

### Changed

* Corrected the invalid inertia tensors and centres of mass in the bundled ABB IRB4600-40/2.55 and RFL URDFs using the collision-mesh approximations contributed by @omnilink-tech in #486. These assume uniform density at the existing declared masses; they are not manufacturer-calibrated dynamic parameters.
* Fixed PyBullet base-frame reads and updates for robots and objects whose inertial frame differs from the base-link frame. This keeps the corrected ABB centre of mass from displacing the robot in planning and collision checks.
* Refreshed the bundled UR5 and UR10e descriptions and meshes from the official Universal Robots ROS 2 description package. All seven supported UR cells now share one `ur_description` mesh package instead of duplicating model assets per cell.
* Bundled UR and Panda visual meshes now use their original Collada files instead of derived OBJ copies, preserving source materials and eliminating redundant converted assets. The ROS package extraction script likewise keeps downloaded DAE files unchanged.
* The tools in `ToolLibrary` now mount along the +Z axis of their base frame instead of +X. Every planning group in `RobotCellLibrary` ends at a link whose +Z points away from the arm (`tool0` for the industrial robots, `panda_hand_tcp` for the Panda), so with this the same tool attaches to any of them with an identity attachment frame — previously each cell carried a rotation to bridge the two conventions, and a tool authored for one robot did not necessarily fit another. Their TCF states the tool's working direction with its own Z axis too, so a `TargetMode.TOOL` target aligns the tool along the target's Z — previously the TCF's X axis ran along the tool, which put every tool-mode target 90 degrees out. The tools are still modelled along +X internally and re-framed on the way out via `ToolModel.reframe_base`. The beams held by the gripper cells are authored in TCF coordinates and were re-authored to match, so they stay put. Poses are unchanged: the attached tools and workpieces of every cell land exactly where they did, only the tool's base frame is now the end effector link's frame rather than a rotated version of it. Requires the `reframe_base` support of `compas_robots >= 1.1`.
Expand Down
17 changes: 13 additions & 4 deletions src/compas_fab/backends/pybullet/client.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
from compas.colors import Color
from compas.datastructures import Mesh
from compas.geometry import Frame
from compas.geometry import Transformation
from compas_robots import Configuration
from compas_robots import RobotModel
from compas_robots import ToolModel
Expand Down Expand Up @@ -652,7 +653,14 @@ def _build_pose_for_pybullet(self, configuration: Configuration) -> list[float]:

def _get_base_frame(self, body_id):
pose = pybullet.getBasePositionAndOrientation(body_id, physicsClientId=self.client_id)
return frame_from_pose(pose)
# PyBullet reports the base's inertial frame, not its URDF link frame.
t_world_inertial = Transformation.from_frame(frame_from_pose(pose))
t_link_inertial = Transformation.from_frame(self._get_base_inertial_frame(body_id))
return Frame.from_transformation(t_world_inertial * t_link_inertial.inverse())

def _get_base_inertial_frame(self, body_id):
dynamics = pybullet.getDynamicsInfo(body_id, const.BASE_LINK_ID, physicsClientId=self.client_id)
return frame_from_pose((dynamics[3], dynamics[4]))

def _get_base_name(self, body_id):
return self._get_body_info(body_id).base_name.decode(encoding="UTF-8")
Expand Down Expand Up @@ -711,7 +719,9 @@ def _get_link_frame(self, link_id, body_id):
return frame_from_pose(pose)

def _set_base_frame(self, frame, body_id):
point, quaternion = pose_from_frame(frame)
# resetBasePositionAndOrientation expects the inertial frame in world coordinates.
inertial_frame = frame.to_world_coordinates(self._get_base_inertial_frame(body_id))
point, quaternion = pose_from_frame(inertial_frame)
pybullet.resetBasePositionAndOrientation(body_id, point, quaternion, physicsClientId=self.client_id)

def _set_joint_position(self, joint_id, value, body_id):
Expand Down Expand Up @@ -860,8 +870,7 @@ def _set_rigid_body_base_frame(self, rigid_body_name: str, frame: Frame):
self._set_object_frame(body_id, frame)

def _set_object_frame(self, body_id, frame):
(point, quat) = pose_from_frame(frame)
pybullet.resetBasePositionAndOrientation(body_id, point, quat, physicsClientId=self.client_id)
self._set_base_frame(frame, body_id)

# ------------------------------------------------------------------------------------
# Helper functions for creating rigid bodies in PyBullet
Expand Down
31 changes: 17 additions & 14 deletions src/compas_fab/data/robot_library/abb_irb4600_40_255/urdf/robot_description.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="abb_irb4600_40_255">
<!-- Inertias and centres of mass approximate uniform-density collision meshes
at the existing declared masses, which have not been validated. These are
not manufacturer dynamic parameters. Source: compas-dev/compas_fab#486. -->
<link name="base_link">
<visual>
<geometry>
Expand All @@ -15,9 +18,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<origin xyz="-0.103747571 0.000230223562 0.0907327715" rpy="0.0 0.0 0.0"/>
<mass value="120.0"/>
<inertia ixx="-4.98" ixy="-0.0" ixz="2.7" iyy="-5.952" iyz="-0.0" izz="0.828"/>
<inertia ixx="1.95287938" ixy="-0.00131034938" ixz="0.230948107" iyy="4.01443637" iyz="0.0010695593" izz="5.2337838"/>
</inertial>
</link>
<link name="link_1">
Expand All @@ -35,9 +38,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.18 0.0 0.5" rpy="0.0 0.0 0.0"/>
<origin xyz="0.0514298063 0.023260392 0.403787887" rpy="0.0 0.0 0.0"/>
<mass value="120.0"/>
<inertia ixx="-4.98" ixy="-0.0" ixz="2.7" iyy="-5.952" iyz="-0.0" izz="0.828"/>
<inertia ixx="3.26767101" ixy="-0.282498551" ixz="-0.564477661" iyy="5.06941746" iyz="-0.208254528" izz="4.08760344"/>
</inertial>
</link>
<link name="link_2">
Expand All @@ -55,9 +58,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.94 0.08 -0.34" rpy="0.0 0.0 0.0"/>
<origin xyz="0.0 -0.180953477 0.488127638" rpy="0.0 0.0 0.0"/>
<mass value="120.0"/>
<inertia ixx="-2.96" ixy="2.256" ixz="-9.588" iyy="-18.72601" iyz="-0.816" izz="-14.95"/>
<inertia ixx="14.4834733" ixy="0.0" ixz="0.0" iyy="14.8136842" iyz="0.282258893" izz="0.813278112"/>
</inertial>
</link>
<link name="link_3">
Expand All @@ -75,9 +78,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.07 -0.266 0.088" rpy="0.0 0.0 0.0"/>
<origin xyz="0.0424978995 0.0439362397 0.119882342" rpy="0.0 0.0 0.0"/>
<mass value="120.0"/>
<inertia ixx="1.345" ixy="-0.5586" ixz="0.1848" iyy="4.65068" iyz="-0.70224" izz="2.76032"/>
<inertia ixx="3.51954993" ixy="0.100722177" ixz="-0.0719537698" iyy="5.13049952" iyz="0.0670880018" izz="4.4811355"/>
</inertial>
</link>
<link name="link_4">
Expand All @@ -95,9 +98,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.0 0.0 1.0" rpy="0.0 0.0 0.0"/>
<origin xyz="0.654158291 0.00113847257 -0.00113873219" rpy="0.0 0.0 0.0"/>
<mass value="40.0"/>
<inertia ixx="-8.0" ixy="-0.0" ixz="-0.0" iyy="-8.0" iyz="-0.0" izz="0.482"/>
<inertia ixx="0.192446239" ixy="-0.0118403926" ixz="0.00589280732" iyy="3.72731927" iyz="-8.82710456e-06" izz="3.76544311"/>
</inertial>
</link>
<link name="link_5">
Expand All @@ -115,9 +118,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.14 0.0 0.0" rpy="0.0 0.0 0.0"/>
<origin xyz="0.0 -0.00149999931 0.0" rpy="0.0 0.0 0.0"/>
<mass value="10.0"/>
<inertia ixx="0.04" ixy="-0.0" ixz="-0.0" iyy="-0.009" iyz="-0.0" izz="-0.004"/>
<inertia ixx="0.0299415877" ixy="0.0" ixz="0.0" iyy="0.0312815066" iyz="0.0" izz="0.0299415877"/>
</inertial>
</link>
<link name="link_6">
Expand All @@ -135,9 +138,9 @@
</geometry>
</collision>
<inertial>
<origin xyz="0.081 -0.091 0.332" rpy="0.0 0.0 0.0"/>
<origin xyz="0.0 0.0 -0.0197532965" rpy="0.0 0.0 0.0"/>
<mass value="5.0"/>
<inertia ixx="-0.29652" ixy="-0.03686" ixz="0.09396" iyy="-0.28792" iyz="-0.10556" izz="-0.04621"/>
<inertia ixx="0.00517999173" ixy="7.79557669e-08" ixz="0.0" iyy="0.00518170835" iyz="0.0" izz="0.0074225942"/>
</inertial>
</link>
<link name="tool0"/>
Expand Down
Loading
Loading