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.

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

[FEATURE] merge entity #424

Merged
merged 4 commits into from
Jan 1, 2025
+540 −1
Merged

[FEATURE] merge entity #424

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
dbb291b
[feat] add entity merging capability
Kashu7100 Dec 31, 2024
0bdcc5b
Merge branch 'main' into feat-merge_entity
Kashu7100 Dec 31, 2024
8f4b348
update
Kashu7100 Jan 1, 2025
b463dd8
Update scene.py
ziyanx02 Jan 1, 2025
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
155 changes: 155 additions & 0 deletions examples/rigid/merge_entities.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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()
109 changes: 109 additions & 0 deletions genesis/assets/urdf/panda_bullet/hand.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,109 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from panda_arm_hand.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="hand">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.04"/>
<mass value=".81"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://meshes/visual/hand.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="package://meshes/collision/hand.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<link name="left_finger">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0.01 0.02"/>
<mass value="0.1"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://meshes/visual/finger.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="package://meshes/collision/finger.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<link name="right_finger">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>

<inertial>
<origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
<mass value="0.1"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="package://meshes/visual/finger.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="package://meshes/collision/finger.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="finger_joint1" type="prismatic">
<parent link="hand"/>
<child link="left_finger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
</joint>
<joint name="finger_joint2" type="prismatic">
<parent link="hand"/>
<child link="right_finger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 -1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="finger_joint1"/>
</joint>
<link name="panda_grasptarget">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<joint name="panda_grasptarget_hand" type="fixed">
<parent link="hand"/>
<child link="panda_grasptarget"/>
<origin rpy="0 0 0" xyz="0 0 0.105"/>
</joint>

</robot>
Loading