From 376ebe336696309cad28b99b1afff08d9f2d2be6 Mon Sep 17 00:00:00 2001 From: Wenqian Date: Sat, 3 Jun 2023 21:37:35 +0100 Subject: [PATCH] correct Delta p --- ...entation_turn_experiment_compliance.launch | 32 +------- ...localsensorAD_6DOF_obstacle_longhorizon.py | 80 +++++++++---------- ...bstacle_wholetrajectory_withOrientation.py | 6 +- ...hOrientation_turn_experiment_compliance.py | 6 +- ...perational_pick_localsensorAD_6DOF_turn.py | 10 ++- ...jectory_withOrientation_turn_experiment.py | 58 +++++++------- ...e_MPC_BC_operational_pick_localsensorAD.py | 4 +- ..._BC_operational_pick_localsensorAD_6DOF.py | 14 ++-- ...onal_pick_localsensorAD_6DOF_experiment.py | 12 +-- ...localsensorAD_6DOF_obstacle_longhorizon.py | 12 +-- ...bstacle_wholetrajectory_withOrientation.py | 60 +++++++++----- ...le_wholetrajectory_withOrientation_turn.py | 64 ++++++++------- ...jectory_withOrientation_turn_experiment.py | 36 +++++---- ...hOrientation_turn_experiment_compliance.py | 38 +++++---- ...al_pick_localsensorAD_6DOF_thisalsowork.py | 12 +-- ...pick_localsensorAD_obstacle_longhorizon.py | 4 +- ..._localsensorAD_obstacle_wholetrajectory.py | 4 +- ...rAD_obstacle_wholetrajectory_experiment.py | 4 +- ...e_wholetrajectory_experiment_compliance.py | 4 +- ...e_wholetrajectory_turn_experimentgazebo.py | 4 +- ...bstacle_wholetrajectory_withOrientation.py | 4 +- ...jectory_withOrientation_turn_experiment.py | 36 +++++---- ...hOrientation_turn_experiment_compliance.py | 4 +- ...y_withOrientation_turn_experimentgazebo.py | 6 +- ...d_pose_MPC_BC_operational_pick_sensorAD.py | 4 +- ...onal_pick_sensorAD_obstacle_longhorizon.py | 4 +- ..._pick_sensorAD_obstacle_wholetrajectory.py | 4 +- ...bstacle_wholetrajectory_withOrientation.py | 4 +- 28 files changed, 269 insertions(+), 261 deletions(-) diff --git a/launch/action_servers_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.launch b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.launch index d605b89..83ebb11 100644 --- a/launch/action_servers_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.launch +++ b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.launch @@ -64,35 +64,5 @@ - - - - - - - - - - - - - - - - - - - - - - + diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py index 8bd541f..57aab3d 100755 --- a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py +++ b/script/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/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py index 57aab3d..ebb4f95 100755 --- a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py +++ b/script/action_client_cmd_pose_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/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py index 01241f4..1e8261b 100755 --- a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py +++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_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 index ad40c82..2876578 100755 --- 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() diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py index c4187a6..fbe2f9f 100755 --- a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py +++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py @@ -106,7 +106,7 @@ def feedback_cb(self, feedback): # read current robot joint positions try: (trans_box,rot_box) = tf_listener.lookupTransform('/vicon/world', '/vicon/eva_box/eva_box', rospy.Time(0)) - trans_box[2] += 0.2 + trans_box[2] += 0.1 box_euler_angle = tf.transformations.euler_from_quaternion([rot_box[0], rot_box[1], rot_box[2], rot_box[3]]) box_Rotation = optas.spatialmath.rotz(box_euler_angle[2]) box_inWorld = np.array([[trans_box[0]], [trans_box[1]], [trans_box[2]]]) @@ -199,7 +199,7 @@ def feedback_cb(self, feedback): client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction) - force = 20 + force = 30 m_box = 0.8 # Initialize node class @@ -242,9 +242,9 @@ def feedback_cb(self, feedback): ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, box_euler_angle[2]]).getquat() args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]] - Right_inbox = np.array([[0.1], [-0.3], [0], [1]]) + Right_inbox = np.array([[0], [-0.3], [0], [1]]) Right_inWorld = (T @ Right_inbox)[0:3] - Left_inbox = np.array([[0.1], [0.3], [0], [1]]) + Left_inbox = np.array([[0], [0.3], [0], [1]]) Left_inWorld = (T @ Left_inbox)[0:3] args['target_position_R'] = [Right_inWorld[0], Right_inWorld[1], trans_box[2]] args['target_position_L'] = [Left_inWorld[0], Left_inWorld[1], trans_box[2]] @@ -265,15 +265,15 @@ def feedback_cb(self, feedback): args['target_torque_L'], args['duration'] ) - Right_inbox = np.array([[0.1], [-0.135], [0], [1]]) + Right_inbox = np.array([[0], [-0.135], [0], [1]]) Right_inWorld = (T @ Right_inbox)[0:3] - Left_inbox = np.array([[0.1], [0.135], [0], [1]]) + Left_inbox = np.array([[0], [0.135], [0], [1]]) Left_inWorld = (T @ Left_inbox)[0:3] args['target_position_R'] = [Right_inWorld[0], Right_inWorld[1], trans_box[2]] args['target_position_L'] = [Left_inWorld[0], Left_inWorld[1], trans_box[2]] # Initialize node class - args['duration']=3.0 + args['duration']=6.0 cmd_pose_client = CmdPoseClient('client', client, args['m_box'], args['target_position_Donkey'], @@ -289,29 +289,29 @@ def feedback_cb(self, feedback): args['duration'] ) - Right_inbox = np.array([[0.1], [-0.135], [0], [1]]) - Right_inWorld = (T @ Right_inbox)[0:3] - Left_inbox = np.array([[0.1], [0.135], [0], [1]]) - Left_inWorld = (T @ Left_inbox)[0:3] - args['target_position_R'] = [Right_inWorld[0], Right_inWorld[1], trans_box[2]] - args['target_position_L'] = [Left_inWorld[0], Left_inWorld[1], trans_box[2]] +# Right_inbox = np.array([[0], [-0.135], [0], [1]]) +# Right_inWorld = (T @ Right_inbox)[0:3] +# Left_inbox = np.array([[0], [0.135], [0], [1]]) +# Left_inWorld = (T @ Left_inbox)[0:3] +# args['target_position_R'] = [Right_inWorld[0], Right_inWorld[1], trans_box[2]] +# args['target_position_L'] = [Left_inWorld[0], Left_inWorld[1], trans_box[2]] - # Initialize node class - args['duration']=6.0 - cmd_pose_client = CmdPoseClient('client', client, - args['m_box'], - args['target_position_Donkey'], - args['target_orientation_Donkey'], - args['target_position_R'], - args['target_orientation_R'], - args['target_force_R'], - args['target_torque_R'], - args['target_position_L'], - args['target_orientation_L'], - args['target_force_L'], - args['target_torque_L'], - args['duration'] - ) +# # Initialize node class +# args['duration']=6.0 +# cmd_pose_client = CmdPoseClient('client', client, +# args['m_box'], +# args['target_position_Donkey'], +# args['target_orientation_Donkey'], +# args['target_position_R'], +# args['target_orientation_R'], +# args['target_force_R'], +# args['target_torque_R'], +# args['target_position_L'], +# args['target_orientation_L'], +# args['target_force_L'], +# args['target_torque_L'], +# args['duration'] +# ) args['m_box'] = m_box # unit kg args['target_force_R'] = [0, 0, force] diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py index 5094acb..8cf0368 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py @@ -220,8 +220,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF.py index d977c80..99e7efc 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF.py @@ -160,7 +160,7 @@ def __init__(self, name): self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left - stiffness_phi = 5000; + stiffness_phi = 500; self.K_phi_Right = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Right self.K_phi_Left = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Left self.D_phi_Right = np.diag([2 * np.sqrt(self.K_phi_Right[0,0]), 2 * np.sqrt(self.K_phi_Right[1,1]), 2 * np.sqrt(self.K_phi_Right[2,2])]) # Damping Right @@ -266,10 +266,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -301,8 +301,8 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], self.qaQb(init_Delta_orientation_Right, ori_R[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], self.qaQb(init_Delta_orientation_Left, ori_L[:, i] ) ) )) builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_experiment.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_experiment.py index 8a74e95..5e97d56 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_experiment.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_experiment.py @@ -267,10 +267,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -302,8 +302,8 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], self.qaQb(init_Delta_orientation_Right, ori_R[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], self.qaQb(init_Delta_orientation_Left, ori_L[:, i] ) ) )) builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py index 5a57974..beff149 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_longhorizon.py @@ -403,10 +403,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -438,8 +438,8 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py index 8e54c1b..255d8e7 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py @@ -175,6 +175,9 @@ def __init__(self, name): r_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner))) r_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner))) + lower_quaternion = np.array([-1, -1, -1, -1]) + upper_quaternion = np.array([1, 1, 1, 1]) + for i in range(self.T_MPC_planner): for j in range(self.T_MPC_planner): # r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j] @@ -190,7 +193,12 @@ def __init__(self, name): # builder_wholebodyMPC_planner.add_leq_inequality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1. + r_ori_ep[1, i]) builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1.) builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1.) -# builder_wholebodyMPC_planner.add_cost_term('Two_arm orientation parallel' + str(i), 20*optas.sumsqr(r_ori_RARM_var_MPC[:, i] - self.qaQb( r_ori_LARM_var_MPC[:, i], quaternion_fixed180 ))) + builder_wholebodyMPC_planner.add_bound_inequality_constraint('quaternion_bound_right' + str(i), lhs=lower_quaternion, mid = R_ori_Right[:, i], rhs=upper_quaternion) + builder_wholebodyMPC_planner.add_bound_inequality_constraint('quaternion_bound_left' + str(i), lhs=lower_quaternion, mid = R_ori_Right[:, i], rhs=upper_quaternion) + + + + # builder_wholebodyMPC_planner.add_cost_term('Two_arm orientation parallel' + str(i), 20*optas.sumsqr(r_ori_RARM_var_MPC[:, i] - self.qaQb( r_ori_LARM_var_MPC[:, i], quaternion_fixed180 ))) builder_wholebodyMPC_planner.add_cost_term('Two_arm end height same' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[2, i] - r_pos_LARM_var_MPC[2, i])) # builder_wholebodyMPC_planner.add_cost_term('Right_arm_align' + str(i), 5*optas.sumsqr( self.skew_optas(self.quatToRotationZ(r_ori_RARM_var_MPC[:, i])) @ (r_pos_RARM_var_MPC[:, i] - r_pos_LARM_var_MPC[:, i]) )) # builder_wholebodyMPC_planner.add_cost_term('Left_arm_align' + str(i), 5*optas.sumsqr( self.skew_optas(self.quatToRotationZ(r_ori_LARM_var_MPC[:, i])) @ (r_pos_RARM_var_MPC[:, i] - r_pos_LARM_var_MPC[:, i]) )) @@ -357,11 +365,11 @@ def __init__(self, name): # setup solver self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={ -# 'knitro.OutLev': 0, - 'print_time': 0, + 'knitro.OutLev': 0, +# 'print_time': 0, # 'knitro.par_msnumthreads': 14, 'knitro.act_qpalg': 1, - 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4, 'knitro.ftol':1e-4, + 'knitro.FeasTol': 1e-3, 'knitro.OptTol': 1e-3, 'knitro.ftol':1e-3, 'knitro.algorithm':3, 'knitro.linsolver':2, # 'knitro.maxtime_real': 4.0e-3, 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1, @@ -427,7 +435,7 @@ def __init__(self, name): self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left - stiffness_phi = 5000; + stiffness_phi = 100000; self.K_phi_Right = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Right self.K_phi_Left = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Left self.D_phi_Right = np.diag([2 * np.sqrt(self.K_phi_Right[0,0]), 2 * np.sqrt(self.K_phi_Right[1,1]), 2 * np.sqrt(self.K_phi_Right[2,2])]) # Damping Right @@ -538,10 +546,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -573,10 +581,14 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) - builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], ori_R_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], ori_L_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) @@ -590,8 +602,8 @@ def __init__(self, name): builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i])) builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i])) builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i])) - builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) - builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) + builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) + builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) ######################################################################################### @@ -651,7 +663,7 @@ def __init__(self, name): # setup solver self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={ 'knitro.OutLev': 0, - 'print_time': 0, +# 'print_time': 0, 'knitro.FeasTol': 5e-6, 'knitro.OptTol': 5e-6, 'knitro.ftol':5e-6, 'knitro.algorithm':1, 'knitro.linsolver':2, @@ -1011,6 +1023,7 @@ def timer_cb(self, event): pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j] ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j] ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j] +# print(optas.sumsqr(ori_R_reasonal[:, i])) else: t_nomalized = 1. for j in range(self.T_MPC_planner): @@ -1018,6 +1031,10 @@ def timer_cb(self, event): pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j] ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j] ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j] + for i in range(self.T_MPC): + for j in range(4): + ori_R_reasonal[j, i] /= optas.sumsqr(ori_R_reasonal[:, i]) + ori_L_reasonal[j, i] /= optas.sumsqr(ori_L_reasonal[:, i]) # print(self._t[self._idx-1]) # print(self.duration_MPC_planner) # print(R_ori_Right) @@ -1025,6 +1042,7 @@ def timer_cb(self, event): # print('left') # print(R_pos_Left[2, :]) # print(pos_L_reasonal[2, :]) + ### --------------------------------------------------------- self.ti_MPC = 0 force_R_goal = [] @@ -1129,11 +1147,11 @@ def timer_cb(self, event): ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j]) # read current robot joint positions - self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) - self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] - self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] - self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) - self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) +# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) +# self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] +# self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] +# self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) +# self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) # compute the donkey velocity in its local frame Global_w_b = np.asarray([0., 0., dq_next[2]]) Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.py index cf6a1d9..f070e23 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.py @@ -357,8 +357,8 @@ def __init__(self, name): # setup solver self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={ -# 'knitro.OutLev': 0, - 'print_time': 0, + 'knitro.OutLev': 0, +# 'print_time': 0, # 'knitro.par_msnumthreads': 14, 'knitro.act_qpalg': 1, 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4, 'knitro.ftol':1e-4, @@ -427,7 +427,7 @@ def __init__(self, name): self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left - stiffness_phi = 5000; + stiffness_phi = 100000; self.K_phi_Right = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Right self.K_phi_Left = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Left self.D_phi_Right = np.diag([2 * np.sqrt(self.K_phi_Right[0,0]), 2 * np.sqrt(self.K_phi_Right[1,1]), 2 * np.sqrt(self.K_phi_Right[2,2])]) # Damping Right @@ -538,10 +538,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -573,10 +573,14 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) - builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], ori_R_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], ori_L_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) @@ -590,8 +594,8 @@ def __init__(self, name): builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i])) builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i])) builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i])) - builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) - builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) + builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) + builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) ######################################################################################### @@ -599,14 +603,14 @@ def __init__(self, name): builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4]) builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof]) builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC))) -# builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC))) -# builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC))) -# builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC))) -# builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC))) -# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 ) -# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 ) - builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[:, 0], rhs = np.zeros(3) ) - builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[:, 0], rhs = np.zeros(3) ) + builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC))) + builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC))) + builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC))) + builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC))) + builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 ) + builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 ) +# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[:, 0], rhs = np.zeros(3) ) +# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[:, 0], rhs = np.zeros(3) ) builder_wholebodyMPC.add_equality_constraint('init_Delta_phi_Right_constraint_y', Phi_Right[:, 0], rhs = 0 ) builder_wholebodyMPC.add_equality_constraint('init_Delta_phi_Left_constraint_y', Phi_Left[:, 0], rhs = 0 ) ######################################################################################### @@ -651,8 +655,8 @@ def __init__(self, name): # setup solver self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={ 'knitro.OutLev': 0, - 'print_time': 0, - 'knitro.FeasTol': 5e-6, 'knitro.OptTol': 5e-6, 'knitro.ftol':5e-6, +# 'print_time': 0, +# 'knitro.FeasTol': 5e-6, 'knitro.OptTol': 5e-6, 'knitro.ftol':5e-6, 'knitro.algorithm':1, 'knitro.linsolver':2, # 'knitro.maxtime_real': 1.8e-2, @@ -1129,11 +1133,11 @@ def timer_cb(self, event): ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j]) # read current robot joint positions - self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) - self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] - self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] - self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) - self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) +# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) +# self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] +# self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] +# self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) +# self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) # compute the donkey velocity in its local frame Global_w_b = np.asarray([0., 0., dq_next[2]]) Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.]) @@ -1228,10 +1232,10 @@ def read_left_ee_grasp_ft_data_cb(self, msg): self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ]) def read_right_ee_grasp_ft_local_data_cb(self, msg): - self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5]]) + self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]]) def read_left_ee_grasp_ft_local_data_cb(self, msg): - self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ]) + self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ]) def read_mux_selection(self, msg): self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment.py index db08d2d..af4aa70 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment.py @@ -429,7 +429,7 @@ def __init__(self, name): self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left - stiffness_phi = 5000; + stiffness_phi = 50; self.K_phi_Right = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Right self.K_phi_Left = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Left self.D_phi_Right = np.diag([2 * np.sqrt(self.K_phi_Right[0,0]), 2 * np.sqrt(self.K_phi_Right[1,1]), 2 * np.sqrt(self.K_phi_Right[2,2])]) # Damping Right @@ -540,10 +540,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -575,10 +575,14 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) - builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], ori_R_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], ori_L_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) @@ -592,8 +596,8 @@ def __init__(self, name): builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i])) builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i])) builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i])) - builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) - builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) + builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) + builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) ######################################################################################### @@ -1138,11 +1142,11 @@ def timer_cb(self, event): ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j]) # read current robot joint positions - self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) - self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] - self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] - self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) - self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) +# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) +# self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] +# self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] +# self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) +# self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) # compute the donkey velocity in its local frame Global_w_b = np.asarray([0., 0., dq_next[2]]) Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py index 0bf6313..2b60ba0 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py @@ -419,7 +419,7 @@ def __init__(self, name): self.m_ee_r = 0.3113; self.m_ee_l = 0.3113; - stiffness = 1500; + stiffness = 500; inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter @@ -429,7 +429,7 @@ def __init__(self, name): self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left - stiffness_phi = 1500; + stiffness_phi = 30; self.K_phi_Right = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Right self.K_phi_Left = np.diag([stiffness_phi, stiffness_phi, stiffness_phi]) # Stiffness Left self.D_phi_Right = np.diag([2 * np.sqrt(self.K_phi_Right[0,0]), 2 * np.sqrt(self.K_phi_Right[1,1]), 2 * np.sqrt(self.K_phi_Right[2,2])]) # Damping Right @@ -540,10 +540,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -575,10 +575,14 @@ def __init__(self, name): # builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i])) # builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i])) - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) - builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L_reasonal[:, i] ) ) )) +# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) +# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], ori_R_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], ori_L_reasonal[:, i] ) )) + builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) + builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i])) ##################################################################################### builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i])) @@ -592,8 +596,8 @@ def __init__(self, name): builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i])) builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i])) builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i])) - builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) - builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.5 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) + builder_wholebodyMPC.add_cost_term('Right_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Right[:, i+1] - Phi_Right[:, i])) + builder_wholebodyMPC.add_cost_term('Left_torque_distance' + str(i), 0.05 * optas.sumsqr(Phi_Left[:, i+1] - Phi_Left[:, i])) ######################################################################################### @@ -1138,11 +1142,11 @@ def timer_cb(self, event): ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j]) # read current robot joint positions - self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) - self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] - self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] - self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) - self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) +# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None) +# self.Derivation_RARM_pos_start = np.asarray(self.pos_fnc_Right(q_next)).T[0] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0] +# self.Derivation_LARM_pos_start = np.asarray(self.pos_fnc_Left(q_next)).T[0] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0] +# self.Derivation_RARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Right(self.q_curr)).T[0], np.asarray(self.ori_fnc_Right(q_next)).T[0] ) +# self.Derivation_LARM_ori_start = self.qaConjugateQb_numpy(np.asarray(self.ori_fnc_Left(self.q_curr)).T[0], np.asarray(self.ori_fnc_Left(q_next)).T[0]) # compute the donkey velocity in its local frame Global_w_b = np.asarray([0., 0., dq_next[2]]) Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_thisalsowork.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_thisalsowork.py index 7c54ad7..01a779f 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_thisalsowork.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_thisalsowork.py @@ -253,10 +253,10 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] - Delta_phi_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] - Delta_phi_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_phi_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Right[:, j] + Delta_phi_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Phi_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -289,8 +289,8 @@ def __init__(self, name): for i in range(self.T_MPC): builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper) # optimization cost: close to target - builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC, self.qaQb(init_Delta_orientation_Right, ori_R[:, i] ) ) )) - builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC, self.qaQb(init_Delta_orientation_Left, ori_L[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Right_var_MPC[:, i], self.qaQb(init_Delta_orientation_Right, ori_R[:, i] ) ) )) + builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])- self.qaQb(Delta_quaternion_Left_var_MPC[:, i], self.qaQb(init_Delta_orientation_Left, ori_L[:, i] ) ) )) # builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel' + str(i), 0.1*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i]).T @ self.ori_fnc_Left(q_var_MPC[:, i]))) builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i])) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py index 664ad40..f63e92c 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py @@ -357,8 +357,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py index 6091b70..dd9812a 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py @@ -419,8 +419,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py index 48ab0de..6178feb 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py @@ -422,8 +422,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment_compliance.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment_compliance.py index d2b6ab0..fda67c4 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment_compliance.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment_compliance.py @@ -422,8 +422,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experimentgazebo.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experimentgazebo.py index a34661a..7d93abb 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experimentgazebo.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experimentgazebo.py @@ -420,8 +420,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py index a1523ff..aae4328 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py @@ -500,8 +500,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py index f7d280e..705e9b7 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment.py @@ -287,10 +287,10 @@ def __init__(self, name): # builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left)) - builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_pos_RARM_var_MPC[0:2, 0], rhs=init_dr_position_Right[0:2]) - builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_pos_LARM_var_MPC[0:2, 0], rhs=init_dr_position_Left[0:2]) -# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Right', dr_ori_RARM_var_MPC[:, 0], rhs=init_dr_orientation_Right) -# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Left', dr_ori_LARM_var_MPC[:, 0], rhs=init_dr_orientation_Left) + builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_pos_RARM_var_MPC[0:3, 0], rhs=init_dr_position_Right[0:3]) + builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_pos_LARM_var_MPC[0:3, 0], rhs=init_dr_position_Left[0:3]) + builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Right', dr_ori_RARM_var_MPC[:, 0], rhs=init_dr_orientation_Right) + builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Left', dr_ori_LARM_var_MPC[:, 0], rhs=init_dr_orientation_Left) # builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3)) builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3)) builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3)) @@ -408,12 +408,12 @@ def __init__(self, name): self.m_ee_r = 0.3113; self.m_ee_l = 0.3113; - stiffness = 1500; + inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter - self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right - self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left + self.K_Right = np.diag([1000, 1000, 1000]) # Stiffness Right + self.K_Left = np.diag([1000, 1000, 1000]) # Stiffness Left self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left ################################################################################### @@ -504,8 +504,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -553,12 +553,14 @@ def __init__(self, name): builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4]) builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof]) builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC))) - builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC))) - builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC))) - builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC))) - builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC))) - builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 ) - builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 ) +# builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC))) +# builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC))) +# builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC))) +# builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC))) +# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 ) +# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 ) + builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[:, 0], rhs = np.zeros(3) ) + builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[:, 0], rhs = np.zeros(3) ) ######################################################################################### dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC))) w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC) @@ -1058,10 +1060,10 @@ def read_base_states_cb(self, msg): # self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ]) def read_right_ee_grasp_ft_local_data_cb(self, msg): - self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]]) + self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5]]) def read_left_ee_grasp_ft_local_data_cb(self, msg): - self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ]) + self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ]) def read_mux_selection(self, msg): self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py index c54a1d9..6a10896 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experiment_compliance.py @@ -504,8 +504,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experimentgazebo.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experimentgazebo.py index 8db65e0..09c5714 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experimentgazebo.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn_experimentgazebo.py @@ -498,8 +498,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) @@ -553,6 +553,8 @@ def __init__(self, name): builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC))) builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 ) builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 ) +# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[:, 0], rhs = np.zeros(3) ) +# builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[:, 0], rhs = np.zeros(3) ) ######################################################################################### dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC))) w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py index ea8f9e1..5f5fea1 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py @@ -216,8 +216,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py index a68971c..daa5d07 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py @@ -355,8 +355,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py index df0cf48..303c725 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py @@ -404,8 +404,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j]) diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py index fc1358b..8db4f4d 100755 --- a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py +++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py @@ -473,8 +473,8 @@ def __init__(self, name): for i in range(self.T_MPC): for j in range(self.T_MPC): q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j] - Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] - Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] + Delta_p_Right_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j] + Delta_p_Left_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j] for j in range(self.T_MPC-1): dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j]) dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])