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 = {}
