diff --git a/RAPID/SERVER_LEFT.mod b/RAPID/SERVER_LEFT.mod
index e3c1a02..6db38de 100644
--- a/RAPID/SERVER_LEFT.mod
+++ b/RAPID/SERVER_LEFT.mod
@@ -4,8 +4,6 @@ MODULE SERVER_L
     !GLOBAL VARIABLES
     !/////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-    VAR num x;
-    
     !//Robot configuration
     PERS tooldata currentTool:=[TRUE,[[0,0,156],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];
     PERS wobjdata currentWobj:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
@@ -511,6 +509,10 @@ MODULE SERVER_L
                     ok:=SERVER_OK;
 
                     ! holdForce range = 0 - 20 N, targetPos = 0 - 25 mm, posAllowance = tolerance of gripper closure value
+                ELSEIF nParams=1 THEN
+                    Hand_GripInward\targetPos:=params{1};
+                    ok:=SERVER_OK;
+
                 ELSEIF nParams=2 THEN
                     Hand_GripInward\holdForce:=params{1}\targetPos:=params{2};
                     ok:=SERVER_OK;
@@ -531,7 +533,7 @@ MODULE SERVER_L
                     ok:=SERVER_OK;
 
                 ELSEIF nParams=1 THEN
-                    Hand_GripOutward\targetPos:=params{2};
+                    Hand_GripOutward_GripOut\targetPos:=params{1};
                     ok:=SERVER_OK;
 
                     ! holdForce range = 0 - 20 N, targetPos = 0 - 25 mm, posAllowance = tolerance of gripper closure value
@@ -653,7 +655,7 @@ MODULE SERVER_L
                 ENDIF
                 !---------------------------------------------------------------------------------------------------------------
             CASE 32:
-                !Get Buffer Size)
+                !Get Buffer Size
                 IF nParams=0 THEN
                     addString:=NumToStr(BUFFER_POS,2);
                     ok:=SERVER_OK;
@@ -691,7 +693,7 @@ MODULE SERVER_L
                 IF nParams=7 THEN
                     circPoint:=[[params{1},params{2},params{3}],
                                 [params{4},params{5},params{6},params{7}],
-                                [0,0,0,0],
+                                L_CONF,
                                 externalAxis];
                     ok:=SERVER_OK;
                 ELSE
@@ -703,7 +705,7 @@ MODULE SERVER_L
                 IF nParams=7 THEN
                     cartesianTarget:=[[params{1},params{2},params{3}],
                                         [params{4},params{5},params{6},params{7}],
-                                        [0,0,0,0],
+                                        L_CONF,
                                         externalAxis];
                     MoveC circPoint,cartesianTarget,currentSpeed,currentZone,currentTool\WObj:=currentWobj;
                     ok:=SERVER_OK;
@@ -716,7 +718,7 @@ MODULE SERVER_L
                 IF nParams=7 THEN
                     cartesianTarget := [[params{1},params{2},params{3}],
                                        [params{4},params{5},params{6},params{7}],
-                                       [-1,-1,0,11],
+                                       L_CONF,
                                        externalAxis];
                     IF isPoseReachable(cartesianTarget, currentTool, currentWobj) THEN
                         addString := "1";
@@ -859,4 +861,4 @@ MODULE SERVER_L
         ENDTEST
     ENDPROC
     
-ENDMODULE
\ No newline at end of file
+ENDMODULE
diff --git a/RAPID/SERVER_RIGHT.mod b/RAPID/SERVER_RIGHT.mod
index c03535d..097a2e3 100644
--- a/RAPID/SERVER_RIGHT.mod
+++ b/RAPID/SERVER_RIGHT.mod
@@ -515,6 +515,10 @@ MODULE SERVER_R
                     ok:=SERVER_OK;
 
                     ! holdForce range = 0 - 20 N, targetPos = 0 - 25 mm, posAllowance = tolerance of gripper closure value
+                ELSEIF nParams=1 THEN
+                    Hand_GripInward\targetPos:=params{1};
+                    ok:=SERVER_OK;
+
                 ELSEIF nParams=2 THEN
                     Hand_GripInward\holdForce:=params{1}\targetPos:=params{2};
                     ok:=SERVER_OK;
@@ -535,7 +539,7 @@ MODULE SERVER_R
                     ok:=SERVER_OK;
 
                 ELSEIF nParams=1 THEN
-                    Hand_GripOutward\targetPos:=params{2};
+                    Hand_GripOutward\targetPos:=params{1};
                     ok:=SERVER_OK;
 
                     ! holdForce range = 0 - 20 N, targetPos = 0 - 25 mm, posAllowance = tolerance of gripper closure value
@@ -638,12 +642,11 @@ MODULE SERVER_R
                             bufferTargets{BUFFER_POS}:=cartesianTarget;
                             bufferSpeeds{BUFFER_POS}:=currentSpeed;
                         ENDIF
+                        ok:=SERVER_OK;
                     ELSE
                         ok := SERVER_BAD_MSG;
                         addString := "Unreachable Pose";
                     ENDIF
-                            
-                    ok:=SERVER_OK;
                 ELSE
                     ok:=SERVER_BAD_MSG;
                 ENDIF
@@ -658,7 +661,7 @@ MODULE SERVER_R
                 ENDIF
                 !---------------------------------------------------------------------------------------------------------------
             CASE 32:
-                !Get Buffer Size)
+                !Get Buffer Size
                 IF nParams=0 THEN
                     addString:=NumToStr(BUFFER_POS,2);
                     ok:=SERVER_OK;
@@ -726,6 +729,7 @@ MODULE SERVER_R
                     IF isPoseReachable(cartesianTarget, currentTool, currentWobj) THEN
                         addString := "1";
                     ELSE
+                        TPWrite "not reachable";
                         addString := "0";
                     ENDIF
                 ELSE
@@ -861,4 +865,4 @@ ERROR
         ENDTEST
     ENDPROC
     
-ENDMODULE
\ No newline at end of file
+ENDMODULE
diff --git a/yumipy/yumi_constants.py b/yumipy/yumi_constants.py
index 2407499..63b4236 100644
--- a/yumipy/yumi_constants.py
+++ b/yumipy/yumi_constants.py
@@ -49,40 +49,40 @@ class YuMiConstants:
     
     CMD_CODES = {
         'ping': 0,
-        'goto_pose_linear':1,
-        'goto_joints': 2,
-        'get_pose': 3,
-        'get_joints':4,
-        'goto_pose':5,
+        'goto_pose_linear':1,  # MoveL
+        'goto_joints': 2,  # MoveAbsJ
+        'get_pose': 3,  # CRobT
+        'get_joints':4,  # CJointT
+        'goto_pose':5,  # MoveJ
         'set_tool':6,
         'set_speed':8,
         'set_zone':9,
-        
-        'goto_pose_sync':11,
-        'goto_joints_sync':12,
-        'goto_pose_delta':13,
-        
-        'close_gripper': 20,
-        'open_gripper': 21,
-        'calibrate_gripper': 22,
-        'set_gripper_max_speed': 23,
-        'set_gripper_force': 24,
-        'move_gripper': 25,
-        'get_gripper_width': 26,
-        
-        'set_circ_point':35,
-        'move_by_circ_point':36,
+
+        'goto_pose_sync':11,  # MoveL with sync
+        'goto_joints_sync':12,  # MoveAbsJ with sync
+        'goto_pose_delta':13,  # MoveL to "current pose + delta"
+
+        'close_gripper': 20,  # g_GripIn
+        'open_gripper': 21,  # g_GripOut
+        'calibrate_gripper': 22,  # g_Init
+        'set_gripper_max_speed': 23, # g_SetMaxSpd
+        'set_gripper_force': 24,  # g_SetForce
+        'move_gripper': 25,  # g_MoveTo
+        'get_gripper_width': 26,  # g_GetPos
+
         'buffer_add': 30,
         'buffer_clear': 31,
         'buffer_size': 32,
-        'buffer_move': 33,
-        
-        'is_pose_reachable': 40,
-        'is_joints_reachable': 41,
+        'buffer_move': 33,  # A series of MoveL
+        'set_circ_point':35,
+        'move_by_circ_point':36,  # MoveC
+
+        'is_pose_reachable': 40, # isPoseReachable
+        'is_joints_reachable': 41,  # isJointsReachable
 
         'close_connection': 99,
-        
-        'reset_home': 100,
+
+        'reset_home': 100,  # MoveAbsJ to Home
     }
     
     RES_CODES = {