correct Delta p

ipab-slmc · Jun 3, 2023 · 376ebe3 · 376ebe3
1 parent 8dcf4a1
commit 376ebe3
Show file tree

Hide file tree

Showing 28 changed files with 269 additions and 261 deletions.
diff --git a/...lsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.launch b/...lsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.launch
@@ -64,35 +64,5 @@
         <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 teleoperating robot in 2D -->
-    <node
-        name="action_server_teleop_2d"
-        pkg="chonk_pushing"
-        ns="/$(arg robot_name)"
-        type="action_server_teleop_2d.py"
-        output="screen"
-        >
-        <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_head" type="string" value="CAMERA_HEAD_HEADING_Link"/>
-        <param name="link_ref" type="string" value="teleop_ref"/>
-        <param name="link_gaze" type="string" value="gaze_ref"/>
-        <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionTeleop2DPositionController/command"/>
-        <param name="x_min" type="double" value="0.2"/>
-        <param name="x_max" type="double" value="0.8"/>
-        <param name="y_min" type="double" value="-0.5"/>
-        <param name="y_max" type="double" value="0.5"/>
-        <!-- <param name="roll_min" type="double" value="-25"/>
-        <param name="roll_max" type="double" value="25"/>
-        <param name="pitch_min" type="double" value="-20"/>
-        <param name="pitch_max" type="double" value="70"/> -->
-        <param name="roll_min" type="double" value="0"/>
-        <param name="roll_max" type="double" value="0"/>
-        <param name="pitch_min" type="double" value="0"/>
-        <param name="pitch_max" type="double" value="0"/>
-        <param name="yaw_min" type="double" value="-120"/>
-        <param name="yaw_max" type="double" value="120"/>
-        <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/...action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py b/...action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py
@@ -125,7 +125,7 @@ def feedback_cb(self, feedback):
         type=float, default=[2, 2-0.22, 1.165],
         metavar=('POS_X', 'POS_Y', 'POS_Z')
     )
-    ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2,    np.pi/2 - np.pi/3-0.2,    0]).getquat()
+    ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2,    np.pi/2 - np.pi/3,    0]).getquat()
     parser.add_argument('--target_orientation_R', nargs=4,
         help="Give target orientation as a quaternion.",
         type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
@@ -148,7 +148,7 @@ def feedback_cb(self, feedback):
         metavar=('POS_X', 'POS_Y', 'POS_Z')
     )
 
-    ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2,    np.pi/2 - np.pi/3-0.2,    0]).getquat()
+    ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2,    np.pi/2 - np.pi/3,    0]).getquat()
     parser.add_argument('--target_orientation_L', nargs=4,
         help="Give target orientation as a quaternion.",
         type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
@@ -177,7 +177,7 @@ def feedback_cb(self, feedback):
     m_box = 1.2
 
     # Initialize node class
-    args['duration']=4.0
+    args['duration']=6.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -192,14 +192,13 @@ def feedback_cb(self, feedback):
         args['target_torque_L'],
         args['duration']
     )
-    # execute node
-#    rospy.spin()
 
-    args['target_position_R'] = [3.8, 2-0.22, 1.165]
-    args['target_position_L'] = [3.8, 2+0.22, 1.165]
+
+    args['target_position_R'] = [3.85, 2-0.22, 1.165]
+    args['target_position_L'] = [3.85, 2+0.22, 1.165]
 
     # Initialize node class
-    args['duration']=4.0
+    args['duration']=6.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -214,11 +213,11 @@ def feedback_cb(self, feedback):
         args['target_torque_L'],
         args['duration']
     )
-    args['target_position_R'] = [3.8, 2-0.142, 1.165]
-    args['target_position_L'] = [3.8, 2+0.142, 1.165]
+    args['target_position_R'] = [3.85, 2-0.143, 1.165]
+    args['target_position_L'] = [3.85, 2+0.143, 1.165]
 
     # Initialize node class
-    args['duration']=3.0
+    args['duration']=5.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -236,12 +235,12 @@ def feedback_cb(self, feedback):
 
     args['m_box'] = m_box       # unit kg
     args['target_force_R'] = [0, 0, force]
-    args['target_force_L'] = [0, 0,  force]
-    args['target_position_R'] = [3.8, 2-0.142, 1.165]
-    args['target_position_L'] = [3.8, 2+0.142, 1.165]
+    args['target_force_L'] = [0, 0, force]
+    args['target_position_R'] = [3.85, 2-0.143, 1.165]
+    args['target_position_L'] = [3.85, 2+0.143, 1.165]
 
     # Initialize node class
-    args['duration']=3.0
+    args['duration']=5.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -258,12 +257,12 @@ def feedback_cb(self, feedback):
     )
 
     args['target_force_R'] = [0, 0, force]
-    args['target_force_L'] = [0, 0,  force]
-    args['target_position_R'] = [3.8, 2-0.142, 1.25]
-    args['target_position_L'] = [3.8, 2+0.142, 1.25]
+    args['target_force_L'] = [0, 0, force]
+    args['target_position_R'] = [3.85, 2-0.143, 1.31]
+    args['target_position_L'] = [3.85, 2+0.143, 1.31]
 
     # Initialize node class
-    args['duration']=3.0
+    args['duration']=4.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -280,9 +279,9 @@ def feedback_cb(self, feedback):
     )
 
     args['target_force_R'] = [0, 0, force]
-    args['target_force_L'] = [0, 0,  force]
-    args['target_position_R'] = [3.8, -2-0.142, 1.25]
-    args['target_position_L'] = [3.8, -2+0.142, 1.25]
+    args['target_force_L'] = [0, 0, force]
+    args['target_position_R'] = [3.85, -2-0.143, 1.31]
+    args['target_position_L'] = [3.85, -2+0.143, 1.31]
 
     # Initialize node class
     args['duration']=10
@@ -302,12 +301,12 @@ def feedback_cb(self, feedback):
     )
 
     args['target_force_R'] = [0, 0, force]
-    args['target_force_L'] = [0, 0,  force]
-    args['target_position_R'] = [3.8, -2-0.142, 1.165]
-    args['target_position_L'] = [3.8, -2+0.142, 1.165]
+    args['target_force_L'] = [0, 0, force]
+    args['target_position_R'] = [3.85, -2-0.143, 1.165]
+    args['target_position_L'] = [3.85, -2+0.143, 1.165]
 
     # Initialize node class
-    args['duration']=3.0
+    args['duration']=5.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -327,11 +326,11 @@ def feedback_cb(self, feedback):
     args['m_box'] = 0 # unit kg
     args['target_force_R'] = [0, 0, 0]
     args['target_force_L'] = [0, 0, 0]
-    args['target_position_R'] = [3.8, -2-0.142, 1.165]
-    args['target_position_L'] = [3.8, -2+0.142, 1.165]
+    args['target_position_R'] = [3.85, -2-0.143, 1.165]
+    args['target_position_L'] = [3.85, -2+0.143, 1.165]
 
     # Initialize node class
-    args['duration']=3.0
+    args['duration']=4.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -349,11 +348,11 @@ def feedback_cb(self, feedback):
 
     args['target_force_R'] = [0, 0, 0]
     args['target_force_L'] = [0, 0, 0]
-    args['target_position_R'] = [3.8, -2-0.22, 1.165]
-    args['target_position_L'] = [3.8, -2+0.22, 1.165]
+    args['target_position_R'] = [3.85, -2-0.25, 1.165]
+    args['target_position_L'] = [3.85, -2+0.25, 1.165]
 
     # Initialize node class
-    args['duration']=3.0
+    args['duration']=5.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -369,11 +368,11 @@ def feedback_cb(self, feedback):
         args['duration']
     )
 
-    args['target_position_R'] = [2, -2-0.22, 1.165]
-    args['target_position_L'] = [2, -2+0.22, 1.165]
+    args['target_position_R'] = [2, -2-0.25, 1.165]
+    args['target_position_L'] = [2, -2+0.25, 1.165]
 
     # Initialize node class
-    args['duration']=4.0
+    args['duration']=5.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -390,14 +389,14 @@ 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
-    args['duration']=4.0
+    args['duration']=5.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -414,7 +413,7 @@ def feedback_cb(self, feedback):
     )
 
     # Initialize node class
-    args['duration']=1.0
+    args['duration']=2.0
     cmd_pose_client = CmdPoseClient('client', client,
         args['m_box'],
         args['target_position_Donkey'],
@@ -431,5 +430,4 @@ def feedback_cb(self, feedback):
     )
 
 
-    # execute node
-#    rospy.spin()
+
diff --git a/...se_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py b/...se_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py
@@ -122,7 +122,7 @@ def feedback_cb(self, feedback):
 
     parser.add_argument('--target_position_R', nargs=3,
         help="Give target position of the robot in meters.",
-        type=float, default=[2, 2-0.22, 1.165],
+        type=float, default=[3, 2-0.22, 1.165],
         metavar=('POS_X', 'POS_Y', 'POS_Z')
     )
     ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2,    np.pi/2 - np.pi/3,    0]).getquat()
@@ -144,7 +144,7 @@ def feedback_cb(self, feedback):
     # parse left arm arguments
     parser.add_argument('--target_position_L', nargs=3,
         help="Give target position of the robot in meters.",
-        type=float, default=[2, 2+0.22, 1.165],
+        type=float, default=[3, 2+0.22, 1.165],
         metavar=('POS_X', 'POS_Y', 'POS_Z')
     )
 
@@ -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 = 30
+    force = 20
     m_box = 1.2
 
     # Initialize node class

diff --git a/...localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py b/...localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py
@@ -111,9 +111,9 @@ def feedback_cb(self, feedback):
         box_Rotation = optas.spatialmath.rotz(box_euler_angle[2])
         box_inWorld = np.array([[trans_box[0]], [trans_box[1]], [trans_box[2]]])
         T = optas.spatialmath.rt2tr(box_Rotation, box_inWorld)
-        print(T)
-        print(box_euler_angle[2])
-        print(box_inWorld)
+#        print(T)
+#        print(box_euler_angle[2])
+#        print(box_inWorld)
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         print("error: cannot find vicon data!!!!")
 #    trans_box = np.zeros(3)

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
@@ -370,10 +370,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
@@ -411,5 +411,11 @@ def feedback_cb(self, feedback):
     )
 
 
+
+
+
+
+
+
     # execute node
 #    rospy.spin()