From 6622ef79a2f079535ead7f25dea4f63feb3c332a Mon Sep 17 00:00:00 2001
From: Kashu Yamazaki <39104615+Kashu7100@users.noreply.github.com>
Date: Wed, 1 Jan 2025 16:10:16 -0600
Subject: [PATCH] [FEATURE] merge entity (#424)
* [feat] add entity merging capability
---
examples/rigid/merge_entities.py | 155 ++++++++++++
genesis/assets/urdf/panda_bullet/hand.urdf | 109 +++++++++
.../urdf/panda_bullet/panda_nohand.urdf | 231 ++++++++++++++++++
.../entities/rigid_entity/rigid_link.py | 2 +
genesis/engine/scene.py | 44 +++-
5 files changed, 540 insertions(+), 1 deletion(-)
create mode 100644 examples/rigid/merge_entities.py
create mode 100644 genesis/assets/urdf/panda_bullet/hand.urdf
create mode 100644 genesis/assets/urdf/panda_bullet/panda_nohand.urdf
diff --git a/examples/rigid/merge_entities.py b/examples/rigid/merge_entities.py
new file mode 100644
index 00000000..ff195bfb
--- /dev/null
+++ b/examples/rigid/merge_entities.py
@@ -0,0 +1,155 @@
+import argparse
+import numpy as np
+import genesis as gs
+
+
+COMB = {
+ "urdf2urdf",
+ "urdf2mjcf",
+ "mjcf2urdf",
+ "mjcf2mjcf",
+}
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("-v", "--vis", action="store_true", default=False)
+ parser.add_argument("-c", "--comb", type=str, default="urdf2urdf", choices=COMB)
+ args = parser.parse_args()
+
+ ########################## init ##########################
+ gs.init(backend=gs.gpu)
+
+ ########################## create a scene ##########################
+ viewer_options = gs.options.ViewerOptions(
+ camera_pos=(0, -3.5, 2.5),
+ camera_lookat=(0.0, 0.0, 0.5),
+ camera_fov=40,
+ max_FPS=60,
+ )
+
+ scene = gs.Scene(
+ viewer_options=viewer_options,
+ sim_options=gs.options.SimOptions(
+ dt=0.01,
+ ),
+ show_viewer=args.vis,
+ )
+
+ ########################## entities ##########################
+ plane = scene.add_entity(
+ gs.morphs.Plane(),
+ )
+
+ if args.comb == "urdf2urdf" or args.comb == "urdf2mjcf":
+ gs.logger.info("loading URDF panda arm")
+ franka = scene.add_entity(
+ gs.morphs.URDF(file="urdf/panda_bullet/panda_nohand.urdf", merge_fixed_links=False, fixed=True),
+ )
+ else:
+ gs.logger.info("loading MJCF panda arm")
+ franka = scene.add_entity(
+ gs.morphs.MJCF(file="xml/franka_emika_panda/panda_nohand.xml"),
+ )
+
+ if args.comb == "urdf2urdf" or args.comb == "mjcf2urdf":
+ gs.logger.info("loading URDF panda hand")
+ # NOTE: you need to fix the base link of the attaching entity
+ hand = scene.add_entity(
+ gs.morphs.URDF(file="urdf/panda_bullet/hand.urdf", merge_fixed_links=False, fixed=True),
+ )
+ else:
+ gs.logger.info("loading MJCF panda hand")
+ hand = scene.add_entity(
+ gs.morphs.MJCF(file="xml/franka_emika_panda/hand.xml"),
+ )
+
+ print([link.name for link in franka.links])
+ print([link.name for link in hand.links])
+ scene.link_entities(franka, hand, "attachment", "hand")
+
+ ########################## build ##########################
+ scene.build()
+
+ arm_jnt_names = [
+ "joint1",
+ "joint2",
+ "joint3",
+ "joint4",
+ "joint5",
+ "joint6",
+ "joint7",
+ ]
+ arm_dofs_idx = [franka.get_joint(name).dof_idx_local for name in arm_jnt_names]
+
+ # Optional: set control gains
+ franka.set_dofs_kp(
+ np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000]),
+ arm_dofs_idx,
+ )
+ franka.set_dofs_kv(
+ np.array([450, 450, 350, 350, 200, 200, 200]),
+ arm_dofs_idx,
+ )
+ franka.set_dofs_force_range(
+ np.array([-87, -87, -87, -87, -12, -12, -12]),
+ np.array([87, 87, 87, 87, 12, 12, 12]),
+ arm_dofs_idx,
+ )
+
+ gripper_jnt_names = [
+ "finger_joint1",
+ "finger_joint2",
+ ]
+ gripper_dofs_idx = [hand.get_joint(name).dof_idx_local for name in gripper_jnt_names]
+
+ # Optional: set control gains
+ hand.set_dofs_kp(
+ np.array([100, 100]),
+ gripper_dofs_idx,
+ )
+ hand.set_dofs_kv(
+ np.array([10, 10]),
+ gripper_dofs_idx,
+ )
+ hand.set_dofs_force_range(
+ np.array([-100, -100]),
+ np.array([100, 100]),
+ gripper_dofs_idx,
+ )
+
+ # PD control
+ for i in range(750):
+ if i == 0:
+ franka.control_dofs_position(
+ np.array([1, 1, 0, 0, 0, 0, 0]),
+ arm_dofs_idx,
+ )
+ hand.control_dofs_position(
+ np.array([0.04, 0.04]),
+ gripper_dofs_idx,
+ )
+ elif i == 250:
+ franka.control_dofs_position(
+ np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5]),
+ arm_dofs_idx,
+ )
+ hand.control_dofs_position(
+ np.array([0.0, 0.0]),
+ gripper_dofs_idx,
+ )
+ elif i == 500:
+ franka.control_dofs_position(
+ np.array([0, 0, 0, 0, 0, 0, 0]),
+ arm_dofs_idx,
+ )
+ hand.control_dofs_position(
+ np.array([0.04, 0.04]),
+ gripper_dofs_idx,
+ )
+
+ scene.step()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/genesis/assets/urdf/panda_bullet/hand.urdf b/genesis/assets/urdf/panda_bullet/hand.urdf
new file mode 100644
index 00000000..f3c3ac2c
--- /dev/null
+++ b/genesis/assets/urdf/panda_bullet/hand.urdf
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/genesis/assets/urdf/panda_bullet/panda_nohand.urdf b/genesis/assets/urdf/panda_bullet/panda_nohand.urdf
new file mode 100644
index 00000000..9d6833e5
--- /dev/null
+++ b/genesis/assets/urdf/panda_bullet/panda_nohand.urdf
@@ -0,0 +1,231 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/genesis/engine/entities/rigid_entity/rigid_link.py b/genesis/engine/entities/rigid_entity/rigid_link.py
index 1e215205..f8d0beea 100644
--- a/genesis/engine/entities/rigid_entity/rigid_link.py
+++ b/genesis/engine/entities/rigid_entity/rigid_link.py
@@ -431,6 +431,7 @@ def parent_idx_local(self):
"""
The local index of the link's parent link in the entity. If the link is the root link, return -1.
"""
+ # TODO: check for parent links outside of the current entity (caused by scene.link_entities())
if self._parent_idx >= 0:
return self._parent_idx - self._entity._link_start
else:
@@ -441,6 +442,7 @@ def child_idxs_local(self):
"""
The local indices of the link's child links in the entity.
"""
+ # TODO: check for child links outside of the current entity (caused by scene.link_entities())
return [idx - self._entity._link_start if idx >= 0 else idx for idx in self._child_idxs]
@property
diff --git a/genesis/engine/scene.py b/genesis/engine/scene.py
index 8d4a248d..efe4aa4a 100644
--- a/genesis/engine/scene.py
+++ b/genesis/engine/scene.py
@@ -82,7 +82,6 @@ def __init__(
show_viewer=True,
show_FPS=True,
):
-
self._uid = gs.UID()
self._t = 0
self._is_built = False
@@ -361,6 +360,49 @@ def add_entity(
return entity
+ @gs.assert_unbuilt
+ def link_entities(
+ self,
+ parent_entity,
+ child_entity,
+ parent_link_name="",
+ child_link_name="",
+ ):
+ """
+ links two entities to act as single entity.
+
+ Parameters
+ ----------
+ parent_entity : genesis.Entity
+ The entity in the scene that will be a parent of kinematic tree.
+ child_entity : genesis.Entity
+ The entity in the scene that will be a child of kinematic tree.
+ parent_link_name : str
+ The name of the link in the parent entity to be linked.
+ child_link_name : str
+ The name of the link in the child entity to be linked.
+ """
+ if not isinstance(parent_entity, gs.engine.entities.RigidEntity):
+ gs.raise_exception("Currently only rigid entities are supported for merging.")
+ if not isinstance(child_entity, gs.engine.entities.RigidEntity):
+ gs.raise_exception("Currently only rigid entities are supported for merging.")
+
+ if not child_link_name:
+ for link in child_entity._links:
+ if link.parent_idx == -1:
+ child_link = link
+ break
+ else:
+ child_link = child_entity.get_link(child_link_name)
+ parent_link = parent_entity.get_link(parent_link_name)
+
+ if child_link._parent_idx != -1:
+ gs.logger.warning(
+ "Child entity already has a parent link. This may cause the entity to break into parts. Make sure this operation is intended."
+ )
+ child_link._parent_idx = parent_link.idx
+ parent_link._child_idxs.append(child_link.idx)
+
@gs.assert_unbuilt
def add_light(
self,