def __init__(self): print "Instantiating..." self._joint_vec = np.zeros(6) self._rate = rospy.Rate(10) self._pc = None self._pc_prev = np.array([0, 0, 0]) self._R_capsule_world = None self._capsule_lin_vel = None self._capsule_ang_vel = None self._pa = None self._pa_prev = np.array([0, 0, 0]) self._R_EPM_world = None self._fm = np.zeros(3) self._fm_prev = np.zeros(3) self._tm = np.zeros(3) self._tm_prev = np.zeros(3) self._coupling_status_msg = Float64() self._df_dx_msg = Vector3() self._df_dx_raw_msg = Vector3() self._dcapz_dEPMz_msg = Vector3() self._dcapz_dEPMz_raw_msg = Vector3() self._mag_wrench_msg = Wrench() # print "To publish: " , self._to_publish self._dipole = DipoleField(1.48, 1.48, SC.ACTUATOR_MAG_H, SC.CAPSULE_MAG_H) # print "Mag info:", SC.ACTUATOR_MAG_H # Initial conditions for Butterworth fulter fs = 100.0 nyq = 0.5 * fs cutoff = 0.5 order = 2 self._butter_zi_dfdx = np.zeros((3, order)) self._butter_params_dfdx = butter(order, cutoff / nyq, btype="lowpass", analog=False) self._butter_zi_dz = np.zeros((3, order)) self._butter_params_dz = butter(order, cutoff / nyq, btype="lowpass", analog=False) # Instantiate publishers and subscribers self._coupling_status_pub = rospy.Publisher("/MAC/coupling_status_topic", Float64, queue_size=1000) self._df_dx_pub = rospy.Publisher("/MAC/df_dx_topic", Vector3, queue_size=1000) self._df_dx_raw_pub = rospy.Publisher("/MAC/df_dx_raw_topic", Vector3, queue_size=1000) self._mag_wrench_pub = rospy.Publisher("/MAC/mag_wrench_topic", Wrench, queue_size=1000) self._dcapz_dEPMz_pub = rospy.Publisher("/MAC/dcapz_dEPMz_topic", Vector3, queue_size=1000) self._dcapz_dEPMz_raw_pub = rospy.Publisher("/MAC/dcapz_dEPMz_raw_topic", Vector3, queue_size=1000) self._robot_sub = rospy.Subscriber("/mitsubishi_arm/joint_states", JointState, self._robot_pose_cb) self._capsule_sub = rospy.Subscriber("/MAC/mac/odom", Odometry, self._capsule_pose_cb) # Robot info self._robot = self._wait_and_get_robot() self._tree = kdl_tree_from_urdf_model(self._robot) self._chain_to_magnet = self._tree.getChain("base_link", "magnet_center") self._fksolver_magnet = KDL.ChainFkSolverPos_recursive(self._chain_to_magnet) self._q_cur = KDL.JntArray(self._chain_to_magnet.getNrOfJoints()) # Initialize timer pub_timer = rospy.Timer(rospy.Duration(0.01), self._determine_coupling_state)
class DecouplingObserver(object): def __init__(self): print "Instantiating..." self._joint_vec = np.zeros(6) self._rate = rospy.Rate(10) self._pc = None self._pc_prev = np.array([0, 0, 0]) self._R_capsule_world = None self._capsule_lin_vel = None self._capsule_ang_vel = None self._pa = None self._pa_prev = np.array([0, 0, 0]) self._R_EPM_world = None self._fm = np.zeros(3) self._fm_prev = np.zeros(3) self._tm = np.zeros(3) self._tm_prev = np.zeros(3) self._coupling_status_msg = Float64() self._df_dx_msg = Vector3() self._df_dx_raw_msg = Vector3() self._dcapz_dEPMz_msg = Vector3() self._dcapz_dEPMz_raw_msg = Vector3() self._mag_wrench_msg = Wrench() # print "To publish: " , self._to_publish self._dipole = DipoleField(1.48, 1.48, SC.ACTUATOR_MAG_H, SC.CAPSULE_MAG_H) # print "Mag info:", SC.ACTUATOR_MAG_H # Initial conditions for Butterworth fulter fs = 100.0 nyq = 0.5 * fs cutoff = 0.5 order = 2 self._butter_zi_dfdx = np.zeros((3, order)) self._butter_params_dfdx = butter(order, cutoff / nyq, btype="lowpass", analog=False) self._butter_zi_dz = np.zeros((3, order)) self._butter_params_dz = butter(order, cutoff / nyq, btype="lowpass", analog=False) # Instantiate publishers and subscribers self._coupling_status_pub = rospy.Publisher("/MAC/coupling_status_topic", Float64, queue_size=1000) self._df_dx_pub = rospy.Publisher("/MAC/df_dx_topic", Vector3, queue_size=1000) self._df_dx_raw_pub = rospy.Publisher("/MAC/df_dx_raw_topic", Vector3, queue_size=1000) self._mag_wrench_pub = rospy.Publisher("/MAC/mag_wrench_topic", Wrench, queue_size=1000) self._dcapz_dEPMz_pub = rospy.Publisher("/MAC/dcapz_dEPMz_topic", Vector3, queue_size=1000) self._dcapz_dEPMz_raw_pub = rospy.Publisher("/MAC/dcapz_dEPMz_raw_topic", Vector3, queue_size=1000) self._robot_sub = rospy.Subscriber("/mitsubishi_arm/joint_states", JointState, self._robot_pose_cb) self._capsule_sub = rospy.Subscriber("/MAC/mac/odom", Odometry, self._capsule_pose_cb) # Robot info self._robot = self._wait_and_get_robot() self._tree = kdl_tree_from_urdf_model(self._robot) self._chain_to_magnet = self._tree.getChain("base_link", "magnet_center") self._fksolver_magnet = KDL.ChainFkSolverPos_recursive(self._chain_to_magnet) self._q_cur = KDL.JntArray(self._chain_to_magnet.getNrOfJoints()) # Initialize timer pub_timer = rospy.Timer(rospy.Duration(0.01), self._determine_coupling_state) def _wait_and_get_robot(self): t_init = rospy.Time.now() t_timeout = rospy.Duration(5) # 10 seconds while rospy.Time.now() - t_init < t_timeout: try: robot = URDF.from_parameter_server("/mitsubishi_arm/robot_description") print "*** Obtained robot_description ***" return robot except KeyError: print "Key error." rospy.sleep(rospy.Duration(1)) raise KeyError("robot_description was not found") def _capsule_pose_cb(self, capsule_pose_data): # Save past data to compute rate of change later self._pc_prev = self._pc self._R_capsule_world_prev = self._R_capsule_world # Get new capsule pose pos = capsule_pose_data.pose.pose.position quat = capsule_pose_data.pose.pose.orientation linear = capsule_pose_data.twist.twist.linear angular = capsule_pose_data.twist.twist.angular self._pc = np.array([pos.x, pos.y, pos.z]) self._R_capsule_world = quaternion_matrix([quat.x, quat.y, quat.z, quat.w])[:3, :3] self._capsule_lin_vel = np.array([linear.x, linear.y, linear.z]) self._capsule_ang_vel = np.array([angular.x, angular.y, angular.z]) # print "Capsule Position: ", self._pc # print "Capsule Orientation: ", self._R_capsule_world # print " ---------- " return None def _robot_pose_cb(self, robot_jnt_data): q_now = KDL.JntArray(6) cur_frame = KDL.Frame() for i in range(0, 6): q_now[i] = robot_jnt_data.position[i] self._fksolver_magnet.JntToCart(q_now, cur_frame) self._pa = kdl_to_numpy(cur_frame.p) self._R_EPM_world = kdl_to_numpy(cur_frame.M) # print "EE Position: " , self._pa # print "EE Orientation: ", self._R_EPM_world # print " ---------- " return None def _determine_coupling_state(self, ev): # if no ev, get error "takes exactly 1 arg" if (self._pc is None) or (self._pa is None) or (self._R_EPM_world is None) or (self._R_capsule_world is None): print "Lacking data. Cannot compute. Waiting..." return print " *** Got all necessary data. Ready for decoupling analysis. *** " ### # Decoupling analysis will consider the following: # (1) Increase in magnitude of df/dx # (2) Minimal threshold of magnetic force applied # (3) Increased z-motion of capsule without significant z motion of EPM ### # Compute new force and torque self._dipole.set_mag_poses(self._pa, self._pc, self._R_EPM_world, self._R_capsule_world) self._fm = self._dipole.mag_force() self._tm = self._dipole.mag_torque() # (1) Get df/dx self._get_dfdx_data() # (2) Check if minimal threshold of fm is met if abs(self._fm[2]) < 0.3: self._coupling_status_msg = 0 # NOT coupled else: self._coupling_status_msg = 1 # Coupled # (3) Get d_capz/d_EPMz self._get_dcapz_dEPMz() # Publish results self._publish_results() # To view what is being published, in another window: "rostopic echo -c /MAC/decouplilng_status_topic" . the -c clears the screen, can ommit. # print "I published" # rospy.sleep(.001) return None # ************************************* def _get_dcapz_dEPMz(self): # Current positions pa = self._pa pc = self._pc # dc/dEE dpa = pa - self._pa_prev dpc = pc - self._pc_prev self._dcapz_dEPMz_raw = np.array([dpc[0] / dpa[0], dpc[1] / dpa[1], dpc[2] / dpa[2]]) # Filter b, a = self._butter_params_dz self._dcapz_dEPMz, self._butter_zi_dz = lfilter( b, a, self._dcapz_dEPMz_raw.reshape(-1, 1), axis=1, zi=self._butter_zi_dz ) self._pa_prev = pa self._pc_prev = pc return None def _get_dfdx_data(self): # Compute raw derivative df/dx self._dfm = self._fm - self._fm_prev self._dpc = self._pc - self._pc_prev self._df_dx_raw = np.array( [self._dfm[0] / self._dpc[0], self._dfm[1] / self._dpc[1], self._dfm[2] / self._dpc[2]] ) # Save these values self._fm_prev = self._fm self._tm_prev = self._tm # idk_what_this_is = self._dipole.mag_field() # Apply Butterworth filter for df/dx b, a = self._butter_params_dfdx self._df_dx, self._butter_zi_dfdx = lfilter( b, a, self._df_dx_raw.reshape(-1, 1), axis=1, zi=self._butter_zi_dfdx ) return None def _publish_results(self): # Publish coupling status (std_msgs/Float64) self._coupling_status_pub.publish(self._coupling_status_msg) # Publish raw df/dx data (geometry_msgs/Vector3) self._df_dx_raw_msg.x = self._df_dx_raw[0] self._df_dx_raw_msg.y = self._df_dx_raw[1] self._df_dx_raw_msg.z = self._df_dx_raw[2] self._df_dx_raw_pub.publish(self._df_dx_raw_msg) # Publish filtered df/dx data (geometry_msgs/Vector3) self._df_dx_msg.x = self._df_dx[0] self._df_dx_msg.y = self._df_dx[1] self._df_dx_msg.z = self._df_dx[2] self._df_dx_pub.publish(self._df_dx_msg) # Publish magnetic wrench data (geometry_msgs/Wrench) self._mag_wrench_msg.force.x = self._fm[0] self._mag_wrench_msg.force.y = self._fm[1] self._mag_wrench_msg.force.z = self._fm[2] self._mag_wrench_msg.torque.x = self._tm[0] self._mag_wrench_msg.torque.y = self._tm[1] self._mag_wrench_msg.torque.z = self._tm[2] self._mag_wrench_pub.publish(self._mag_wrench_msg) # Publish raw d_EPMz/d_capsulez data (geometry_msgs/Vector3) self._dcapz_dEPMz_msg.x = 0 self._dcapz_dEPMz_msg.y = 0 self._dcapz_dEPMz_msg.z = self._dcapz_dEPMz[2] self._dcapz_dEPMz_pub.publish(self._dcapz_dEPMz_msg) # Publish raw d_EPMz/d_capsulez data (geometry_msgs/Vector3) self._dcapz_dEPMz_raw_msg.x = 0 self._dcapz_dEPMz_raw_msg.y = 0 self._dcapz_dEPMz_raw_msg.z = self._dcapz_dEPMz[2] self._dcapz_dEPMz_raw_pub.publish(self._dcapz_dEPMz_raw_msg) return None def stop(self): pass