carm 0.0.20250815__tar.gz → 0.0.20250911__tar.gz

{carm-0.0.20250815 → carm-0.0.20250911}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: carm
-Version: 0.0.20250815
+Version: 0.0.20250911
 Summary: Python interface for cvte arm.
 Author-email: Yong Zhao <zhaoyong11933@cvte.com>
 Classifier: Programming Language :: Python :: 3

{carm-0.0.20250815 → carm-0.0.20250911}/carm/carm.py

@@ -10,20 +10,20 @@ class Carm:
         self.last_msg = None
         self.ws = websocket.WebSocketApp(
             addr,  # 测试用的公开WebSocket服务
-            on_open   = lambda ws: self.on_open(ws),
-            on_close  = lambda ws, code, close_msg: self.on_close(ws, code, close_msg),
-            on_message= lambda ws, msg: self.on_message(ws, msg),
+            on_open   = lambda ws: self.__on_open(ws),
+            on_close  = lambda ws, code, close_msg: self.__on_close(ws, code, close_msg),
+            on_message= lambda ws, msg: self.__on_message(ws, msg),
         )
 
         self.ops = {
-            "webSendRobotState": lambda msg: self.cbk_status(msg),
-            "taskFinished": lambda msg: self.cbk_taskfinish(msg),
+            "webSendRobotState": lambda msg: self.__cbk_status(msg),
+            "taskFinished": lambda msg: self.__cbk_taskfinish(msg),
             "onCarmError": lambda msg: print("Error:", msg)
         }
         self.res_pool  = {}
         self.task_pool = {}
 
-        self.reader = threading.Thread(target=self.recv_loop).start()
+        self.reader = threading.Thread(target=self.__recv_loop, daemon=True).start()
         self.open_ready = threading.Event()
         self.open_ready.wait()
         self.limit = self.get_limits()["params"]
@@ -32,12 +32,22 @@ class Carm:
 
     @property
     def version(self):
+        """
+        Get the version of the arm.
+        Returns:
+            dict: The version of the arm.
+        """
         return self.request({"command":"getArmIntrinsicProperties",
                              "arm_index":0,
                              "type":"version"})
     
 
     def get_limits(self):
+        """
+        获取主要配置参数，包括关节限位、关节最大速度、加速度、加加速度等。
+        Returns:
+            dict: The limits of the arm.
+        """
         return self.request({"command":"getJointParams",
                              "arm_index":0,
                              "type":"version"})
@@ -58,16 +68,42 @@ class Carm:
         return self.set_control_mode(1)
 
     def set_servo_enable(self, enable=True):
+        """
+        Set the servo enable of the arm.
+        Args:
+            enable (bool): The servo enable to set.
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"setServoEnable",
                              "arm_index":0,
                              "enable":enable})
     
     def set_control_mode(self, mode=1):
+        """
+        Set the control mode of the arm.
+        Args:
+            mode (int): The control mode to set.
+            * 0-IDLE空闲模式
+            * 1-点位控制模式
+            * 2-MIT控制模式
+            * 3-关节拖动模式
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"setControlMode",
                              "arm_index":0,
                              "mode":mode})
     
     def set_end_effector(self, pos, tau):
+        """
+        Set the end effector of the arm.
+        Args:
+            pos (float): The position of the end effector. 0-0.08m
+            tau (float): The torque of the end effector. 0-9.0N.m
+        Returns:
+            dict: The response from the arm.
+        """
         pos = self.__clip(pos, 0, 0.08)
         tau = self.__clip(tau, 0, 9)
 
@@ -77,6 +113,13 @@ class Carm:
                              "tau": tau})
     
     def get_tool_coordinate(self, tool):
+        """
+        获取指定工具的坐标系（工具末端相对法兰的位姿关系）.
+        Args:
+            tool (int): The tool index.
+        Returns:
+            dict: The tool coordinate of the arm.
+        """
         return self.request({"command":"getCoordinate",
                              "arm_index":0,
                              "type": "tool",
@@ -84,12 +127,31 @@ class Carm:
 
     
     def set_collision_config(self, flag = True, level = 10):
+        """
+        设置碰撞检测配置.
+        Args:
+            flag (bool): 是否开启碰撞检测.
+            level (int): 碰撞检测等级. 灵敏度等级 0-2，0最高，2最低
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"setCollisionConfig",
                              "arm_index":0,
                              "flag": flag,
                              "level": level})
     
     def stop(self, type=0):
+        """
+        停止carm.
+        Args:
+            type (int): 停止类型.
+            * 0-暂停
+            * 1-停止
+            * 2-禁用
+            * 3-紧急停止
+        Returns:
+            dict: The response from the arm.
+        """
         stop_id = ["SIG_ARM_PAUSE", "SIG_ARM_STOP", 
                    "SIG_ARM_DISABLE","SIG_EMERGENCY_STOP"]
         return self.request({"command":"stopSignals",
@@ -98,57 +160,119 @@ class Carm:
                              "step_cnt":5})
     
     def stop_task(self, at_once=False):
+        """
+        停止当前任务.
+        Args:
+            at_once (bool): 是否立即停止任务.
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"stopSignals",
                              "arm_index":0,
                              "stop_id": "SIG_TASK_STOP",
                              "stop_at_once": at_once})
     
     def recover(self):
+        """
+        恢复carm.
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"stopSignals",
                              "arm_index":0,
                              "stop_id": "SIG_ARM_RECOVER",
                              "step_cnt": 5})
     
     def clean_carm_error(self):
+        """
+        清除carm错误.
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"setControllerErrorReset",
                              "arm_index":0})
     
-    def set_speed_level(self, level, response_level):
+    def set_speed_level(self, level = 5.0, response_level = 20):
+        """
+        Set the speed level of the arm.
+        Args:
+            level (float): The speed level to set. range from 0 to 10. 0 is the slowest, 10 is the fastest.
+            response_level (int): The response level to set, after how many cicles to set. From 10 to 100.
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"setSpeedLevel",
                              "arm_index":0,
                              "level":level,
                              "response_level":response_level})
-    
+        
     def set_debug(self, flag):
+        """
+        设置调试模式.
+        Args:
+            flag (bool): 是否开启调试模式.
+        Returns:
+            dict: The response from the arm.
+        """
         return self.request({"command":"setDebugMode",
                              "arm_index":0,
                              "trigger":flag})
 
     def get_urdf(self):
+        """
+        获取urdf模型.
+        Returns:
+            dict: The urdf model of the arm.
+        """
         return self.request({"command":"getArmIntrinsicProperties",
                              "arm_index":0,
                              "type":"urdf"})   
     @property
     def joint_pos(self):
+        """
+        获取当前关节位置.
+        Returns:
+            list: The joint position of the arm.
+        """
         return self.state["arm"][0]["reality"]["pose"]
     
     @property
     def joint_vel(self):
+        """
+        获取当前关节速度.
+        Returns:
+            list: The joint velocity of the arm.
+        """
         return self.state["arm"][0]["reality"]["vel"]
     
     @property
     def joint_tau(self):
+        """
+        获取当前关节力矩.
+        Returns:
+            list: The joint torque of the arm.
+        """
         return self.state["arm"][0]["reality"]["torque"]
     
     @property
     def cart_pose(self):
+        """
+        获取当前笛卡尔位置.
+        Returns:
+            list: The cartesian position of the arm.
+        """
         return self.state["arm"][0]["pose"]
     
     @property
     def gripper_state(self):
+        """
+        获取当前夹爪状态.
+        Returns:
+            dict: The gripper state of the arm.
+        """
         return self.state["arm"][0]["gripper"]
     
-    def clip_joints(self, joints):
+    def __clip_joints(self, joints):
         lower = self.limit['limit_lower']
         upper = self.limit["limit_upper"]
         for i,v in enumerate(joints):
@@ -156,7 +280,15 @@ class Carm:
         return joints
     
     def track_joint(self, pos, end_effector = -1):
-        pos = self.clip_joints(pos)
+        """
+        关节轨迹跟踪.
+        Args:
+            pos (list): The joint position to track.
+            end_effector (float, optional): The end effector width. -1 means not set.
+        Returns:
+            dict: The response from the arm.
+        """
+        pos = self.__clip_joints(pos)
         req = {"command":"trajectoryTrackingTasks",
                "task_id":"TASK_TRACKING",
                "arm_index":0,
@@ -169,6 +301,14 @@ class Carm:
         return self.request(req)
     
     def track_pose(self, pos, end_effector = -1):
+        """
+        笛卡尔轨迹跟踪.
+        Args:
+            pos (list): The cartesian position to track.
+            end_effector (float, optional): The end effector width. -1 means not set.
+        Returns:
+            dict: The response from the arm.
+        """
         req = {"command":"trajectoryTrackingTasks",
                "task_id":"TASK_TRACKING",
                "arm_index":0,
@@ -181,7 +321,18 @@ class Carm:
         return self.request(req)
 
     def move_joint(self, pos, tm=-1, sync=True, user=0, tool=0):
-        pos = self.clip_joints(pos)
+        """
+        关节到点运动
+        Args:
+            pos (list): The joint position to move.
+            tm (float, optional): The time to move. -1 means not set.
+            sync (bool, optional): Whether to wait for the movement to finish.
+            user (int, optional): The user index.
+            tool (int, optional): The tool index.
+        Returns:
+            dict: The response from the arm.
+        """
+        pos = self.__clip_joints(pos)
         
         res =  self.request({"command":"webRecieveTasks",
                              "task_id":"TASK_MOVJ",
@@ -191,11 +342,22 @@ class Carm:
                              "data":{"user":user,"tool":tool, "target_pos": pos, "speed":100}}) 
         
         if sync and res["recv"]=="Task_Recieve":
-            self.wait_task(res["task_key"])
+            self.__wait_task(res["task_key"])
 
         return res
 
     def move_pose(self, pos, tm=-1, sync=True, user=0, tool=0):
+        """
+        笛卡尔到点运动
+        Args:
+            pos (list): The cartesian position to move.
+            tm (float, optional): The time to move. -1 means not set.
+            sync (bool, optional): Whether to wait for the movement to finish.
+            user (int, optional): The user index.
+            tool (int, optional): The tool index.
+        Returns:
+            dict: The response from the arm.
+        """
         res =  self.request({"command":"webRecieveTasks",
                              "task_id":"TASK_MOVJ",
                              "task_level":"Task_General",
@@ -204,11 +366,42 @@ class Carm:
                              "data":{"user":user,"tool":tool, "target_pos": pos, "speed":100}}) 
         
         if sync and res["recv"]=="Task_Recieve":
-            self.wait_task(res["task_key"])
+            self.__wait_task(res["task_key"])
+
+        return res
+    
+
+    def move_line(self, pos, speed=50, sync=True, user=0, tool=0):
+        """
+        笛卡尔直线运动
+        Args:
+            pos (list): The cartesian position to move.
+            speed (float, optional): The speed to move. 0 - 100%.
+            sync (bool, optional): Whether to wait for the movement to finish.
+            user (int, optional): The user index.
+            tool (int, optional): The tool index.
+        Returns:
+            dict: The response from the arm.
+        """
+        ret = self.invK(pos, self.joint_pos, user, tool) # check IK first
+        if ret["recv"] != "Task_Recieve":
+            print("Inverse kinematics failed:", ret)
+            return ret
+
+        pos = ret["data"]["joint1"]
+        res =  self.request({"command":"webRecieveTasks",
+                             "task_id":"TASK_MOVL",
+                             "task_level":"Task_General",
+                             "arm_index":0,
+                             "point_type":{"space":0},
+                             "data":{"user":user,"tool":tool, "point": pos, "speed":speed}}) 
+        
+        if sync and res["recv"]=="Task_Recieve":
+            self.__wait_task(res["task_key"])
 
         return res
         
-    def wait_task(self, task_key):
+    def __wait_task(self, task_key):
         event = threading.Event()
         self.task_pool[task_key] = {"event":event}
         event.wait()
@@ -233,13 +426,28 @@ class Carm:
         pass # TODO
 
     def invK(self, cart_pose, ref_joints, user=0, tool=0):
+        """
+        逆解
+        Args:
+            cart_pose (list): The cartesian position to move.
+            ref_joints (list): The reference joint position.
+            user (int, optional): The user index.
+            tool (int, optional): The tool index.
+        Returns:
+            dict: The response from the arm.
+        """
         if not type(cart_pose[0]) is list:
             cart_pose = [cart_pose, ]
+            ref_joints = [ref_joints, ]
         assert(len(cart_pose) == len(ref_joints))
+        data = {"user":user,"tool":tool,"point_cnt":len(cart_pose)}
+        for i in range(len(ref_joints)):
+            data[f"point{i+1}"] = cart_pose[i]
+            data[f"refer{i+1}"] = ref_joints[i]
         return self.request({"command":"getKinematics",
                              "task_id":"inverse",
                              "arm_index":0,
-                             "data":{"user":user,"tool":tool,"point_cnt":len(cart_pose)}})
+                             "data":data})
 
     def request(self, req):
         event = threading.Event()
@@ -247,43 +455,43 @@ class Carm:
         req["task_key"] = task_key
         self.res_pool[task_key] = {"req":req,"event":event}
 
-        self.send(req)
+        self.__send(req)
 
         event.wait()
         return self.res_pool.pop(task_key)["res"]
         
-    def send(self, msg):
+    def __send(self, msg):
         self.ws.send(json.dumps(msg))
         
-    def cbk_status(self, message):
+    def __cbk_status(self, message):
         self.state = message
 
-    def cbk_taskfinish(self, message):
+    def __cbk_taskfinish(self, message):
         task = message["task_key"]
         self.task_pool[task]["event"].set()
         
-    def on_open(self, ws):
+    def __on_open(self, ws):
         self.open_ready.set()
         print("Connected successfully.")
 
-    def on_close(self, ws, code, close_msg):
+    def __on_close(self, ws, code, close_msg):
         print("Disconnected, please check your --addr",code, close_msg)
 
-    def on_message(self, ws, message):
+    def __on_message(self, ws, message):
         msg = json.loads(message)
         self.last_msg = msg
         cmd = msg["command"]
-        op = self.ops.get(cmd, lambda msg: self.response_op(msg))
+        op = self.ops.get(cmd, lambda msg: self.__response_op(msg))
         op(msg)
 
-    def response_op(self, res):
+    def __response_op(self, res):
         id   = res.get("task_key","")
         data = self.res_pool[id]
         data["res"] = res
         data["event"].set() # notify request thread
 
 
-    def recv_loop(self):
+    def __recv_loop(self):
         print("Recv loop started.")
         self.ws.run_forever()
 

carm-0.0.20250815/scripts/carm → carm-0.0.20250911/carm/carm_dds.py

@@ -18,8 +18,11 @@ class ArmDriver:
         else:
             self.arm.set_control_mode(1)
 
+        self.arm.set_speed_level(args.speed)
+
         self.pub_joint = messenger.advertise(args.joint_topic, 0)
         self.pub_end   = messenger.advertise(args.end_topic, 0)
+        self.pub_joint_ik = messenger.advertise(args.joint_cmd_ik_topic, 0)
         
         self.sub_joint = messenger.subscribe(args.joint_cmd_topic, 0, lambda msg:self.joint_callback(msg))
         self.sub_end   = messenger.subscribe(args.end_cmd_topic, 0, lambda msg:self.end_callback(msg))
@@ -28,6 +31,16 @@ class ArmDriver:
         position = np.frombuffer(msg["position"],dtype=np.float64).tolist()
         self.arm.track_pose(position)
 
+        if self.args.enable_ik:
+            ret = self.arm.invK(position, ref_joints=self.arm.joint_pos)
+            if ret["recv"] != "Task_Recieve":
+                print("Inverse kinematics failed:", ret)
+                return 
+
+            pos = ret["data"]["joint1"]
+            self.pub_joint_ik.publish({"header": msg["header"], "position": pos})
+
+
     def joint_callback(self, msg):        
         position = np.frombuffer(msg["position"],dtype=np.float64).tolist()        
         self.arm.track_joint(position)
@@ -48,7 +61,7 @@ class ArmDriver:
             time.sleep(0.005)
 
 def send_cmd(args):
-    arm       = carm.Carm(args.addr,mit=args.mit)
+    arm       = carm.Carm(args.addr)
 
     cmd = args.cmd
 
@@ -66,16 +79,14 @@ def driver_main(args):
     transfer = ros2.Transfer({"node": "carm_driver" + args.device, 
                               "publishers":[[args.joint_topic, "sensor_msgs/msg/JointState",10],
                                             [args.end_topic, "sensor_msgs/msg/JointState",10],
+                                            [args.joint_cmd_ik_topic, "sensor_msgs/msg/JointState",10],
                                             ],
                               "subscriptions":[[args.joint_cmd_topic, "sensor_msgs/msg/JointState",10],
                                                [args.end_cmd_topic, "sensor_msgs/msg/JointState",10]]})
     
     driver.loop()
     
-    
-    
-# 测试代码
-if __name__ == "__main__":
+def main():
     parser = argparse.ArgumentParser()
     parser.add_argument("--addr", type=str, default="ws://localhost:8090", help="Device address, including ip and port")
     parser.add_argument("--cmd", type=str, default="", help="Send command instead of start driver, support enable,disable,remote")
@@ -85,7 +96,9 @@ if __name__ == "__main__":
     parser.add_argument("--joint_topic", type=str, default="", help="the joints status topic")
     parser.add_argument("--end_topic", type=str, default="", help="the joints status topic")
     parser.add_argument("--joint_cmd_topic", type=str, default="", help="the joints cmd topic")
+    parser.add_argument("--joint_cmd_ik_topic", type=str, default="", help="the joints cmd ik topic, this means both end_cmd_topic will be transformed with ik")
     parser.add_argument("--end_cmd_topic", type=str, default="", help="the end cmd topic")
+    parser.add_argument("--speed", type=float, default=5.0, help="the speed level")
     
     args = parser.parse_args()
     
@@ -97,10 +110,20 @@ if __name__ == "__main__":
         args.joint_cmd_topic = "/"+args.device + "/joints_cmd"
     if args.end_cmd_topic == "":
         args.end_cmd_topic = "/"+args.device + "/end_cmd"
+    if args.joint_cmd_ik_topic == "":
+        args.joint_cmd_ik_topic = "/"+args.device + "/joints_cmd_ik"
+        args.__dict__["enable_ik"] = False
+    else:
+        args.__dict__["enable_ik"] = True
     
     if args.cmd == "":
         driver_main(args)
     else:
         send_cmd(args)
+   
+    
+# 测试代码
+if __name__ == "__main__":
+    main()
     
     

carm-0.0.20250815/scripts/carm_ros2 → carm-0.0.20250911/carm/carm_ros2.py

@@ -19,7 +19,7 @@ class ArmDriver(Node):
         
         self.arm       = carm.Carm(args.addr)
         if args.mit:
-            self.arm.set_control_mode(3)
+            self.arm.set_control_mode(2)
         else:
             self.arm.set_control_mode(1)
 
@@ -60,9 +60,8 @@ class ArmDriver(Node):
             self.pub_end.publish(end_msg)
 
             time.sleep(0.005)
-   
-# 测试代码
-if __name__ == "__main__":
+
+def main():
     parser = argparse.ArgumentParser()
     parser.add_argument("--addr", type=str, default="ws://localhost:8090", help="Device address, including ip and port")
     parser.add_argument("--device", type=str, default="carm", help="device name, used as topic prefix")
@@ -90,5 +89,8 @@ if __name__ == "__main__":
     rclpy.spin(node)
     node.destroy_node()
     rclpy.shutdown()
-    
+
+# 测试代码
+if __name__ == "__main__":
+    main()
     

{carm-0.0.20250815 → carm-0.0.20250911}/carm.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: carm
-Version: 0.0.20250815
+Version: 0.0.20250911
 Summary: Python interface for cvte arm.
 Author-email: Yong Zhao <zhaoyong11933@cvte.com>
 Classifier: Programming Language :: Python :: 3

{carm-0.0.20250815 → carm-0.0.20250911}/carm.egg-info/SOURCES.txt

@@ -2,11 +2,11 @@ README.md
 pyproject.toml
 carm/__init__.py
 carm/carm.py
+carm/carm_dds.py
+carm/carm_ros2.py
 carm.egg-info/PKG-INFO
 carm.egg-info/SOURCES.txt
 carm.egg-info/dependency_links.txt
 carm.egg-info/entry_points.txt
 carm.egg-info/requires.txt
-carm.egg-info/top_level.txt
-scripts/carm
-scripts/carm_ros2
+carm.egg-info/top_level.txt

carm-0.0.20250911/carm.egg-info/entry_points.txt

@@ -0,0 +1,3 @@
+[console_scripts]
+carm = carm.carm_dds:main
+carm_ros2 = carm.carm_ros2:main

{carm-0.0.20250815 → carm-0.0.20250911}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "carm"
-version = "0.0.20250815"
+version = "0.0.20250911"
 authors = [
     {name = "Yong Zhao", email = "zhaoyong11933@cvte.com"},
 ]
@@ -18,8 +18,8 @@ classifiers = [
 ]
 
 [project.scripts]
-carm = "carm:main"
-carm_ros2 = "carm:ros2_main"
+carm = "carm.carm_dds:main"
+carm_ros2 = "carm.carm_ros2:main"
 
 [tool.setuptools]
 packages = ["carm"]
@@ -28,4 +28,5 @@ packages = ["carm"]
 carm = [
     "__init__.py",
     "carm.py",
+    "carm_dds.py",
 ]

carm-0.0.20250815/carm.egg-info/entry_points.txt

@@ -1,3 +0,0 @@
-[console_scripts]
-carm = carm:main
-carm_ros2 = carm:ros2_main