def __init__(self): """ """ self.lib = EtherCAT_Hand_Lib() self.joint_name = "FFJ3" if rospy.has_param("~joint_name"): self.joint_name = rospy.get_param("~joint_name") self.min = 0.0 if rospy.has_param("~min"): self.min = rospy.get_param("~min") self.max = 1.55 if rospy.has_param("~max"): self.max = rospy.get_param("~max") self.nb_repetition = 2 if rospy.has_param("~nb_repetition"): self.nb_repetition = rospy.get_param("~nb_repetition") print " nb repetition: ", self.nb_repetition self.sign = -1 if rospy.has_param("~sign"): self.sign = rospy.get_param("~sign") rospy.loginfo("sign: " + str(self.sign)) self.publisher = rospy.Publisher( "/sr_friction_compensation/" + self.joint_name, Float64) self.rate = rospy.Rate(100) self.forces = [] self.positions = []
def __init__(self): """ """ self.lib = EtherCAT_Hand_Lib() self.joint_name = "FFJ3" if rospy.has_param("~joint_name"): self.joint_name = rospy.get_param("~joint_name") self.min = 0.0 if rospy.has_param("~min"): self.min = rospy.get_param("~min") self.max = 1.55 if rospy.has_param("~max"): self.max = rospy.get_param("~max") self.nb_repetition = 2 if rospy.has_param("~nb_repetition"): self.nb_repetition = rospy.get_param("~nb_repetition") print " nb repetition: ", self.nb_repetition self.sign = -1 if rospy.has_param("~sign"): self.sign = rospy.get_param("~sign") rospy.loginfo("sign: "+str(self.sign)) self.publisher = rospy.Publisher("/sr_friction_compensation/"+self.joint_name, Float64) self.rate = rospy.Rate(100) self.forces = [] self.positions = []
def __init__(self, context): super(SrGuiManipulation, self).__init__(context) self.setObjectName('SrGuiManipulation') # Set by call to process_collision_map() self.collision_support_surface_name = None self.raw_objects = None self.found_objects = {} self.unknown_object_counter = 1 #starts at 1 as it's only used for display self.service_tabletop_collision_map = None self.service_db_get_model_description = None self.service_object_detector = None self_dir = os.path.dirname(os.path.realpath(__file__)) self.config_dir = os.path.join(self_dir, '../config') self.ui_dir = os.path.join(self_dir, '../../ui') # UI setup self._widget = QWidget() ui_file = os.path.join(self.ui_dir, 'SrGuiManipulation.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('SrGuiManipulationUi') context.add_widget(self._widget) self.object_chooser = ObjectChooser(self._widget, self, "Objects Detected") self._widget.chooser_layout.addWidget(self.object_chooser) self.object_chooser.draw() # Bind button clicks self._widget.btn_detect_objects.pressed.connect(self.detect_objects) self._widget.btn_collision_map.pressed.connect( self.process_collision_map) self._widget.btn_collision_map.setEnabled(False) self._widget.btn_start_grab_position.pressed.connect( self.start_grab_position) # self._widget.btn_zero_position.pressed.connect(self.zero_position) self._widget.btn_zero_position.pressed.connect(self.redo_place) self.robot_lib_eth = EtherCAT_Hand_Lib() # Guillaume: Currently removed because requires ethercat to be active to run, ethercat does exist in sim # if not self.robot_lib_eth.activate_joint_states(): # logerr("The EtherCAT Hand node doesn't seem to be running") # self.robot_lib_can = ShadowHand_ROS() # if not self.robot_lib_can.has_arm(): # logerr("The CAN Arm node doesn't seem to be running") self.init_services() self.init_joint_pubs()
class SrFrictionCompensation(object): """ """ def __init__(self): """ """ self.lib = EtherCAT_Hand_Lib() self.joint_name = "FFJ3" if rospy.has_param("~joint_name"): self.joint_name = rospy.get_param("~joint_name") self.min = 0.0 if rospy.has_param("~min"): self.min = rospy.get_param("~min") self.max = 1.55 if rospy.has_param("~max"): self.max = rospy.get_param("~max") self.nb_repetition = 2 if rospy.has_param("~nb_repetition"): self.nb_repetition = rospy.get_param("~nb_repetition") print " nb repetition: ", self.nb_repetition self.sign = -1 if rospy.has_param("~sign"): self.sign = rospy.get_param("~sign") rospy.loginfo("sign: " + str(self.sign)) self.publisher = rospy.Publisher( "/sr_friction_compensation/" + self.joint_name, Float64) self.rate = rospy.Rate(100) self.forces = [] self.positions = [] def run(self): self.lib.activate() time.sleep(0.5) for i in range(0, self.nb_repetition): self.move_to_start() time.sleep(1) self.record_map(-self.sign) time.sleep(1) print "forces length: ", len(self.forces), " pos length: ", len( self.positions) self.interpolate_map(1) self.forces = [] self.positions = [] for i in range(0, self.nb_repetition): self.move_to_start(False) time.sleep(1) self.record_map(self.sign, False) time.sleep(1) print "forces length: ", len(self.forces), " pos length: ", len( self.positions) self.interpolate_map(2) def move_to_start(self, start_at_min=True): msg = Float64() #try to get past the minimum if start_at_min: rospy.loginfo("Moving to the starting position (" + str(self.min) + ")") else: rospy.loginfo("Moving to the starting position (" + str(self.max) + ")") if (start_at_min): #250 should be big enough to move the finger to the end of its range msg.data = self.sign * 250 while (self.lib.get_position(self.joint_name) > self.min) and not rospy.is_shutdown(): self.publisher.publish(msg) self.rate.sleep() else: #250 should be big enough to move the finger to the end of its range msg.data = -self.sign * 250 while (self.lib.get_position(self.joint_name) < self.max) and not rospy.is_shutdown(): self.publisher.publish(msg) self.rate.sleep() msg.data = 0.0 for i in range(0, 20): self.publisher.publish(msg) self.rate.sleep() rospy.loginfo("Starting position reached") def record_map(self, sign, increasing=True): if increasing: rospy.loginfo("Recording increasing map, until " + str(self.max)) else: rospy.loginfo("Recording decreasing map, until" + str(self.min)) msg = Float64() if (increasing): while (self.lib.get_position(self.joint_name) < self.max) and not rospy.is_shutdown(): self.map_step(sign, msg) else: while (self.lib.get_position(self.joint_name) > self.min) and not rospy.is_shutdown(): self.map_step(sign, msg, False) def map_step(self, sign, msg, increasing=True): #keep the tendon under tension min_force = 0. if increasing: min_force = 0. else: min_force = 70. force, pos = self.find_smallest_force( self.lib.get_position(self.joint_name), sign, min_force) if force != False: self.forces.append(force) self.positions.append(pos) msg.data = sign * min_force self.publisher.publish(msg) rospy.Rate(2).sleep() def find_smallest_force(self, first_position, sign, min_force): msg = Float64() for force in range(min_force, 400): if rospy.is_shutdown(): break if abs(self.lib.get_position(self.joint_name) - first_position) > epsilon: #ok, the finger moved, return the necessary force measured_force = self.lib.get_effort(self.joint_name) print "finger moved from ", first_position, " with force ", measured_force return measured_force, first_position else: msg.data = sign * force self.publisher.publish(msg) self.rate.sleep() return False, False def interpolate_map(self, index): interp_pos = numpy.linspace(self.min, self.max, 10) fp = lambda v, x: v[0] + v[1] * x v = self.linear_interp(-1, -1) print v plot(interp_pos, fp(v, interp_pos), '-') print "forces length: ", len(self.forces), " pos length: ", len( self.positions) plot(self.positions, self.forces, 'o') savefig("friction_compensation_" + self.joint_name + ".png") def linear_interp(self, min_index, max_index): fp = lambda v, x: v[0] + v[1] * x e = lambda v, x, y: (fp(v, x) - y) v0 = [3., 1, 4.] v = 0 if min_index == -1 and max_index == -1: v, success = leastsq(e, v0, args=(self.positions, self.forces), maxfev=10000) else: v, success = leastsq(e, v0, args=(self.positions[min_index:max_index], self.forces[min_index:max_index]), maxfev=10000) return v
class SrFrictionCompensation(object): """ """ def __init__(self): """ """ self.lib = EtherCAT_Hand_Lib() self.joint_name = "FFJ3" if rospy.has_param("~joint_name"): self.joint_name = rospy.get_param("~joint_name") self.min = 0.0 if rospy.has_param("~min"): self.min = rospy.get_param("~min") self.max = 1.55 if rospy.has_param("~max"): self.max = rospy.get_param("~max") self.nb_repetition = 2 if rospy.has_param("~nb_repetition"): self.nb_repetition = rospy.get_param("~nb_repetition") print " nb repetition: ", self.nb_repetition self.sign = -1 if rospy.has_param("~sign"): self.sign = rospy.get_param("~sign") rospy.loginfo("sign: "+str(self.sign)) self.publisher = rospy.Publisher("/sr_friction_compensation/"+self.joint_name, Float64) self.rate = rospy.Rate(100) self.forces = [] self.positions = [] def run(self): self.lib.activate() time.sleep(0.5) for i in range(0,self.nb_repetition): self.move_to_start() time.sleep(1) self.record_map( -self.sign ) time.sleep(1) print "forces length: ", len(self.forces), " pos length: ", len(self.positions) self.interpolate_map(1) self.forces = [] self.positions = [] for i in range(0, self.nb_repetition): self.move_to_start(False) time.sleep(1) self.record_map( self.sign, False ) time.sleep(1) print "forces length: ", len(self.forces), " pos length: ", len(self.positions) self.interpolate_map(2) def move_to_start(self, start_at_min = True): msg = Float64() #try to get past the minimum if start_at_min: rospy.loginfo("Moving to the starting position ("+str(self.min)+")") else: rospy.loginfo("Moving to the starting position ("+str(self.max)+")") if( start_at_min ): #250 should be big enough to move the finger to the end of its range msg.data = self.sign * 250 while (self.lib.get_position(self.joint_name) > self.min) and not rospy.is_shutdown(): self.publisher.publish(msg) self.rate.sleep() else: #250 should be big enough to move the finger to the end of its range msg.data = -self.sign * 250 while (self.lib.get_position(self.joint_name) < self.max) and not rospy.is_shutdown(): self.publisher.publish(msg) self.rate.sleep() msg.data = 0.0 for i in range(0,20): self.publisher.publish(msg) self.rate.sleep() rospy.loginfo("Starting position reached") def record_map(self, sign, increasing = True): if increasing: rospy.loginfo("Recording increasing map, until "+str(self.max)) else: rospy.loginfo("Recording decreasing map, until"+str(self.min)) msg = Float64() if( increasing ): while (self.lib.get_position(self.joint_name) < self.max) and not rospy.is_shutdown(): self.map_step(sign, msg) else: while (self.lib.get_position(self.joint_name) > self.min) and not rospy.is_shutdown(): self.map_step(sign, msg, False) def map_step(self, sign, msg, increasing=True): #keep the tendon under tension min_force = 0. if increasing: min_force = 0. else: min_force = 70. force, pos = self.find_smallest_force(self.lib.get_position(self.joint_name), sign, min_force) if force != False: self.forces.append(force) self.positions.append(pos) msg.data = sign*min_force self.publisher.publish(msg) rospy.Rate(2).sleep() def find_smallest_force(self, first_position, sign, min_force): msg = Float64() for force in range(min_force, 400): if rospy.is_shutdown(): break if abs(self.lib.get_position(self.joint_name) - first_position) > epsilon: #ok, the finger moved, return the necessary force measured_force = self.lib.get_effort(self.joint_name) print "finger moved from ",first_position, " with force ", measured_force return measured_force, first_position else: msg.data = sign*force self.publisher.publish(msg) self.rate.sleep() return False, False def interpolate_map(self, index): interp_pos = numpy.linspace(self.min, self.max, 10) fp = lambda v, x: v[0]+ v[1]*x v = self.linear_interp(-1, -1) print v plot(interp_pos, fp(v, interp_pos), '-') print "forces length: ", len(self.forces), " pos length: ", len(self.positions) plot(self.positions, self.forces, 'o') savefig("friction_compensation_"+self.joint_name+".png") def linear_interp(self, min_index, max_index): fp = lambda v, x: v[0]+ v[1]*x e = lambda v, x, y: (fp(v,x)-y) v0 = [3., 1, 4.] v = 0 if min_index == -1 and max_index == -1: v, success = leastsq(e, v0, args=(self.positions,self.forces), maxfev=10000) else: v, success = leastsq(e, v0, args=(self.positions[min_index:max_index],self.forces[min_index:max_index]), maxfev=10000) return v