Merge pull request #23 from YoruCathy/main

Sync with local

pyrcareworld/Examples/example_robot_move.py

@@ -2,15 +2,17 @@
 
 env = RCareWorld()
 robot = env.create_robot(
-    id=315893,
+    id=315892,
     gripper_list=["3158920"],
     robot_name="kinova_gen3_7dof-robotiq85",
-    base_pos=[0, 0, 1],
+    # base_pos=[7.74100018,14.8170004,10.4515963],
 )
+target = env.create_object(id=1000, name="Cube", is_in_scene=True)
 # robot.load()
-cube = env.create_object(id=315867, name="Cube", is_in_scene=True)
 while True:
-    position = cube.getPosition()
-    print(position)
-    robot.moveTo(position)
+    position = target.getPosition()
     env._step()
+    # print(position)
+    robot.directlyMoveTo(position)
+    print(robot.getRobotState())
+    env._step()

pyrcareworld/Examples/example_robot_move_directly.py

@@ -1,16 +1,23 @@
 from pyrcareworld.envs import RCareWorld
 
 env = RCareWorld()
-robot = env.create_robot(
-    id=315893,
-    gripper_list=["3158930"],
-    robot_name="kinova_gen3_7dof-robotiq85",
-    base_pos=[0, 0, 1],
+franka = env.create_robot(
+    id=965874, gripper_list=["9658740"], robot_name="franka-panda", base_pos=[0, 0, 0]
 )
-cube = env.create_object(id=315867, name="Cube", is_in_scene=True)
 
+# target = env.create_object(id=197454, name="Cube", is_in_scene=True)
+# position = target.getPosition()
+# print(position)
+# franka.directlyMoveTo(position)
+# for i in range(20):
+#     env._step()
 while True:
-    position = cube.getPosition()
-    print(position)
-    robot.directlyMoveTo(position)
+    # position = target.getPosition()
+    # print(position)
+    # franka.directlyMoveTo(position)
+    j = franka.getJointPositions()
+    print(j)
+    # for i in range(20):
+    # env._step()
+    franka.setJointPositionsDirectly(j)
     env._step()

pyrcareworld/Examples/pose_converter.py

@@ -0,0 +1,31 @@
+import pickle
+import json
+
+# Load the pickle file
+with open("/home/cathy/Downloads/18-0-7.pkl", "rb") as f:
+    data = pickle.load(f)
+
+# Convert the data to the desired JSON structure (This depends on your pickle structure)
+# For this example, I'm assuming your pickle file has a similar structure as mentioned in the earlier JSON example.
+
+data = data[0]
+# json_data = []
+print(data[100]["caregiver_info"])
+
+# for frame_data in data:
+#     frame_entry = {"frame": frame_data["frame"], "bones": []}
+
+#     for bone_data in frame_data["bones"]:
+#         bone_entry = {
+#             "startPoint": bone_data["startPoint"],
+#             "endPoint": bone_data["endPoint"],
+#             "startName": bone_data["startName"],
+#             "endName": bone_data["endName"],
+#         }
+#         frame_entry["bones"].append(bone_entry)
+
+#     json_data.append(frame_entry)
+
+# # Save the data as JSON
+# with open("output_data.json", "w") as json_file:
+#     json.dump(json_data, json_file)

pyrcareworld/Examples/pose_visualizer.py

@@ -0,0 +1,175 @@
+import pickle
+import matplotlib.pyplot as plt
+
+JOINT_NAMES = [
+    "pelvis",
+    "left_hip",
+    "right_hip",
+    "spine1",
+    "left_knee",
+    "right_knee",
+    "spine2",
+    "left_ankle",
+    "right_ankle",
+    "spine3",
+    "left_foot",
+    "right_foot",
+    "neck",
+    "left_collar",
+    "right_collar",
+    "head",
+    "left_shoulder",
+    "right_shoulder",
+    "left_elbow",
+    "right_elbow",
+    "left_wrist",
+    "right_wrist",
+    "jaw",
+    "left_eye_smplhf",
+    "right_eye_smplhf",
+    "left_index1",
+    "left_index2",
+    "left_index3",
+    "left_middle1",
+    "left_middle2",
+    "left_middle3",
+    "left_pinky1",
+    "left_pinky2",
+    "left_pinky3",
+    "left_ring1",
+    "left_ring2",
+    "left_ring3",
+    "left_thumb1",
+    "left_thumb2",
+    "left_thumb3",
+    "right_index1",
+    "right_index2",
+    "right_index3",
+    "right_middle1",
+    "right_middle2",
+    "right_middle3",
+    "right_pinky1",
+    "right_pinky2",
+    "right_pinky3",
+    "right_ring1",
+    "right_ring2",
+    "right_ring3",
+    "right_thumb1",
+    "right_thumb2",
+    "right_thumb3",
+    "nose",
+    "right_eye",
+    "left_eye",
+    "right_ear",
+    "left_ear",
+    "left_big_toe",
+    "left_small_toe",
+    "left_heel",
+    "right_big_toe",
+    "right_small_toe",
+    "right_heel",
+    "left_thumb",
+    "left_index",
+    "left_middle",
+    "left_ring",
+    "left_pinky",
+    "right_thumb",
+    "right_index",
+    "right_middle",
+    "right_ring",
+    "right_pinky",
+    "right_eye_brow1",
+    "right_eye_brow2",
+    "right_eye_brow3",
+    "right_eye_brow4",
+    "right_eye_brow5",
+    "left_eye_brow5",
+    "left_eye_brow4",
+    "left_eye_brow3",
+    "left_eye_brow2",
+    "left_eye_brow1",
+    "nose1",
+    "nose2",
+    "nose3",
+    "nose4",
+    "right_nose_2",
+    "right_nose_1",
+    "nose_middle",
+    "left_nose_1",
+    "left_nose_2",
+    "right_eye1",
+    "right_eye2",
+    "right_eye3",
+    "right_eye4",
+    "right_eye5",
+    "right_eye6",
+    "left_eye4",
+    "left_eye3",
+    "left_eye2",
+    "left_eye1",
+    "left_eye6",
+    "left_eye5",
+    "right_mouth_1",
+    "right_mouth_2",
+    "right_mouth_3",
+    "mouth_top",
+    "left_mouth_3",
+    "left_mouth_2",
+    "left_mouth_1",
+    "left_mouth_5",  # 59 in OpenPose output
+    "left_mouth_4",  # 58 in OpenPose output
+    "mouth_bottom",
+    "right_mouth_4",
+    "right_mouth_5",
+    "right_lip_1",
+    "right_lip_2",
+    "lip_top",
+    "left_lip_2",
+    "left_lip_1",
+    "left_lip_3",
+    "lip_bottom",
+    "right_lip_3",
+    # Face contour
+    "right_contour_1",
+    "right_contour_2",
+    "right_contour_3",
+    "right_contour_4",
+    "right_contour_5",
+    "right_contour_6",
+    "right_contour_7",
+    "right_contour_8",
+    "contour_middle",
+    "left_contour_8",
+    "left_contour_7",
+    "left_contour_6",
+    "left_contour_5",
+    "left_contour_4",
+    "left_contour_3",
+    "left_contour_2",
+    "left_contour_1",
+]
+
+# from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+
+# env = RCareWorldBaseEnv()
+
+# Open the file in binary read mode
+with open("/home/cathy/Downloads/test.pkl", "rb") as file:
+    data = pickle.load(file)
+# print(data.keys())
+# dict_keys(['reye_pose', 'leye_pose', 'left_hand_pose', 'betas', 'joints', 'jaw_pose', 'global_orient', 'body_pose', 'transl', 'expression', 'right_hand_pose', 'full_pose', 'gender'])
+print(len(data["joints"][0][0]))
+
+for i in range(len(data["joints"])):
+    positions = data["joints"][i]
+    x, y, z = positions[:, 0], positions[:, 1], positions[:, 2]
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection="3d")
+    ax.scatter(x, y, z)
+
+    ax.set_xlabel("X Label")
+    ax.set_ylabel("Y Label")
+    ax.set_zlabel("Z Label")
+
+    plt.show()

pyrcareworld/Test/scripts/launch_data_export.sh

@@ -0,0 +1,4 @@
+#!/bin/sh
+
+rm data/*
+python test_camera_image.py