def __init__(self): # Variables self.contact = False self.first_config = False self.curr = np.array([0.,0.,0.,0.,0.,0.]) self.curr_buff = np.array([0.,0.,0.,0.,0.,0.]) self.pos_buff = np.array([0.,0.,0.,0.,0.,0.]) K_mot = np.array([0.,1.122861,0.695228,0.,0.,0.]) # Change K self.param = np.array([[0.,0.,0.,0.,0.,0.],[0.,0.,0.,0.,0.,0.],[0.,0.,0.,0.,0.,0.]]) # initial is 0 self.z = np.array([0.,0.,0.,0.,0.,0.]) # initial is 0 self.time = [0] self.qdd = np.array([0.,0.,0.,0.,0.,0.]) # Generic configuration np.set_printoptions(precision=4, suppress=True) # Read configuration parameters self.ns = rospy.get_namespace() robot_name = self.ns.replace('/','') if len(robot_name) == 0: #~ robot_name = 'right' rospy.logerr('Namespace is required by this script.') return js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 125.0) self.dt = 1. / js_rate # Populate the OpenRAVE environment and robot rospack = rospkg.RosPack() models_dir = rospack.get_path('denso_openrave') env = Environment() denso_base = env.ReadRobotXMLFile(models_dir + '/objects/denso_base.kinbody.xml') denso_base.SetName('denso_base') env.Add(denso_base) robot = env.ReadRobotXMLFile(models_dir + '/robots/denso_ft_sensor_handle.robot.xml') robot.SetName(robot_name) env.Add(robot) self.rave = Manipulator(env, robot, 'denso_ft_sensor_handle') self.rave.EnableCollisionChecker() RaveSetDebugLevel(DebugLevel.Fatal) # enable OpenRave GUI #~ env.SetViewer('QtCoin') # Denso Controllers self.denso = JointPositionController(robot_name) self.rave.SetDOFValues(self.denso.get_joint_positions()) self.rate = rospy.Rate(js_rate) # Subscribe to FT sensor and joint state rospy.Subscriber('/%s/ft_sensor/raw' % robot_name, WrenchStamped, self.ft_sensor_cb) rospy.Subscriber('/%s/joint_states' % robot_name, JointState, self.joint_states_cb) rospy.Subscriber('/%s/ft_sensor/diagnostics' % robot_name, DiagnosticArray, self.diagnostics_cb) rospy.loginfo('Waiting for [/%s/ft_sensor/] topics' % robot_name) while not rospy.is_shutdown(): rospy.sleep(0.1) if hasattr(self, 'wrench') and hasattr(self, 'ft_status') and hasattr(self, 'effort') and hasattr(self, 'position'): break rospy.loginfo('Succefully connected to [/%s/ft_sensor/] topics' % robot_name) rospy.loginfo('Succefully connected to [/%s/joint_states/] topics' % robot_name) # Move to first pre-contact position Tprecontact = tr.euler_matrix(0, -np.pi/2 - np.pi/4 + 10/180.0*np.pi, 0) Tprecontact[:3,3] = np.array([0.3, 0., -0.018]) self.move_robot(Tprecontact) # Move to second pre-contact position Tprecontact[0,3] -= 0.01 self.move_robot(Tprecontact) rospy.sleep(0.5) # Move until contact is happening (speed = 0.25 cm/s) q = self.rave.GetDOFValues() J = np.zeros((6,6)) curr_ref = self.curr[-1] dx = np.array([-0.002*self.dt, 0., 0., 0., 0., 0.]) dq_buf = np.array([0.,0.,0.,0.,0.,0.]) Wref = np.array(self.wrench) contact_force = np.array([1.0,10.0,5.0,0.0]),num = 0 while not rospy.is_shutdown(): if self.ft_status == DiagnosticStatus().OK: J[:3,:] = rave.manipulator.CalculateJacobian() J[3:,:] = rave.manipulator.CalculateAngularVelocityJacobian() dq = np.dot(np.linalg.pinv(J), dx) self.qdd = (dq - dq_buf) / self.dt / self.dt dq_buf = dq q += dq self.rave.SetDOFValues(q) self.rave.SetDOFVelocities(dq/self.dt) self.denso.set_joint_positions(q) self.rate.sleep() # Calculate F/T Tor = [] for (i,Ki) in enumerate(K_mot): Tor.append(Ki*(self.curr[i] - curr_ref[i]) + self.param[2,i] + self.z[i]) global_ft_calc = - np.dot(np.linalg.pinv(J.T), Tor) T = self.rave.GetEndEffectorTransform(self.rave.GetDOFValues()) force_frame_transform bXeF = force_frame_transform(T) local_ft_calc = np.dot(np.linalg.pinv(bXeF), global_ft_calc) print 'local_ft_calc: ' for (i,loc_i) in enumerate(local_ft_calc): print('%f ' %loc_i) print '\n' # FT sensor W = np.array(self.wrench) F = W[:3] - Wref[:3] if ((np.linalg.norm(F) > contact_force[num]) and (num<2)) or ((np.linalg.norm(F) < contact_force[num]) and (num>=2)): rospy.loginfo('Contact %d detected' %(num+1)) if(num==0): self.contact = True self.first_config = True num +=1 if(num==2): dx[0] = -dx[0] rospy.loginfo('Last local_ft_calc:') local_ft_calc rospy.loginfo('Last F:') F rospy.loginfo('Move to next contact point') rospy.sleep(3) continue else: rospy.loginfo('FT Sensor failed')
J[:3,:] = rave.manipulator.CalculateJacobian() J[3:,:] = rave.manipulator.CalculateAngularVelocityJacobian() global_ft_calc.append(np.linalg.solve(J.T, Tor)) ext_jnt_torque.append(np.array(Tor)) elif topic.find('/ft_sensor/raw') >= 0: force = np.array([msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z]) torque = np.array([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z]) ft_queue.append(-np.hstack((force,torque))) if (len(ft_queue) < js_window) or not js_alive: continue wrench_queue = ft_filter(np.array(ft_queue)) #~ wrench_queue = np.array(ft_queue) T = rave.GetEndEffectorTransform(jnt_position[-1]) force_frame_transform bXeF = force_frame_transform(T) global_wrench = np.dot(bXeF, wrench_queue[-1]) time_ft_list.append(t.to_sec()) time_ft = np.array(time_ft_list[:]) - time_ft_list[0] if(time_ft[-1] < t_precon): wrench_offset = global_wrench global_wrench = np.array([0.,0.,0.,0.,0.,0.]) else: global_wrench -= wrench_offset ext_tor.append(np.dot(J.T, global_wrench)) wrench.append(global_wrench) bag.close() wrench = np.array(wrench) mot_torque = np.array(mot_torque)