finish parallel programming for experiment

ipab-slmc · Jun 15, 2023 · a090427 · a090427
1 parent 017f990
commit a090427
Show file tree

Hide file tree

Showing 27 changed files with 3,380 additions and 61 deletions.
diff --git a/...ck_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn_experiment.launch b/...ck_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn_experiment.launch
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+
+<launch>
+    <!-- set global arguments -->
+    <arg name="robot_name" default="chonk"/>
+    <param name="robot_description_wholebody" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing_wholebody.urdf'"/>
+
+    <!-- Spawn position controller -->
+    <node
+        name="trajectory_controller"
+        pkg="controller_manager"
+        type="spawner"
+        ns="/$(arg robot_name)"
+        args="/$(arg robot_name)/trajectory_controller"
+        respawn="false"
+        output="screen"
+    />
+
+    <!-- Launch a mux to send commands to the robot only from one source/controller -->
+    <node
+        name="mux_joint_position"
+        pkg="topic_tools"
+        type="mux"
+        ns="/$(arg robot_name)"
+        args="trajectory_controller/command
+            streaming_controller/command
+            DefaultPositionController/command
+            ActionCmdPosePositionController/command
+            ActionCmdConfigPositionController/command
+            ActionTeleop2DPositionController/command
+            mux:=mux_joint_position"
+    />
+    <!-- Action Service Node for commanding end-effector poses -->
+    <node
+        name="action_server_cmd_config"
+        pkg="chonk_pushing"
+        ns="/$(arg robot_name)"
+        type="action_server_cmd_config_wholetrajectory.py"
+        output="screen"
+        >
+        <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdConfigPositionController/command"/>
+        <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/>
+        <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/>
+    </node>
+    <!-- Action Service Node for planning joint positions and build MPC to track the planning results -->
+    <node
+        name="planner_server"
+        pkg="chonk_pushing"
+        ns="/$(arg robot_name)"
+        type="action_server_planner_MPC_BC_operational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn_experiment.py"
+        output="screen"
+        >
+        <param name="link_donkey" type="string" value="link_donkey"/>
+<!--        <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/>
+        <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/>-->
+        <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_grasp"/>
+        <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_grasp"/>
+<!--        <param name="link_sensor_right" type="string" value="RARM_changer_link_base"/>
+        <param name="link_sensor_left" type="string" value="LARM_changer_link_base"/>-->
+        <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/>
+        <param name="link_gaze" type="string" value="gaze_ref"/>
+        <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdPosePositionController/command"/>
+        <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/>
+        <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/>
+    </node>
+</launch>
diff --git a/...ck_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn_experiment.launch b/...ck_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn_experiment.launch
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+    <!-- set global arguments -->
+    <arg name="robot_name" default="chonk"/>
+
+    <!-- Action Service Node for tracking joint positions and build MPC to track the planning results -->
+    <node
+        name="tracking_server"
+        pkg="chonk_pushing"
+        ns="/$(arg robot_name)"
+        type="action_server_tracking_MPC_BC_operational_pick_localsensorAD_6DOF_bothobstacle_wholetrajectory_withOrientation_turn_experiment.py"
+        output="screen"
+        >
+        <param name="link_donkey" type="string" value="link_donkey"/>
+<!--        <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/>
+        <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/>-->
+        <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_grasp"/>
+        <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_grasp"/>
+<!--        <param name="link_sensor_right" type="string" value="RARM_changer_link_base"/>
+        <param name="link_sensor_left" type="string" value="LARM_changer_link_base"/>-->
+        <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/>
+        <param name="link_gaze" type="string" value="gaze_ref"/>
+        <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdPosePositionController/command"/>
+        <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/>
+        <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/>
+    </node>
+</launch>
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_turn.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_turn.py
@@ -377,7 +377,7 @@ def feedback_cb(self, feedback):
     args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
 
     # Initialize node class
-    args['duration']=10.0
+    args['duration']=4.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],