delete notGood servers

ipab-slmc · May 26, 2023 · 3368ff9 · 3368ff9
1 parent 9e75727
commit 3368ff9
Show file tree

Hide file tree

Showing 8 changed files with 66 additions and 3,735 deletions.
diff --git a/..._servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch b/..._servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch
@@ -34,7 +34,7 @@
         name="action_server_cmd_config"
         pkg="chonk_pushing"
         ns="/$(arg robot_name)"
-        type="action_server_cmd_config.py"
+        type="action_server_cmd_config_wholetrajectory.py"
         output="screen"
         >
         <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdConfigPositionController/command"/>

diff --git a/...ent_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py b/...ent_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py
@@ -173,7 +173,7 @@ def feedback_cb(self, feedback):
     rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
     client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
 
-    force = 50
+    force = 30
     m_box = 1.2
 
     # Initialize node class
@@ -258,8 +258,8 @@ def feedback_cb(self, feedback):
 
     args['target_force_R'] = [0, force, 0]
     args['target_force_L'] = [0, -force, 0]
-    args['target_position_R'] = [3.85, 2-0.142, 1.27]
-    args['target_position_L'] = [3.85, 2+0.142, 1.27]
+    args['target_position_R'] = [3.85, 2-0.142, 1.31]
+    args['target_position_L'] = [3.85, 2+0.142, 1.31]
 
     # Initialize node class
     args['duration']=4.0
@@ -280,8 +280,8 @@ def feedback_cb(self, feedback):
 
     args['target_force_R'] = [0, force, 0]
     args['target_force_L'] = [0, -force, 0]
-    args['target_position_R'] = [3.85, -2-0.142, 1.27]
-    args['target_position_L'] = [3.85, -2+0.142, 1.27]
+    args['target_position_R'] = [3.85, -2-0.142, 1.31]
+    args['target_position_L'] = [3.85, -2+0.142, 1.31]
 
     # Initialize node class
     args['duration']=8
@@ -389,10 +389,10 @@ def feedback_cb(self, feedback):
     )
 
     args['target_position_R'] = [0.865, -0.12, 0.92]
-    ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi,    np.pi/2,    0]).getquat()
+    ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2,    np.pi/2,    0]).getquat()
     args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
     args['target_position_L'] = [0.865, 0.12, 0.92]
-    ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi,    np.pi/2,    0]).getquat()
+    ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2,    np.pi/2,    0]).getquat()
     args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
 
     # Initialize node class

diff --git a/script/action_server_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py b/script/action_server_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py
diff --git a/...md_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py b/...md_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py
@@ -100,7 +100,7 @@ def __init__(self, name):
         )
         self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
 #        self.dt_MPC_planner = 0.1 # time step
-        self.T_MPC_planner = 10 # T is number of time steps
+        self.T_MPC_planner = 12 # T is number of time steps
 #        self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
         # nominal robot configuration
         self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes

diff --git a/...tion_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_notgood.py b/...tion_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_notgood.py
diff --git a/...ver_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py b/...ver_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py
diff --git a/...pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood.py b/...pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood.py
diff --git a/...ose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood2.py b/...ose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood2.py