add reset function in the python wrapper

uwgraphics · Jun 16, 2023 · c15d69e · c15d69e
1 parent 8afda65
commit c15d69e
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/wrappers/python_wrapper.py b/wrappers/python_wrapper.py
@@ -19,6 +19,7 @@ class RelaxedIKS(ctypes.Structure):
 lib.solve_position.restype = Opt
 lib.solve_velocity.argtypes = [ctypes.POINTER(RelaxedIKS), ctypes.POINTER(ctypes.c_double), ctypes.c_int, ctypes.POINTER(ctypes.c_double), ctypes.c_int, ctypes.POINTER(ctypes.c_double), ctypes.c_int]
 lib.solve_velocity.restype = Opt
+lib.reset.argtypes = [ctypes.POINTER(RelaxedIKS)]
 
 class RelaxedIKRust:
     def __init__(self, setting_file_path = None):
@@ -72,6 +73,12 @@ def solve_velocity(self, linear_velocities, angular_velocities, tolerances):
             tole_arr[i] = tolerances[i]
         xopt = lib.solve_velocity(self.obj, linear_arr, len(linear_arr), angular_arr, len(angular_arr), tole_arr, len(tole_arr))
         return xopt.data[:xopt.length]
+
+    def reset(self, joint_state):
+        js_arr = (ctypes.c_double * len(joint_state))()
+        for i in range(len(joint_state)):
+            js_arr[i] = joint_state[i]
+        lib.reset(self.obj, js_arr, len(js_arr))
 
 if __name__ == '__main__':
     pass