def __init__(self): # 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: rospy.logerr('Namespace is required by this script.') robot_name = 'right' js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 125.0) # Populate the OpenRAVE environment and robot rospack = rospkg.RosPack() models_dir = rospack.get_path('denso_openrave') # load yaml yaml_path = models_dir + '/worlds/stefan_chair_env.yaml' envdict = yaml.load(file(yaml_path, 'r'))['environment'] openRave = YamlEnv(envdict) # Get the model right = openRave.models['right'] bars = add_connecting_bars(openRave) # enable OpenRave GUI env = openRave.get_environment() env.SetViewer('QtCoin') # Attach kinematics manipulator rave = Manipulator(env, right, 'denso_ft_sensor_handle') rave.EnableCollisionChecker() RaveSetDebugLevel(DebugLevel.Fatal) # Denso Controllers denso = 0 #~ denso = JointPositionController(robot_name) #~ rave.SetDOFValues(denso.get_joint_positions()) rate = rospy.Rate(js_rate) # Subscribe to FT sensor #~ rospy.Subscriber('/%s/ft_sensor/raw' % robot_name, WrenchStamped, self.ft_sensor_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'): #~ break #~ rospy.loginfo('Succefully connected to [/%s/ft_sensor/] topics' % robot_name) # Move to first pre-contact position tdelta = 1/js_rate t = 0.0 tend = 5.0 q = np.array([0.,0.,0.,0.,0.,0.]) while (t<tend): q[1] = t / tend * np.pi/3 q[2] = t / tend * np.pi/6 rave.SetDOFValues(q) #~ denso.set_joint_positions(q) t += tdelta rate.sleep() # compute the current data here while(True):
# Initialize node optimize_part = False # True : optimize the graph testing = False # True: to test the parameter with actual data, False : Search for optimization node_name = 'data_13' # Connected to the corresponding rosbag sample_n = 1 # change the sample in here (must be equal or less than 'sample' in param.yaml) rospy.init_node(node_name) rospy.loginfo('Starting [%s] node' % node_name) js_rate = 250.0 rate = rospy.Rate(js_rate) # Load denso_openrave path rospack = rospkg.RosPack() models_dir = rospack.get_path('denso_openrave') # Load YAML yaml_path = models_dir + '/worlds/stefan_chair_env.yaml' envdict = yaml.load(file(yaml_path, 'r'))['environment'] openRave = YamlEnv(envdict) # Get environment env = openRave.get_environment() #~ env.SetViewer('QtCoin') # Add right arm right = openRave.models['right'] rave = Manipulator(env, right, 'denso_ft_sensor_handle') rave.EnableCollisionChecker() RaveSetDebugLevel(DebugLevel.Fatal) # Initialize Filter parameters np.set_printoptions(precision=4, suppress=True) order = read_parameter('~lowpass_filter_order', 3) cutoff = read_parameter('~lowpass_cutoff', 5) js_window = int(js_rate/5) js_filter = ButterLowPass(cutoff, js_rate, order=order) ft_filter = ButterLowPass(cutoff, js_rate, order=order)
def __init__(self): # 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: rospy.logerr('Namespace is required by this script.') robot_name = 'right' #~ return js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 250.0) # Populate the OpenRAVE environment and robot rospack = rospkg.RosPack() models_dir = rospack.get_path('denso_openrave') # Load YAML yaml_path = models_dir + '/worlds/stefan_chair_env.yaml' envdict = yaml.load(file(yaml_path, 'r'))['environment'] openRave = YamlEnv(envdict) # Get environment env = openRave.get_environment() # Add right arm right = openRave.models['right'] self.rave = Manipulator(env, right, '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()) denso = 0 self.rate = rospy.Rate(js_rate) # Move to first pre-contact position q = self.rave.GetDOFValues() T0 = self.rave.GetEndEffectorTransform(q) # get end effector T0[0:3,3] += [0.255, 0.23, 0.0] T0[2,3] = 0.034 Tprecon = tr.euler_matrix(0, np.pi, np.pi/2) Tprecon[:3,3] = T0[:3,3] self.move_robot(Tprecon) # subscribe to force_state #~ rospy.sleep(1.0) #~ rospy.Subscriber('/%s/force_state' % robot_name, ForceState, self.est_force_cb) #~ rospy.loginfo('Waiting for [/%s/force_state/] topics' % robot_name) #~ while not rospy.is_shutdown(): #~ rospy.sleep(0.1) #~ if hasattr(self, 'wrench'): #~ break #~ rospy.loginfo('Succefully connected to [/%s/force_state/] topics' % robot_name) raw_input('enter to continue') rospy.loginfo('Moving to contact') q = self.rave.GetDOFValues() dt = 1./js_rate dx = np.array([0., 0., -0.0005*dt, 0., 0., 0.]) J = np.zeros((6,6)) #~ Wref = np.array(self.wrench) while not rospy.is_shutdown(): J[:3,:] = self.rave.manipulator.CalculateJacobian() J[3:,:] = self.rave.manipulator.CalculateAngularVelocityJacobian() dq = np.linalg.solve(J, dx) q += dq self.rave.SetDOFValues(q) #~ denso.set_joint_positions(q) self.rate.sleep() #~ W = np.array(self.wrench) #~ F = W[:3] - Wref[:3] if (np.linalg.norm(F) > 20.): rospy.loginfo('Contact detected') break rospy.loginfo('stay for a while....') rospy.sleep(5.0) raw_input('enter to start force control') # Start force control q = self.rave.GetDOFValues() Fcontrol = 20.0 Kp = 0.00001 Kv = 0.0001 Fe = collections.deque(maxlen=10) timeout = 30 initime = rospy.get_time while not rospy.is_shutdown() and (rospy.get_time()-initime)<timeout: F = W[2] - Wref[2] Fe.append(Fcontrol - F) if(len(Fe)<10): continue dFe = np.diff(np.array(Fe), n=1, axis=0)* js_rate dx[2] = Kp*np.array(Fe)[-1] + Kv*dFe[-1] J[:3,:] = self.rave.manipulator.CalculateJacobian() J[3:,:] = self.rave.manipulator.CalculateAngularVelocityJacobian() dq = np.linalg.solve(J, dx) q += dq self.rave.SetDOFValues(q) #~ denso.set_joint_positions(q) self.rate.sleep() t += dt rospy.loginfo('Finish the experiment....') rospy.sleep(5.0) self.move_robot(Tprecon)