Repository: empriselab/RCareWorld (language: python)

Issue:
Justin/cloth anchor
## Description:

Adds scripts to allow python to specify a particle group anchoring. Leverages new `Attr` functions seen in [this PR to RCareCommon.](https://github.com/empriselab/RCareCommon/pull/6)


Discussion / hints:

Fix patch:
diff --git a/.gitignore b/.gitignore
index 6ef3a3ea..7ad062e3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -25,6 +25,7 @@
 
 # Visual Studio cache directory
 .vs/
+.vscode/
 
 # Gradle cache directory
 .gradle/
@@ -230,4 +231,4 @@ cython_debug/
 #  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
-#.idea/
\ No newline at end of file
+#.idea/
diff --git a/planning/urdf_gen/python_script/combine2fbx.py b/planning/urdf_gen/python_script/combine2fbx.py
index fa2fdf53..efbddcfc 100644
--- a/planning/urdf_gen/python_script/combine2fbx.py
+++ b/planning/urdf_gen/python_script/combine2fbx.py
@@ -2,13 +2,14 @@
 import sys
 import os
 
+
 def combine2fbx(from_path, to_path, hier_file):
     # load all the ply object
     # and combine to a fbx
     # hier file: this object -> parent
 
     for item in os.listdir(from_path):
-        bpy.ops.import_scene.obj(filepath=from_path+"/"+item)
+        bpy.ops.import_scene.obj(filepath=from_path + "/" + item)
 
     objs = bpy.data.objects
     objs.remove(objs["Cube"], do_unlink=True)
@@ -20,14 +21,17 @@ def combine2fbx(from_path, to_path, hier_file):
             this_object, parent, _ = line.strip().split(" ")
             if parent != "None":
                 try:
-                    objs[this_object].parent = objs[parent]    
-                    objs[this_object].matrix_parent_inverse = objs[parent].matrix_world.inverted()
+                    objs[this_object].parent = objs[parent]
+                    objs[this_object].matrix_parent_inverse = objs[
+                        parent
+                    ].matrix_world.inverted()
                 except KeyError:
                     print("Warning: Virtual link exists!")
-    
+
     bpy.ops.export_scene.fbx(filepath=to_path)
 
+
 argv = sys.argv
-argv = argv[argv.index("--") + 1:]
+argv = argv[argv.index("--") + 1 :]
 print(argv)
-combine2fbx(argv[0], argv[1], argv[2])
\ No newline at end of file
+combine2fbx(argv[0], argv[1], argv[2])
diff --git a/planning/urdf_gen/python_script/extract_parts.py b/planning/urdf_gen/python_script/extract_parts.py
index 4ae97b63..94f1617e 100644
--- a/planning/urdf_gen/python_script/extract_parts.py
+++ b/planning/urdf_gen/python_script/extract_parts.py
@@ -2,15 +2,16 @@
 import argparse
 import os
 
+
 def parse_obj_with_group(obj_name):
     vlines = []
     vnlines = []
     glines = {}
     with open(obj_name) as f:
         for line in f:
-            if line.startswith("v "): # it is vertex line
+            if line.startswith("v "):  # it is vertex line
                 vlines.append(line)
-            if line.startswith("vn "): # it is normal line
+            if line.startswith("vn "):  # it is normal line
                 vnlines.append(line)
             if line.startswith("g"):
                 g_name = line.strip().split(" ")[-1]
@@ -30,15 +31,15 @@ def parse_obj_with_group(obj_name):
     parser.add_argument("-o", "--output_dir", type=str, default="smpl_parts")
     parser.add_argument("--hier", type=str)
     args = parser.parse_args()
-    #obj_name = "SMPL_f_unityDoubleBlends_lbs_10_scale5_207_v1.0.0.obj"
-    #obj_name = "SMPL_f_unityDoubleBlends_lbs_10_scale5_207_v1.0.0_groups.obj"
+    # obj_name = "SMPL_f_unityDoubleBlends_lbs_10_scale5_207_v1.0.0.obj"
+    # obj_name = "SMPL_f_unityDoubleBlends_lbs_10_scale5_207_v1.0.0_groups.obj"
     obj_name = args.object_name
 
     output_dir = args.output_dir
 
     if not os.path.exists(output_dir):
         os.makedirs(output_dir)
-    
+
     hier_path = args.hier
     hier_dict = {}
     co_vertex_dict = {}
@@ -47,12 +48,12 @@ def parse_obj_with_group(obj_name):
             child, parent, _ = line.strip().split()
             if parent != "None" and parent != "f_avg_root":
                 hier_dict[child] = parent
-    
+
     vlines, vnlines, glines = parse_obj_with_group(obj_name)
     all_counter = 0
     face_id_group_mapping = {}
     part_v_seq_new_index_mapping = {}
-    for part_name in glines.keys(): # for each part
+    for part_name in glines.keys():  # for each part
         v_seq = []
         vn_seq = []
         for line in glines[part_name]:
@@ -62,7 +63,7 @@ def parse_obj_with_group(obj_name):
             v3i, vt3i, vn3i = face3.split("/")
             v_seq.append([int(v1i), int(v2i), int(v3i)])
             vn_seq.append([int(vn1i), int(vn2i), int(vn3i)])
-        
+
         v_seq_unique = np.unique(v_seq)
         vn_seq_unique = np.unique(vn_seq)
 
@@ -71,16 +72,16 @@ def parse_obj_with_group(obj_name):
         v_seq_new_index_mapping = {}
         vn_seq_new_index_mapping = {}
         for i in range(len(v_seq_unique)):
-            v_seq_new_index_mapping[v_seq_unique[i]] = i+1
+            v_seq_new_index_mapping[v_seq_unique[i]] = i + 1
         for i in range(len(vn_seq_unique)):
             vn_seq_new_index_mapping[vn_seq_unique[i]] = i + 1
         part_v_seq_new_index_mapping[part_name] = v_seq_new_index_mapping
-        
-        with open(output_dir+"/"+part_name+".obj", "w") as fout:
+
+        with open(output_dir + "/" + part_name + ".obj", "w") as fout:
             for vi in v_seq_unique:
-                fout.write(vlines[vi-1])
+                fout.write(vlines[vi - 1])
             for vni in vn_seq_unique:
-                fout.write(vnlines[vni-1])
+                fout.write(vnlines[vni - 1])
             for line in glines[part_name]:
                 head, face1, face2, face3 = line.strip().split()
                 v1i, vt1i, vn1i = face1.split("/")
@@ -92,12 +93,42 @@ def parse_obj_with_group(obj_name):
                 new_vn1i = vn_seq_new_index_mapping[int(vn1i)]
                 new_vn2i = vn_seq_new_index_mapping[int(vn2i)]
                 new_vn3i = vn_seq_new_index_mapping[int(vn3i)]
-                fout.write(head+" "+str(new_v1i)+"//"+str(new_vn1i)+" "+str(new_v2i)+"//"+str(new_vn2i)+" "+str(new_v3i)+"//"+str(new_vn3i)+"\n")
-    
+                fout.write(
+                    head
+                    + " "
+                    + str(new_v1i)
+                    + "//"
+                    + str(new_vn1i)
+                    + " "
+                    + str(new_v2i)
+                    + "//"
+                    + str(new_vn2i)
+                    + " "
+                    + str(new_v3i)
+                    + "//"
+                    + str(new_vn3i)
+                    + "\n"
+                )
+
     with open(hier_path.replace("hier", "covertex"), "w") as f:
         for key in hier_dict.keys():
-            intersect_ids = face_id_group_mapping[key].intersection(face_id_group_mapping[hier_dict[key]])
+            intersect_ids = face_id_group_mapping[key].intersection(
+                face_id_group_mapping[hier_dict[key]]
+            )
             selected_id = int(list(intersect_ids)[0])
             current_child = int(part_v_seq_new_index_mapping[key][selected_id])
-            current_parent = int(part_v_seq_new_index_mapping[hier_dict[key]][selected_id])
-            f.write(key+" "+hier_dict[key]+" "+str(selected_id)+" "+str(current_child)+" "+str(current_parent)+"\n")
\ No newline at end of file
+            current_parent = int(
+                part_v_seq_new_index_mapping[hier_dict[key]][selected_id]
+            )
+            f.write(
+                key
+                + " "
+                + hier_dict[key]
+                + " "
+                + str(selected_id)
+                + " "
+                + str(current_child)
+                + " "
+                + str(current_parent)
+                + "\n"
+            )
diff --git a/planning/urdf_gen/python_script/extract_vertex_group.py b/planning/urdf_gen/python_script/extract_vertex_group.py
index 7dfdefdd..d5de9d7c 100644
--- a/planning/urdf_gen/python_script/extract_vertex_group.py
+++ b/planning/urdf_gen/python_script/extract_vertex_group.py
@@ -2,6 +2,7 @@
 import sys
 import os
 
+
 def find_max_group(weights):
     max_weight = 0
     max_index = 0
@@ -12,6 +13,7 @@ def find_max_group(weights):
             max_index = i
     return max_index
 
+
 def extractVertexGroup(from_path, to_path, hier_path):
     to_dir = "/".join(to_path.split("/")[:-1])
     res_dir = "/".join(hier_path.split("/")[:-1])
@@ -24,24 +26,27 @@ def extractVertexGroup(from_path, to_path, hier_path):
     objs.remove(objs["Cube"], do_unlink=True)
     objs.remove(objs["Camera"], do_unlink=True)
     objs.remove(objs["Light"], do_unlink=True)
-    
+
     bpy.ops.import_scene.fbx(filepath=from_path)
     print(bpy.data.armatures.keys())
-    real_armature = bpy.data.armatures['SMPLX-female']
+    real_armature = bpy.data.armatures["SMPLX-female"]
     print(real_armature.bones.keys())
-    
+
     with open(hier_path, "w") as f:
         for key in real_armature.bones.keys():
             if real_armature.bones[key].parent is None:
-                f.write(key+" None 0\n")
-            elif real_armature.bones[key].parent.name.find("root")!=-1:
-                f.write(key+" "+real_armature.bones[key].parent.name+" 0\n")
+                f.write(key + " None 0\n")
+            elif real_armature.bones[key].parent.name.find("root") != -1:
+                f.write(key + " " + real_armature.bones[key].parent.name + " 0\n")
             else:
-                f.write(key+" "+real_armature.bones[key].parent.name+" 1\n")
+                f.write(key + " " + real_armature.bones[key].parent.name + " 1\n")
+
+    bpy.ops.export_scene.obj(
+        filepath=to_path, keep_vertex_order=True, use_vertex_groups=True
+    )
 
-    bpy.ops.export_scene.obj(filepath=to_path, keep_vertex_order=True,use_vertex_groups=True)
 
 argv = sys.argv
-argv = argv[argv.index("--") + 1:]
+argv = argv[argv.index("--") + 1 :]
 
-extractVertexGroup(argv[0], argv[1], argv[2])
\ No newline at end of file
+extractVertexGroup(argv[0], argv[1], argv[2])
diff --git a/planning/urdf_gen/python_script/generate_rough_urdf.py b/planning/urdf_gen/python_script/generate_rough_urdf.py
index 6c49db2c..a7dba361 100644
--- a/planning/urdf_gen/python_script/generate_rough_urdf.py
+++ b/planning/urdf_gen/python_script/generate_rough_urdf.py
@@ -1,57 +1,155 @@
 import argparse
 import numpy as np
 
+
 def process_a_base(f, link_name, xyz, rpy, inertial=None):
     f.write('   <link name="{}">\n'.format(link_name))
-    f.write('       <inertial>\n')
-    f.write('           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]))
+    f.write("       <inertial>\n")
+    f.write(
+        '           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(
+            xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]
+        )
+    )
     f.write('           <mass value="0" />\n')
     if inertial is None:
-        f.write('           <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />\n')  
+        f.write(
+            '           <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />\n'
+        )
     else:
-        f.write('           <inertia ixx="{}" ixy="{}" ixz="{}" iyy="{}" iyz="{}" izz="{}" />\n'.format(inertial[0],inertial[1],inertial[2], inertial[3], inertial[4],inertial[5]))
-    f.write('       </inertial></link>\n') 
+        f.write(
+            '           <inertia ixx="{}" ixy="{}" ixz="{}" iyy="{}" iyz="{}" izz="{}" />\n'.format(
+                inertial[0],
+                inertial[1],
+                inertial[2],
+                inertial[3],
+                inertial[4],
+                inertial[5],
+            )
+        )
+    f.write("       </inertial></link>\n")
+
 
 def process_a_link(f, link_name, link_path, xyz, rpy, inertial=None, color=None):
     f.write('   <link name="{}">\n'.format(link_name))
-    f.write('       <inertial>\n')
-    f.write('           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]))
+    f.write("       <inertial>\n")
+    f.write(
+        '           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(
+            xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]
+        )
+    )
     f.write('           <mass value="1" />\n')
     if inertial is None:
-        f.write('           <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />\n')  
+        f.write(
+            '           <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />\n'
+        )
     else:
-        f.write('           <inertia ixx="{}" ixy="{}" ixz="{}" iyy="{}" iyz="{}" izz="{}" />\n'.format(inertial[0],inertial[1],inertial[2], inertial[3], inertial[4],inertial[5]))
-    f.write('       </inertial>\n')  
-    f.write('       <visual>\n')
-    f.write('           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]))
-    f.write('           <geometry><mesh filename="{}" /></geometry>\n'.format(link_path))
+        f.write(
+            '           <inertia ixx="{}" ixy="{}" ixz="{}" iyy="{}" iyz="{}" izz="{}" />\n'.format(
+                inertial[0],
+                inertial[1],
+                inertial[2],
+                inertial[3],
+                inertial[4],
+                inertial[5],
+            )
+        )
+    f.write("       </inertial>\n")
+    f.write("       <visual>\n")
+    f.write(
+        '           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(
+            xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]
+        )
+    )
+    f.write(
+        '           <geometry><mesh filename="{}" /></geometry>\n'.format(link_path)
+    )
     if color is None:
-        f.write('           <material name=""><color rgba="0.827450980392157 0.83921568627451 0.827450980392157 1" /></material>\n')
+        f.write(
+            '           <material name=""><color rgba="0.827450980392157 0.83921568627451 0.827450980392157 1" /></material>\n'
+        )
     else:
-        f.write('           <material name=""><color rgba="{} {} {} 1" /></material>\n'.format(color[0], color[1], color[2]))
-    f.write('       </visual>\n')
-    f.write('       <collision>\n')
-    f.write('           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]))
-    f.write('           <geometry><mesh filename="{}" /></geometry>\n'.format(link_path))
-    f.write('       </collision></link>\n')
-
-def process_a_1d_joint(f, joint_name, parent, child, xyz, rpy, joint_type="revolute", limits=[0,0,0,0], axis=[0,1,0]):
+        f.write(
+            '           <material name=""><color rgba="{} {} {} 1" /></material>\n'.format(
+                color[0], color[1], color[2]
+            )
+        )
+    f.write("       </visual>\n")
+    f.write("       <collision>\n")
+    f.write(
+        '           <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(
+            xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]
+        )
+    )
+    f.write(
+        '           <geometry><mesh filename="{}" /></geometry>\n'.format(link_path)
+    )
+    f.write("       </collision></link>\n")
+
+
+def process_a_1d_joint(
+    f,
+    joint_name,
+    parent,
+    child,
+    xyz,
+    rpy,
+    joint_type="revolute",
+    limits=[0, 0, 0, 0],
+    axis=[0, 1, 0],
+):
     f.write('   <joint name="{}" type="{}">\n'.format(joint_name, joint_type))
-    f.write('       <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]))
+    f.write(
+        '       <origin xyz="{} {} {}" rpy="{} {} {}" />\n'.format(
+            xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2]
+        )
+    )
     f.write('       <parent link="{}" />\n'.format(parent))
     f.write('       <child link="{}" />\n'.format(child))
-    f.write('       <axis xyz="{} {} {}" />\n'.format(axis[0],axis[1], axis[2]))
-    f.write('       <limit lower="{}" upper="{}" effort="{}" velocity="{}" />\n'.format(limits[0], limits[1], limits[2], limits[3]))
-    f.write('   </joint>\n')
+    f.write('       <axis xyz="{} {} {}" />\n'.format(axis[0], axis[1], axis[2]))
+    f.write(
+        '       <limit lower="{}" upper="{}" effort="{}" velocity="{}" />\n'.format(
+            limits[0], limits[1], limits[2], limits[3]
+        )
+    )
+    f.write("   </joint>\n")
+
 
 def process_a_3d_joint(f, parent, child, xyz, rpy, limits=None):
-    fake_link_name_x = parent+"_"+child+"_rot_link_x"
-    fake_link_name_y = parent+"_"+child+"_rot_link_y"
-    f.write('   <link name={}></link>\n'.format(fake_link_name_x))
-    f.write('   <link name={}></link>\n'.format(fake_link_name_y))
-    process_a_1d_joint(f, parent+"_"+child+"_joint_x", parent, fake_link_name_x, [0,0,0], rpy, limits=limits, axis=[1,0,0])
-    process_a_1d_joint(f, parent+"_"+child+"_joint_y", fake_link_name_x, fake_link_name_y, [0,0,0], rpy, limits=limits, axis=[0, 1,0])
-    process_a_1d_joint(f, parent+"_"+child+"_joint_z",fake_link_name_y, child, xyz, rpy, limits=limits, axis=[0,0,1])
+    fake_link_name_x = parent + "_" + child + "_rot_link_x"
+    fake_link_name_y = parent + "_" + child + "_rot_link_y"
+    f.write("   <link name={}></link>\n".format(fake_link_name_x))
+    f.write("   <link name={}></link>\n".format(fake_link_name_y))
+    process_a_1d_joint(
+        f,
+        parent + "_" + child + "_joint_x",
+        parent,
+        fake_link_name_x,
+        [0, 0, 0],
+        rpy,
+        limits=limits,
+        axis=[1, 0, 0],
+    )
+    process_a_1d_joint(
+        f,
+        parent + "_" + child + "_joint_y",
+        fake_link_name_x,
+        fake_link_name_y,
+        [0, 0, 0],
+        rpy,
+        limits=limits,
+        axis=[0, 1, 0],
+    )
+    process_a_1d_joint(
+        f,
+        parent + "_" + child + "_joint_z",
+        fake_link_name_y,
+        child,
+        xyz,
+        rpy,
+        limits=limits,
+        axis=[0, 0, 1],
+    )
+
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
@@ -62,7 +160,6 @@ def process_a_3d_joint(f, parent, child, xyz, rpy, limits=None):
     parser.add_argument("--hier_path", type=str)
     args = parser.parse_args()
 
-
     urdf_name = args.urdf_name
     part_dir_path = args.part_dir
 
@@ -75,45 +172,96 @@ def process_a_3d_joint(f, parent, child, xyz, rpy, limits=None):
     # for unity
     unity_urdf_name = urdf_name.replace(".urdf", "_for_unity.urdf")
     with open(unity_urdf_name, "w") as f:
-        f.write('<robot name="{}">\n'.format(unity_urdf_name.split(".")[0].split("/")[-1]))
+        f.write(
+            '<robot name="{}">\n'.format(unity_urdf_name.split(".")[0].split("/")[-1])
+        )
 
         with open(args.hier_path) as f1:
             for line in f1:
                 child, parent, _ = line.strip().split(" ")
                 if parent == "None":
-                    process_a_base(f, child, xyz=[0,0,0], rpy=[0,0,0])
+                    process_a_base(f, child, xyz=[0, 0, 0], rpy=[0, 0, 0])
                 else:
-                    link_path = "package://"+part_dir_path+"/"+child+".stl"
-                    process_a_link(f, child, link_path, xyz=[0,0,0], rpy=[0,0,0])
-                    if parent.find("root")!=-1:
-                        process_a_1d_joint(f, child+"_joint", parent, child, xyz=[0,0,0], rpy=[0,0,0], joint_type="fixed")
+                    link_path = "package://" + part_dir_path + "/" + child + ".stl"
+                    process_a_link(f, child, link_path, xyz=[0, 0, 0], rpy=[0, 0, 0])
+                    if parent.find("root") != -1:
+                        process_a_1d_joint(
+                            f,
+                            child + "_joint",
+                            parent,
+                            child,
+                            xyz=[0, 0, 0],
+                            rpy=[0, 0, 0],
+                            joint_type="fixed",
+                        )
                     else:
                         xyz = part_pos_dict[child] - part_pos_dict[parent]
-                        process_a_1d_joint(f, child+"_joint", parent, child, xyz=xyz, rpy=[0,0,0], limits=[0,np.pi,0,0])
+                        process_a_1d_joint(
+                            f,
+                            child + "_joint",
+                            parent,
+                            child,
+                            xyz=xyz,
+                            rpy=[0, 0, 0],
+                            limits=[0, np.pi, 0, 0],
+                        )
 
         f.write("</robot>\n")
-    
+
     # for moveit
     moveit_urdf_name = urdf_name.replace(".urdf", "_for_moveit.urdf")
     with open(moveit_urdf_name, "w") as f:
-        f.write('<robot name="{}">\n'.format(moveit_urdf_name.split(".")[0].split("/")[-1]))
+        f.write(
+            '<robot name="{}">\n'.format(moveit_urdf_name.split(".")[0].split("/")[-1])
+        )
 
         with open(args.hier_path) as f1:
             for line in f1:
                 child, parent, level = line.strip().split(" ")
                 if parent == "None":
-                    process_a_base(f, child, xyz=[0,0,0], rpy=[0,0,0])
+                    process_a_base(f, child, xyz=[0, 0, 0], rpy=[0, 0, 0])
                 else:
-                    link_path = "package://"+args.package_name+"/"+part_dir_path+"/"+child+".stl"
-                    process_a_link(f, child, link_path, xyz=[0,0,0], rpy=[0,0,0])
+                    link_path = (
+                        "package://"
+                        + args.package_name
+                        + "/"
+                        + part_dir_path
+                        + "/"
+                        + child
+                        + ".stl"
+                    )
+                    process_a_link(f, child, link_path, xyz=[0, 0, 0], rpy=[0, 0, 0])
                     if level == "0":
-                        if parent.find("root")!=-1:
-                            process_a_1d_joint(f, child+"_joint", parent, child, xyz=[0,0,0], rpy=[0,0,0], joint_type="fixed")
+                        if parent.find("root") != -1:
+                            process_a_1d_joint(
+                                f,
+                                child + "_joint",
+                                parent,
+                                child,
+                                xyz=[0, 0, 0],
+                                rpy=[0, 0, 0],
+                                joint_type="fixed",
+                            )
                         else:
                             xyz = part_pos_dict[child] - part_pos_dict[parent]
-                            process_a_1d_joint(f, child+"_joint", parent, child, xyz=xyz, rpy=[0,0,0], limits=[0,np.pi,0,0])
+                            process_a_1d_joint(
+                                f,
+                                child + "_joint",
+                                parent,
+                                child,
+                                xyz=xyz,
+                                rpy=[0, 0, 0],
+                                limits=[0, np.pi, 0, 0],
+                            )
                     else:
                         xyz = part_pos_dict[child] - part_pos_dict[parent]
-                        process_a_3d_joint(f, parent, child, xyz=xyz, rpy=[0,0,0], limits=[0, np.pi, 0, 0])
+                        process_a_3d_joint(
+                            f,
+                            parent,
+                            child,
+                            xyz=xyz,
+                            rpy=[0, 0, 0],
+                            limits=[0, np.pi, 0, 0],
+                        )
 
-        f.write("</robot>\n")
\ No newline at end of file
+        f.write("</robot>\n")
diff --git a/planning/urdf_gen/python_script/normalize_part.py b/planning/urdf_gen/python_script/normalize_part.py
index 48246781..64ae832f 100644
--- a/planning/urdf_gen/python_script/normalize_part.py
+++ b/planning/urdf_gen/python_script/normalize_part.py
@@ -2,25 +2,30 @@
 import argparse
 import numpy as np
 
+
 def parse_obj(obj_name):
     vertex_data = []
     flines = []
     with open(obj_name) as f:
         for line in f:
-            if line.startswith("v "): # it is vertex line
+            if line.startswith("v "):  # it is vertex line
                 head, x, y, z = line.strip().split(" ")
                 vertex_data.append([float(x), float(y), float(z)])
             if line.startswith("f"):
                 flines.append(line)
     return np.array(vertex_data), flines
 
+
 def get_center(vertex):
     return np.mean(vertex, axis=0)
 
+
 def write_obj(path, vertex, faces):
     with open(path, "w") as f:
         for item in vertex:
-            f.write("v "+str(item[0])+" "+str(item[1])+" "+str(item[2])+"\n")
+            f.write(
+                "v " + str(item[0]) + " " + str(item[1]) + " " + str(item[2]) + "\n"
+            )
         for line in faces:
             f.write(line)
 
@@ -38,11 +43,20 @@ def write_obj(path, vertex, faces):
 
     if not os.path.exists(write_dir):
         os.makedirs(write_dir)
-    
-    with open(args.offset_path,"w") as f:
+
+    with open(args.offset_path, "w") as f:
         for part in part_list:
-            vertex, faces = parse_obj(input_dir+"/"+part)
+            vertex, faces = parse_obj(input_dir + "/" + part)
             center = get_center(vertex)
-            f.write(part+" "+str(center[0])+" "+str(center[1])+" "+str(center[2])+"\n")
+            f.write(
+                part
+                + " "
+                + str(center[0])
+                + " "
+                + str(center[1])
+                + " "
+                + str(center[2])
+                + "\n"
+            )
             vertex -= center
-            write_obj(write_dir+"/"+part, vertex, faces)
\ No newline at end of file
+            write_obj(write_dir + "/" + part, vertex, faces)
diff --git a/planning/urdf_gen/python_script/obj2stl.py b/planning/urdf_gen/python_script/obj2stl.py
index d38f82ed..db264cbc 100644
--- a/planning/urdf_gen/python_script/obj2stl.py
+++ b/planning/urdf_gen/python_script/obj2stl.py
@@ -2,7 +2,7 @@
 import os
 import argparse
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     parser = argparse.ArgumentParser()
     parser.add_argument("-i", "--input_dir", type=str, default="smpl_stl")
     parser.add_argument("-o", "--output_dir", type=str, default="smpl_parts")
@@ -17,5 +17,5 @@
         if item.split(".")[-1] != "obj":
             continue
         ms = pymeshlab.MeshSet()
-        ms.load_new_mesh(input_dir+"/"+item)
-        ms.save_current_mesh(output_dir+"/"+item.replace(".obj", ".stl"))
\ No newline at end of file
+        ms.load_new_mesh(input_dir + "/" + item)
+        ms.save_current_mesh(output_dir + "/" + item.replace(".obj", ".stl"))
diff --git a/planning/urdf_gen/python_script/write_package_info.py b/planning/urdf_gen/python_script/write_package_info.py
index 8aaa27b6..2326b5ac 100644
--- a/planning/urdf_gen/python_script/write_package_info.py
+++ b/planning/urdf_gen/python_script/write_package_info.py
@@ -1,24 +1,28 @@
 import argparse
 from asyncore import write
 
-def write_packge(res_dir,version, description, email, author, license, buildtool_dep, run_dep):
-    with open(res_dir+"/package.xml", "w") as f:
+
+def write_packge(
+    res_dir, version, description, email, author, license, buildtool_dep, run_dep
+):
+    with open(res_dir + "/package.xml", "w") as f:
         f.write('<?xml version="1.0"?>\n')
-        f.write('<package>\n')
-        f.write('<name>{}</name>\n'.format(res_dir))
-        f.write('<version>{}</version>\n'.format(version))
-        f.write('<description>{}</description>\n'.format(description))
+        f.write("<package>\n")
+        f.write("<name>{}</name>\n".format(res_dir))
+        f.write("<version>{}</version>\n".format(version))
+        f.write("<description>{}</description>\n".format(description))
         f.write('<maintainer email="{}">{}</maintainer>\n'.format(email, author))
-        f.write('<license>{}</license>\n'.format(license))
-        f.write('<author>{}</author>\n'.format(author))
+        f.write("<license>{}</license>\n".format(license))
+        f.write("<author>{}</author>\n".format(author))
         for item in buildtool_dep:
-            f.write('<buildtool_depend>{}</buildtool_depend>\n'.format(item))
+            f.write("<buildtool_depend>{}</buildtool_depend>\n".format(item))
         for item in run_dep:
             f.write("<run_depend>{}</run_depend>\n".format(item))
         f.write("</package>\n")
 
+
 def write_cmake_list(res_dir, part_dir, buildtool_dep):
-    with open(res_dir+"/CMakeLists.txt", "w") as f:
+    with open(res_dir + "/CMakeLists.txt", "w") as f:
         f.write("cmake_minimum_required(VERSION 2.8.3)\n")
         f.write("project({})\n".format(res_dir))
 
@@ -45,8 +49,17 @@ def write_cmake_list(res_dir, part_dir, buildtool_dep):
     args = parser.parse_args()
 
     buildtool_dep = ["catkin"]
-    run_dep = ["robot_state_publisher","joint_state_publisher","tf2_ros","rviz"]
+    run_dep = ["robot_state_publisher", "joint_state_publisher", "tf2_ros", "rviz"]
 
-    write_packge(args.name, args.version, args.desc, args.email, args.author, args.license, buildtool_dep, run_dep)
+    write_packge(
+        args.name,
+        args.version,
+        args.desc,
+        args.email,
+        args.author,
+        args.license,
+        buildtool_dep,
+        run_dep,
+    )
 
-    write_cmake_list(args.name, args.part_dir, buildtool_dep)
\ No newline at end of file
+    write_cmake_list(args.name, args.part_dir, buildtool_dep)
diff --git a/planning/urdf_gen/run_extract_parts.sh b/planning/urdf_gen/run_extract_parts.sh
index a326128a..5cad25a0 100644
--- a/planning/urdf_gen/run_extract_parts.sh
+++ b/planning/urdf_gen/run_extract_parts.sh
@@ -18,4 +18,4 @@ python $py_script_path/obj2stl.py -i $mid_output_dir/parts_obj_normal -o $res_di
 # the -i part_dir path is used for writing in urdf, package://part_dir
 python $py_script_path/generate_rough_urdf.py -i $part_dir -o $res_dir/$urdf_name.urdf -n $res_dir --offset_path $mid_output_dir/offset.txt --hier_path $mid_output_dir/hier.txt
 
-python $py_script_path/write_package_info.py -n $res_dir -i $part_dir
\ No newline at end of file
+python $py_script_path/write_package_info.py -n $res_dir -i $part_dir
diff --git a/pyrcareworld/Examples/example_robot_move.py b/pyrcareworld/Examples/example_robot_move.py
index 2946d750..f2f703f8 100644
--- a/pyrcareworld/Examples/example_robot_move.py
+++ b/pyrcareworld/Examples/example_robot_move.py
@@ -2,17 +2,13 @@
 
 env = RCareWorld()
 robot = env.create_robot(
-    id=315892,
-    gripper_list=["3158920"],
+    id=315893,
+    gripper_list=["3158930"],
     robot_name="kinova_gen3_7dof-robotiq85",
-    # base_pos=[7.74100018,14.8170004,10.4515963],
+    base_pos=[0, 0, 1],
 )
-target = env.create_object(id=1000, name="Cube", is_in_scene=True)
-# robot.load()
+target = env.create_object(id=315867, name="Cube", is_in_scene=True)
 while True:
     position = target.getPosition()
-    env._step()
-    # print(position)
     robot.directlyMoveTo(position)
-    print(robot.getRobotState())
-    env._step()
\ No newline at end of file
+    env.step()
diff --git a/pyrcareworld/Examples/example_robot_move_directly.py b/pyrcareworld/Examples/example_robot_move_directly.py
deleted file mode 100644
index 3ecb8097..00000000
--- a/pyrcareworld/Examples/example_robot_move_directly.py
+++ /dev/null
@@ -1,23 +0,0 @@
-from pyrcareworld.envs import RCareWorld
-
-env = RCareWorld()
-franka = env.create_robot(
-    id=965874, gripper_list=["9658740"], robot_name="franka-panda", base_pos=[0, 0, 0]
-)
-
-# 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 = target.getPosition()
-    # print(position)
-    # franka.directlyMoveTo(position)
-    j = franka.getJointPositions()
-    print(j)
-    # for i in range(20):
-    # env._step()
-    franka.setJointPositionsDirectly(j)
-    env._step()
diff --git a/pyrcareworld/Examples/pose_converter.py b/pyrcareworld/Examples/pose_converter.py
deleted file mode 100644
index 34c49fef..00000000
--- a/pyrcareworld/Examples/pose_converter.py
+++ /dev/null
@@ -1,31 +0,0 @@
-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)
diff --git a/pyrcareworld/Examples/pose_visualizer.py b/pyrcareworld/Examples/pose_visualizer.py
deleted file mode 100644
index 01185c51..00000000
--- a/pyrcareworld/Examples/pose_visualizer.py
+++ /dev/null
@@ -1,175 +0,0 @@
-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()
diff --git a/pyrcareworld/URDF/tobor/debug.txt b/pyrcareworld/URDF/tobor/debug.txt
index a696b24a..1a194917 100755
--- a/pyrcareworld/URDF/tobor/debug.txt
+++ b/pyrcareworld/URDF/tobor/debug.txt
@@ -36,4 +36,4 @@ rfuniverse right: [[0.07000000029802322, 1.0, 0.2199999988079071],
     [0.8055415749549866, 0.9999440908432007, 0.22000013291835785], 
     [1.025987982749939, 0.9998888969421387, 0.22000034153461456], 
     [1.1711138486862183, 0.9998526573181152, 0.2200004905462265], 
-    [1.4220223426818848, 0.9997892379760742, 0.22000059485435486]]
\ No newline at end of file
+    [1.4220223426818848, 0.9997892379760742, 0.22000059485435486]]
diff --git a/pyrcareworld/pyrcareworld/agents/cloth.py b/pyrcareworld/pyrcareworld/agents/cloth.py
new file mode 100644
index 00000000..eac568b0
--- /dev/null
+++ b/pyrcareworld/pyrcareworld/agents/cloth.py
@@ -0,0 +1,31 @@
+from pyrcareworld.objects.object import RCareWorldBaseObject
+
+
+class Cloth(RCareWorldBaseObject):
+    """
+    An Obi Cloth Actor in RCareWorld.
+    """
+
+    def __init__(self, env, id: int, name: str, is_in_scene: bool = False):
+        super().__init__(env=env, id=id, name=name, is_in_scene=is_in_scene)
+
+    def addParticleAnchor(self, particle_group_name: str, anchor_id: int):
+        """
+        Add a particle anchor to the cloth actor for the specified particle group and anchored to the object given by anchor_id.
+        """
+        self.env.instance_channel.set_action(
+            "AddParticleAnchor",
+            id=self.id,
+            particle_group_name=particle_group_name,
+            anchor_id=anchor_id,
+        )
+
+    def removeParticleAnchor(self, particle_group_name: str):
+        """
+        Remove a particle anchor from the cloth actor for the specified particle group. Removes ALL anchors for the particle group.
+        """
+        self.env.instance_channel.set_action(
+            "RemoveParticleAnchor",
+            id=self.id,
+            particle_group_name=particle_group_name,
+        )
diff --git a/pyrcareworld/pyrcareworld/agents/human.py b/pyrcareworld/pyrcareworld/agents/human.py
index cad8bbbd..5ef820a0 100644
--- a/pyrcareworld/pyrcareworld/agents/human.py
+++ b/pyrcareworld/pyrcareworld/agents/human.py
@@ -1,4 +1,5 @@
 from pyrcareworld.objects import RCareWorldBaseObject
+from typing import Optional
 
 
 class Human(RCareWorldBaseObject):
@@ -8,20 +9,7 @@ class Human(RCareWorldBaseObject):
 
     def __init__(self, env, id: int, name: str, is_in_scene: bool = False):
         super().__init__(env=env, id=id, name=name, is_in_scene=is_in_scene)
-
-    def setBasePosition(self, position: list):
-        self.env.instance_channel.set_action(
-            "SetTransform", id=self.id, position=position
-        )
-
-    def setRootRotation(self, rotation: list):
-        self.env.instance_channel.set_action(
-            "SetTransform", id=self.id, rotation=rotation
-        )
-
-    def setJointRotationByName(self, joint_name: str, position: list):
-        # bone name list:
-        name_list = [
+        self.name_list = [
             "Pelvis",
             "Spine1",
             "Spine2",
@@ -78,7 +66,19 @@ def setJointRotationByName(self, joint_name: str, position: list):
             "RightPinky2",
             "RightPinky3",
         ]
-        if joint_name not in name_list:
+
+    def setBasePosition(self, position: list):
+        self.env.instance_channel.set_action(
+            "SetTransform", id=self.id, position=position
+        )
+
+    def setRootRotation(self, rotation: list):
+        self.env.instance_channel.set_action(
+            "SetTransform", id=self.id, rotation=rotation
+        )
+
+    def setJointRotationByName(self, joint_name: str, position: list):
+        if joint_name not in self.name_list:
             raise ValueError("The joint name is not in the list")
         self.env.instance_channel.set_action(
             "SetNameBonePosition",
@@ -89,65 +89,45 @@ def setJointRotationByName(self, joint_name: str, position: list):
             bone_position_z=position[2],
         )
 
+    def setJointLimits(
+        self,
+        joint_name: str,
+        lower_limit: float,
+        upper_limit: float,
+        axis: Optional[str] = None,
+    ):
+        """
+        Sets the joint limits for this human object in Unity.
+
+        Args:
+            joint_name: The name of the joint (or bone).
+            lower_limit: The lower limit of the joint.
+            upper_limit: The upper limit of the joint.
+            axis: The axis of the joint. If None, all axes are set.
+        """
+        if joint_name not in self.name_list:
+            raise ValueError("The joint name is not in the list")
+
+        if (axis in ["X", "Y", "Z"]) is False:
+            self.env.instance_channel.set_action(
+                "SetJointLimits",
+                id=self.id,
+                bone_name=joint_name,
+                lower_limit=lower_limit,
+                upper_limit=upper_limit,
+            )
+        else:
+            self.env.instance_channel.set_action(
+                "SetJointLimits",
+                id=self.id,
+                bone_name=joint_name,
+                lower_limit=lower_limit,
+                upper_limit=upper_limit,
+                axis=axis,
+            )
+
     def setJointRotationByNameDirectly(self, joint_name: str, position: list):
-        name_list = [
-            "Pelvis",
-            "Spine1",
-            "Spine2",
-            "Spine3",
-            "LeftShoulder",
-            "LeftUpperArm",
-            "LeftLowerArm",
-            "LeftHand",
-            "RightShoulder",
-            "RightUpperArm",
-            "RightLowerArm",
-            "RightHand",
-            "LeftUpperLeg",
-            "LeftLowerLeg",
-            "LeftFoot",
-            "LeftToes",
-            "RightUpperLeg",
-            "RightLowerLeg",
-            "RightFoot",
-            "RightToes",
-            "Neck",
-            "Head",
-            "LeftEye",
-            "RightEye",
-            "Jaw",
-            "LeftThumb1",
-            "LeftThumb2",
-            "LeftThumb3",
-            "LeftIndex1",
-            "LeftIndex2",
-            "LeftIndex3",
-            "LeftMiddle1",
-            "LeftMiddle2",
-            "LeftMiddle3",
-            "LeftRing1",
-            "LeftRing2",
-            "LeftRing3",
-            "LeftPinky1",
-            "LeftPinky2",
-            "LeftPinky3",
-            "RightThumb1",
-            "RightThumb2",
-            "RightThumb3",
-            "RightIndex1",
-            "RightIndex2",
-            "RightIndex3",
-            "RightMiddle1",
-            "RightMiddle2",
-            "RightMiddle3",
-            "RightRing1",
-            "RightRing2",
-            "RightRing3",
-            "RightPinky1",
-            "RightPinky2",
-            "RightPinky3",
-        ]
-        if joint_name not in name_list:
+        if joint_name not in self.name_list:
             raise ValueError("The joint name is not in the list")
         self.env.instance_channel.set_action(
             "SetNameBonePositionDirectly",
@@ -159,64 +139,7 @@ def setJointRotationByNameDirectly(self, joint_name: str, position: list):
         )
 
     def getJointStateByName(self, joint_name: str):
-        name_list = [
-            "Pelvis",
-            "Spine1",
-            "Spine2",
-            "Spine3",
-            "LeftShoulder",
-            "LeftUpperArm",
-            "LeftLowerArm",
-            "LeftHand",
-            "RightShoulder",
-            "RightUpperArm",
-            "RightLowerArm",
-            "RightHand",
-            "LeftUpperLeg",
-            "LeftLowerLeg",
-            "LeftFoot",
-            "LeftToes",
-            "RightUpperLeg",
-            "RightLowerLeg",
-            "RightFoot",
-            "RightToes",
-            "Neck",
-            "Head",
-            "LeftEye",
-            "RightEye",
-            "Jaw",
-            "LeftThumb1",
-            "LeftThumb2",
-            "LeftThumb3",
-            "LeftIndex1",
-            "LeftIndex2",
-            "LeftIndex3",
-            "LeftMiddle1",
-            "LeftMiddle2",
-            "LeftMiddle3",
-            "LeftRing1",
-            "LeftRing2",
-            "LeftRing3",
-            "LeftPinky1",
-            "LeftPinky2",
-            "LeftPinky3",
-            "RightThumb1",
-            "RightThumb2",
-            "RightThumb3",
-            "RightIndex1",
-            "RightIndex2",
-            "RightIndex3",
-            "RightMiddle1",
-            "RightMiddle2",
-            "RightMiddle3",
-            "RightRing1",
-            "RightRing2",
-            "RightRing3",
-            "RightPinky1",
-            "RightPinky2",
-            "RightPinky3",
-        ]
-        if joint_name not in name_list:
+        if joint_name not in self.name_list:
             raise ValueError("The joint name is not in the list")
         # print(self.env.instance_channel.data)
         joint_states = self.env.instance_channel.data[self.id][joint_name]
@@ -274,11 +197,9 @@ def saveArticulationBoneData(self, path: str):
         self.env.instance_channel.set_action(
             "SaveArticulationBoneData", id=self.id, path=path
         )
-    
+
     def enableSoftBody(self):
         """
         TODO: enable soft body
         """
-        self.env.instance_channel.set_action(
-            "EnableSoftBody", id=self.id
-        )
+        self.env.instance_channel.set_action("EnableSoftBody", id=self.id)
diff --git a/pyrcareworld/pyrcareworld/agents/robot.py b/pyrcareworld/pyrcareworld/agents/robot.py
index ffdd3068..36dd44b4 100644
--- a/pyrcareworld/pyrcareworld/agents/robot.py
+++ b/pyrcareworld/pyrcareworld/agents/robot.py
@@ -438,7 +438,6 @@ def moveTo(self, targetPose: list, targetRot=None) -> None:
             joint_positions = self.ik_controller.calculate_ik_recursive(targetPose)
         self.setJointPositions(joint_positions)
 
-
     def directlyMoveTo(self, targetPose: list, targetRot: list = None) -> None:
         if targetRot is not None:
             joint_positions = self.ik_controller.calculate_ik_recursive(
@@ -481,9 +480,8 @@ def BioIKRotateQua(
     def GripperOpen(self) -> None:
         self.env.instance_channel.set_action("GripperOpen", id=self.gripper_id[0])
 
-
     def GripperClose(self) -> None:
         self.env.instance_channel.set_action("GripperClose", id=self.gripper_id[0])
 
     def reset(self) -> None:
-        self.ik_controller.reset()
\ No newline at end of file
+        self.ik_controller.reset()
diff --git a/pyrcareworld/pyrcareworld/attributes/activelightsensor_attr.py b/pyrcareworld/pyrcareworld/attributes/activelightsensor_attr.py
index 1c862985..5bf11a80 100755
--- a/pyrcareworld/pyrcareworld/attributes/activelightsensor_attr.py
+++ b/pyrcareworld/pyrcareworld/attributes/activelightsensor_attr.py
@@ -49,27 +49,27 @@ def parse_message(msg: IncomingMessage) -> dict:
                 [0.0, 0.0, 0.0, 1.0],
             ]
         )
-        this_object_data[
-            "active_depth"
-        ] = active_depth.calc_main_depth_from_left_right_ir(
-            image_left,
-            image_right,
-            left_extrinsic_matrix,
-            right_extrinsic_matrix,
-            main_extrinsic_matrix,
-            ir_intrinsic_matrix,
-            ir_intrinsic_matrix,
-            main_intrinsic_matrix,
-            lr_consistency=False,
-            main_cam_size=(
-                main_intrinsic_matrix[0, 2] * 2,
-                main_intrinsic_matrix[1, 2] * 2,
-            ),
-            ndisp=128,
-            use_census=True,
-            register_depth=True,
-            census_wsize=7,
-            use_noise=False,
+        this_object_data["active_depth"] = (
+            active_depth.calc_main_depth_from_left_right_ir(
+                image_left,
+                image_right,
+                left_extrinsic_matrix,
+                right_extrinsic_matrix,
+                main_extrinsic_matrix,
+                ir_intrinsic_matrix,
+                ir_intrinsic_matrix,
+                main_intrinsic_matrix,
+                lr_consistency=False,
+                main_cam_size=(
+                    main_intrinsic_matrix[0, 2] * 2,
+                    main_intrinsic_matrix[1, 2] * 2,
+                ),
+                ndisp=128,
+                use_census=True,
+                register_depth=True,
+                census_wsize=7,
+                use_noise=False,
+            )
         )
         this_object_data["active_depth"][this_object_data["active_depth"] > 8.0] = 0
         this_object_data["active_depth"][this_object_data["active_depth"] < 0.1] = 0
diff --git a/pyrcareworld/pyrcareworld/attributes/cloth_attr.py b/pyrcareworld/pyrcareworld/attributes/cloth_attr.py
index 50c65917..24d6b472 100755
--- a/pyrcareworld/pyrcareworld/attributes/cloth_attr.py
+++ b/pyrcareworld/pyrcareworld/attributes/cloth_attr.py
@@ -9,3 +9,38 @@
 def parse_message(msg: IncomingMessage) -> dict:
     this_object_data = attr.base_attr.parse_message(msg)
     return this_object_data
+
+
+def AddParticleAnchor(kwargs: dict) -> OutgoingMessage:
+    """
+    Sends a message containing a request to add a particle anchor to the cloth actor for the specified particle group and anchored to the object given by anchor_id.
+    """
+    compulsory_params = ["id", "particle_group_name", "anchor_id"]
+    utility.CheckKwargs(kwargs, compulsory_params)
+
+    msg = OutgoingMessage()
+
+    msg.write_int32(kwargs["id"])
+    msg.write_string("AddParticleAnchor")
+
+    msg.write_string(kwargs["particle_group_name"])
+    msg.write_int32(kwargs["anchor_id"])
+
+    return msg
+
+
+def RemoveParticleAnchor(kwargs: dict) -> OutgoingMessage:
+    """
+    Sends a message containing a request to remove a particle anchor from the cloth actor for the specified particle group. Removes ALL anchors for the particle group.
+    """
+    compulsory_params = ["id", "particle_group_name"]
+    utility.CheckKwargs(kwargs, compulsory_params)
+
+    msg = OutgoingMessage()
+
+    msg.write_int32(kwargs["id"])
+    msg.write_string("RemoveParticleAnchor")
+
+    msg.write_string(kwargs["particle_group_name"])
+
+    return msg
diff --git a/pyrcareworld/pyrcareworld/attributes/digit_attr.py b/pyrcareworld/pyrcareworld/attributes/digit_attr.py
index 63e07e55..77a5db00 100755
--- a/pyrcareworld/pyrcareworld/attributes/digit_attr.py
+++ b/pyrcareworld/pyrcareworld/attributes/digit_attr.py
@@ -19,7 +19,7 @@ def GetData(kwargs: dict) -> OutgoingMessage:
     compulsory_params = ["id"]
     optional_params = []
     utility.CheckKwargs(kwargs, compulsory_params)
-    msg = OutgoingMessage() 
+    msg = OutgoingMessage()
 
     msg.write_int32(kwargs["id"])
     msg.write_string("GetData")
diff --git a/pyrcareworld/pyrcareworld/attributes/humanarticulation_attr.py b/pyrcareworld/pyrcareworld/attributes/humanarticulation_attr.py
index af3c6f55..963e1a98 100644
--- a/pyrcareworld/pyrcareworld/attributes/humanarticulation_attr.py
+++ b/pyrcareworld/pyrcareworld/attributes/humanarticulation_attr.py
@@ -145,3 +145,36 @@ def SaveArticulationBoneData(kwargs: dict) -> OutgoingMessage:
     msg.write_string(kwargs["path"])
 
     return msg
+
+
+def SetJointLimits(kwargs: dict) -> OutgoingMessage:
+    """
+    Set joint limits for a specific bone in an ArticulationBody. Will apply to the joint connecting that bone to the previous bone in the hierarchy.
+
+    Args:
+        Compulsory:
+            id: The id of the ArticulationBody object.
+            bone_name: The name of the bone to select a joint for.
+            lower_limit: The lower limit of the joint.
+            upper_limit: The upper limit of the joint.
+        Optional:
+            axis: The axis of the joint to set the limits for: {"X", "Y", or "Z"}.
+
+            If not specified, sets all limits (often fine since most joints only have 1 free axis).
+    """
+    compulsory_params = ["id", "bone_name", "lower_limit", "upper_limit"]
+    optional_params = ["axis"]
+    utility.CheckKwargs(kwargs, compulsory_params)
+
+    msg = OutgoingMessage()
+
+    msg.write_int32(kwargs["id"])
+    msg.write_string("SetJointLimits")
+    msg.write_string(kwargs["bone_name"])
+    msg.write_float32(kwargs["lower_limit"])
+    msg.write_float32(kwargs["upper_limit"])
+    msg.write_bool("axis" in kwargs)
+    if "axis" in kwargs:
+        msg.write_string(kwargs["axis"])
+
+    return msg
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/agent_action_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/agent_action_pb2.py
index 4f1f539d..fe86a3a6 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/agent_action_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/agent_action_pb2.py
@@ -125,7 +125,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_AGENTACTIONPROTO,
-        __module__="pyrcareworld.communicator_objects.agent_action_pb2"
+        __module__="pyrcareworld.communicator_objects.agent_action_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.AgentActionProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/agent_info_action_pair_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/agent_info_action_pair_pb2.py
index 1b746e83..1cc33e96 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/agent_info_action_pair_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/agent_info_action_pair_pb2.py
@@ -93,14 +93,10 @@
     serialized_end=340,
 )
 
-_AGENTINFOACTIONPAIRPROTO.fields_by_name[
-    "agent_info"
-].message_type = (
+_AGENTINFOACTIONPAIRPROTO.fields_by_name["agent_info"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_agent__info__pb2._AGENTINFOPROTO
 )
-_AGENTINFOACTIONPAIRPROTO.fields_by_name[
-    "action_info"
-].message_type = (
+_AGENTINFOACTIONPAIRPROTO.fields_by_name["action_info"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_agent__action__pb2._AGENTACTIONPROTO
 )
 DESCRIPTOR.message_types_by_name["AgentInfoActionPairProto"] = _AGENTINFOACTIONPAIRPROTO
@@ -111,7 +107,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_AGENTINFOACTIONPAIRPROTO,
-        __module__="pyrcareworld.communicator_objects.agent_info_action_pair_pb2"
+        __module__="pyrcareworld.communicator_objects.agent_info_action_pair_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.AgentInfoActionPairProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/agent_info_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/agent_info_pb2.py
index f7ffb158..f187750d 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/agent_info_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/agent_info_pb2.py
@@ -197,9 +197,7 @@
     serialized_end=379,
 )
 
-_AGENTINFOPROTO.fields_by_name[
-    "observations"
-].message_type = (
+_AGENTINFOPROTO.fields_by_name["observations"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_observation__pb2._OBSERVATIONPROTO
 )
 DESCRIPTOR.message_types_by_name["AgentInfoProto"] = _AGENTINFOPROTO
@@ -210,7 +208,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_AGENTINFOPROTO,
-        __module__="pyrcareworld.communicator_objects.agent_info_pb2"
+        __module__="pyrcareworld.communicator_objects.agent_info_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.AgentInfoProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/brain_parameters_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/brain_parameters_pb2.py
index f7b3bd3b..7ff711d9 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/brain_parameters_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/brain_parameters_pb2.py
@@ -269,7 +269,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_ACTIONSPECPROTO,
-        __module__="pyrcareworld.communicator_objects.brain_parameters_pb2"
+        __module__="pyrcareworld.communicator_objects.brain_parameters_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.ActionSpecProto)
     ),
 )
@@ -280,7 +280,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_BRAINPARAMETERSPROTO,
-        __module__="pyrcareworld.communicator_objects.brain_parameters_pb2"
+        __module__="pyrcareworld.communicator_objects.brain_parameters_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.BrainParametersProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/capabilities_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/capabilities_pb2.py
index 9388548d..0ea8cec5 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/capabilities_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/capabilities_pb2.py
@@ -179,7 +179,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_UNITYRLCAPABILITIESPROTO,
-        __module__="pyrcareworld.communicator_objects.capabilities_pb2"
+        __module__="pyrcareworld.communicator_objects.capabilities_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLCapabilitiesProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/custom_reset_parameters_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/custom_reset_parameters_pb2.py
index 4d0327b6..7c1321b3 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/custom_reset_parameters_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/custom_reset_parameters_pb2.py
@@ -44,9 +44,9 @@
     serialized_end=117,
 )
 
-DESCRIPTOR.message_types_by_name[
-    "CustomResetParametersProto"
-] = _CUSTOMRESETPARAMETERSPROTO
+DESCRIPTOR.message_types_by_name["CustomResetParametersProto"] = (
+    _CUSTOMRESETPARAMETERSPROTO
+)
 _sym_db.RegisterFileDescriptor(DESCRIPTOR)
 
 CustomResetParametersProto = _reflection.GeneratedProtocolMessageType(
@@ -54,7 +54,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_CUSTOMRESETPARAMETERSPROTO,
-        __module__="pyrcareworld.communicator_objects.custom_reset_parameters_pb2"
+        __module__="pyrcareworld.communicator_objects.custom_reset_parameters_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.CustomResetParametersProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/demonstration_meta_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/demonstration_meta_pb2.py
index 3f9401e0..6bfaf08b 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/demonstration_meta_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/demonstration_meta_pb2.py
@@ -143,7 +143,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_DEMONSTRATIONMETAPROTO,
-        __module__="pyrcareworld.communicator_objects.demonstration_meta_pb2"
+        __module__="pyrcareworld.communicator_objects.demonstration_meta_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.DemonstrationMetaProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/engine_configuration_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/engine_configuration_pb2.py
index 2eb730b4..597b476f 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/engine_configuration_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/engine_configuration_pb2.py
@@ -161,7 +161,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_ENGINECONFIGURATIONPROTO,
-        __module__="pyrcareworld.communicator_objects.engine_configuration_pb2"
+        __module__="pyrcareworld.communicator_objects.engine_configuration_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.EngineConfigurationProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/header_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/header_pb2.py
index cd5888ba..9376cce0 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/header_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/header_pb2.py
@@ -89,7 +89,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_HEADERPROTO,
-        __module__="pyrcareworld.communicator_objects.header_pb2"
+        __module__="pyrcareworld.communicator_objects.header_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.HeaderProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/observation_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/observation_pb2.py
index 661b5ec1..af9dac6b 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/observation_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/observation_pb2.py
@@ -287,22 +287,22 @@
 
 _OBSERVATIONPROTO_FLOATDATA.containing_type = _OBSERVATIONPROTO
 _OBSERVATIONPROTO.fields_by_name["compression_type"].enum_type = _COMPRESSIONTYPEPROTO
-_OBSERVATIONPROTO.fields_by_name[
-    "float_data"
-].message_type = _OBSERVATIONPROTO_FLOATDATA
+_OBSERVATIONPROTO.fields_by_name["float_data"].message_type = (
+    _OBSERVATIONPROTO_FLOATDATA
+)
 _OBSERVATIONPROTO.fields_by_name["observation_type"].enum_type = _OBSERVATIONTYPEPROTO
 _OBSERVATIONPROTO.oneofs_by_name["observation_data"].fields.append(
     _OBSERVATIONPROTO.fields_by_name["compressed_data"]
 )
-_OBSERVATIONPROTO.fields_by_name[
-    "compressed_data"
-].containing_oneof = _OBSERVATIONPROTO.oneofs_by_name["observation_data"]
+_OBSERVATIONPROTO.fields_by_name["compressed_data"].containing_oneof = (
+    _OBSERVATIONPROTO.oneofs_by_name["observation_data"]
+)
 _OBSERVATIONPROTO.oneofs_by_name["observation_data"].fields.append(
     _OBSERVATIONPROTO.fields_by_name["float_data"]
 )
-_OBSERVATIONPROTO.fields_by_name[
-    "float_data"
-].containing_oneof = _OBSERVATIONPROTO.oneofs_by_name["observation_data"]
+_OBSERVATIONPROTO.fields_by_name["float_data"].containing_oneof = (
+    _OBSERVATIONPROTO.oneofs_by_name["observation_data"]
+)
 DESCRIPTOR.message_types_by_name["ObservationProto"] = _OBSERVATIONPROTO
 DESCRIPTOR.enum_types_by_name["CompressionTypeProto"] = _COMPRESSIONTYPEPROTO
 DESCRIPTOR.enum_types_by_name["ObservationTypeProto"] = _OBSERVATIONTYPEPROTO
@@ -317,12 +317,12 @@
             (_message.Message,),
             dict(
                 DESCRIPTOR=_OBSERVATIONPROTO_FLOATDATA,
-                __module__="pyrcareworld.communicator_objects.observation_pb2"
+                __module__="pyrcareworld.communicator_objects.observation_pb2",
                 # @@protoc_insertion_point(class_scope:communicator_objects.ObservationProto.FloatData)
             ),
         ),
         DESCRIPTOR=_OBSERVATIONPROTO,
-        __module__="pyrcareworld.communicator_objects.observation_pb2"
+        __module__="pyrcareworld.communicator_objects.observation_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.ObservationProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/training_analytics_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/training_analytics_pb2.py
index 687e7bad..d7b644cc 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/training_analytics_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/training_analytics_pb2.py
@@ -444,12 +444,12 @@
     serialized_end=734,
 )
 
-DESCRIPTOR.message_types_by_name[
-    "TrainingEnvironmentInitialized"
-] = _TRAININGENVIRONMENTINITIALIZED
-DESCRIPTOR.message_types_by_name[
-    "TrainingBehaviorInitialized"
-] = _TRAININGBEHAVIORINITIALIZED
+DESCRIPTOR.message_types_by_name["TrainingEnvironmentInitialized"] = (
+    _TRAININGENVIRONMENTINITIALIZED
+)
+DESCRIPTOR.message_types_by_name["TrainingBehaviorInitialized"] = (
+    _TRAININGBEHAVIORINITIALIZED
+)
 _sym_db.RegisterFileDescriptor(DESCRIPTOR)
 
 TrainingEnvironmentInitialized = _reflection.GeneratedProtocolMessageType(
@@ -457,7 +457,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_TRAININGENVIRONMENTINITIALIZED,
-        __module__="pyrcareworld.communicator_objects.training_analytics_pb2"
+        __module__="pyrcareworld.communicator_objects.training_analytics_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.TrainingEnvironmentInitialized)
     ),
 )
@@ -468,7 +468,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_TRAININGBEHAVIORINITIALIZED,
-        __module__="pyrcareworld.communicator_objects.training_analytics_pb2"
+        __module__="pyrcareworld.communicator_objects.training_analytics_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.TrainingBehaviorInitialized)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_input_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_input_pb2.py
index 0d4b9ab6..dfd670ab 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_input_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_input_pb2.py
@@ -93,14 +93,10 @@
     serialized_end=369,
 )
 
-_UNITYINPUTPROTO.fields_by_name[
-    "rl_input"
-].message_type = (
+_UNITYINPUTPROTO.fields_by_name["rl_input"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_unity__rl__input__pb2._UNITYRLINPUTPROTO
 )
-_UNITYINPUTPROTO.fields_by_name[
-    "rl_initialization_input"
-].message_type = (
+_UNITYINPUTPROTO.fields_by_name["rl_initialization_input"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_unity__rl__initialization__input__pb2._UNITYRLINITIALIZATIONINPUTPROTO
 )
 DESCRIPTOR.message_types_by_name["UnityInputProto"] = _UNITYINPUTPROTO
@@ -111,7 +107,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_UNITYINPUTPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_input_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_input_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityInputProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_message_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_message_pb2.py
index 6672646c..7fe53250 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_message_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_message_pb2.py
@@ -115,17 +115,13 @@
     serialized_end=427,
 )
 
-_UNITYMESSAGEPROTO.fields_by_name[
-    "header"
-].message_type = pyrcareworld_dot_communicator__objects_dot_header__pb2._HEADERPROTO
-_UNITYMESSAGEPROTO.fields_by_name[
-    "unity_output"
-].message_type = (
+_UNITYMESSAGEPROTO.fields_by_name["header"].message_type = (
+    pyrcareworld_dot_communicator__objects_dot_header__pb2._HEADERPROTO
+)
+_UNITYMESSAGEPROTO.fields_by_name["unity_output"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_unity__output__pb2._UNITYOUTPUTPROTO
 )
-_UNITYMESSAGEPROTO.fields_by_name[
-    "unity_input"
-].message_type = (
+_UNITYMESSAGEPROTO.fields_by_name["unity_input"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_unity__input__pb2._UNITYINPUTPROTO
 )
 DESCRIPTOR.message_types_by_name["UnityMessageProto"] = _UNITYMESSAGEPROTO
@@ -136,7 +132,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_UNITYMESSAGEPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_message_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_message_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityMessageProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_output_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_output_pb2.py
index aa13a3aa..bda8d2da 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_output_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_output_pb2.py
@@ -93,14 +93,10 @@
     serialized_end=377,
 )
 
-_UNITYOUTPUTPROTO.fields_by_name[
-    "rl_output"
-].message_type = (
+_UNITYOUTPUTPROTO.fields_by_name["rl_output"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_unity__rl__output__pb2._UNITYRLOUTPUTPROTO
 )
-_UNITYOUTPUTPROTO.fields_by_name[
-    "rl_initialization_output"
-].message_type = (
+_UNITYOUTPUTPROTO.fields_by_name["rl_initialization_output"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_unity__rl__initialization__output__pb2._UNITYRLINITIALIZATIONOUTPUTPROTO
 )
 DESCRIPTOR.message_types_by_name["UnityOutputProto"] = _UNITYOUTPUTPROTO
@@ -111,7 +107,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_UNITYOUTPUTPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_output_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_output_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityOutputProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_input_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_input_pb2.py
index 979a43dd..d9485c3d 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_input_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_input_pb2.py
@@ -125,14 +125,12 @@
     serialized_end=323,
 )
 
-_UNITYRLINITIALIZATIONINPUTPROTO.fields_by_name[
-    "capabilities"
-].message_type = (
+_UNITYRLINITIALIZATIONINPUTPROTO.fields_by_name["capabilities"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_capabilities__pb2._UNITYRLCAPABILITIESPROTO
 )
-DESCRIPTOR.message_types_by_name[
-    "UnityRLInitializationInputProto"
-] = _UNITYRLINITIALIZATIONINPUTPROTO
+DESCRIPTOR.message_types_by_name["UnityRLInitializationInputProto"] = (
+    _UNITYRLINITIALIZATIONINPUTPROTO
+)
 _sym_db.RegisterFileDescriptor(DESCRIPTOR)
 
 UnityRLInitializationInputProto = _reflection.GeneratedProtocolMessageType(
@@ -140,7 +138,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_UNITYRLINITIALIZATIONINPUTPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_rl_initialization_input_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_rl_initialization_input_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLInitializationInputProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_output_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_output_pb2.py
index 1423e8c6..ab20c275 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_output_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_initialization_output_pb2.py
@@ -165,19 +165,15 @@
     serialized_end=477,
 )
 
-_UNITYRLINITIALIZATIONOUTPUTPROTO.fields_by_name[
-    "brain_parameters"
-].message_type = (
+_UNITYRLINITIALIZATIONOUTPUTPROTO.fields_by_name["brain_parameters"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_brain__parameters__pb2._BRAINPARAMETERSPROTO
 )
-_UNITYRLINITIALIZATIONOUTPUTPROTO.fields_by_name[
-    "capabilities"
-].message_type = (
+_UNITYRLINITIALIZATIONOUTPUTPROTO.fields_by_name["capabilities"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_capabilities__pb2._UNITYRLCAPABILITIESPROTO
 )
-DESCRIPTOR.message_types_by_name[
-    "UnityRLInitializationOutputProto"
-] = _UNITYRLINITIALIZATIONOUTPUTPROTO
+DESCRIPTOR.message_types_by_name["UnityRLInitializationOutputProto"] = (
+    _UNITYRLINITIALIZATIONOUTPUTPROTO
+)
 _sym_db.RegisterFileDescriptor(DESCRIPTOR)
 
 UnityRLInitializationOutputProto = _reflection.GeneratedProtocolMessageType(
@@ -185,7 +181,7 @@
     (_message.Message,),
     dict(
         DESCRIPTOR=_UNITYRLINITIALIZATIONOUTPUTPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_rl_initialization_output_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_rl_initialization_output_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLInitializationOutputProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_input_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_input_pb2.py
index 6026081b..33397f67 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_input_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_input_pb2.py
@@ -208,22 +208,20 @@
     serialized_end=566,
 )
 
-_UNITYRLINPUTPROTO_LISTAGENTACTIONPROTO.fields_by_name[
-    "value"
-].message_type = (
+_UNITYRLINPUTPROTO_LISTAGENTACTIONPROTO.fields_by_name["value"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_agent__action__pb2._AGENTACTIONPROTO
 )
 _UNITYRLINPUTPROTO_LISTAGENTACTIONPROTO.containing_type = _UNITYRLINPUTPROTO
-_UNITYRLINPUTPROTO_AGENTACTIONSENTRY.fields_by_name[
-    "value"
-].message_type = _UNITYRLINPUTPROTO_LISTAGENTACTIONPROTO
+_UNITYRLINPUTPROTO_AGENTACTIONSENTRY.fields_by_name["value"].message_type = (
+    _UNITYRLINPUTPROTO_LISTAGENTACTIONPROTO
+)
 _UNITYRLINPUTPROTO_AGENTACTIONSENTRY.containing_type = _UNITYRLINPUTPROTO
-_UNITYRLINPUTPROTO.fields_by_name[
-    "agent_actions"
-].message_type = _UNITYRLINPUTPROTO_AGENTACTIONSENTRY
-_UNITYRLINPUTPROTO.fields_by_name[
-    "command"
-].enum_type = pyrcareworld_dot_communicator__objects_dot_command__pb2._COMMANDPROTO
+_UNITYRLINPUTPROTO.fields_by_name["agent_actions"].message_type = (
+    _UNITYRLINPUTPROTO_AGENTACTIONSENTRY
+)
+_UNITYRLINPUTPROTO.fields_by_name["command"].enum_type = (
+    pyrcareworld_dot_communicator__objects_dot_command__pb2._COMMANDPROTO
+)
 DESCRIPTOR.message_types_by_name["UnityRLInputProto"] = _UNITYRLINPUTPROTO
 _sym_db.RegisterFileDescriptor(DESCRIPTOR)
 
@@ -236,7 +234,7 @@
             (_message.Message,),
             dict(
                 DESCRIPTOR=_UNITYRLINPUTPROTO_LISTAGENTACTIONPROTO,
-                __module__="pyrcareworld.communicator_objects.unity_rl_input_pb2"
+                __module__="pyrcareworld.communicator_objects.unity_rl_input_pb2",
                 # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLInputProto.ListAgentActionProto)
             ),
         ),
@@ -245,12 +243,12 @@
             (_message.Message,),
             dict(
                 DESCRIPTOR=_UNITYRLINPUTPROTO_AGENTACTIONSENTRY,
-                __module__="pyrcareworld.communicator_objects.unity_rl_input_pb2"
+                __module__="pyrcareworld.communicator_objects.unity_rl_input_pb2",
                 # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLInputProto.AgentActionsEntry)
             ),
         ),
         DESCRIPTOR=_UNITYRLINPUTPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_rl_input_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_rl_input_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLInputProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_output_pb2.py b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_output_pb2.py
index ed6ea73a..358bb9cd 100755
--- a/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_output_pb2.py
+++ b/pyrcareworld/pyrcareworld/communicator_objects/unity_rl_output_pb2.py
@@ -186,19 +186,17 @@
     serialized_end=447,
 )
 
-_UNITYRLOUTPUTPROTO_LISTAGENTINFOPROTO.fields_by_name[
-    "value"
-].message_type = (
+_UNITYRLOUTPUTPROTO_LISTAGENTINFOPROTO.fields_by_name["value"].message_type = (
     pyrcareworld_dot_communicator__objects_dot_agent__info__pb2._AGENTINFOPROTO
 )
 _UNITYRLOUTPUTPROTO_LISTAGENTINFOPROTO.containing_type = _UNITYRLOUTPUTPROTO
-_UNITYRLOUTPUTPROTO_AGENTINFOSENTRY.fields_by_name[
-    "value"
-].message_type = _UNITYRLOUTPUTPROTO_LISTAGENTINFOPROTO
+_UNITYRLOUTPUTPROTO_AGENTINFOSENTRY.fields_by_name["value"].message_type = (
+    _UNITYRLOUTPUTPROTO_LISTAGENTINFOPROTO
+)
 _UNITYRLOUTPUTPROTO_AGENTINFOSENTRY.containing_type = _UNITYRLOUTPUTPROTO
-_UNITYRLOUTPUTPROTO.fields_by_name[
-    "agentInfos"
-].message_type = _UNITYRLOUTPUTPROTO_AGENTINFOSENTRY
+_UNITYRLOUTPUTPROTO.fields_by_name["agentInfos"].message_type = (
+    _UNITYRLOUTPUTPROTO_AGENTINFOSENTRY
+)
 DESCRIPTOR.message_types_by_name["UnityRLOutputProto"] = _UNITYRLOUTPUTPROTO
 _sym_db.RegisterFileDescriptor(DESCRIPTOR)
 
@@ -211,7 +209,7 @@
             (_message.Message,),
             dict(
                 DESCRIPTOR=_UNITYRLOUTPUTPROTO_LISTAGENTINFOPROTO,
-                __module__="pyrcareworld.communicator_objects.unity_rl_output_pb2"
+                __module__="pyrcareworld.communicator_objects.unity_rl_output_pb2",
                 # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLOutputProto.ListAgentInfoProto)
             ),
         ),
@@ -220,12 +218,12 @@
             (_message.Message,),
             dict(
                 DESCRIPTOR=_UNITYRLOUTPUTPROTO_AGENTINFOSENTRY,
-                __module__="pyrcareworld.communicator_objects.unity_rl_output_pb2"
+                __module__="pyrcareworld.communicator_objects.unity_rl_output_pb2",
                 # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLOutputProto.AgentInfosEntry)
             ),
         ),
         DESCRIPTOR=_UNITYRLOUTPUTPROTO,
-        __module__="pyrcareworld.communicator_objects.unity_rl_output_pb2"
+        __module__="pyrcareworld.communicator_objects.unity_rl_output_pb2",
         # @@protoc_insertion_point(class_scope:communicator_objects.UnityRLOutputProto)
     ),
 )
diff --git a/pyrcareworld/pyrcareworld/control/robot_control.py b/pyrcareworld/pyrcareworld/control/robot_control.py
new file mode 100644
index 00000000..0da00f91
--- /dev/null
+++ b/pyrcareworld/pyrcareworld/control/robot_control.py
@@ -0,0 +1,42 @@
+import numpy as np
+
+
+class Control:
+    def __init__(self, robot):
+        self.robot = robot
+        self.ik_controller = robot.ik_controller
+
+    def osc_ee_control(self, currentJointPos, currentJointVel, desPos, desVel, Kp, Kd):
+        def unity_pos_to_bullet_pos(pos: list):
+            return np.array([pos[2], -pos[0], pos[1]])
+
+        currState = self.getRobotState()
+        currentEEPos = currState["positions"][-1]
+        currentEEVel = currState["velocities"][-3]
+
+        currentEEPos = unity_pos_to_bullet_pos(currentEEPos)
+        currentEEVel = unity_pos_to_bullet_pos(currentEEVel)
+        desPos = unity_pos_to_bullet_pos(desPos)
+        desVel = unity_pos_to_bullet_pos(desVel)
+
+        des_x_dd = Kp * (desPos - currentEEPos) + Kd * (desVel - currentEEVel)
+
+        # # assume end effector link num = 8
+        Jee, Jr = self.ik_controller.calc_jacobian(
+            7, [0, 0, 0], currentJointPos, currentJointVel, [0, 0, 0, 0, 0, 0, 0]
+        )
+
+        Mq = self.ik_controller.calc_Mq(currentJointPos)
+
+        # gq = self.ik_controller.calc_gq(currentPos, currentVel, [0, 0, 0, 0, 0, 0 , 0])
+        Jee_T = np.transpose(Jee)
+
+        Mxee = np.linalg.inv(np.matmul(Jee, np.matmul(np.linalg.inv(Mq), Jee_T)))
+
+        internal = np.transpose(np.matmul(Mxee, des_x_dd))
+
+        # u = np.transpose(gq)
+        u = np.matmul(Jee_T, internal)  # + np.transpose(gq)
+
+        # return u
+        return u  # jointVels, jointVelocities, diff #, com
diff --git a/pyrcareworld/pyrcareworld/envs/__init__.py b/pyrcareworld/pyrcareworld/envs/__init__.py
index f310ad40..3a14285f 100644
--- a/pyrcareworld/pyrcareworld/envs/__init__.py
+++ b/pyrcareworld/pyrcareworld/envs/__init__.py
@@ -1,1 +1,1 @@
-from .base_env import RCareWorld, RCareWorldGymWrapper
+from .rcareworld_env import RCareWorld, RCareWorldGymWrapper
diff --git a/pyrcareworld/pyrcareworld/envs/base_env.py b/pyrcareworld/pyrcareworld/envs/rcareworld_env.py
similarity index 96%
rename from pyrcareworld/pyrcareworld/envs/base_env.py
rename to pyrcareworld/pyrcareworld/envs/rcareworld_env.py
index b7404a25..86f9bf02 100644
--- a/pyrcareworld/pyrcareworld/envs/base_env.py
+++ b/pyrcareworld/pyrcareworld/envs/rcareworld_env.py
@@ -3,6 +3,7 @@
 from abc import ABC
 
 import pyrcareworld
+from pyrcareworld.agents.cloth import Cloth
 from pyrcareworld.environment import UnityEnvironment
 from pyrcareworld.side_channel.environment_parameters_channel import (
     EnvironmentParametersChannel,
@@ -275,6 +276,7 @@ def __init__(
         )
         self.robot_dict = {}
         self.object_dict = {}
+        self.cloth_dict = {}
         self.camera_dict = {}
         self.human_dict = {}
         self.lighting_dict = {}
@@ -335,7 +337,7 @@ def create_object(self, id: int, name: str, is_in_scene: bool):
         this_object = self.object_dict[id]
         return this_object
 
-    def create_human(self, id: int, name: str, is_in_scene: bool):
+    def create_human(self, id: int, name: str, is_in_scene: bool) -> Human:
         """create human
 
         Args:
@@ -350,6 +352,22 @@ def create_human(self, id: int, name: str, is_in_scene: bool):
         this_human = self.human_dict[id]
         return this_human
 
+    def create_cloth(self, id: int, name: str, is_in_scene: bool) -> Cloth:
+        """
+        Creates a cloth object in the scene.
+
+        Args:
+            id: Int. The ID of the cloth object.
+            name: Str. The name of the cloth object.
+            is_in_scene: Bool. Whether the cloth object is in the scene.
+
+        Returns:
+            The cloth Object.
+        """
+        self.cloth_dict[id] = Cloth(self, id, name, is_in_scene)
+        this_cloth = self.cloth_dict[id]
+        return this_cloth
+
     def create_skin(self, id: int, name: str, is_in_scene: bool):
         """create skin
 
diff --git a/pyrcareworld/pyrcareworld/objects/object.py b/pyrcareworld/pyrcareworld/objects/object.py
index e7a583a3..aba9c99b 100644
--- a/pyrcareworld/pyrcareworld/objects/object.py
+++ b/pyrcareworld/pyrcareworld/objects/object.py
@@ -20,7 +20,7 @@ def __init__(self, env, id: int, name: str, is_in_scene: bool = False):
         self.name = name
         self.copy_ids = []
 
-    def load(self, position=None, rotation=[0,0,0]) -> None:
+    def load(self, position=None, rotation=[0, 0, 0]) -> None:
         """
         Load object into the Scene in Unity
         The object need to be an addressbale file and not in scene.
diff --git a/pyrcareworld/pyrcareworld/rfuniverse_channel/instance_channel.py b/pyrcareworld/pyrcareworld/rfuniverse_channel/instance_channel.py
index 6d29d718..711988df 100755
--- a/pyrcareworld/pyrcareworld/rfuniverse_channel/instance_channel.py
+++ b/pyrcareworld/pyrcareworld/rfuniverse_channel/instance_channel.py
@@ -40,7 +40,7 @@ def set_action(self, action: str, attr_name=None, **kwargs) -> None:
                         self.send_message(msg)
         except AttributeError:
             print(
-                "There is no action called '%s' or this function has bug, please fix it."
+                "There is no action called '%s' or this function has a bug, please fix it."
                 % action
             )
             exit(-1)
diff --git a/pyrcareworld/pyrcareworld/rpc_utils.py b/pyrcareworld/pyrcareworld/rpc_utils.py
index 775903a6..2ea7c4ec 100755
--- a/pyrcareworld/pyrcareworld/rpc_utils.py
+++ b/pyrcareworld/pyrcareworld/rpc_utils.py
@@ -40,11 +40,11 @@ def behavior_spec_from_proto(
                 name=obs.name,
                 shape=tuple(obs.shape),
                 observation_type=ObservationType(obs.observation_type),
-                dimension_property=tuple(
-                    DimensionProperty(dim) for dim in obs.dimension_properties
-                )
-                if len(obs.dimension_properties) > 0
-                else (DimensionProperty.UNSPECIFIED,) * len(obs.shape),
+                dimension_property=(
+                    tuple(DimensionProperty(dim) for dim in obs.dimension_properties)
+                    if len(obs.dimension_properties) > 0
+                    else (DimensionProperty.UNSPECIFIED,) * len(obs.shape)
+                ),
             )
         )
 
diff --git a/pyrcareworld/pyrcareworld/sensors/__init__.py b/pyrcareworld/pyrcareworld/sensors/__init__.py
index 8a95c7b7..ff250e58 100644
--- a/pyrcareworld/pyrcareworld/sensors/__init__.py
+++ b/pyrcareworld/pyrcareworld/sensors/__init__.py
@@ -1,2 +1,2 @@
 from .camera import Camera
-from .skin import Skin
\ No newline at end of file
+from .skin import Skin
diff --git a/pyrcareworld/pyrcareworld/sensors/camera.py b/pyrcareworld/pyrcareworld/sensors/camera.py
index 8880dddb..14556f36 100644
--- a/pyrcareworld/pyrcareworld/sensors/camera.py
+++ b/pyrcareworld/pyrcareworld/sensors/camera.py
@@ -18,9 +18,9 @@ def __init__(
         id: int,
         name: str,
         intrinsic_matrix=[600, 0, 0, 0, 600, 0, 240, 240, 1],
-        width: int = 480,
-        height: int = 480,
-        fov: float = 60,
+        width: int = 256,
+        height: int = 256,
+        fov: float = None,
         is_in_scene: bool = True,
     ):
         super().__init__(env=env, id=id, name=name, is_in_scene=is_in_scene)
@@ -43,55 +43,50 @@ def getCameraInfo(self):
         info["fov"] = fov
         return info
 
-    def initializeRGBWithIntrinsic(self):
+    def getRGB(self, mode="wh"):
         """
-        Initialize the camera for RGB images with the intrinsic matrix
+        Initialize the camera for RGB images with width, height, and fov, or intrinsic matrix
+        mode = "fov" or "intrinsic" or "wh"
         """
-        self.env.instance_channel.set_action(
-            "GetRGB", id=self.id, intrinsic_matrix=self.intrinsic_matrix
-        )
-        self.is_initialized.append("rgb_intrinsic")
-
-    def initializeRGB(self):
-        """
-        Initialize the camera for RGB images with width, height, and fov
-        """
-        if self.fov is not None:
+        assert mode in [
+            "fov",
+            "intrinsic",
+            "wh",
+        ], "mode should be 'fov' or 'intrinsic' or 'wh'"
+        if mode == "fov":
             self.env.instance_channel.set_action(
                 "GetRGB", id=self.id, width=self.width, height=self.height, fov=self.fov
             )
             self.is_initialized.append("rgb_fov")
-        else:
+        elif mode == "wh":
             self.env.instance_channel.set_action(
                 "GetRGB", id=self.id, width=self.width, height=self.height
             )
             self.is_initialized.append("rgb_wh")
-
-    def getRGB(self):
-        """
-        Returns the RGB image as an image array
-        """
+        elif mode == "intrinsic":
+            self.env.instance_channel.set_action(
+                "GetRGB", id=self.id, intrinsic_matrix=self.intrinsic_matrix
+            )
+            self.is_initialized.append("rgb_intrinsic")
+        self.env._step()
         image_byte = self.env.instance_channel.data[self.id]["rgb"]
         image_rgb = np.frombuffer(image_byte, dtype=np.uint8)
-        image_rgb = cv2.imdecode(image_rgb, cv2.IMREAD_COLOR)
-        return image_rgb
-
-    def initializeDepthEXRWithIntrinsic(self):
-        """
-        Initialize the camera for depth images with the intrinsic matrix
-        """
-        self.env.instance_channel.set_action(
-            "GetDepthEXR",
-            id=self.id,
-            intrinsic_matrix=self.intrinsic_matrix,
-        )
-        self.is_initialized.append("depth_intrinsic")
+        reshaped_image_rgb = image_rgb.reshape(self.height, self.width, 3)
+        # upside down and convert bgr to rgb
+        reshaped_image_rgb = np.flipud(reshaped_image_rgb)[:, :, ::-1]
+        return reshaped_image_rgb
 
-    def initializeDepthEXR(self):
+    def getDepthEXR(self, mode="wh"):
         """
-        Initialize the camera for depth images with width, height, and fov
+        Initialize the camera for depth images in EXR format with width, height, and fov, or intrinsic matrix
+        mode = "fov" or "intrinsic" or "wh"
         """
-        if self.fov is not None:
+        assert mode in [
+            "fov",
+            "intrinsic",
+            "wh",
+        ], "mode should be 'fov' or 'intrinsic' or 'wh'"
+        if mode == "fov":
             self.env.instance_channel.set_action(
                 "GetDepthEXR",
                 id=self.id,
@@ -100,33 +95,33 @@ def initializeDepthEXR(self):
                 fov=self.fov,
             )
             self.is_initialized.append("depth_fov")
-        else:
+        elif mode == "wh":
             self.env.instance_channel.set_action(
                 "GetDepthEXR", id=self.id, width=self.width, height=self.height
             )
             self.is_initialized.append("depth_wh")
-
-    def getDepthEXR(self):
-        """
-        Returns the depth image as a numpy array
-        """
+        elif mode == "intrinsic":
+            self.env.instance_channel.set_action(
+                "GetDepthEXR", id=self.id, intrinsic_matrix=self.intrinsic_matrix
+            )
+            self.is_initialized.append("depth_intrinsic")
+        self.env._step()
         depth = self.env.instance_channel.data[self.id]["depth_exr"]
-        return depth
+        depth = np.frombuffer(depth, dtype=np.float32)
+        reshaped_image_depth = depth.reshape(self.height, self.width)
+        return reshaped_image_depth
 
-    def initializeNormalWithIntrinsic(self):
+    def getNormal(self, mode="wh"):
         """
-        Initialize the camera for surface normals with the intrinsic matrix
+        Initialize the camera for surface normals with width, height, and fov, or intrinsic matrix
+        mode = "fov" or "intrinsic" or "wh"
         """
-        self.env.instance_channel.set_action(
-            "GetNormal", id=self.id, intrinsic_matrix=self.intrinsic_matrix
-        )
-        self.is_initialized.append("normal_intrinsic")
-
-    def initializeNormal(self):
-        """
-        Initialize the camera for surface normals with width, height, and fov
-        """
-        if self.fov is not None:
+        assert mode in [
+            "fov",
+            "intrinsic",
+            "wh",
+        ], "mode should be 'fov' or 'intrinsic' or 'wh'"
+        if mode == "fov":
             self.env.instance_channel.set_action(
                 "GetNormal",
                 id=self.id,
@@ -135,88 +130,79 @@ def initializeNormal(self):
                 fov=self.fov,
             )
             self.is_initialized.append("normal_fov")
-        else:
+        elif mode == "wh":
             self.env.instance_channel.set_action(
                 "GetNormal", id=self.id, width=self.width, height=self.height
             )
             self.is_initialized.append("normal_wh")
-
-    def getNormal(self):
-        """
-        Returns the surface normals as a numpy array
-        """
+        elif mode == "intrinsic":
+            self.env.instance_channel.set_action(
+                "GetNormal", id=self.id, intrinsic_matrix=self.intrinsic_matrix
+            )
+            self.is_initialized.append("normal_intrinsic")
+        self.env._step()
         normal = self.env.instance_channel.data[self.id]["normal"]
-        return normal
-
-    def initializeInstanceMaskWithIntrinsic(self):
-        """
-        Initialize the camera for instance masks with the intrinsic matrix
-        """
-        self.env.instance_channel.set_action(
-            "GetID", id=self.id, intrinsic_matrix=self.intrinsic_matrix
-        )
-        self.is_initialized.append("instance_intrinsic")
+        normal = np.frombuffer(normal, dtype=np.uint8)
+        reshaped_image_normal = normal.reshape(self.height, self.width, 3)
+        return reshaped_image_normal
 
-    def initializeInstanceMask(self, w, h, fov=None):
+    def getInstanceMask(self, mode="wh"):
         """
-        Initialize the camera for instance masks with width, height, and fov
+        Initialize the camera for instance masks with width, height, and fov, or intrinsic matrix
+        mode = "fov" or "intrinsic" or "wh"
         """
-        if fov is not None:
+        assert mode in [
+            "fov",
+            "intrinsic",
+            "wh",
+        ], "mode should be 'fov' or 'intrinsic' or 'wh'"
+        if mode == "fov":
             self.env.instance_channel.set_action(
                 "GetID", id=self.id, width=self.width, height=self.height, fov=self.fov
             )
             self.is_initialized.append("instance_fov")
-        else:
+        elif mode == "wh":
             self.env.instance_channel.set_action(
                 "GetID", id=self.id, width=self.width, height=self.height
             )
             self.is_initialized.append("instance_wh")
-
-    def getInstanceMask(self):
-        """
-        Returns the instance mask as a numpy array
-        """
-        image_id = self.env.instance_channel.data[self.id]["id_map"]
-        image_id = np.frombuffer(image_id, dtype=np.uint8)
-        image_id = cv2.imdecode(image_id, cv2.IMREAD_COLOR)
-        return image_id
-
-    def initializeAmodalMaskWithIntrinsic(self):
-        """
-        Initialize the camera for amodal masks with the intrinsic matrix
-        """
-        self.env.instance_channel.set_action(
-            "GetAmodalMask", id=self.id, intrinsic_matrix=self.intrinsic_matrix
-        )
-        self.is_initialized.append("amodal_intrinsic")
-
-    def initializeAmodalMask(self, w, h, fov=None):
-        """
-        Initialize the camera for amodal masks with width, height, and fov
-        """
-        if fov is not None:
+        elif mode == "intrinsic":
             self.env.instance_channel.set_action(
-                "GetAmodalMask",
-                id=self.id,
-                width=self.width,
-                height=self.height,
-                fov=self.fov,
+                "GetID", id=self.id, intrinsic_matrix=self.intrinsic_matrix
             )
-            self.is_initialized.append("amodal_fov")
-        else:
-            self.env.instance_channel.set_action(
-                "GetAmodalMask", id=self.id, width=self.width, height=self.height
-            )
-            self.is_initialized.append("amodal_wh")
-
-    def getAmodalMask(self):
-        """
-        Returns the amodal mask as a numpy array
-        """
-        image_id = self.env.instance_channel.data[self.id]["amodal_mask"]
+            self.is_initialized.append("instance_intrinsic")
+        self.env._step()
+        image_id = self.env.instance_channel.data[self.id]["id_map"]
         image_id = np.frombuffer(image_id, dtype=np.uint8)
-        image_id = cv2.imdecode(image_id, cv2.IMREAD_COLOR)
-        return image_id
+        reshaped_image_id = image_id.reshape(self.height, self.width, 3)
+        return reshaped_image_id
+
+    # def getAmodalInstanceMask(self, mode = "wh"):
+    #     """
+    #     Initialize the camera for amodal instance masks with width, height, and fov, or intrinsic matrix
+    #     mode = "fov" or "intrinsic" or "wh"
+    #     """
+    #     assert mode in ["fov", "intrinsic", "wh"], "mode should be 'fov' or 'intrinsic' or 'wh'"
+    #     if mode == "fov":
+    #         self.env.instance_channel.set_action(
+    #             "GetAmodalMask", id=self.id, width=self.width, height=self.height, fov=self.fov
+    #         )
+    #         self.is_initialized.append("amodal_instance_fov")
+    #     elif mode == "wh":
+    #         self.env.instance_channel.set_action(
+    #             "GetAmodalMask", id=self.id, width=self.width, height=self.height
+    #         )
+    #         self.is_initialized.append("amodal_instance_wh")
+    #     elif mode == "intrinsic":
+    #         self.env.instance_channel.set_action(
+    #             "GetAmodalMask", id=self.id, intrinsic_matrix=self.intrinsic_matrix
+    #         )
+    #         self.is_initialized.append("amodal_instance_intrinsic")
+    #     self.env._step()
+    #     image_id = self.env.instance_channel.data[self.id]["amodal_mask"]
+    #     image_id = np.frombuffer(image_id, dtype=np.uint8)
+    #     reshaped_image_id = image_id.reshape(self.height, self.width, 3)
+    #     return reshaped_image_id
 
     def initializeActiveDepthWithIntrinsic(self, ir_intrinsic_matrix):
         """
diff --git a/pyrcareworld/pyrcareworld/side_channel/side_channel_manager.py b/pyrcareworld/pyrcareworld/side_channel/side_channel_manager.py
index b5d00366..4a4c5a4d 100755
--- a/pyrcareworld/pyrcareworld/side_channel/side_channel_manager.py
+++ b/pyrcareworld/pyrcareworld/side_channel/side_channel_manager.py
@@ -21,7 +21,7 @@ def process_side_channel_message(self, data: bytes) -> None:
             try:
                 channel_id = uuid.UUID(bytes_le=bytes(data[offset : offset + 16]))
                 offset += 16
-                message_len, = struct.unpack_from("<i", data, offset)
+                (message_len,) = struct.unpack_from("<i", data, offset)
                 offset = offset + 4
                 message_data = data[offset : offset + message_len]
                 offset = offset + message_len
@@ -63,7 +63,7 @@ def generate_side_channel_messages(self) -> bytearray:
 
     @staticmethod
     def _get_side_channels_dict(
-        side_channels: Optional[List[SideChannel]]
+        side_channels: Optional[List[SideChannel]],
     ) -> Dict[uuid.UUID, SideChannel]:
         """
         Converts a list of side channels into a dictionary of channel_id to SideChannel
@@ -72,7 +72,7 @@ def _get_side_channels_dict(
         side_channels_dict: Dict[uuid.UUID, SideChannel] = {}
 
         # Print side channel id in use
-        print('Side channel id in use:')
+        print("Side channel id in use:")
         for _sc in side_channels:
             print(_sc.channel_id)
 
diff --git a/pyrcareworld/pyrcareworld/utils/pose_convert.py b/pyrcareworld/pyrcareworld/utils/pose_convert.py
index bec5f411..07e6bb10 100644
--- a/pyrcareworld/pyrcareworld/utils/pose_convert.py
+++ b/pyrcareworld/pyrcareworld/utils/pose_convert.py
@@ -2,6 +2,7 @@
 import time
 import numpy as np
 
+
 class PoseUtils:
     def __init__(self):
         pass
@@ -24,18 +25,28 @@ def bullet_scale_to_unity_scale(self, scale: list):
     def unity_scale_to_bullet_scale(self, scale: list):
         return [scale[2], scale[0], scale[1]]
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     unity_pos = [0.52, 2.28, 0.75]
     unity_qua = [0, 0.258819103, 0, 0.965925813]
     unity_euler = [0, 30, 0]
-    unity_sca = [1., 2., 1.5]
+    unity_sca = [1.0, 2.0, 1.5]
 
     id_simulator = p.connect(p.GUI)
     p.setTimeStep(0.02)
     pose = PoseUtils()
-    box = p.createVisualShape(shapeType=p.GEOM_BOX, halfExtents=pose.unity_scale_to_bullet_scale((np.array(unity_sca) / 2).tolist()))
-    p.createMultiBody(baseVisualShapeIndex=box, basePosition=pose.unity_pos_to_bullet_pos(unity_pos), baseOrientation=pose.unity_qua_to_bullet_qua(unity_qua))
+    box = p.createVisualShape(
+        shapeType=p.GEOM_BOX,
+        halfExtents=pose.unity_scale_to_bullet_scale(
+            (np.array(unity_sca) / 2).tolist()
+        ),
+    )
+    p.createMultiBody(
+        baseVisualShapeIndex=box,
+        basePosition=pose.unity_pos_to_bullet_pos(unity_pos),
+        baseOrientation=pose.unity_qua_to_bullet_qua(unity_qua),
+    )
 
     while 1:
         p.stepSimulation(id_simulator)
-        time.sleep(0.02)
\ No newline at end of file
+        time.sleep(0.02)
diff --git a/pyrcareworld/pyrcareworld/utils/pose_converter.py b/pyrcareworld/pyrcareworld/utils/pose_converter.py
index c8208314..cd5196f9 100644
--- a/pyrcareworld/pyrcareworld/utils/pose_converter.py
+++ b/pyrcareworld/pyrcareworld/utils/pose_converter.py
@@ -1,7 +1,7 @@
 import numpy as np
 
 
-class CoordinateSystemConverter():
+class CoordinateSystemConverter:
     """
 
     Coordinate System Converter class.
@@ -21,9 +21,18 @@ class CoordinateSystemConverter():
                         "-left"/"-right"/"-up"/"-down"/"-forward"/"-back"/
                         "-l"/"-r"/"-u"/"-d"/"-f"/"-b"]
     """
-    def __init__(self, cs1_direction=["right", "up", "forward"], cs2_direction=["right", "up", "forward"]):
-        self.cs1_dir, self.cs1_axis, self.cs1_sign, self.cs1_system = self._preprocessing(cs1_direction)
-        self.cs2_dir, self.cs2_axis, self.cs2_sign, self.cs2_system = self._preprocessing(cs2_direction)
+
+    def __init__(
+        self,
+        cs1_direction=["right", "up", "forward"],
+        cs2_direction=["right", "up", "forward"],
+    ):
+        self.cs1_dir, self.cs1_axis, self.cs1_sign, self.cs1_system = (
+            self._preprocessing(cs1_direction)
+        )
+        self.cs2_dir, self.cs2_axis, self.cs2_sign, self.cs2_system = (
+            self._preprocessing(cs2_direction)
+        )
         self.cs1_to_cs2_map = [
             self.cs1_axis[self.cs2_dir[0]],
             self.cs1_axis[self.cs2_dir[1]],
@@ -47,10 +56,12 @@ def __init__(self, cs1_direction=["right", "up", "forward"], cs2_direction=["rig
             self.cs1_sign[self.cs1_dir[2]] * self.cs2_sign[self.cs1_dir[2]],
         ]
 
-        self.cs1_left_or_right = self.cs1_sign['right'] * self.cs1_sign['up'] * self.cs1_sign['forward']
-        self.cs2_left_or_right = self.cs2_sign['right'] * self.cs2_sign['up'] * self.cs2_sign['forward']
-
-
+        self.cs1_left_or_right = (
+            self.cs1_sign["right"] * self.cs1_sign["up"] * self.cs1_sign["forward"]
+        )
+        self.cs2_left_or_right = (
+            self.cs2_sign["right"] * self.cs2_sign["up"] * self.cs2_sign["forward"]
+        )
 
     def _preprocessing(self, direction):
         dir = ["", "", ""]
@@ -65,31 +76,31 @@ def _preprocessing(self, direction):
                 sign["right"] = 1
                 direction_in_left[i] = [1, 0, 0]
 
-            elif direction[i] in ["up",  "u",  "-d",  "-down"]:
+            elif direction[i] in ["up", "u", "-d", "-down"]:
                 dir[i] = "up"
                 axis["up"] = i
                 sign["up"] = 1
                 direction_in_left[i] = [0, 1, 0]
 
-            elif direction[i] in ["forward",  "f",  "-b",  "-back"]:
+            elif direction[i] in ["forward", "f", "-b", "-back"]:
                 dir[i] = "forward"
                 axis["forward"] = i
                 sign["forward"] = 1
                 direction_in_left[i] = [0, 0, 1]
 
-            elif direction[i] in ["-right",  "-r",  "l",  "left"]:
+            elif direction[i] in ["-right", "-r", "l", "left"]:
                 dir[i] = "right"
                 axis["right"] = i
                 sign["right"] = -1
                 direction_in_left[i] = [-1, 0, 0]
 
-            elif direction[i] in ["-up",  "-u",  "d",  "down"]:
+            elif direction[i] in ["-up", "-u", "d", "down"]:
                 dir[i] = "up"
                 axis["up"] = i
                 sign["up"] = -1
                 direction_in_left[i] = [0, -1, 0]
 
-            elif direction[i] in ["-forward",  "-f",  "b",  "back"]:
+            elif direction[i] in ["-forward", "-f", "b", "back"]:
                 dir[i] = "forward"
                 axis["forward"] = i
                 sign["forward"] = -1
@@ -98,8 +109,6 @@ def _preprocessing(self, direction):
             else:
                 raise f"{direction[i]} is Unrecognized axis"
 
-
-
         z = self._cross(direction_in_left[0], direction_in_left[1])
         if z == direction_in_left[2]:
             system = 1
@@ -109,15 +118,14 @@ def _preprocessing(self, direction):
 
         return dir, axis, sign, system
 
-
-
     def _cross(self, lhs: list, rhs: list) -> list:
-        return [lhs[1] * rhs[2] - lhs[2] * rhs[1], lhs[2] * rhs[0] - lhs[0] * rhs[2], lhs[0] * rhs[1] - lhs[1] * rhs[0]]
-
-
+        return [
+            lhs[1] * rhs[2] - lhs[2] * rhs[1],
+            lhs[2] * rhs[0] - lhs[0] * rhs[2],
+            lhs[0] * rhs[1] - lhs[1] * rhs[0],
+        ]
 
     def cs1_pos_to_cs2_pos(self, pos: list) -> list:
-
         """
         Convert position form Coordinate System 1 to Coordinate System 2.
         Args:
@@ -136,10 +144,7 @@ def cs1_pos_to_cs2_pos(self, pos: list) -> list:
 
         return [x, y, z]
 
-
-
     def cs2_pos_to_cs1_pos(self, pos: list) -> list:
-
         """
         Convert position form Coordinate System 2 to Coordinate System 1.
 
@@ -161,10 +166,7 @@ def cs2_pos_to_cs1_pos(self, pos: list) -> list:
 
         return [x, y, z]
 
-
-
     def cs1_quat_to_cs2_quat(self, quat: list) -> list:
-
         """
         Convert quaternion form Coordinate System 1 to Coordinate System 2.
 
@@ -185,10 +187,7 @@ def cs1_quat_to_cs2_quat(self, quat: list) -> list:
 
         return [x, y, z, w]
 
-
-
     def cs2_quat_to_cs1_quat(self, quat: list) -> list:
-
         """
         Convert quaternion form Coordinate System 2 to Coordinate System 1.
         Args:
@@ -208,10 +207,7 @@ def cs2_quat_to_cs1_quat(self, quat: list) -> list:
 
         return [x, y, z, w]
 
-
-
     def cs1_scale_to_cs2_scale(self, scale: list) -> list:
-
         """
         Convert scale form Coordinate System 1 to Coordinate System 2.
 
@@ -229,10 +225,7 @@ def cs1_scale_to_cs2_scale(self, scale: list) -> list:
 
         return [x, y, z]
 
-
-
     def cs2_scale_to_cs1_scale(self, scale: list) -> list:
-
         """
         Convert scale form Coordinate System 2 to Coordinate System 1.
 
@@ -250,10 +243,7 @@ def cs2_scale_to_cs1_scale(self, scale: list) -> list:
 
         return [x, y, z]
 
-
-
     def cs1_matrix_to_cs2_matrix(self, matrix) -> np.ndarray:
-
         """
         Convert rotation matrix form Coordinate System 1 to Coordinate System 2.
         Args:
@@ -269,10 +259,7 @@ def cs1_matrix_to_cs2_matrix(self, matrix) -> np.ndarray:
         quaternion = self.cs1_quat_to_cs2_quat(quaternion)
         return self.quat_to_matrix(quaternion)
 
-
-
     def cs2_matrix_to_cs1_matrix(self, matrix) -> np.ndarray:
-
         """
         Convert rotation matrix form Coordinate System 2 to Coordinate System 1.
 
@@ -289,32 +276,41 @@ def cs2_matrix_to_cs1_matrix(self, matrix) -> np.ndarray:
 
         return self.quat_to_matrix(quaternion)
 
-
-
-    def quat_to_matrix(self, quat=[0,0,0,1]) -> np.ndarray:
+    def quat_to_matrix(self, quat=[0, 0, 0, 1]) -> np.ndarray:
         """
         Convert quaternion to rotation matrix.
         Args:
             quat: List of length 4, representing quaternion in [x, y, z, w] order.
-            
+
         Return:
             np.ndarray: shape[3,3], rotation matrix.
         """
-        return np.array([
-            [1 - 2 * (quat[1] ** 2 + quat[2] ** 2), 2 * (quat[0] * quat[1] - quat[2] * quat[3]), 2 * (quat[0] * quat[2] + quat[1] * quat[3])],
-            [2 * (quat[0] * quat[1] + quat[2] * quat[3]), 1 - 2 * (quat[0] ** 2 + quat[2] ** 2), 2 * (quat[1] * quat[2] - quat[0] * quat[3])],
-            [2 * (quat[0] * quat[2] - quat[1] * quat[3]), 2 * (quat[1] * quat[2] + quat[0] * quat[3]), 1 - 2 * (quat[0] ** 2 + quat[1] ** 2)]
-
-        ])
-
-
+        return np.array(
+            [
+                [
+                    1 - 2 * (quat[1] ** 2 + quat[2] ** 2),
+                    2 * (quat[0] * quat[1] - quat[2] * quat[3]),
+                    2 * (quat[0] * quat[2] + quat[1] * quat[3]),
+                ],
+                [
+                    2 * (quat[0] * quat[1] + quat[2] * quat[3]),
+                    1 - 2 * (quat[0] ** 2 + quat[2] ** 2),
+                    2 * (quat[1] * quat[2] - quat[0] * quat[3]),
+                ],
+                [
+                    2 * (quat[0] * quat[2] - quat[1] * quat[3]),
+                    2 * (quat[1] * quat[2] + quat[0] * quat[3]),
+                    1 - 2 * (quat[0] ** 2 + quat[1] ** 2),
+                ],
+            ]
+        )
 
     def matrix_to_quat(self, matrix) -> list:
         """
         Convert rotation matrix to quaternion.
         Args:
             matrix: list or np.ndarray shape[3,3], rotation matrix.
-            
+
         Return:
             list: List of length 4, representing quaternion in [x, y, z, w] order.
         """
@@ -349,4 +345,4 @@ def matrix_to_quat(self, matrix) -> list:
             y = (m[1, 2] + m[2, 1]) / s
             z = 0.25 * s
 
-        return [x, y, z, w]
\ No newline at end of file
+        return [x, y, z, w]
diff --git a/pyrcareworld/pyrcareworld/utils/pose_utils.py b/pyrcareworld/pyrcareworld/utils/pose_utils.py
index 9dddb8d8..f0cb6528 100644
--- a/pyrcareworld/pyrcareworld/utils/pose_utils.py
+++ b/pyrcareworld/pyrcareworld/utils/pose_utils.py
@@ -2,10 +2,12 @@
 import time
 import numpy as np
 
+
 class PoseUtils:
     """
     A simple pose converter between Unity and Bullet
     """
+
     def __init__(self):
         pass
 
@@ -27,18 +29,28 @@ def bullet_scale_to_unity_scale(self, scale: list):
     def unity_scale_to_bullet_scale(self, scale: list):
         return [scale[2], scale[0], scale[1]]
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     unity_pos = [0.52, 2.28, 0.75]
     unity_qua = [0, 0.258819103, 0, 0.965925813]
     unity_euler = [0, 30, 0]
-    unity_sca = [1., 2., 1.5]
+    unity_sca = [1.0, 2.0, 1.5]
 
     id_simulator = p.connect(p.GUI)
     p.setTimeStep(0.02)
     pose = PoseUtils()
-    box = p.createVisualShape(shapeType=p.GEOM_BOX, halfExtents=pose.unity_scale_to_bullet_scale((np.array(unity_sca) / 2).tolist()))
-    p.createMultiBody(baseVisualShapeIndex=box, basePosition=pose.unity_pos_to_bullet_pos(unity_pos), baseOrientation=pose.unity_qua_to_bullet_qua(unity_qua))
+    box = p.createVisualShape(
+        shapeType=p.GEOM_BOX,
+        halfExtents=pose.unity_scale_to_bullet_scale(
+            (np.array(unity_sca) / 2).tolist()
+        ),
+    )
+    p.createMultiBody(
+        baseVisualShapeIndex=box,
+        basePosition=pose.unity_pos_to_bullet_pos(unity_pos),
+        baseOrientation=pose.unity_qua_to_bullet_qua(unity_qua),
+    )
 
     while 1:
         p.stepSimulation(id_simulator)
-        time.sleep(0.02)
\ No newline at end of file
+        time.sleep(0.02)
diff --git a/pyrcareworld/pyrcareworld/utils/visualization.py b/pyrcareworld/pyrcareworld/utils/visualization.py
index b09b0604..ab3c31e4 100644
--- a/pyrcareworld/pyrcareworld/utils/visualization.py
+++ b/pyrcareworld/pyrcareworld/utils/visualization.py
@@ -1,6 +1,6 @@
 from abc import ABC
+
+
 class RCareViz(ABC):
     def __init__(self):
         pass
-
-
diff --git a/pyrcareworld/requirements.txt b/pyrcareworld/requirements.txt
index 7a5fc95f..02a696f5 100755
--- a/pyrcareworld/requirements.txt
+++ b/pyrcareworld/requirements.txt
@@ -9,4 +9,4 @@ pandas==1.5.3
 Pillow==9.4.0
 protobuf==3.19.0
 pyaml==21.10.1
-pybullet==3.2.5
\ No newline at end of file
+pybullet==3.2.5
diff --git a/pyrcareworld/run.py b/pyrcareworld/run.py
index 685105e6..15824381 100644
--- a/pyrcareworld/run.py
+++ b/pyrcareworld/run.py
@@ -11,7 +11,6 @@ def create_environment(
     user_input: bool = False,
     record: bool = False,
 ):
-
     """
     This function creates an instance of the specified environment class
     and then runs its 'demo_env' method.


Test patch:
diff --git a/pyrcareworld/Test/test_active_depth.py b/pyrcareworld/Test/test_active_depth.py
index 3fd3de42..acc8de1f 100755
--- a/pyrcareworld/Test/test_active_depth.py
+++ b/pyrcareworld/Test/test_active_depth.py
@@ -8,7 +8,7 @@
     raise
 import pyrcareworld.utils.utility as utility
 import pyrcareworld.utils.depth_processor as dp
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 env = RCareWorldBaseEnv(scene_file="ActiveDepth.json")
 
diff --git a/pyrcareworld/Test/test_articulation_ik.py b/pyrcareworld/Test/test_articulation_ik.py
index 08d37b28..033cb494 100755
--- a/pyrcareworld/Test/test_articulation_ik.py
+++ b/pyrcareworld/Test/test_articulation_ik.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 import pyrcareworld.utils.utility as utility
 
 env = RCareWorldBaseEnv(
diff --git a/pyrcareworld/Test/test_camera_image.py b/pyrcareworld/Test/test_camera_image.py
index 540e2e15..4845e606 100755
--- a/pyrcareworld/Test/test_camera_image.py
+++ b/pyrcareworld/Test/test_camera_image.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 import cv2
 import numpy as np
 import os.path as osp
@@ -34,11 +34,11 @@
 # # file.write(env.instance_channel.data[123456]['depth'])
 # # file.close()
 env.instance_channel.set_action(
-        "GetDepthEXR",
-        id=123456,
-        width=512,
-        height=1024,
-    )
+    "GetDepthEXR",
+    id=123456,
+    width=512,
+    height=1024,
+)
 env.instance_channel.set_action("GetRGB", id=123456, width=512, height=1024)
 env._step()
 
@@ -49,7 +49,7 @@
 num_frames = num_animation_seconds * 30 + 10
 
 for i in range(num_frames):
-    print(f'==> frame {i}')
+    print(f"==> frame {i}")
     env.instance_channel.set_action(
         "GetDepthEXR",
         id=123456,
@@ -61,17 +61,17 @@
     position = info["position"]
     quaternion = info["quaternion"]
     env._step()
-    
-    
+
     depth_bytes = env.instance_channel.data[123456]["depth_exr"]
     # # temp_file_path = osp.join(tempfile.gettempdir(), "temp_img.exr")
     # # with open(temp_file_path, "wb") as f:
     # #     f.write(depth_bytes)
     # # depth_exr = cv2.imread(temp_file_path, cv2.IMREAD_UNCHANGED)
     # # os.remove(temp_file_path)
-    
-    depth_exr = cv2.imdecode(np.frombuffer(depth_bytes, dtype=np.uint8), cv2.IMREAD_UNCHANGED)
 
+    depth_exr = cv2.imdecode(
+        np.frombuffer(depth_bytes, dtype=np.uint8), cv2.IMREAD_UNCHANGED
+    )
 
     # mask = np.asarray(o3d.io.read_image(osp.join(mask_path, video_name, view_name, base_name + '.png')))[:, :, 0]
     # foregound_mask = mask == 11
@@ -79,7 +79,6 @@
     # print(np.unique(depth_png))
     # cv2.imwrite(f"data/depth_{i}.jpg", depth_png)
 
-
     image_byte = env.instance_channel.data[123456]["rgb"]
     image_rgb = np.frombuffer(image_byte, dtype=np.uint8)
     image_rgb = cv2.imdecode(image_rgb, cv2.IMREAD_COLOR)
@@ -94,6 +93,6 @@
     cv2.imwrite(f"data/depth_{i}.png", depth_png)
 
     with open(f"data/pose_{i}.txt", "w") as f:
-        f.write(f"{position[0]} {position[1]} {position[2]} {quaternion[0]} {quaternion[1]} {quaternion[2]} {quaternion[3]}")
-
-
+        f.write(
+            f"{position[0]} {position[1]} {position[2]} {quaternion[0]} {quaternion[1]} {quaternion[2]} {quaternion[3]}"
+        )
diff --git a/pyrcareworld/Test/test_cloth_anchor.py b/pyrcareworld/Test/test_cloth_anchor.py
new file mode 100644
index 00000000..f6e3e209
--- /dev/null
+++ b/pyrcareworld/Test/test_cloth_anchor.py
@@ -0,0 +1,22 @@
+from pyrcareworld.envs.rcareworld_env import RCareWorld
+
+# Script to test cloth anchoring.
+if __name__ == "__main__":
+    env = RCareWorld()
+
+    # Create a new cloth representation.
+    cloth = env.create_cloth(id=100, name="Cloth", is_in_scene=True)
+
+    # Anchors the cloth's particle group "corner" to the object with id 200.
+    cloth.addParticleAnchor("corner", 200)
+
+    # Step 100 times.
+    for _ in range(100):
+        env.step()
+
+    # Unanchor. Observe the cloth fall.
+    cloth.removeParticleAnchor("corner")
+
+    # Continue stepping.
+    for _ in range(10000):
+        env.step()
diff --git a/pyrcareworld/Test/test_custom_message.py b/pyrcareworld/Test/test_custom_message.py
index aa9ca536..e656454d 100755
--- a/pyrcareworld/Test/test_custom_message.py
+++ b/pyrcareworld/Test/test_custom_message.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 from pyrcareworld.side_channel.side_channel import (
     IncomingMessage,
     OutgoingMessage,
diff --git a/pyrcareworld/Test/test_grasp_pose.py b/pyrcareworld/Test/test_grasp_pose.py
index 3ad0f839..a1bb1f71 100755
--- a/pyrcareworld/Test/test_grasp_pose.py
+++ b/pyrcareworld/Test/test_grasp_pose.py
@@ -5,7 +5,7 @@
 except ImportError:
     print("This feature requires pandas, please install with `pip install pandas`")
     raise
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 mesh_path = "../Mesh/drink1/drink1.obj"
 pose_path = "../Mesh/drink1/grasps_rfu.csv"
diff --git a/pyrcareworld/Test/test_grasp_sim.py b/pyrcareworld/Test/test_grasp_sim.py
index 9d0d2fec..eeaaf9fc 100755
--- a/pyrcareworld/Test/test_grasp_sim.py
+++ b/pyrcareworld/Test/test_grasp_sim.py
@@ -12,7 +12,7 @@
     print("This feature requires open3d, please install with `pip install open3d`")
     raise
 
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 
 def get_grasp_pose(file: str, points_count, scale: float = 1):
diff --git a/pyrcareworld/Test/test_humanbody_ik.py b/pyrcareworld/Test/test_humanbody_ik.py
index f4983dbe..69a986ad 100755
--- a/pyrcareworld/Test/test_humanbody_ik.py
+++ b/pyrcareworld/Test/test_humanbody_ik.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 env = RCareWorldBaseEnv()
 env._step()
diff --git a/pyrcareworld/Test/test_joint_limits.py b/pyrcareworld/Test/test_joint_limits.py
new file mode 100644
index 00000000..c173f877
--- /dev/null
+++ b/pyrcareworld/Test/test_joint_limits.py
@@ -0,0 +1,38 @@
+import random
+from pyrcareworld.envs.rcareworld_env import RCareWorld
+
+# Script to test joint limits. Paired with `Set Joint Limits` scene.
+if __name__ == "__main__":
+    env = RCareWorld()
+
+    # Corresponds to the human in the scene.
+    person = env.create_human(id=100, name="Person", is_in_scene=True)
+
+    for _ in range(10000):
+        # Randomly selects a set of 3 upper and lower joint limits every frame.
+        lower1, upper1 = random.uniform(-50, 0), random.uniform(1, 50)
+        lower2, upper2 = random.uniform(-50, 0), random.uniform(1, 50)
+        lower3, upper3 = random.uniform(-50, 0), random.uniform(1, 50)
+
+        # Sets the joint limits of LeftUpperArm, LeftHand, and RightHand.
+        person.setJointLimits("LeftUpperArm", lower1, upper1)
+        person.setJointLimits("LeftHand", lower2, upper2, "X")
+        person.setJointLimits("RightHand", lower3, upper3, "Z")
+
+        # You should observe the joint limits being set in the Unity scene.
+        # (Causes the human mesh to spasm due to the random joint limits.)
+
+        print(
+            "Setting joint limits to:\n\tLeftUpperArm: ",
+            lower1,
+            upper1,
+            "\n\tLeftHand: ",
+            lower2,
+            upper2,
+            "\n\tRightHand: ",
+            lower3,
+            upper3,
+            "\n\n",
+        )
+
+        env.step()
diff --git a/pyrcareworld/Test/test_load_mesh.py b/pyrcareworld/Test/test_load_mesh.py
index 162c632b..2ab41845 100755
--- a/pyrcareworld/Test/test_load_mesh.py
+++ b/pyrcareworld/Test/test_load_mesh.py
@@ -1,6 +1,6 @@
 import random
 import os
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 env = RCareWorldBaseEnv()
 env._step()
diff --git a/pyrcareworld/Test/test_load_urdf.py b/pyrcareworld/Test/test_load_urdf.py
index 6ac25e0c..9dc15e39 100755
--- a/pyrcareworld/Test/test_load_urdf.py
+++ b/pyrcareworld/Test/test_load_urdf.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 import pyrcareworld.utils.utility as utility
 import os
 
diff --git a/pyrcareworld/Test/test_pick_and_place.py b/pyrcareworld/Test/test_pick_and_place.py
index 36e42808..6656c89b 100644
--- a/pyrcareworld/Test/test_pick_and_place.py
+++ b/pyrcareworld/Test/test_pick_and_place.py
@@ -1,5 +1,5 @@
 import random
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 env = RCareWorldBaseEnv()
 
diff --git a/pyrcareworld/Test/test_pick_and_place_flexiv.py b/pyrcareworld/Test/test_pick_and_place_flexiv.py
index 54d7d4d8..135c0def 100644
--- a/pyrcareworld/Test/test_pick_and_place_flexiv.py
+++ b/pyrcareworld/Test/test_pick_and_place_flexiv.py
@@ -1,5 +1,5 @@
 import random
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 
 env = RCareWorldBaseEnv(assets=["flexivArm_ag95"])
 
diff --git a/pyrcareworld/Test/test_pick_and_place_gym.py b/pyrcareworld/Test/test_pick_and_place_gym.py
index 681d4084..80444c7b 100644
--- a/pyrcareworld/Test/test_pick_and_place_gym.py
+++ b/pyrcareworld/Test/test_pick_and_place_gym.py
@@ -2,9 +2,9 @@
 
 env = FrankaRoboticsEnv(
     # executable_file='@editor',
-    scene_file='FrankaRobotics.json',
+    scene_file="FrankaRobotics.json",
     max_episode_length=500,
-    reward_type='sparse',
+    reward_type="sparse",
     seed=None,
     tolerance=0.05,
     load_object=True,
@@ -14,7 +14,7 @@
     target_y_range=0.6,
     object_xz_range=0.15,
     asset_bundle_file=None,
-    assets=['Rigidbody_Box']
+    assets=["Rigidbody_Box"],
 )
 env.reset()
 
diff --git a/pyrcareworld/Test/test_point_cloud.py b/pyrcareworld/Test/test_point_cloud.py
index 0d972a4f..e0d6bebc 100755
--- a/pyrcareworld/Test/test_point_cloud.py
+++ b/pyrcareworld/Test/test_point_cloud.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 import pyrcareworld.utils.depth_processor as dp
 import numpy as np
 
diff --git a/pyrcareworld/Test/test_point_cloud_with_intrinsic_matrix.py b/pyrcareworld/Test/test_point_cloud_with_intrinsic_matrix.py
index 01b76924..c99520cd 100755
--- a/pyrcareworld/Test/test_point_cloud_with_intrinsic_matrix.py
+++ b/pyrcareworld/Test/test_point_cloud_with_intrinsic_matrix.py
@@ -1,4 +1,4 @@
-from pyrcareworld.envs.base_env import RCareWorldBaseEnv
+from pyrcareworld.envs.rcareworld_env import RCareWorldBaseEnv
 import pyrcareworld.utils.depth_processor as dp
 import numpy as np
 
diff --git a/pyrcareworld/URDF/tobor/test_pybullet.py b/pyrcareworld/URDF/tobor/test_pybullet.py
index a19dce6f..58c8230b 100755
--- a/pyrcareworld/URDF/tobor/test_pybullet.py
+++ b/pyrcareworld/URDF/tobor/test_pybullet.py
@@ -3,11 +3,11 @@
 import time
 
 physicsClient = p.connect(p.GUI)
-p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
-p.setGravity(0,0,-10)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())  # optionally
+p.setGravity(0, 0, -10)
 planeId = p.loadURDF("plane.urdf")
-startPos = [0,0,1]
-startOrientation = p.getQuaternionFromEuler([0,0,0])
+startPos = [0, 0, 1]
+startOrientation = p.getQuaternionFromEuler([0, 0, 0])
 
 # robotId = p.loadURDF("tobor.urdf", startPos, startOrientation)
 robotId = p.loadURDF("tobor_arm.urdf", startPos, startOrientation)
@@ -15,13 +15,13 @@
 joint_num = p.getNumJoints(robotId)
 print("joint_number:", joint_num)
 
-#for i in range(joint_num):
+# for i in range(joint_num):
 #    print("joint ", i, " info:", p.getJointInfo(robotId, i))
 
 camTargetPos = [0, 0, 0]
 cameraUp = [0, 0, 1]
 cameraPos = [1, 1, 1]
-yaw=90
+yaw = 90
 pitch = -10.0
 roll = 0
 upAxisIndex = 2
@@ -34,20 +34,22 @@
 fov = 60
 
 
-
-for i in range (10000):
+for i in range(10000):
     p.stepSimulation()
-    viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
-                                                            roll, upAxisIndex)
+    viewMatrix = p.computeViewMatrixFromYawPitchRoll(
+        camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex
+    )
     aspect = pixelWidth / pixelHeight
     projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
-    img_arr = p.getCameraImage(pixelWidth,
-                                        pixelHeight,
-                                        viewMatrix,
-                                        projectionMatrix,
-                                        shadow=1,
-                                        lightDirection=[1, 1, 1],
-                                        renderer=p.ER_BULLET_HARDWARE_OPENGL)
-    time.sleep(1./240.)
-
-p.disconnect()
\ No newline at end of file
+    img_arr = p.getCameraImage(
+        pixelWidth,
+        pixelHeight,
+        viewMatrix,
+        projectionMatrix,
+        shadow=1,
+        lightDirection=[1, 1, 1],
+        renderer=p.ER_BULLET_HARDWARE_OPENGL,
+    )
+    time.sleep(1.0 / 240.0)
+
+p.disconnect()
diff --git a/pyrcareworld/test_install.sh b/pyrcareworld/test_install.sh
new file mode 100644
index 00000000..d5eb88ad
--- /dev/null
+++ b/pyrcareworld/test_install.sh
@@ -0,0 +1,1 @@
+python Examples/example_base_object.py