def haptic_force(self, F):
haptic_output = OmniFeedback()
haptic_output.force.x = max(min(-F[0] * self.gains['K_output'], 3), -3)
haptic_output.force.y = max(min(F[1] * self.gains['K_output'], 3), -3)
haptic_output.force.z = max(min(-F[2] * self.gains['K_output'], 3), -3)
haptic_output.lock = [False, False, False]
self.haptic_pub.publish(haptic_output)
def __init__(self):
# Controller Settings
self.action_boundary = 100
self.gains = {'K_input': 1,
'V_input': 0,
'K_output': 0.05,
}
# PyGame Initialization
self.player = Rect((helper.windowWidth*0.5, helper.windowHeight*0.5, helper.PLAYERSIZE_X, helper.PLAYERSIZE_Y) )
self.swarmbot = Rect((helper.windowWidth*0.5, helper.windowHeight*0.5, helper.BLOCKSIZE_X, helper.BLOCKSIZE_Y) )
self.display_surf = pygame.display.set_mode((helper.windowWidth, helper.windowHeight))
pygame.display.set_caption('Use the haptic device to push the swarm bot')
pygame.init()
pygame.font.init()
# Variable Initialization
self.ee_state = np.asarray([[0], [0], [0], [0], [0], [0]])
self.swarm_state = np.asarray([[300],[300],[0],[0],[0],[0]])
self.swarm_heading = 0
self.F = [0,0,0]
self.haptic_output = OmniFeedback()
self.swarm_output = Haptic()
self.time0 = time.time()
# ROS Initialization
rospy.init_node("python_controller")
rospy.Subscriber("/bot0/state", State, self.update_from_bot)
rospy.Subscriber("/Geomagic/end_effector_pose", JointState, self.get_input_haptic)
rospy.Subscriber("/Geomagic/button", PhantomButtonEvent, self.button_callback)
self.one_bot_pub = rospy.Publisher("/bot0/haptic", Haptic, queue_size=1)
self.haptic_pub = rospy.Publisher("/Geomagic/force_feedback", OmniFeedback, queue_size=1)
def __init__(self):
# self.listener = tf.TransformListener()
self.of = OmniFeedback()
self.of.lock = [False for i in xrange(3)]
self.pub = rospy.Publisher("Geomagic/force_feedback",
OmniFeedback,
queue_size=1)
self.sub = rospy.Subscriber("Geomagic/end_effector_pose",
JointState,
self.callback,
queue_size=1)
self.lock = 0.0
def __init__(self):
self.goal_rec = None
self.start_rec = None
self.action_boundary = 100
self.player = Rect(
(helper.windowWidth * 0.5, helper.windowHeight * 0.5,
helper.PLAYERSIZE_X, helper.PLAYERSIZE_Y))
self.swarmbot = Rect(
(helper.windowWidth * 0.5, helper.windowHeight * 0.5,
helper.BLOCKSIZE_X, helper.BLOCKSIZE_Y))
self.display_surf = pygame.display.set_mode(
(helper.windowWidth, helper.windowHeight))
self.ee_state = np.asarray([[0], [0], [0], [0], [0], [0]])
self.swarm_state = np.asarray([[300], [300], [0], [0], [0], [0]])
self.swarm_mass = 5
self.swarm_heading = 0
self.F = [0, 0, 0]
self.haptic_output = OmniFeedback()
self.swarm_output = Haptic()
self.running = True
self.am_i_at_goal = False
self.am_i_at_start = False
self.time0 = time.time()
self.gains = {
'K_input': 1,
'V_input': 0,
'K_output': 0.05,
}
pygame.display.set_caption('Use the cursor to move the swarm bot')
#self.csv = open("/home/cibr-strokerehab/Documents/JointStatesRecording.csv", "w")
rospy.init_node("python_controller")
rospy.Subscriber("/bot0/state", State, self.update_from_bot)
rospy.Subscriber("/Geomagic/end_effector_pose", JointState,
self.get_input_haptic)
rospy.Subscriber("/Geomagic/button", PhantomButtonEvent,
self.button_callback)
self.one_bot_pub = rospy.Publisher("/bot0/haptic",
Haptic,
queue_size=1)
self.haptic_pub = rospy.Publisher("/Geomagic/force_feedback",
OmniFeedback,
queue_size=1)
pygame.init()
pygame.font.init()
def __init__(self, maze_name="maze1"):
self.goal_rec = None
self.start_rec = None
self.push_boundary = 60
self.pull_boundary = 120
self.player = Rect(
(helper.windowWidth * 0.5, helper.windowHeight * 0.5,
helper.PLAYERSIZE_X, helper.PLAYERSIZE_Y))
self.swarmbot = Rect(
(helper.windowWidth * 0.5, helper.windowHeight * 0.5,
helper.BLOCKSIZE_X, helper.BLOCKSIZE_Y))
self.display_surf = pygame.display.set_mode(
(helper.windowWidth, helper.windowHeight))
self.ee_state = np.asarray([[0], [0], [0], [0], [0], [0]])
self.swarm_state = np.asarray([[300], [300], [0], [0], [0], [0]])
self.swarm_mass = 5
self.output = OmniFeedback()
self.F = [0, 0, 0]
self.running = True
self.am_i_at_goal = False
self.am_i_at_start = False
self.time0 = time.time()
self.gains = {
'K_pull': 0.4,
'V_pull': 0.1,
'K_output': 0.05,
'V_output': 0.1,
'K_push': 1,
'V_push': 1,
}
rospy.Subscriber("/Geomagic/end_effector_pose", JointState,
self.get_input)
self.pub = rospy.Publisher("/Geomagic/force_feedback",
OmniFeedback,
queue_size=1)
pygame.display.set_caption('Use the cursor to move the swarm bot')
# self.csv = open("/home/cibr-strokerehab/Documents/JointStatesRecording.csv", "w")
pygame.init()
pygame.font.init()