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: robot_name = 'right' rospy.logerr('Namespace is required by this script.') self.js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 250.0) # Create low pass filter order = read_parameter('~lowpass_filter_order', 3) cutoff = read_parameter('~lowpass_cutoff', 5) self.js_window = int(self.js_rate/5) self.js_filter = ButterLowPass(cutoff, self.js_rate, order=order) self.mot_tor_filter = ButterLowPass(1.0, self.js_rate, order=5) self.js_queue = collections.deque(maxlen=self.js_window ) self.mot_tor_queue = collections.deque(maxlen=self.js_window ) self.mot_torque_avg = collections.deque(maxlen=self.js_window ) self.mot_torque = collections.deque(maxlen=self.js_window ) self.jnt_position = collections.deque(maxlen=self.js_window ) self.Tmcg = collections.deque(maxlen=(self.js_window) ) # Variables self.contact = False self.param = np.array([[0.989438, 1.146991, 0.501995, 0.173604, 0.246897, 0.083080],[0.656342, 0.710369, 0.352390, -0.407913, 0.181837, -0.136770],[0.076302, 0.024945, 0.069612, 0.957069, 0.032772, 0.523249]]) self.z = np.array([10.340966, 16.791938, 13.832300, -1.771149, 12.406588, 8.797445]) self.Tor = np.array([0.,0.,0.,0.,0.,0.]) self.i = 1 self.k = 0 # 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) self.rate = rospy.Rate(self.js_rate) # Subscribe to joint state rospy.Subscriber('/%s/joint_states' % robot_name, JointState, self.joint_states_cb) rospy.loginfo('Waiting for [/%s/joint_states] topics' % robot_name) self.js_alive = False while not (self.js_alive): rospy.sleep(0.1) if rospy.is_shutdown(): return rospy.loginfo('Succefully connected to [/%s/joint_states/] topics' % robot_name) while(True): continue
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):
def __init__(self, sim=False): """ Constructor Construct all 3D world's information. Then constructs both planner and executor object. """ # Get parameters robot_name = read_parameter('~robot_name', 'left') table_id = int(read_parameter('~table_id', 2)) # Init openrave env self.env = Environment() if sim: self.env.SetViewer('qtcoin') # Get the robot rospack = rospkg.RosPack() path = rospack.get_path('light_drawing') + '/robots/denso_light.robot.xml' self.robot = self.env.ReadRobotXMLFile(path) self.robot.SetName(robot_name) manip_name = 'light' # Get the objects table_path = rospack.get_path('denso_openrave') + '/objects/table.kinbody.xml' table = self.env.ReadKinBodyXMLFile(table_path) Ttable = np.eye(4) Ttable[:3, 3] = np.array([0.7, 0.3, 0.126]) # Ttable[2, 3] += 9.0e-3 # Uncomment if using the acrylic printing board table.SetTransform(Ttable) # Add the objects to the environment. self.env.AddRobot(self.robot) self.env.AddKinBody(table) self.sim = sim # Init planner and executor self.pl = PrintPlanner(self.env, robot_name, manip_name, hardware=False) self.ex = DensoRobot(self.env, robot_name, manip_name) self.ex.ScaleVelocityLimits(1) self.ex.ScaleAccelerationLimits(1) # Advertise the service srv_name = 'draw_light' s = rospy.Service(srv_name, DrawingRequest, self.handle_request) rospy.loginfo("Service %s is ready" % srv_name)
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') 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()) denso = 0 self.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 q = self.rave.GetDOFValues() q[4] = np.pi/2 q[2] = np.pi/6 Tprecontact = self.rave.GetEndEffectorTransform(q) self.move_robot(Tprecontact) while not rospy.is_shutdown(): continue
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')
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') 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()) denso = 0 self.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) #~ joint_num = 2 a0 = np.pi/6 a = np.pi/65 freq = 0.3 w = 2.0*np.pi* freq dt = 1./js_rate t = 0. q = self.rave.GetDOFValues() #~ q[0] = np.pi/2 q[1] = np.pi/2 #~ q[2] = np.pi/2 - q[1] self.move_robot(self.rave.GetEndEffectorTransform(q)) #~ while(q[2]<np.pi/2): #~ q[2] += 0.1*dt #~ q[3] += 0.1*dt #~ self.rave.SetDOFValues(q) #~ self.denso.set_joint_positions(q) #~ self.rate.sleep() rospy.sleep(5.0) q = self.rave.GetDOFValues() a1 = 5./180.*np.pi f0 = 0.1 B = 0.05 while(t < 5./freq): q[joint_num] += a1*np.cos(2*np.pi*(f0*t+B*t*t))*2*np.pi*(f0+2*B*t)*dt #~ q[joint_num] += (a*np.sin(w*t) + w*(a0+a*t)*np.cos(w*t))*dt self.rave.SetDOFValues(q) #~ self.denso.set_joint_positions(q) self.rate.sleep() t+=dt
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') 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) rave = Manipulator(env, robot, 'denso_ft_sensor_handle') rave.EnableCollisionChecker() RaveSetDebugLevel(DebugLevel.Fatal) # enable OpenRave GUI env.SetViewer('QtCoin') # Denso Controllers #~ denso = JointPositionController(robot_name) #~ rave.SetDOFValues(denso.get_joint_positions()) denso = 0 rate = rospy.Rate(js_rate) # Move to first pre-contact position Tprecontact = tr.euler_matrix(0, -np.pi/2 - np.pi/4, 0) Tprecontact[:3,3] = np.array([0.3, 0., -0.025]) self.move_robot(denso, rave, Tprecontact, rate) # Move to second pre-contact position Tprecontact[0,3] -= 0.01 self.move_robot(denso, rave, Tprecontact, rate) rospy.sleep(0.5) # 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 until contact is happening (speed = 0.25 cm/s) q = rave.GetDOFValues() dt = 1. / js_rate dx = np.array([-0.0025*dt, 0, 0]) #~ Wref = np.array(self.wrench) while not rospy.is_shutdown(): if True or self.ft_status == DiagnosticStatus().OK: J = rave.manipulator.CalculateJacobian() dq = np.dot(np.linalg.pinv(J), dx) q += dq rave.SetDOFValues(q) #~ denso.set_joint_positions(q) rate.sleep() # FT sensor #~ W = np.array(self.wrench) #~ F = W[:3] - Wref[:3] if (rave.GetEndEffectorTransform(q)[0,3] < 0.25):#(np.linalg.norm(F) > 1.): rospy.loginfo('Contact detected') break else: rospy.loginfo('FT Sensor failed') # Apply sinusoidal motion profile using jacobian velocity #~ x(t) = A * math.sin(w*t - (np.pi / 2.)) for freq in np.array([0.1 , 0.2 , 0.3 , 0.4, 0.5]): # 0.01 , 0.03 , 0.1 , 0.3, 1.0 A = 0.001 # 0.028 degree w = 2.*np.pi*freq t = 0 rospy.loginfo('Applying motion profile with freq %f Hz' %freq) while (t < 4./freq) and not rospy.is_shutdown(): if True or self.ft_status == DiagnosticStatus().OK: J = rave.manipulator.CalculateJacobian() dq = -w * A*math.cos(w*t - (np.pi / 2.)) * dt q[4] += dq #~ denso.set_joint_positions(q) rate.sleep() rave.SetDOFValues(q) t += dt else: rospy.loginfo('FT Sensor failed') rospy.loginfo('Finished motion profile freq %f Hz' %freq) rospy.sleep(1)
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) mot_tor_filter = ButterLowPass(1.0, js_rate, order=5) # Process rosbag data directory = os.path.expanduser('~/data/rosbag_data/') for bag_file in os.listdir(directory): if not (os.path.splitext(bag_file)[0] == node_name): continue bag = rosbag.Bag(os.path.join(directory, bag_file)) bag_info = yaml.load(bag._get_yaml_info()) # The two topics must have the same number of messages
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)
def __init__(self): # Generic configuration np.set_printoptions(precision = 4, suppress=True) # Read configuration parameters left_robot_name = read_parameter('~left_robot_name', 'left') self.js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % left_robot_name, 125.0) self.table_id = int(read_parameter('~table_id', 2)) self.stick_id = int(read_parameter('~stick_id', 3)) # Populate the OpenRAVE environment rospack = rospkg.RosPack() models_dir = rospack.get_path('denso_openrave') env = Environment() left_robot = env.ReadRobotXMLFile(models_dir + '/robots/denso_ft_gripper_with_base.robot.xml') left_robot.SetName('left_robot') table = env.ReadKinBodyXMLFile(models_dir + '/objects/table.kinbody.xml') pin = env.ReadKinBodyXMLFile(models_dir + '/objects/pin.kinbody.xml') wood_stick = env.ReadKinBodyXMLFile(models_dir + '/objects/wood_stick.kinbody.xml') env.Add(left_robot) env.Add(table) env.Add(pin) env.Add(wood_stick) self.left_rave = Manipulator(env, left_robot, 'denso_ft_sensor_gripper') #Change the limits of the gripper in the model so that the model in openrave can close until touch the pin [lowerlimits, upperlimits] = left_robot.GetDOFLimits() upperlimits[-1] = np.deg2rad(43) left_robot.SetDOFLimits(lowerlimits, upperlimits) RaveSetDebugLevel(DebugLevel.Fatal) # Place elements in OpenRAVE using TF information self.tf_updater = TFtoOpenRAVE(env, self.global_frame) self.tf_updater.add_body('%s/base_link' % left_robot_name, left_robot, duration=5.) self.tf_updater.add_body('rigid_body_%d' % self.table_id, table, duration=5.) self.tf_updater.add_body('rigid_body_%d' % self.stick_id, wood_stick) self.tf_updater.start() env.SetViewer('QtCoin') # Denso Controllers self.left_denso = JointPositionController(left_robot_name) self.left_rave.SetDOFValues(self.left_denso.get_joint_positions()) self.left_rave.EnableCollisionChecker() rospy.Subscriber('/optitrack_rigid_bodies', RigidBodyArray, self.optitrack_cb) rospy.loginfo('Reading object poses from optitrack...') #Set the pin into the position-in openrave env (grasped by the gripper) q_denso = self.left_denso.get_joint_positions() T_denso = self.left_rave.GetEndEffectorTransform(q_denso) c90 = np.cos(np.pi/2) s90 = np.sin(np.pi/2) T_pininhand = np.array( [[c90,-s90, 0, 0.015], [s90, c90, 0, 0 ], [0 ,0 , 1, 0.005 ], [0 ,0 , 0, 1 ]]) pin.SetTransform(np.dot(T_denso, T_pininhand)) self.left_rave.CloseGripper() self.left_rave.robot.WaitForController(5) with self.left_rave.env: self.left_rave.robot.Grab(pin) # Position-based explicit force control self.state_alive = False rospy.Subscriber('/%s/endpoint_state' % left_robot_name, EndpointState, self.endpoint_state_cb) rospy.loginfo('Waiting for [/%s/endpoint_state] topic' % left_robot_name) self.state_alive = False while not self.state_alive and not rospy.is_shutdown(): rospy.sleep(0.1) rospy.Subscriber('/%s/ft_sensor/diagnostics' % left_robot_name, DiagnosticArray, self.diagnostics_cb) rospy.loginfo('Waiting for [/%s/ft_sensor/] topics' % left_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' % left_robot_name) # Real code that make the robot move exploration_data = [] # TODO: identify 3 points of 3 planes to explore (will be developed further, using region growing or planar detector..) point1 = np.array([0,0,0,1]) direction = np.array([0,0,-1]) ex1 = [point1,direction] exploration_data.append(ex1) point2 = np.array([-0.03,-0.01,-0.05,1]) direction2 = np.array([1,0,0]) ex2 = [point2,direction2] exploration_data.append(ex2) point3 = np.array([0., -0.01,-0.05,1]) direction3 = np.array([0,1,0]) ex3 = [point3,direction3] exploration_data.append(ex3) #TODO: Estimate the pin position do_the_pin_estimation = raw_input('Press E if you want to estimate the pin position ') if do_the_pin_estimation == 'e' or do_the_pin_estimation == 'E': estimate_hand2pin() else: self.T_hand2pin = np.eye(4) self.T_hand2pin[:3,3] = np.array([-0.03,0,-0.015]) # TODO: Collect measurements o_p = 2e-3 o_n = 15/180.0*np.pi Tws = wood_stick.GetTransform() Tws[:3,3] = Tws[:3,3] + np.array([0.00,0.0,0.017]) collecteddata = [] self.forcelist = [] for i in range(3): waitkey = raw_input("Press Enter to collect new data point...") num_points = 0 point = np.dot(Tws,exploration_data[i][0]) direction = np.dot(Tws[:3,:3],exploration_data[i][1]) touched = self.touch_surface(point[:3], direction, 0.4 , 0.03) if not((touched==np.zeros(3)).all()): num_points +=1 d = [touched, direction, o_p,o_n] collecteddata.append(d) print "Found ", num_points, " point(s) on the surface\n" print "New point found: ", touched ,"\n" else: print "Not touched! Moving to another exploration position" while num_points < 2: #TODO: plan new point waitkey = raw_input("Press Enter to continue...") radius = np.random.uniform(0.01,0.015) rand_perpendicular_vector = perpendicular_vector(direction)/np.linalg.norm(perpendicular_vector(direction)) new_point = point[:3] + radius*rand_perpendicular_vector new_direction = direction #TODO: approach and touch touched = self.touch_surface(new_point, new_direction, 0.4 , 0.03) #TODO: update num_points if not((touched==np.zeros(3)).all()): num_points +=1 d = [touched, new_direction, o_p,o_n] collecteddata.append(d) print "Found ", num_points, " point(s) on the surface\n" print "New point found: ", touched ,"\n" else: print "Not touched! Moving to another exploration position" if rospy.is_shutdown(): return rospy.loginfo("COLLECT MEASUREMENT DONE") print collecteddata # TODO: Insert the pin into the hole rospy.loginfo("Move the pin to the correct position and start inserting") delta0 = 22 dim = 6 # 6 DOFs ptcl0 = ParticleFilterLib.Particle(Tws[:3,3], tr.euler_from_matrix(Tws[:3,:3])) V0 = ParticleFilterLib.Region([ptcl0], delta0) M = 6 # No. of particles per delta-neighbohood delta_desired = 0.9 # Terminal value of delta woodstick = env.ReadKinBodyXMLFile('wood_stick.xml') env.Add(woodstick,True) env.Remove(wood_stick) list_particles, weights = ParticleFilterLib.ScalingSeries(V0, collecteddata, M, delta0, delta_desired, dim, body = woodstick, env = env) est = ParticleFilterLib.VisualizeParticles(list_particles, weights, env= env, body=woodstick, showestimated = True) # print "Real transf. ", Tws # self.touch_surface(est[:3,3], direction, 0.4 , 0.03) rospy.loginfo("DONE")