API tools faq
paste
chiku007

High-level and Mid-level controller architecture

chiku007
Oct 28th, 2024
51
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 23.28 KB | Science | 0 0
raw download clone embed print report
  1. #! /usr/bin/env python
  2. # 1) Broadcast the demanded BHand states as a topic,
  3. # 2) Broadcast the demanded BHand states as an action-server: should be able to tell it when to start and give it a user-provided trajectory to follow.
  4. # 3) Start the ros-node in a seperate-process to allow all events in that process access to ros-topics: allow the cart-MPC access to the current cart-states when predicting the intial-conditions of the next time-step (which will be used as initial-conditions for the MPC)
  5. # 4) Set-up the original-model (cart-mpc in a seperate-process, and the QP-solver in the main-process (may need to eventually move it into a seperate-process since the main-process may be used in many-places))
  6. # import rospy
  7. # For creating the action-server
  8. # import actionlib
  9. import numpy as np
  10. import queue # imported for using queue.Empty exception
  11. import time
  12. # For bradcasting the demanded bhand-state, and to get the planned cart-trajectory to follow, along with the start-time for broadcasting the demanded EE-poses
  13. # from cart_mpc.msg import bhand_demand, BHand_dem_poseFeedback, BHand_dem_poseResult, BHand_dem_poseAction
  14. # Get cart and BHand pose
  15. # from gazebo_msgs.msg import LinkStates
  16. # For reading the off-setted Force-Torque reading
  17. # from geometry_msgs.msg import PoseStamped, WrenchStamped
  18. # For running the ros-node on a seperate process, and for ensuring that the data currently-available in said-variable are not in the middle of being written to by the topic-subscriber callback-function
  19. from multiprocessing import Lock, Process, Queue
  20. # EE-pose trajectory follower
  21. # from QP_mover_V1 import Diff_IK
  22. # For sensding the demanded joint velocities to the robot.
  23. # from SummitClass_V10_mujoco import SummitMover
  24.  
  25.  
  26. class BHand_dem_pose(object):
  27.  
  28.     # create messages that are used to publish feedback/result
  29.     # _feedback = BHand_dem_poseFeedback()
  30.     # _result = BHand_dem_poseResult()
  31.     # Constructor: All variables and functions with the self. keyword are initialised in the constructor
  32.     def __init__(self):
  33.  
  34.         # Initialize the Cart-MPC module
  35.         #cart_mpc = Cart_MPC()
  36.  
  37.         # Initialize variables
  38.         # Initial process-time
  39.         self.init_process_time = 0 # sec
  40.         # Cart-MPC time-step
  41.         self.delta_t_mpc = 0.5 # sec
  42.         # QP-solver time-step
  43.         self.delta_t_ee = 0.5 # sec
  44.         # Number of rounds
  45.         self.num_rounds = 1
  46.         # Length
  47.         self.radius = 0.3
  48.         # Starting angle
  49.         self.starting_angle = 0
  50.         gains_EE = [7.0, 1, 0.1, 0.1] # PID
  51.  
  52.         # Subscribe to the topic that will send the demanded EE-state.
  53.         # Subscribe to the topic that will give the current WAM joint-states.
  54.         # Subscribe to the topic that will give the current off-setted FT-sensor data
  55.         # Publish joint velocites to the MM joints
  56.         # self.mm_mover = SummitMover()
  57.  
  58.         # # Build the next feedback msg to be sent
  59.         # self._feedback.cart_pose_error = PoseStamped()
  60.  
  61.         # # Initialize QP Solver
  62.         # with self.nostdout(): # To mute the output of the qpOASES solver when it is being initialized
  63.         #     self.mob_man = Diff_IK(k_p=gains_EE[0],k_d=gains_EE[1], k_i=gains_EE[2], k_w=gains_EE[3], delta_t=self.delta_t_ee)
  64.  
  65.         # # Creates the action server
  66.         # self._as = actionlib.SimpleActionServer("cart_mpc_as", BHand_dem_poseAction, self.goal_callback, False)
  67.         # self._as.start()
  68.  
  69.         # rospy.loginfo("Ready to plan cart-motion ...")
  70.    
  71.     def goal_callback(self, goal):
  72.         # rospy.loginfo("Starting to broadcast bhand demanded state ...")
  73.  
  74.         # Start-time
  75.         self.start_time = time.time()+goal#goal.start_time
  76.         # Initialize some variables
  77.         success = True
  78.  
  79.         # Create Lock object to make sure there is no memory conflict!
  80.         lock_state = Lock()
  81.  
  82.         # Prepare four queues:
  83.         # 1) For providing values to a queue dedicated for the cart-MPC (2 Hz),
  84.         # 2) For providing values to a queue dedicated for the QP-solver (20 Hz),
  85.         # 3) For providing the cart-mpc with the current-state (2 Hz),
  86.         # 4) For providing the QP-solver with the current-state (20 Hz),
  87.  
  88.         # Initialize the queues
  89.         # Queue of in-coming current system-state (for feedback | @ 2 Hz)
  90.         queue_of_curr_state_cart_mpc = Queue()
  91.         # Queue of in-coming current system-state (for feedback | @ 20 Hz)
  92.         queue_of_curr_state_qp_sol = Queue()
  93.         # Queue to be sent to the QP-solver (@ 20 Hz)
  94.         queue_of_ref_EE_states = Queue()
  95.         # Queue to be sent out as action-server feedback (@ 2 Hz)
  96.         queue_of_cart_pose_error = Queue()
  97.         # Queue to be sent out as joint-velocities ( @ 20 Hz)
  98.         queue_of_joint_vel = Queue()
  99.  
  100.         # Initialize the Queue
  101.         # Current system-state
  102.         curr_sys_state = np.zeros((16,1))#self.mm_mover.get_current_mm_state()
  103.         # For the Cart-MPC (2 Hz)
  104.         queue_of_curr_state_cart_mpc.put(curr_sys_state)
  105.         # Next instant
  106.         curr_state_cart_mpc_nxt_ist = self.init_process_time + self.delta_t_mpc
  107.         # For the QP-Solver (20 Hz)
  108.         queue_of_curr_state_qp_sol.put(curr_sys_state)
  109.         # Next instant
  110.         curr_state_qp_sol_nxt_ist = self.init_process_time + self.delta_t_ee
  111.  
  112.         # Reference EE-state
  113.         # Initial joint velocity
  114.         init_joint_vel = np.array([[0],[0],[0], # MB velocity
  115.                                    [0],[0],[0],[0],[0],[0],[0]]) # WAM velocity
  116.         # Initial EE position
  117.         mat_w_G_10_curr = np.eye(4) #self.mob_man.curr_EE_pose(curr_sys_state[0:10,0])
  118.         # Initial EE position and velocity (as a reference)
  119.         ref_EE_state = np.concatenate((mat_w_G_10_curr[0:3,3:4],np.zeros((3,1)),np.zeros((6,1))),axis=0)
  120.         # Add initial EE-state to the Queue (20 Hz)
  121.         queue_of_ref_EE_states.put(ref_EE_state)
  122.  
  123.         # Joint velocity
  124.         queue_of_joint_vel.put(init_joint_vel)
  125.  
  126.         # Next instant
  127.         cart_pose_error_nxt_ist = self.init_process_time + self.delta_t_mpc
  128.  
  129.         # Initialize the process
  130.         # Cart-MPC
  131.         p_cart_mpc = Process(target=self.add_new_EE_pose, args=(queue_of_curr_state_cart_mpc,   # In  (2 Hz)
  132.                                                                 queue_of_ref_EE_states,         # Out (20 Hz)
  133.                                                                 queue_of_cart_pose_error,       # Out (2 Hz)
  134.                                                                 lock_state))                    # Pass the lock-state that will control the sequence in which the locked-variables are accessed
  135.         # QP-Solver
  136.         p_qp_solver = Process(target=self.follow_new_EE_pose, args=(queue_of_curr_state_qp_sol, # In  (20 Hz)
  137.                                                                     queue_of_ref_EE_states,     # In  (20 Hz)
  138.                                                                     queue_of_joint_vel,         # Out (20 Hz)
  139.                                                                     lock_state))                # Pass the lock-state that will control the sequence in which the locked-variables are accessed
  140.        
  141.         # Start the process:
  142.         # To add jobs
  143.         p_cart_mpc.start()
  144.         print("Adding EE-poses to go to ...")
  145.         # rospy.loginfo("Adding EE-poses to go to ...")
  146.         # To do jobs
  147.         p_qp_solver.start()
  148.         print("Going to said EE-poses ...")
  149.         # rospy.loginfo("Going to said EE-poses ...")
  150.  
  151.         # Wait till the off-setted time has elapsed
  152.         while((time.time()-self.start_time)<self.init_process_time):
  153.             continue
  154.         # Now start keeping track of when to populate the relevant queues
  155.         # print(str(time.time()-self.start_time)+" Start")
  156.         while True:
  157.             # Check if preempted/cancelled
  158.             # if self._as.is_preempt_requested():
  159.             #     rospy.logwarn('The base goal has been cancelled/preempted!')
  160.             #     self._as.set_preempted()
  161.             #     success = False
  162.             #     break
  163.            
  164.             # Update system-state input to the cart-mpc (@ 2 Hz)
  165.             if ((time.time()-self.start_time)>curr_state_cart_mpc_nxt_ist):
  166.                 # Update new time
  167.                 curr_state_cart_mpc_nxt_ist = curr_state_cart_mpc_nxt_ist + self.delta_t_mpc
  168.                 # Current system-state                
  169.                 curr_mm_state = np.zeros((16,1))#self.mm_mover.get_current_mm_state()
  170.                 try:
  171.                     # print(str(time.time()-self.start_time)+" Outside A: Before")
  172.                     # Finish getting and putting data in queue
  173.                     # Acquire the lock
  174.                     # with lock_state:
  175.                     # lock_state.acquire()
  176.                     # For the Cart-MPC (2 Hz)
  177.                     # queue_of_curr_state_cart_mpc.put(curr_mm_state) # Un-comment it when you need it.
  178.                     # Extract the cart pose-error into a pose-object and send it out as feedback
  179.                     # print("Getting cart_pose_error")
  180.                     cart_pose_error = queue_of_cart_pose_error.get_nowait()
  181.                     # print("cart_pose_error")
  182.                     # Release the lock
  183.                     # lock_state.release()
  184.                     # print(str(time.time()-self.start_time)+" Outside A: After")
  185.                 except queue.Empty:
  186.                     print("Finished! A")
  187.                     break
  188.                 else:
  189.                     a=1
  190.                     # print("Got cart_pose_error")
  191.                 # # Set the header
  192.                 # self._feedback.cart_pose_error.header.seq = cart_pose_error[0,0]
  193.                 # self._feedback.cart_pose_error.header.stamp = rospy.Time.now()
  194.                 # self._feedback.cart_pose_error.header.frame_id = "global-frame"
  195.                 # # Set the pose error
  196.                 # self._feedback.cart_pose_error.pose.position.x = 0
  197.                 # self._feedback.cart_pose_error.pose.position.y = 0
  198.                 # self._feedback.cart_pose_error.pose.position.z = 0
  199.                 # self._feedback.cart_pose_error.pose.orientation.x = 0
  200.                 # self._feedback.cart_pose_error.pose.orientation.y = 0
  201.                 # self._feedback.cart_pose_error.pose.orientation.z = 0
  202.                 # self._feedback.cart_pose_error.pose.orientation.w = 0
  203.                 # # publish the feedback
  204.                 # self._as.publish_feedback(self._feedback)
  205.  
  206.             # Update system-state input to the QP-Solver
  207.             # Send the computed joint-velocities to the ros-topic (@ 20 Hz)
  208.             if ((time.time()-self.start_time)>curr_state_qp_sol_nxt_ist):
  209.                 # Update new time
  210.                 curr_state_qp_sol_nxt_ist = curr_state_qp_sol_nxt_ist + self.delta_t_ee
  211.  
  212.                 # Update new system-states
  213.                 # Current system-state
  214.                 curr_mm_state = np.zeros((16,1))#self.mm_mover.get_current_mm_state()
  215.                 # print(str(time.time()-self.start_time)+" Outside B: Before")
  216.                 # Finish getting and putting data in queue
  217.  
  218.                 try:
  219.                     # Get current commanded joint-velocities
  220.                     # print("Getting new_joint_vel")
  221.                     new_joint_vel = queue_of_joint_vel.get_nowait()
  222.                     # print("new_joint_vel")
  223.                 except queue.Empty:
  224.                     print("Finished! B")
  225.                     break
  226.                 else:
  227.                     a=1
  228.                     #print("Got new_joint_vel")
  229.                 # Acquire the lock
  230.                 # with lock_state:
  231.                 # lock_state.acquire()
  232.                 # For the QP-Solver (20 Hz)
  233.                 queue_of_curr_state_qp_sol.put(curr_mm_state)
  234.                 # Release the lock
  235.                 # lock_state.release()
  236.                 # print(str(time.time()-self.start_time)+" Outside B: After")
  237.  
  238.                 # Send the commanded joint velocities
  239.                 # self.mm_mover.summit_move_velocity(new_joint_vel)
  240.                 print(new_joint_vel)
  241.         print("Send terminal velocity, since the if-statement checking the joint-vel was not updated : "+str(new_joint_vel))
  242.         # Completing a process
  243.         p_cart_mpc.join()
  244.         # rospy.loginfo("Finished adding EE-poses to go to.")
  245.         print("Finished adding EE-poses to go to.")
  246.         p_qp_solver.join()
  247.         # rospy.loginfo("Finished going to said EE-poses.")
  248.         print("Finished going to said EE-poses.")
  249.  
  250.         # At this point, either the goal has been achieved (success==true)
  251.         # or the client preempted the goal (success==false).
  252.         # If success, then we publish the final result.
  253.         # If not success, we do not publish anything in the result
  254.         # if success:
  255.         #     self._result.cart_pose_error_final = self._feedback.cart_pose_error
  256.         #     rospy.loginfo('Successfully covered the planned trajectory' )
  257.         #     self._as.set_succeeded(self._result)
  258.    
  259.     def add_new_EE_pose(self,
  260.                         queue_of_curr_state_cart_mpc,   # Queue of in-coming current system-state (for feedback | @ 2 Hz)
  261.                         queue_of_ref_EE_states,         # Queue to be sent to the QP-solver (@ 2 Hz)
  262.                         queue_of_cart_pose_error,       # Queue to be sent out as action-server feedback (@ 2 Hz)
  263.                         lock_state):                    # Pass the lock-state that will control the sequence in which the locked-variables are accessed
  264.         # Next time-instant for adding an EE-pose.
  265.         next_time_inst = self.init_process_time + self.delta_t_mpc
  266.         # Number of instances per revolution
  267.         num_instances_rev = 500
  268.         # Number of instances
  269.         num_instances = 5#num_instances_rev*self.num_rounds
  270.         # Wait till the off-setted time has elapsed
  271.         while((time.time()-self.start_time)<self.init_process_time):
  272.             continue
  273.         # Keep adding velocities to the queue.
  274.         for instant_ind in range(num_instances):
  275.             # Finish getting data from queue
  276.             # Acquire the lock
  277.             # self.lock_state.acquire()
  278.             # System joint position (from ROS-topics)
  279.             # Wait for the data to be loaded
  280.             # curr_joint_pos = queue_of_curr_state_cart_mpc.get()
  281.             # Release the lock
  282.             # self.lock_state.release()
  283.            
  284.             # New theta
  285.             curr_theta = self.starting_angle + 2*np.pi*(instant_ind/num_instances_rev)
  286.             # Reference position
  287.             ref_EE_position = np.array([[0.2],
  288.                                         [self.radius*np.sin(curr_theta)],
  289.                                         [0.6]])
  290.             # print(ref_EE_position)
  291.             # Reference velocity
  292.             ref_EE_velocity = np.array([[0],
  293.                                         [self.radius*2*np.pi/num_instances_rev/self.delta_t_mpc*np.cos(curr_theta)],
  294.                                         [0]])
  295.             # Reference wrench (at the FT-sensor, the wrench experienced by it, NOT the wrench experienced by the BHand Free-body)
  296.             ref_EE_wrench = np.array([[0],  # This force-component must NOT be included in the cost function, as we will be imposing position-control to allow the BHand palm to follow a desired trajectory along that direction.
  297.                                       [-1], # The BHand will be attempting to exert this magnitude of force on the horizontal handle.
  298.                                       [0],  # This force-component must NOT be included in the cost function, as we will be imposing position-control to make the BHand palm remain at a fixed height from the ground.
  299.                                       [0],  # This torque-component must NOT be included in the cost function, as we will be imposing pose-control to make the BHand palm remain vertical to the ground.
  300.                                       [0],  # This torque-component must NOT be included in the cost function, as we will be imposing pose-control to make the BHand palm remain vertical to the ground.
  301.                                       [0]]) # The BHand must always be alighned with the horizontal-bar; not facing it would result in a torque to be experienced by the FT-sensor when the BHand comes in contact with it.
  302.             # Reference state
  303.             ref_EE_state = np.concatenate((ref_EE_position,ref_EE_velocity,ref_EE_wrench),axis=0)
  304.  
  305.             # Build the next feedback msg to be sent
  306.             cart_pose_error = [instant_ind,     # Set the header
  307.                                0,0,0,0,0,0,0] # Set the pose error
  308.            
  309.             # print(str(time.time()-self.start_time)+" Inside A: Before")
  310.             # Finish putting data in queue
  311.             # Acquire the lock
  312.             # with lock_state:
  313.             # lock_state.acquire()
  314.             # Add the cart-pose error
  315.             queue_of_cart_pose_error.put(cart_pose_error)
  316.             # Add the reference position and velocity
  317.             # When the cart-MPC is called, make sure to linearly interpolate from the previous state to the new-state and send 10 values (20 Hz demand from a 2 Hz process).
  318.             queue_of_ref_EE_states.put(ref_EE_state)
  319.             # Release the lock
  320.             # lock_state.release()
  321.  
  322.             # Wait till a set amount of time has elapsed
  323.             # print(str(time.time()-self.start_time)+" Inside A: After")
  324.             while((time.time()-self.start_time)<next_time_inst):
  325.                 continue
  326.             # Update next-time
  327.             next_time_inst = next_time_inst+self.delta_t_mpc
  328.    
  329.     def follow_new_EE_pose(self,
  330.                            queue_of_curr_state_qp_sol,  # Queue of in-coming current system-state (for feedback | @ 20 Hz)
  331.                            queue_of_ref_EE_states,      # Queue being sent to the QP-solver ( @ 20 Hz)
  332.                            queue_of_joint_vel,          # Queue to be sent out as joint-velocities ( @ 20 Hz)
  333.                            lock_state):                 # Pass the lock-state that will control the sequence in which the locked-variables are accessed
  334.         # This processes' job is to follow the demanded EE-state.
  335.         # Next time-instant for giving jint-velocities
  336.         next_time_inst = self.init_process_time+self.delta_t_ee
  337.         # Wait till the off-setted time has elapsed
  338.         while((time.time()-self.start_time)<self.init_process_time):
  339.             continue
  340.         while True:
  341.             try:
  342.                 '''
  343.                    try to get new_vel from the `queue of ref EE states`. get_nowait() function will
  344.                    raise `queue.Empty` exception if the queue is empty.
  345.                    queue(False) function would do the same task aswell.
  346.                '''
  347.                 # print(str(time.time()-self.start_time)+" Inside B_0: Before")
  348.                 # Finish getting data from queue
  349.                
  350.                 # Reference EE state (from the cart-MPC)
  351.                 ref_EE_state = queue_of_ref_EE_states.get_nowait()
  352.                 # print("ref_EE_state")
  353.                 # System joint position (from ROS-topics)
  354.                 # Acquire the lock
  355.                 # with lock_state:
  356.                 # lock_state.acquire()
  357.                 # Wait for the data to be loaded
  358.                 curr_joint_pos = queue_of_curr_state_qp_sol.get()
  359.                 # print("curr_joint_pos")
  360.                 # Release the lock
  361.                 # lock_state.release()
  362.                 # print(str(time.time()-self.start_time)+" Inside B_0: After")
  363.             except queue.Empty:
  364.                 # Need to import `queue` for this to work
  365.                 # Since no new velocities are in the queue, we will be brining the system to a stop.
  366.                 # New (terminal) joint velocity
  367.                 new_joint_vel = np.zeros((10,1))
  368.                 # print(str(time.time()-self.start_time)+" Inside B_2: Before")
  369.                 # Finish putting data in queue
  370.                 # Acquire the lock
  371.                 # with lock_state:
  372.                 # lock_state.acquire()
  373.                 # Send zero-velocity-command to robot.
  374.                 queue_of_joint_vel.put(new_joint_vel)
  375.                 # Release the lock
  376.                 # lock_state.release()
  377.                 # print(str(time.time()-self.start_time)+" Inside B_2: After")
  378.                 # Wait till a set amount of time has elapsed
  379.                 while((time.time()-self.start_time)<next_time_inst):
  380.                     continue
  381.                 # Update next-time
  382.                 # print(curr_joint_pos)
  383.                 break
  384.             else:
  385.                 '''
  386.                    if no exception has been raised, add the task completion
  387.                    message to task_that_are_done queue
  388.                '''
  389.                 # Reference EE position
  390.                 mat_w_G_10_ref = np.zeros((4,4))
  391.                 mat_w_G_10_ref[0,2] = -1
  392.                 mat_w_G_10_ref[1,1] = 1
  393.                 mat_w_G_10_ref[2,0] = 1
  394.                 mat_w_G_10_ref[0:3,3:4] = ref_EE_state[0:3,0:1]
  395.                 mat_w_G_10_ref[3,3] = 1
  396.                 # Reference FT wrench
  397.                 ft_wrench_ref = ref_EE_state[6:12,0:1]
  398.                 # Calculate new joint velocity
  399.                 # new_joint_vel = self.mob_man.run_QPSOL(curr_joint_pos,
  400.                 #                                        mat_w_G_10_ref,
  401.                 #                                        ref_EE_state[3:6,0:1],
  402.                 #                                        ft_wrench_ref-curr_joint_pos[10:16,0])
  403.                 new_joint_vel = np.zeros((10,1))
  404.                 new_joint_vel[9,0] = next_time_inst
  405.                 # Re-orient the MB linear-velocity from world-frame to local-frame
  406.                 new_joint_vel[0:2,0] = np.matmul(np.array([[np.cos(curr_joint_pos[2,0]),-np.sin(curr_joint_pos[2,0])],
  407.                                                            [np.sin(curr_joint_pos[2,0]),np.cos(curr_joint_pos[2,0])]]),
  408.                                                  new_joint_vel[0:2,0])
  409.                
  410.                 # print(str(time.time()-self.start_time)+" Inside B_1: Before")
  411.                 # Finish putting data in queue
  412.                 # Acquire the lock
  413.                 # with lock_state:
  414.                 # lock_state.acquire()
  415.                 # Send velocity-commands to Robot
  416.                 queue_of_joint_vel.put(new_joint_vel)
  417.                 # Release the lock
  418.                 # lock_state.release()
  419.                 # Wait till a set amount of time has elapsed
  420.                 # print(str(time.time()-self.start_time)+" Inside B_1: After")
  421.                 while((time.time()-self.start_time)<next_time_inst):
  422.                     continue
  423.                 # Update next-time
  424.                 next_time_inst = next_time_inst+self.delta_t_ee
  425.  
  426. if __name__ == '__main__':
  427.     # rospy.loginfo("Demanded BHand-pose-class initialising...")
  428.     # rospy.init_node('cart_mpc_action_server')
  429.     # rospy.loginfo(rospy.get_name() + ' start')
  430.  
  431.     # # Go to class functions that do all the heavy lifting. Do error checking.
  432.     # try:
  433.     #     BHand_dem_pose()
  434.     #     rospy.spin()
  435.     # except rospy.ROSInterruptException as e:
  436.     #     print(str(e))
  437.  
  438.     print("Starting test ...")
  439.     follow_bhand_pose = BHand_dem_pose()
  440.     follow_bhand_pose.goal_callback(goal=2)
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!