def shape(self, shape_name, time=rospy.Time(0)): """ Applies a Shape to robot """ if self.robot_shapes.get(shape_name): self.getController().applyShape(self.robot_shapes.get(shape_name), time) else: Logger.warning("No shape with name '{}' for robot '{}'".format( shape_name, self.robot_name))
def reset(self): self.is_active = False self.done = False # self.velocity = 0 # self.axis = PyKDL.Vector() thr = self._estimateDisturbance(20) * 1.3 self.setParameters({"damp_force_threshold": thr}) Logger.warning("damp_force_threshold={}".format(thr))
def stepForward(self): """ Moves one step forward (1 cycle) the robot's state machine """ try: self.sfm.loop() if self.robotSFM.isTrajectoryAdmissible() and self.sfm.getModel( ).state == "moving": self.robotSFM.applyTrajectory() except Exception as e: Logger.warning("Exception: {}".format(e))
def _send_command_result(self, success): if self.command_server_last_message: Logger.warning("Command result:{}".format(success)) if success: self.command_server.resolveCommand( self.command_server_last_message) else: self.command_server.rejectCommand( self.command_server_last_message) self.command_server_last_message = None
def command_callback(self, msg): try: if msg.isValid(): if msg.getReceiver() == "{}_supervisor".format( self.sensor_name): command = msg.getCommand() self.result_flg = True if command == "reset": self.reset_publisher.publish("") time.sleep(0.5) Logger.warning("Sensor '{}': Reset".format( self.sensor_name)) elif command == "filteron": Logger.warning("Sensor '{}': Filter ON".format( self.sensor_name)) # TODO elif command == "filteroff": Logger.warning("Sensor '{}': Filter OFF".format( self.sensor_name)) # TODO elif command == "filterreset": Logger.warning("Sensor '{}': Filter Reset".format( self.sensor_name)) # TODO else: self.result_flg = False Logger.error("INVALID input") self._send_command_result(self.result_flg) except Exception as e: print(e)
def robot_done_callback(self, command): sent_message = command.getSentMessage() if sent_message.getCommand().startswith("goto"): Logger.warning("trajectory DONE") try: source_done = command.response_data["trajectory_done"] if source_done: print("trajectory OK {}".format( command.response_data["q_target_distance"])) self.send_command.setStyleSheet("background-color: green") else: print("trajectory FAIL {}".format( command.response_data["q_target_distance"])) self.send_command.setStyleSheet("background-color: white") except Exception as e: print(e) self.send_command.setStyleSheet("background-color: red")
def updateAndBroadcast(self, node, current_tf, size): if self.current_tf_name != '': if current_tf: if self.filtered_tf_slots[self.current_tf_name] > 0: self.filtered_tf_slots[self.current_tf_name] -= 1 print(self.current_tf_name, self.filtered_tf_slots[self.current_tf_name]) if len(self.filtered_tf_map[self.current_tf_name]) < size: self.filtered_tf_map[self.current_tf_name].append( current_tf) else: Logger.warning( "TF Buffer for '{}' is full! Clear it...".format( current_tf_name)) else: self._send_command_result(True) p = np.array([0, 0, 0]) q = np.array([0, 0, 0, 0]) for tf_name, tf_list in self.filtered_tf_map.iteritems(): if len(tf_list) <= 0: continue for tf in tf_list: newp = np.array([tf.p.x(), tf.p.y(), tf.p.z()]) qx, qy, qz, qw = tf.M.GetQuaternion() newq = np.array([qx, qy, qz, qw]) p = p + newp q = q + newq p = p / float(len(tf_list)) q = q / float(len(tf_list)) q = q / np.linalg.norm(q) filtered_tf = PyKDL.Frame() filtered_tf.p = PyKDL.Vector(p[0], p[1], p[2]) filtered_tf.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3]) node.broadcastTransform(filtered_tf, tf_name + "_filtered", node.getParameter("world_tf"), node.getCurrentTime())
def _send_command(self, supervisor_id, command, data={}): Logger.warning("Sending {} {} to {}".format(command, data, supervisor_id)) message = SimpleMessage(receiver="{}_supervisor".format(supervisor_id), command=command) for k in data.keys(): message.setData(k, data[k]) if supervisor_id in self.sensor_name_list: self.active_command = self.sensor_proxy_client_dict[ supervisor_id].sendCommand(message.toString()) self.action() elif supervisor_id in self.robot_name_list: self.active_command = self.robot_proxy_client_dict[ supervisor_id].sendCommand(message.toString()) self.action() elif supervisor_id in self.subtask_name_list: self.active_command = self.subtask_proxy_client_dict[ supervisor_id].sendCommand(message.toString()) self.action() else: self.alarm()
def on_enter_error(self, error_type=""): Logger.log("State: ERROR") self._sendFeedback(state="error") self.trajectory_generator.clearTrajectory() if error_type == "NO_COMMAND": self.err = self.result.ERROR_NO_COMMAND Logger.warning( "Command string NOT recognized. Waiting for a new command") elif error_type == "NO_TRAJECTORY": self.err = self.result.ERROR_NO_TRAJECTORY Logger.warning( "Trajectory NOT computed. Waiting for a new command") elif error_type == "START_MOVING": self.err = self.result.ERROR_START_MOVING Logger.warning( "Moving Time Out. Target NOT Reached. Waiting for a new command" ) elif error_type == "END_TRAJECTORY": self.err = self.result.ERROR_END_TRAJECTORY Logger.error( "Trajectory Time Out. Target NOT Reached. Waiting for a new command" ) elif error_type == "ROBOT_ALARM": self.err = self.result.ERROR_ROBOT_ALARM self.in_allarm = True Logger.error( "\n\n\n ROBOT ALARM!!! \n Restore the machine and Reset the Alarm \n\n\n" ) elif error_type == "COMMAND_CONFLICT": self.err = self.result.ERROR_COMMAND_CONFLICT Logger.error( "\n\n\n COMMAND CONFLICT!!! \n Waiting for a new command \n\n\n" ) else: self.err = self.result.ERROR_UNKNOWN Logger.error("Unknown Error --- {}".format(error_type)) # send result self._sendResult(success=False, error=self.err, command=self.current_command)
# Creates an Enable Service enable_service = node.createService("~enable", SetBool, serviceCallback) # Remote Service remote_name = "ping" if "pong" in node.getName() else "pong" remote_enable_service = node.getServiceProxy(remote_name + "/enable", SetBool) # start start_time = 0.0 start_offset = 0.0 if "ping" in node.getName() else math.pi # Main Loop while node.isActive(): # Generates new Float message value = Float32() value.data = math.sin(node.getElapsedTimeInSecs() * 2 * math.pi * 0.2 - start_offset) # Publish if enabled if enabled: Logger.warning("{} -> {}".format(node.getName(), value.data)) output_publisher.publish(value) if value.data < 0: remote_enable_service(True) enabled = False else: output_publisher.publish(Float32(0.0)) node.tick()
def _goal_callback(self): if not self.action_server.is_active(): # Logger.log("New goal accepted!") self._setNewGoal(self.action_server.accept_new_goal()) else: Logger.warning("New goal not accepted! Another goal is active")
def startFiltering(self, name, slot_size): Logger.warning("FILTER START: {}".format(name)) self.current_tf_name = name self.filtered_tf_slots[name] = slot_size if name not in self.filtered_tf_map: self.filtered_tf_map[name] = []
def command_callback(self, msg): if msg.isValid(): if msg.getReceiver() == "{}_supervisor".format(self.robot_name): print msg.toString() command = msg.getCommand() try: #⬢⬢⬢⬢⬢➤ RESET if command == "reset": self.command_category = "setting" Logger.warning(" !!!!!!!! ALARM RESET !!!!!!!! ") self.alarm_proxy.resetAlarm() self.target_follower.resetAlarm() self._reject_input_command(GlobalErrorList.ROBOT_ALARM) self.idle() elif self.state != "alarm": #⬢⬢⬢⬢⬢➤ SET TOOL if command == "settool": # change multiple data structure in a single multi-entry dictionary self.command_category = "setting" tool_name = msg.getData("tool_name") new_tool_name = msg.getData("new_tool_name") transf = msg.getData("transformation") tool = self._getToolFromName(tool_name) transf = FrameVectorToKDL(transf) self.new_tools[new_tool_name] = tool * transf print("Tools: {}".format(self.new_tools)) self._send_command_result(True) self.idle() #⬢⬢⬢⬢⬢➤ SELECT TOOL elif command == "selecttool": self.command_category = "setting" tool_name = msg.getData("tool_name") tool = self._getToolFromName(tool_name) self._setTool(tool) self._send_command_result(True) self.idle() #⬢⬢⬢⬢⬢➤ SET TRAJECTORY elif command == "settrajectory": self.command_category = "setting" points = msg.getData( "points" ) # if it returns none, this wasn't the set parameter time = msg.getData("time") if points is not None: print Parameters.get(obj=self.robot_name, param="TRAJECTORY_POINTS") Parameters.change(obj=self.robot_name, param="TRAJECTORY_POINTS", value=int(points)) print Parameters.get(obj=self.robot_name, param="TRAJECTORY_POINTS") elif time is not None: Parameters.change(obj=self.robot_name, param="TRAJECTORY_TIME", value=int(points)) self._send_command_result(True) self.idle() #⬢⬢⬢⬢⬢➤ MOVE TO TF elif command == "movetotf": self.command_category = "trajectory" tf_name = msg.getData("tf_name") tool_name = msg.getData("tool_name") self._toTf(tf_name, tool_name, mode=command) print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format( tf_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ JUMP TO TF elif command == "jumptotf": self.command_category = "trajectory" tf_name = msg.getData("tf_name") tool_name = msg.getData("tool_name") self._toTf(tf_name, tool_name, mode=command) print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format( tf_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ GO TO TF elif command == "gototf": self.command_category = "trajectory" tf_name = msg.getData("tf_name") tool_name = msg.getData("tool_name") self._toTf(tf_name, tool_name, mode=command) print("Tf: \033[1m\033[4m\033[94m{}\033[0m".format( tf_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ GO TO JOINTS elif command == "gotojoints": self.command_category = "trajectory" joints_str = msg.getData("joints_vector") self._toJoints(joints_str) print("Joints: \033[1m\033[4m\033[94m{}\033[0m". format(joints_str)) self.trajectory() #⬢⬢⬢⬢⬢➤ GO TO SHAPE elif command == "gotoshape": self.command_category = "trajectory" shape_name = msg.getData("shape_name", str) self._toShape(shape_name) print("Shape: \033[1m\033[4m\033[94m{}\033[0m". format(shape_name)) self.trajectory() #⬢⬢⬢⬢⬢➤ RESET CONTROLLERS elif command == "directreset": self.command_category = "setting" self._resetDirectCommander() #⬢⬢⬢⬢⬢➤ ENABLE CONTROLLERS elif command == "direct": self.command_category = "setting" active = msg.getData("active") if active: self._switchCommander(self.DIRECT_COMMANDER) self.direct() else: self._switchCommander( self.TRAJECTORY_COMMANDER) self.idle() #⬢⬢⬢⬢⬢➤ START CONTROLLERS elif command == "controllerstart": self.command_category = "setting" self._sendCommandActionToController(msg) print("Inputs: {}".format( msg.getData("input_data"))) self.controlling() #⬢⬢⬢⬢⬢➤ UPDATE CONTROLLERS PARAMETERS elif command == "controllerparameters": self.command_category = "setting" self._sendCommandActionToController(msg) print("Params: {}".format( msg.getData("parameters"))) #⬢⬢⬢⬢⬢➤ SELECT CONTROLLERS elif command == "controllerselect": self.command_category = "setting" self._sendCommandActionToController(msg) print("Controller selected: {}".format( msg.getData("id"))) #⬢⬢⬢⬢⬢➤ DISABLE CONTROLLERS elif command == "controllerdisable": self.command_category = "setting" self._sendCommandActionToController(msg) print("Controllers removed: {}".format( msg.getData("id"))) else: self.command_category = "unknown" Logger.error("INVALID input") self._reject_input_command() except Exception as e: Logger.error(e) self._reject_input_command()
def stepForward(self): try: if self.active: self.current_tf = transformations.retrieveTransform( self.listener, self.robot_name + "/base_link", # TODO aggiungere base_link in shunck self.robot_name + "/tool", none_error=True, time=rospy.Time(0), print_error=True) if self.current_tf is not None: if self.target_tf is None: self.target_tf = self.current_tf #➤ Joystick if self.joystick_flg: self.target_tf = self.joystick.transformT( self.target_tf) #➤ Control Action else: self.feedback_data["current_tf"] = self.current_tf self.feedback_data["target_tf"] = self.target_tf for ctrl_id in self.active_controllers.keys(): self.target_tf = self.active_controllers[ ctrl_id].output(self.feedback_data) ####### Publish and Set Target ####### transformations.broadcastTransform(self.br, self.target_tf, self.tf_target_name, self.robot_name + "/base_link", time=rospy.Time.now()) dist = 1 # diff_tf = self.current_tf.Inverse() * self.target_tf # transformations.broadcastTransform(self.br, diff_tf, "diff_tf", # self.robot_name + "/base_link", time = rospy.Time.now()) # dist_lin, distr_ang=transformations.frameDistance(diff_tf, # PyKDL.Frame()) # distr_ang=np.linalg.norm(distr_ang) # dist_lin=np.linalg.norm( # transformations.KDLVectorToNumpyArray(dist_lin)) # dist=distr_ang + dist_lin # print dist if dist > 0.0003 or self.joystick_flg: self.target_follower.setTarget( target=self.target_tf, target_type=TargetFollowerProxy.TYPE_POSE, target_source=TargetFollowerProxy.SOURCE_DIRECT) else: Logger.error("tf not ready!!") else: pass except Exception as e: Logger.warning(e) pass
def __init__(self, robot_name, controllers_dict=None): self.robot_name = robot_name self.current_tf = None self.target_tf = None self.tf_target_name = "follow_target" self.active = False self.command_success_flg = False # target follower self.target_follower = TargetFollowerProxy(self.robot_name) # alarm self.alarm_proxy = AlarmProxy(self.robot_name) self.alarm_proxy.registerAlarmCallback(self.alarm_callback) self.alarm_proxy.registerResetCallback(self.alarm_reset_callback) # command message self.message_proxy = SimpleMessageProxy("{}_inner_message".format( self.robot_name)) self.message_proxy.register(self.command_callback) # command action self.command_server_last_message = None self.command_server = CommandProxyServer("{}_direct_commander".format( self.robot_name)) self.command_server.registerCallback(self.command_action_callback) # tf self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() # Joystick Contorol joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback, queue_size=1) self.joystick = Joystick(translation_mag=0.0005, rotation_mag=0.0008) self.joystick.registerCallback(self.joystick_key_callback) self.joystick_flg = False # Disable by default # Closed-Loop Control if controllers_dict is None or controllers_dict == {}: controllers_dict = {"none": NeutralController()} self.controllers_dict = controllers_dict self.active_controllers = {} self.active_controllers["none"] = NeutralController() Logger.warning("READY!!!") # Feedback self.feedback_data = {} # Force rospy.Subscriber('/atift', Twist, self.force_feedback_callback, queue_size=1) # Tactile rospy.Subscriber('/tactile', Float64MultiArray, self.tactile_feedback_callback, queue_size=1)
def stopFiltering(self): Logger.warning("FILTER STOP") self.current_tf_name = ''
def alarm_reset_callback(self): Logger.warning(" ******** ALARM CLEAR ******** ") self.target_tf = self.current_tf self._reset()
def command_callback(self, msg): try: self.command_success_flg = True if msg.isValid(): receiver = msg.getReceiver() if msg.getReceiver() == "{}_direct_commander".format( self.robot_name): command = msg.getCommand() #⬢⬢⬢⬢⬢➤ ON if command == "_on_": Logger.log( " ******** Direct Commander: ENABLE ******** ") self._reset() self._removeController() self.active = True #⬢⬢⬢⬢⬢➤ OFF elif command == "_off_": Logger.log( " ******** Direct Commander: DISABLE ******** ") self.current_tf = None self._reset() self._removeController() self.active = False #⬢⬢⬢⬢⬢➤ RESET elif command == "_reset_": Logger.log(" ******** RESET ******** ") self._reset() #⬢⬢⬢⬢⬢➤ JOYSTICK elif command == "_joyon_": Logger.log(" Joystick: ENABLE ") self.joystick_flg = True elif command == "_joyoff_": Logger.log(" Joystick: DISABLE ") self.joystick_flg = False #⬢⬢⬢⬢⬢➤ SELECT CONTROLLER elif command.startswith("controllerselect"): ctrl_id_list = msg.getData("id") Logger.log( " Controller Selection: {}".format(ctrl_id_list)) for ctrl_id in ctrl_id_list: if ctrl_id == "none": Logger.log(" Neutral Controller ") self._removeController() else: try: self._addController(ctrl_id) except: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format( ctrl_id)) #⬢⬢⬢⬢⬢➤ CONTROLLER PARAMETERS elif command.startswith("controllerparameters"): parameters = msg.getData("parameters") Logger.log( " Parameters Update: {} ".format(parameters)) for ctrl_id in parameters.keys(): if ctrl_id in self.active_controllers.keys(): self.active_controllers[ctrl_id].setParameters( parameters[ctrl_id]) else: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format(ctrl_id)) #⬢⬢⬢⬢⬢➤ REMOVE CONTROLLER elif command.startswith("controllerdisable"): Logger.log(" Removing Controller ") ctrl_id_list = msg.getData("id") for ctrl_id in ctrl_id_list: if ctrl_id == "_all_": self._reset() self._removeController() elif ctrl_id in self.active_controllers.keys(): self._reset(ctrl_id) self._removeController(ctrl_id) else: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format(ctrl_id)) # ⬢⬢⬢⬢⬢➤ START CONTROLLERS elif command.startswith("controllerstart"): Logger.log(" Controllers Start {}".format( self.active_controllers.keys())) input_data = msg.getData("input_data") time.sleep(0.1) for ctrl_id in input_data.keys(): if ctrl_id in self.active_controllers.keys(): self.active_controllers[ctrl_id].start( input_data[ctrl_id]) else: self.command_success_flg = False Logger.error( "Wrong Controller ID: {}".format(ctrl_id)) else: self.command_success_flg = False Logger.warning("INVALID COMMAND: {}".format(command)) else: receiver = msg.getReceiver() if msg.getReceiver() == "{}_direct_commander".format( self.robot_name): self.command_success_flg = False except Exception as e: self.command_success_flg = False print(e) Logger.log(" \nActive Controllers: {}\n".format( self.active_controllers.keys()))
def clearFiltering(self, name): Logger.warning("FILTER CLEAR: {}".format(name)) self.filtered_tf_map[name] = [] self.filtered_tf_slots[name] = 0
def generateTrajectoryFreeFromFrames(self, q_in, frame1, frame2, steps=1, middle_frames=1, number_of_tries=5, agumented_middle_joints=True, perturbate_middle_frames=True): perturbation_gain = 0 p1 = transformations.KDLVectorToList(frame1.p) p2 = transformations.KDLVectorToList(frame2.p) dist_frames = np.linalg.norm(np.subtract(p1, p2)) while number_of_tries > 0: if middle_frames > 1: cartesian_trajectory = self.generateCartesianFrames( frame1, frame2, middle_frames, 1) else: cartesian_trajectory = [frame2] number_of_tries -= 1 trajectory_points = [] if not self.checkIKService(): self.setCurrentTrajectory(trajectory_points) return trajectory_points else: current_q = q_in last_q = q_in success_counter = 0.0 for frame in cartesian_trajectory: if perturbate_middle_frames and frame != cartesian_trajectory[ -1]: # we perturbate the frame hoping to avoid the singularities in the IK calculation frame = FuzzyTrajectoryGenerator._perturbateFrameRotation( frame, perturbation_gain) trajectory_point = TrajectoryPoint(frame, last_q, 0.0, invalid=True) response = self.robot.getIKService().simple_ik( frame, last_q) if response.status == IKServiceResponse.STATUS_OK: trajectory_point.q = response.q_out.data trajectory_point.invalid = False success_counter += 1.0 last_q = trajectory_point.q success_counter = success_counter / float( len(cartesian_trajectory)) Logger.log("IK Computation (#{} segments) Success {}%".format( len(cartesian_trajectory), success_counter * 100)) if success_counter > 0.99: inc_steps = np.linalg.norm( np.arccos(np.cos(np.subtract(q_in, last_q)))) q_trajectory_steps = steps + int( agumented_middle_joints * inc_steps * 500) return self.generateTrajectoryFromShapes( q_in, last_q, q_trajectory_steps, 1) # self.setCurrentTrajectory(trajectory_points) self.status = FuzzyTrajectoryGenerator.STATUS_OK # return trajectory_points else: perturbation_gain = 1 middle_frames *= 5 Logger.warning("Fail IK") self.status = FuzzyTrajectoryGenerator.STATUS_INVALID_POINTS trajectory_points = [] self.setCurrentTrajectory(trajectory_points) return trajectory_points
def as_goal_callback(self): """ Callback chiamata SEMPRE all'arrivo di un nuovo Goal """ if not self.action_server.is_active(): self._setNewGoal(self.action_server.accept_new_goal()) else: Logger.warning("New goal not accepted! Another goal is active")
def alarm_reset_callback(self): if self.state == "error" and self.in_allarm: Logger.warning("\nAlarm Reset\n") self.in_allarm = False
def reset(self): self.is_active = False self.done = False thr = self._estimateDisturbance(20) * 1.10 self.setParameters({"thresholds": thr}) Logger.warning("thresholds={}".format(thr))
def resetFiltering(self): Logger.warning("FILTER RESET") self.filtered_tf_map = {} self.filtered_tf_slots = {}