def __init__(self, robot):
# tf Listener
self.listener = tf.TransformListener()
# robot
self.robot = robot
self.robot_name = robot.getName()
# trajectory
self.trajectory_generator = FuzzyTrajectoryGenerator(self.robot)
self.trajectory = []
self.generate_trajectory_failure = False
self.trajectory_correct = False
# command
self.current_command = None
self.interpreter = CommandInterpreter(robot_name=self.robot_name,
robot_object=self.robot)
# frames
self.ee_frame = None
# target
self.target = None
self.target_q = None
self.dist_target = -1
# parameters
self.node_frequency = Parameters.get(obj=self.robot_name,
param="NODE_FREQUENCY")
self.trajectory_points = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
self.trajectory_time = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_TIME")
self.joints_tollerance = self.robot.getParam("joints_tollerance")
# timer
self.init_generate_timer = 15 * self.node_frequency # [sec]
self.init_start_moving_timer = 10 * self.node_frequency # [sec]
# external callback
self.feedback_ext_callback = None
self.feedback = RobotMotionFeedback()
self.result_ext_callback = None
self.result = RobotMotionResult()
# error
self.err = self.result.NO_ERROR
# noise velocity in steady state
self.vel_noise_mean = 0.015
# alarm
self.in_allarm = False
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
self.alarm_proxy.registerResetCallback(self.alarm_reset_callback)
def _generateTrajectoryFromCommand(self):
self.initial_q = self.robot.getController().getQ()
self.trajectory_generator.clearTrajectory()
self.generate_trajectory_failure = False
steps = self.trajectory_time * self.node_frequency
if self.target:
self.trajectory_points = Parameters.get(obj=self.robot_name,
param="TRAJECTORY_POINTS")
if self.trajectory_points is None:
self.trajectory_points = self.node_frequency * self.trajectory_time
try:
if self.interpreter.trajectory_specs == "linear_joints":
self.trajectory = self.trajectory_generator.generateTrajectoryFromShapes(
self.initial_q, self.target, self.trajectory_points, 1)
elif self.interpreter.trajectory_specs == "linear_frames_to_free_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFromFrames(
self.initial_q, self.robot.getEEFrame(), self.target,
self.node_frequency, self.trajectory_time)
elif self.interpreter.trajectory_specs == "free_frames_to_linear_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames(
self.initial_q,
self.robot.getEEFrame(),
self.target,
steps=steps,
agumented_middle_joints=True,
middle_frames=10,
number_of_tries=1,
perturbate_middle_frames=False)
elif self.interpreter.trajectory_specs == "target_frame_to_target_joints":
self.target = self.target * self.robot.getCurrentTool(
).Inverse()
self.trajectory = self.trajectory_generator.generateTrajectoryFreeFromFrames(
self.initial_q,
self.robot.getEEFrame(),
self.target,
steps=1,
agumented_middle_joints=False,
middle_frames=20,
number_of_tries=5,
perturbate_middle_frames=False)
else:
self.trajectory = []
except Exception as e:
Logger.error(e)
self.trajectory = []
else:
self.trajectory = []
# Trajectory target update
if len(self.trajectory) > 0:
self.target_q = self.trajectory_generator.getTargetQ()
else:
self.generate_trajectory_failure = True
self.target_q = self.initial_q
def __init__(self, robot_name):
self.robot_name = robot_name
self.listener = tf.TransformListener()
self.message_database = MessageStorage()
# tool
self.tools_list = Parameters.get(obj=self.robot_name, param="TOOLS")
print self.tools_list
self.new_tools = {}
self.new_tools["dynamic"] = FrameVectorToKDL(
self.tools_list["gripper"])
# alarm
self.alarm_proxy = AlarmProxy(self.robot_name)
self.alarm_proxy.registerAlarmCallback(self.alarm_callback)
# target follower proxy
self.target_follower = TargetFollowerProxy(self.robot_name)
# robot outer message (no feedback/response)
self.outer_message_proxy = SimpleMessageProxy()
self.outer_message_proxy.register(self.command_callback)
# robot outer command
self.command_server = CommandProxyServer("{}_supervisor".format(
self.robot_name))
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.command_category = "unknown"
# robot inner message
self.inner_message_proxy = SimpleMessageProxy(
"{}_inner_message".format(self.robot_name))
# robot inner command
self.robot_trajectory_client = RobotMotionClient(
self.robot_name, ext_callback=self.robot_trajectory_callback)
self.robot_control_client = CommandProxyClient(
"{}_direct_commander".format(self.robot_name))
self.robot_control_client.registerDoneCallback(
self.control_done_callback)
import superros.transformations as transformations
from superros.logger import Logger
from rocup.msg import *
from geometry_msgs.msg import Pose
from std_msgs.msg import Float64MultiArray
import tf
from scipy.interpolate import interp1d
from std_msgs.msg import String
from transitions import Machine
from rocup.param.global_parameters import Parameters
WORLD_FRAME_ID = Parameters.get("WORLD_FRAME_ID")
class CommandInterpreter(object):
def __init__(self, robot_name="", robot_object=None, callback=None):
# tf Listener
self.listener = tf.TransformListener()
# robot
self.robot_name = robot_name
self.robot = robot_object
# trajectory
self.target = None
self.trajectory_specs = ""
from rocup.proxy.target_follower_proxy import TargetFollowerProxy
from rocup.proxy.alarm_proxy import AlarmProxy
from rocup.proxy.proxy_message import SimpleMessage, SimpleMessageProxy
from rocup.robotsupervisor.robot_supervisor_state_machine import SupervisorSM
from std_msgs.msg import String, Float64, Bool
from sensor_msgs.msg import JointState
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
import threading
import PyKDL
import copy
import math
import tf
from rocup.param.global_parameters import Parameters
if __name__ == '__main__':
robot_name = Parameters.get("BONMET_NAME")
rospy.init_node('{}_supervisor_node'.format(robot_name))
node_rate = 30
rate = rospy.Rate(node_rate)
bonmet_supervisor = SupervisorSM(robot_name)
bonmet_supervisor.start()
try:
while not rospy.is_shutdown():
bonmet_supervisor.stepForward()
rate.sleep()
except rospy.ROSInterruptException:
pass
import numpy as np
import rospy
from superros.logger import Logger
from rocup.param.global_parameters import Parameters
from superros.comm import RosNode
from sensor_msgs.msg import JointState
def command_callback(msg):
global joints
joints = msg.position
if __name__ == '__main__':
robot_name = Parameters.get("BONMET_NAME")
node = RosNode('joint_command_sequencer'.format(robot_name))
node_rate = Parameters.get(obj=robot_name, param="NODE_FREQUENCY")
node.setupParameter("hz", node_rate)
node.createSubscriber("/bonmetc60/joint_command", JointState, command_callback)
pub = node.createPublisher("/bonmetc60/joint_command_seq", JointState)
joints = [0, 0, 0, 0, 0, 0]
joints_msg = JointState()
while node.isActive():
joints_msg.position = joints
pub.publish(joints_msg)
node.tick()
from rocup.param.global_parameters import Parameters
from rocup.proxy.proxy_message import SimpleMessage, SimpleMessageProxy
from rocup.proxy.command_proxy import CommandProxyClient, CommandMessage, CommandProxyServer
from rocup.partdb.cad import Scene, Shape, Face, STLUtils, Transform, Part, Terminal, PartCollection, Gearbox, Channel
from rocup.partdb.data_dictionaries import *
from std_msgs.msg import String
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Pose
from std_srvs.srv import Trigger, TriggerResponse
from rocup.partdb.data_dictionaries import GearboxPartsData
from rocup.param.global_parameters import Parameters
#⬢⬢⬢⬢⬢➤ ROBOT
robot_name = Parameters.get("GRASSHOPPER_NAME") # @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
Hb_open = "\033[94m\033[1m\033[4m"
Hg_open = "\033[92m\033[1m\033[4m"
Hr_open = "\033[91m\033[1m\033[4m"
H_close = "\033[0m"
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
class ConnectionsScanProxyServer(object):
def __init__(self):
self.command_proxy_server_name = "grasshopper_grasp_supervisor"
self.command_server = CommandProxyServer(self.command_proxy_server_name)
self.command_server.registerCallback(self.command_action_callback)
self.command_server_last_message = None
self.message_proxy = SimpleMessageProxy()
from rocup.proxy.alarm_proxy import AlarmProxy
from rocup.proxy.proxy_message import SimpleMessage, SimpleMessageProxy
from rocup.robotsupervisor.robot_supervisor_state_machine import SupervisorSM
from std_msgs.msg import String, Float64, Bool
from sensor_msgs.msg import JointState
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
import threading
import PyKDL
import copy
import math
import tf
from rocup.param.global_parameters import Parameters
if __name__ == '__main__':
robot_name = Parameters.get("SCHUNK_NAME")
rospy.init_node('{}_supervisor_node'.format(robot_name))
node_rate = 30
rate = rospy.Rate(node_rate)
schunk_supervisor = SupervisorSM(robot_name)
schunk_supervisor.start()
try:
while not rospy.is_shutdown():
schunk_supervisor.stepForward()
rate.sleep()
except rospy.ROSInterruptException:
pass
from rocup.proxy.target_follower_proxy import TargetFollowerProxy
from rocup.proxy.alarm_proxy import AlarmProxy
from rocup.proxy.proxy_message import SimpleMessage, SimpleMessageProxy
from rocup.robotsupervisor.robot_supervisor_state_machine import SupervisorSM
from std_msgs.msg import String, Float64, Bool
from sensor_msgs.msg import JointState
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
import threading
import PyKDL
import copy
import math
import tf
from rocup.param.global_parameters import Parameters
if __name__ == '__main__':
robot_name = Parameters.get("COMAU_NAME")
rospy.init_node('{}_supervisor_node'.format(robot_name))
node_rate = 30
rate = rospy.Rate(node_rate)
comau_supervisor = SupervisorSM(robot_name)
comau_supervisor.start()
try:
while not rospy.is_shutdown():
comau_supervisor.stepForward()
rate.sleep()
except rospy.ROSInterruptException:
pass
from std_msgs.msg import Float64MultiArray
import superros.transformations as transformations
from superros.logger import Logger
from rocup.srv import IKService, IKServiceResponse
from rocup.msg import RobotFollowStatus
from rocup.param.global_parameters import Parameters
""" CONSTANTS """
ROBOT_CONTROLLER_PUBLISHER_NAME = "joint_states"
ROBOT_CONTROLLER_SUBSCRIBER_NAME = "joint_command"
ROBOT_CONTROLLER_REPLACE_ROBOT_NAME_PREFIX_IN_JOINTS = True
ROBOT_CONTROLLER_MOVING_ROTATION_EPSILON = 0.002
ROBOT_DEFAULT_IK_SERVICE_SUFFIX = "/ik_service_node/ik_service"
ROBOTS_DEFAULT_WORLD_NAME = Parameters.get("WORLD_FRAME_ID")
ROBOTS_DEBUG_MODE = True
class GeneralIKService:
""" General Service Wrapper for IKService messages """
def __init__(self, service_name, service_manifest=IKService, timeout=5.0):
self.service_name = service_name
self.service_manifest = service_manifest
self.active = False
Logger.log("GeneralIKService: waiting for service " +
service_name + "...")
try:
rospy.wait_for_service(self.service_name, timeout=timeout)
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()
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from superros.logger import Logger
import superros.transformations as transformations
from superros.transformations import TwistToKDLVector, TwistFormKDLVector
from rocup.utils.devices import Joystick
from rocup.param.global_parameters import Parameters
from rocup.proxy.target_follower_proxy import TargetFollowerProxy
from rocup.proxy.command_proxy import CommandProxyClient, CommandMessage, CommandProxyServer
from rocup.proxy.alarm_proxy import AlarmProxy
from rocup.proxy.proxy_message import SimpleMessage, SimpleMessageProxy
from rocup.robotcontrol.controllers.other_controllers import *
WRIST_FT_SENSOR_POSITION = Parameters.get("WRIST_FT_SENSOR_POSITION_WRT_EEF")
class DirectCommander(object):
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
return None
if __name__ == '__main__':
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("wires_task")
node.setupParameter("hz", 50)
node.setHz(node.getParameter("hz"))
task_name = "wires_task_comau"
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇ Parameters ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
#⬢⬢⬢⬢⬢➤ Robots
comau_name = Parameters.get("COMAU_NAME")
gripper_name = Parameters.get("SCHUNK_NAME")
robot_list = [comau_name, gripper_name]
#⬢⬢⬢⬢⬢➤ Sensors
tactile_name = "tactile"
sensor_list = [tactile_name]
#⬢⬢⬢⬢⬢➤ Subtasks
insertion_task_name = "insertion_task"
tool_correction_task_name = "tool_correction_task"
wire_tf_filter_task_name = "tf_filter"
subtask_list = [
insertion_task_name, tool_correction_task_name,
wire_tf_filter_task_name
]