unilab添加moveit启动

1,整合所有moveit节点到一个move_group中，并整合所有的controller依次激活
2,添加pymoveit2的节点，使用json可直接启动
3,修改机械臂规划方式，添加约束，让冗余关节不会进行过多移动

test/experiments/plr_test_converted.json

@@ -163,7 +163,7 @@
             "type": "plate",
             "class": "opentrons_96_filtertiprack_1000ul",
             "position": {
-                "x": 0,
+                "x": 265.0,
                 "y": 0,
                 "z": 69
             },
@@ -5762,8 +5762,8 @@
             "type": "plate",
             "class": "nest_96_wellplate_2ml_deep",
             "position": {
-                "x": 265.0,
-                "y": 0,
+                "x": 0,
+                "y": 90.5,
                 "z": 69
             },
             "config": {
@@ -9623,6 +9623,65 @@
                 "pending_liquids": [],
                 "liquid_history": []
             }
+        },
+        {
+            "id": "benyao",
+            "name": "benyao",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "moveit.benyao_arm",
+            "position": {
+                "x": -500,
+                "y": 1000,
+                "z": -100
+            },
+            "config": {
+                "moveit_type": "benyao_arm",
+                "joint_poses": {
+                    "arm": {
+                        "hotel_1": [1.05, 0.568, -1.0821, 0.0, 1.0821],
+                        "home": [0.865, 0.09, 0.8727, 0.0, -0.8727]
+                    }
+                },
+                "rotation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": -1.5708,
+                    "type": "Rotation"
+                },
+                "device_config": {
+                }
+            },
+            "data": {
+            }
+        },
+        {
+            "id": "hotel",
+            "name": "hotel",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "hotel.thermo_orbitor_rs2_hotel",
+            "position": {
+                "x": 0,
+                "y": -700,
+                "z": -10
+            },
+            "config": {
+                "rotation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 0,
+                    "type": "Rotation"
+                },
+                "device_config": {
+                }
+            },
+            "data": {
+            }
         }
     ],
     "links": []

test/experiments/test_moveit.json

@@ -0,0 +1,35 @@
+{
+    "nodes": [
+
+        {
+            "id": "benyao",
+            "name": "benyao",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "moveit.benyao_arm",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+                "moveit_type": "benyao_arm",
+                "joint_poses": {
+                    "arm": {
+                        "home": [0.0, 0.2, 0.0, 0.0, 0.0],
+                        "pick": [1.2, 0.0, 0.0, 0.0, 0.0]
+                    }
+                },
+                "device_config": {
+                }
+            },
+            "data": {
+            }
+        }
+    ],
+    "links": [
+        
+    ]
+}