if there a demo binding camera the the end of joint or the body of the robot? #258
Description
I am now trying to using genesis to generate some data for VLA model,but I can not binding my camera to the end of joint correctly,while I trying to get the joint global position,some error occured:
this is my code:
import genesis as gs
import numpy as np
import cv2
gs.init(backend=gs.gpu)
# -----------场景初始化-----------
scene = gs.Scene(show_viewer=True)
# -----------背景-----------
plane = scene.add_entity(gs.morphs.Plane())
# -----------机械臂-----------
cam = scene.add_camera(
res = (1280, 960),
pos = (3.5, 0.0, 2.5),
lookat = (0, 0, 0.5),
fov = 30,
GUI = True
)
rm_dual = scene.add_entity(
gs.morphs.URDF(file='/home/nvidia/Genesis/rm_models/dual_arm_robot/urdf/dual_arm_robot.urdf'),
)
# -----------生成初始化-----------
scene.build()
# 相机操作
# rgb, depth, segmentation, normal = cam.render(depth=True, segmentation=True, normal=True)
# 绑定对应左手控制关节
left_jnt_names = [f"Joint_left{i}" for i in range(1,7)]+[f"Joint_l_finger{i}" for i in range(1,3)]
l_dofs_idx = [rm_dual.get_joint(name).dof_idx_local for name in left_jnt_names]
# 绑定对应右手控制关节
right_jnt_names = [f"Joint_right{i}" for i in range(1,7)]+[f"Joint_r_finger{i}" for i in range(1,3)]
r_dofs_idx = [rm_dual.get_joint(name).dof_idx_local for name in right_jnt_names]
# 绑定车轮
left_wheel_names = [f'joint_wheel_lf{i}' for i in range(1,3)]+[f'joint_wheel_lb{i}' for i in range(1,3)]
right_wheel_names = [f'joint_wheel_rf{i}' for i in range(1,3)]+[f'joint_wheel_rb{i}' for i in range(1,3)]
l_wheel_dofs_idx = [rm_dual.get_joint(name).dof_idx_local for name in left_wheel_names]
r_wheel_dofs_idx = [rm_dual.get_joint(name).dof_idx_local for name in right_wheel_names]
wheel_go_names =['joint_wheel_right','joint_wheel_left']
wheel_go_index = [rm_dual.get_joint(name).dof_idx_local for name in wheel_go_names]
# 绑定相机
cam_names =['joint_camera_ro']
cam_index = [rm_dual.get_joint(name).dof_idx_local for name in cam_names]
# 设置一些运动参数
# -----------左臂参数-----------
# 设置位置增益参数
rm_dual.set_dofs_kp(
kp = np.array([4500, 4500, 3500, 3500, 2000, 2000, 100, 100]),
dofs_idx_local = l_dofs_idx,
)
# 设置速度增益参数：
rm_dual.set_dofs_kv(
kv = np.array([1000, 1000, 1000, 1000, 1000, 1000, 10, 10]),
dofs_idx_local = l_dofs_idx,
)
# 设置力控安全区间
rm_dual.set_dofs_force_range(
lower = np.array([-87, -87, -87, -87, -12, -12, -100, -100]),
upper = np.array([ 87, 87, 87, 87, 12, 12, 100, 100]),
dofs_idx_local = l_dofs_idx,
)
# -----------右臂参数-----------
# 设置位置增益参数
rm_dual.set_dofs_kp(
kp = np.array([4500, 4500, 3500, 3500, 2000, 2000, 100, 100]),
dofs_idx_local = r_dofs_idx,
)
# 设置速度增益参数
rm_dual.set_dofs_kv(
kv = np.array([1000, 1000, 1000, 1000, 1000, 1000, 10, 10]),
dofs_idx_local = r_dofs_idx,
)
# 设置力控安全区间
rm_dual.set_dofs_force_range(
lower = np.array([-87, -87, -87, -87, -12, -12, -100, -100]),
upper = np.array([ 87, 87, 87, 87, 12, 12, 100, 100]),
dofs_idx_local = r_dofs_idx,
)
# -----------设置机械轮子参数-----------
# 左侧轮子
# 设置位置增益参数
rm_dual.set_dofs_kp(
kp = np.array([100,100,100,100]),
dofs_idx_local = l_wheel_dofs_idx,
)
# 设置速度增益参数
rm_dual.set_dofs_kv(
kv = np.array([1000, 1000, 1000, 1000]),
dofs_idx_local = l_wheel_dofs_idx,
)
# 设置力控安全区间
rm_dual.set_dofs_force_range(
lower = np.array([-87, -87, -87, -87]),
upper = np.array([ 87, 87, 87, 87]),
dofs_idx_local = l_wheel_dofs_idx,
)
# 右侧轮子
rm_dual.set_dofs_kp(
kp = np.array([100,100,100,100]),
dofs_idx_local = r_wheel_dofs_idx,
)
# 设置速度增益参数
rm_dual.set_dofs_kv(
kv = np.array([1000, 1000, 1000, 1000]),
dofs_idx_local = r_wheel_dofs_idx,
)
# 设置力控安全区间
rm_dual.set_dofs_force_range(
lower = np.array([-87, -87, -87, -87]),
upper = np.array([ 87, 87, 87, 87]),
dofs_idx_local = r_wheel_dofs_idx,
)
# 前进轮
rm_dual.set_dofs_kp(
kp = np.array([100,100]),
dofs_idx_local = wheel_go_index,
)
# 设置速度增益参数
rm_dual.set_dofs_kv(
kv = np.array([1000, 1000]),
dofs_idx_local = wheel_go_index,
)
# 设置力控安全区间
rm_dual.set_dofs_force_range(
lower = np.array([-87, -87]),
upper = np.array([ 87, 87]),
dofs_idx_local = wheel_go_index,
)
# -----------运动动作预览-----------
# Hard reset
for i in range(50):
if i < 50:
rm_dual.set_dofs_position(
np.array([0,0,0,0,0,0,0.04,0.04]),
l_dofs_idx
)
rm_dual.set_dofs_position(
np.array([0,0,0,0,0,0,0.04,0.04]),
r_dofs_idx
)
rm_dual.set_dofs_position(
np.array([1.57]),
r_wheel_dofs_idx[:1]
)
rm_dual.set_dofs_position(
np.array([1.57]),
cam_index[:1]
)
scene.step()
# cam.render()
for i in range(1200):
if i == 0:
rm_dual.control_dofs_position(
np.array([0,0,0,0,0,0,0.03,0.03]),
l_dofs_idx
)
rm_dual.control_dofs_position(
np.array([0,0,0,0,0,0,0.03,0.03]),
r_dofs_idx
)
rm_dual.control_dofs_velocity(
np.array([5,-5]),
wheel_go_index
)
if i == 300:
rm_dual.control_dofs_position(
np.array([0,1,0,0,0,-1,0,0]),
l_dofs_idx
)
rm_dual.control_dofs_position(
np.array([0,-1,0,0,0,1,0,0]),
r_dofs_idx
)
rm_dual.control_dofs_velocity(
np.array([0,0]),
wheel_go_index
)
if i==600:
rm_dual.control_dofs_position(
np.array([0,1,0,0,0,-1,0.03,0.03]),
l_dofs_idx
)
rm_dual.control_dofs_position(
np.array([0,-1,0,0,0,1,0.03,0.03]),
r_dofs_idx
)
rm_dual.control_dofs_velocity(
np.array([5,5]),
wheel_go_index
)
if i==900:
rm_dual.control_dofs_position(
np.array([0,0,0,0,0,0,0,0]),
l_dofs_idx
)
rm_dual.control_dofs_position(
np.array([0,0,0,0,0,0,0.1,1]),
r_dofs_idx
)
rm_dual.control_dofs_velocity(
np.array([5,-5]),
wheel_go_index
)
# cam_pos = rm_dual.get_dofs_position(cam_index[:1])
# print(rm_dual.joints[cam_index[0]])
print(rm_dual.joints[cam_index[0]].get_pos())
scene.step()
cam.render()
and the error is:
Traceback (most recent call last):
File "/home/nvidia/Genesis/try.py", line 218, in
print(rm_dual.joints[cam_index[0]].get_pos())
File "/home/nvidia/Genesis/genesis/utils/misc.py", line 48, in wrapper
return method(self, *args, **kwargs)
File "/home/nvidia/Genesis/genesis/engine/entities/rigid_entity/rigid_joint.py", line 84, in get_pos
self._kernel_get_pos(tensor)
File "/home/nvidia/.conda/envs/Genesis/lib/python3.10/site-packages/taichi/lang/kernel_impl.py", line 1178, in call
raise type(e)("\n" + str(e)) from None
taichi.lang.exception.TaichiIndexError:
File "/home/nvidia/Genesis/genesis/engine/entities/rigid_entity/rigid_joint.py", line 94, in _kernel_get_pos:
l_info = self._solver.links_info[self._idx, i_b]
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Field with dim 1 accessed with indices of dim 2