def get_group():
"""
Helper function to create a group from named modules, and set specified gains on the modules in that group.
"""
families = ['Test Family']
names = [
'J1_base', 'J2_shoulder', 'J3_elbow', 'J4_wrist1', 'J5_wrist2',
'J6_wrist3'
]
lookup = hebi.Lookup()
sleep(2.0)
group = lookup.get_group_from_names(families, names)
if group is None:
return None
# Set gains
gains_command = hebi.GroupCommand(group.size)
try:
gains_command.read_gains("gains/A-2085-06.xml")
except Exception as e:
print('Failed to read gains: {0}'.format(e))
return None
if not group.send_command_with_acknowledgement(gains_command):
print('Failed to receive ack from group')
return None
return group
def create_group(config):
"""
Used by :class:`Hexapod` to create the group to interface with modules
:param config: The runtime configuration
"""
imitation = config.is_imitation
if imitation:
num_modules = len(config.module_names)
from hebi.util import create_imitation_group
return create_imitation_group(num_modules)
else:
names = config.module_names
families = [config.family]
lookup = hebi.Lookup()
def connect():
group = lookup.get_group_from_names(families, names)
if group is None:
print('Cannot find hexapod on network...')
raise RuntimeError()
elif group.size != len(names):
raise RuntimeError()
return group
# Let the lookup object discover modules, before trying to connect
sleep(2.0)
return retry_on_error(connect)
def __init__(self, actuation_interval, maxAngle, minAngle, bias, mac):
self.lookup = hebi.Lookup()
time.sleep(2.)
self.group = self.lookup.get_group_from_macs([mac])
self.group.feedback_frequency = 100
self.cmd = hebi.GroupCommand(1)
self.interval = actuation_interval
self.angleBias = bias
self.currAngle = 0
self.maxAngle = self.angleBias + maxAngle
self.minAngle = self.angleBias + minAngle
self.degInc = 2 * (maxAngle - minAngle) / (self.interval * 7.)
self.positiveAngular = True
self.fixed = False
self.angular_vel = math.pi
self.killswitch = False
self.oscillating = False
self.lastTime = 0.
self.init_hebi()
def main():
hebi_modules = [('arm', 'joint_0'), ('arm', 'joint_1')]
# Discover modules
count_found = 0
lookup = hebi.Lookup()
sleep(2) # Give the Lookup process 2 seconds to discover modules
print('Modules found on network:')
for entry in lookup.entrylist:
print('{0} | {1}'.format(entry.family, entry.name))
if (entry.family, entry.name) in hebi_modules:
count_found = count_found + 1
if len(hebi_modules) != count_found:
print('Some HEBI module(s) is missing (%d/%d)...' % (count_found, len(hebi_modules)))
exit()
# Create a group from a set of names
group = lookup.get_group_from_names([m[0] for m in hebi_modules], [m[1] for m in hebi_modules])
print('Control group created...')
print(group)
sleep(3)
exit()
def setup():
lookup = hebi.Lookup()
group = lookup.get_group_from_names(
['HEBI'], ['base', 'shoulder', 'elbow', 'wrist1', 'wrist2', 'wrist3'])
if group is None:
print(
'Group not found: Did you forget to set the module family and names above?'
)
exit(1)
group.feedback_frequency = 100
group.command_lifetime = 100
model = hebi.robot_model.RobotModel()
model.add_actuator('X8-9')
model.add_bracket('X5-HeavyBracket', 'Right-Outside')
model.add_actuator('X8-9')
model.add_link('X5', 0.325, numpy.pi)
model.add_actuator('X5-9')
model.add_link('X5', 0.325, numpy.pi)
model.add_actuator('X5-9')
model.add_bracket('X5-LightBracket', 'Right')
model.add_actuator('X5-4')
model.add_bracket('X5-LightBracket', 'Right')
model.add_actuator('X5-4')
payload_mass = 0.6
model.add_rigid_body([0, 0, 0], [1, 1, 1, 0, 0, 0], payload_mass,
numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]]), True)
return [group, model]
def run():
arm = arm_container.create_3_dof()
state = State(arm)
from threading import Thread
cmd_thread = Thread(target=command_proc,
name='Command Thread',
args=(state,))
cmd_thread.start()
print_and_cr("Press 'b2' to add waypoint ('b3' for stopping at this waypoint), 'b4' to clear waypoints, 'b5' to playback, and 'b1' to quit.")
print_and_cr("When in playback mode, 'b6' resumes training, and 'b1' quits.")
# Mobile device setup
phone_family = 'HEBI'
phone_name = "mobileIO"
lookup = hebi.Lookup()
sleep(2)
print('Waiting for Mobile IO device to come online...')
m = create_mobile_io(lookup, phone_family, phone_name)
if m is None:
raise RuntimeError("Could not find Mobile IO device")
m.update()
while not m.get_button_state(1):
# Update MobileIO state
if not m.update():
print("Failed to get feedback from MobileIO")
continue
state.lock()
current_mode = state.mode
if current_mode == 'training':
m.set_led_color("blue")
if m.get_button_state(2):
add_waypoint(state, False)
elif m.get_button_state(3):
add_waypoint(state, True)
elif m.get_button_state(4):
clear_waypoints(state)
elif m.get_button_state(5):
if state.number_of_waypoints > 1:
state._mode = 'playback'
else:
print_and_cr('Need at least two waypoints to enter playback mode!')
elif current_mode == 'playback':
m.set_led_color("green")
if m.get_button_state(6):
state._mode = 'training'
state.unlock()
m.set_led_color("red")
state._quit = True
print_and_cr('')
def setup():
lookup = hebi.Lookup()
group = lookup.get_group_from_names(
['ShipBotB'], ['Base', 'Shoulder', 'Elbow', 'Wrist', 'EndEffector'])
group.feedback_frequency = 100
group.command_lifetime = 250
model = hebi.robot_model.import_from_hrdf("HEBI Robot 2021-04-19.hrdf")
return [group, model]
def _controller_by_mobile_io_selector(family, name, feedback_frequency):
import hebi
from time import sleep
lookup = hebi.Lookup()
sleep(2)
group = lookup.get_group_from_names([family], [name])
if group is None:
msg = 'Could not find Mobile IO on network with\nfamily: {0}\nname: {1}'.format(
family, name)
print(msg)
raise RuntimeError(msg)
group.feedback_frequency = feedback_frequency
return HebiModuleController(group)
def initialize_arm(self):
lookup = hebi.Lookup()
# Wait 2 seconds for the module list to populate
sleep(2.0)
self.group = lookup.get_group_from_names([self.family_name],
self.module_name)
self.base = lookup.get_group_from_names([self.family_name],
[self.module_name[0]])
self.shoulder = lookup.get_group_from_names([self.family_name],
[self.module_name[1]])
self.elbow = lookup.get_group_from_names([self.family_name],
[self.module_name[2]])
if self.group is None:
print(
'Group not found: Did you forget to set the module family and name above?'
)
exit(1)
self.group_feedback = hebi.GroupFeedback(self.group.size)
self.base_feedback = hebi.GroupFeedback(self.base.size)
self.shoulder_feedback = hebi.GroupFeedback(self.shoulder.size)
self.elbow_feedback = hebi.GroupFeedback(self.elbow.size)
if all(joint is None for joint in [
self.group.get_next_feedback(reuse_fbk=self.group_feedback),
self.base.get_next_feedback(reuse_fbk=self.base_feedback),
self.shoulder.get_next_feedback(
reuse_fbk=self.shoulder_feedback),
self.elbow.get_next_feedback(reuse_fbk=self.elbow_feedback)
]):
print('Error getting feedback.')
exit(1)
# Start logging in the background
self.group.start_log('logs', mkdirs=True)
self.base_command = hebi.GroupCommand(self.base.size)
self.shoulder_command = hebi.GroupCommand(self.shoulder.size)
self.elbow_command = hebi.GroupCommand(self.elbow.size)
self.joints = {
"base": ["base", self.base, self.base_feedback, self.base_command],
"shoulder": [
"shoulder", self.shoulder, self.shoulder_feedback,
self.shoulder_command
],
"elbow":
["elbow", self.elbow, self.elbow_feedback, self.elbow_command]
}
def __init__(self, family, gripperName):
self.family = family
self.name = gripperName
self.group = hebi.Lookup().get_group_from_names([family], [gripperName])
if self.group is None:
print ("Could not find gripper on network! Exiting...")
sys.exit()
self.openEffort = 1
self.closeEffort = -5
self.state = 0; # 0 is open, 1 is closed
# set up command structs
self.cmd = hebi.GroupCommand(1) # only 1 module in the group
def check_actuator():
"""
Check if actuator is connected
return: none
"""
all_devices = hebi.Lookup(
) # shows broadcast number, class containing modules
sleep(2) # give 2 seconds to discover modules
print('Modules found on network:')
for entry in all_devices.entrylist:
print('{0} | {1}'.format(entry.family, entry.name))
return all_devices
def __init__(self):
# finds available actuators on network
lookup = hebi.Lookup()
# Give the Lookup process 2 seconds to discover modules
sleep(2)
self.group = lookup.get_group_from_names(
['HEBI'], ['X5-9', 'X-00147']) #change depending on
self.group.feedback_frequency = 24.0
# this is a list of components that can be iterated through that represent the robot
# i.e. components[0] is linked to components[1] etc.
self.components = [['actuator', 'X5-1'],
['bracket', 'X5-LightBracket', 'right'],
['link', 'X5', 5.0, 0], ['actuator', 'X5-1'],
['link', 'X5', 5.0, 0]]
self.model = self.make_model(self.components)
self.mesh = []
self.base_rotation = [1, 0, 0, 0]
def setup():
lookup = hebi.Lookup()
group = lookup.get_group_from_names(
['ShipBotB'], ['Base', 'Shoulder', 'Elbow', 'Wrist', 'EndEffector'])
group.feedback_frequency = 200
group.command_lifetime = 100
model = hebi.robot_model.RobotModel()
model.add_actuator('X5-4')
model.add_bracket('X5-HeavyBracket', 'Right-Outside')
model.add_actuator('X8-9')
model.add_link('X5', 0.265, pi)
model.add_actuator('X5-4')
model.add_link('X5', 0.325, pi)
model.add_actuator('X5-1')
model.add_bracket('X5-HeavyBracket', 'Left-Outside')
model.add_actuator('X5-1')
return [group, model]
def setup_arm_params(name, family, has_gas_spring=False, lookup=None):
"""
:param name: This argument currently supports the following names
* '6-DoF + gripper'
* '6-DoF'
* '5-DoF + gripper'
* '5-DoF'
* '4-DoF
* '4-DoF SCARA'
* '3-DoF'
:param family: The family name of the modules which should be selected.
:param has_gas_spring: Specifies whether there is a gas spring supporting the shoulder joint
of the arm to provide extra payload.
"""
if name not in __arm_setup_params_dict:
valid_vals = __arm_setup_params_dict.keys()
print('Invalid `name` input {0}'.format(name))
print('Valid names include: {0}'.format(valid_vals))
raise KeyError('Invalid name')
arm_setup_params = __arm_setup_params_dict[name]
params = arm_setup_params[0]
group_names = arm_setup_params[1]
kin = hebi.robot_model.import_from_hrdf(
os.path.join(_arm_resource_local_dir, "hrdf",
params.robot_name + ".hrdf"))
if lookup is None:
lookup = hebi.Lookup()
from time import sleep
sleep(2)
group = lookup.get_group_from_names([family], group_names)
if group is None:
raise RuntimeError("Could not find group")
if not group.feedback_frequency > 0.0:
group.send_feedback_request()
params.update_gravity(group.get_next_feedback())
return group, kin, params
def get_group():
families = ['Test Family']
names = ["Base", "Shoulder", "Elbow"]
lookup = hebi.Lookup()
sleep(2.0)
group = lookup.get_group_from_names(families, names)
if group is None:
return None
# Set gains
gains_command = hebi.GroupCommand(group.size)
try:
gains_command.read_gains("gains/3-DoF_arm_gains.xml")
except Exception as e:
print('Failed to read gains: {0}'.format(e))
return None
if not group.send_command_with_acknowledgement(gains_command):
print('Failed to receive ack from group')
return None
return group
def get_group():
families = ['arm']
names = ["Base", 'Elbow']
lookup = hebi.Lookup()
sleep(2.0)
group = lookup.get_group_from_names(families, names)
if group is None:
print('inititalize_hebi: no group found')
return None
# Set gains
gains_command = hebi.GroupCommand(group.size)
try:
gains_command.read_gains("gains/default_gains.xml")
except:
print('inititalize_hebi: failed to read gains')
return None
if not group.send_command_with_acknowledgement(gains_command):
print('inititalize_hebi: failed to receive ack from group')
return None
return group
def create_group(config, has_camera):
"""
Used by :class:`Igor` to create the group to interface with modules
:param config: The runtime configuration
:type config: .configuration.Igor2Config
:param has_camera:
:type has_camera: bool
"""
imitation = config.is_imitation
if imitation:
if has_camera:
num_modules = len(config.module_names)
else:
num_modules = len(config.module_names_no_cam)
from hebi.util import create_imitation_group
return create_imitation_group(num_modules)
else:
if has_camera:
names = config.module_names
else:
names = config.module_names_no_cam
families = [config.family]
lookup = hebi.Lookup()
def connect():
group = lookup.get_group_from_names(families, names)
if group is None:
print('Cannot find igor on network...')
raise RuntimeError()
elif group.size != len(names):
raise RuntimeError()
return group
# Let the lookup object discover modules, before trying to connect
sleep(2.0)
return retry_on_error(connect)
def create_3_dof():
lookup = hebi.Lookup()
# You can modify the names here to match modules found on your network
module_family = 'HEBI'
module_names = ['base', 'shoulder', 'elbow']
from time import sleep
sleep(2)
arm = lookup.get_group_from_names([module_family], module_names)
if arm is None:
print(
'\nCould not find arm group: Did you forget to set the module family and names?'
)
print('Searched for modules named:')
print("{0} with family '{1}'".format(
', '.join(["'{0}'".format(entry) for entry in module_names]),
module_family))
print('Modules on the network:')
for entry in lookup.entrylist:
print(entry)
else:
print('[No Modules Found]')
exit(1)
model = hebi.robot_model.RobotModel()
model.add_actuator('X5-4')
model.add_bracket('X5-LightBracket', 'right')
model.add_actuator('X5-4')
model.add_link('X5', extension=0.18, twist=np.pi)
model.add_actuator('X5-4')
model.add_link('X5', extension=0.18, twist=0)
assert arm.size == model.dof_count
return ArmContainer(arm, model)
def __init__(self, params, lookup = None):
if lookup is None:
# Create arm from lookup
self.lookup = hebi.Lookup()
sleep(2)
else:
self.lookup = lookup
# Lookup modules for arm
self.group = self.lookup.get_group_from_names([params.family], params.moduleNames)
if self.group is None:
print("Could not find arm on network")
modules_on_network = [entry for entry in self.lookup.entrylist]
if len(modules_on_network) == 0:
print("No modules on network")
else:
print("modules on network:")
for entry in modules_on_network:
print(entry)
raise RuntimeError()
# Create robot model
try:
self.model = hebi.robot_model.import_from_hrdf(params.hrdf)
except Exception as e:
print('Could not load HRDF: {0}'.format(e))
sys.exit()
# Create Gripper, if exists
if params.hasGripper:
self.gripper = Gripper(params.family, params.gripperName)
else:
self.gripper = None
# Create comand and feedback
self.cmd = hebi.GroupCommand(self.group.size)
self.fbk = hebi.GroupFeedback(self.group.size)
self.prev_fbk = self.fbk
self.group.get_next_feedback(reuse_fbk=self.fbk)
# Zero our initial comand setting vars
self.pos_cmd = self.fbk.position
self.vel_cmd = np.zeros(self.group.size)
self.accel_cmd = np.zeros(self.group.size)
self.eff_cmd = np.zeros(self.group.size)
# Setup gravity vector for grav comp
self.gravity_vec = [0, 0, 1]
gravity_from_quaternion(self.fbk.orientation[0], output=self.gravity_vec)
self.gravity_vec = np.array(self.gravity_vec)
# Zero times
self.time_now = time()
self.traj_start_time = self.time_now
self.duration = 0
self.t_traj = 0
self.set_goal_first = True
self.at_goal = True
self.goal = "grav comp"
def auto_pointing():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'auto_pointing' node so that multiple nodes can run
# simultaneously.
rospy.init_node('auto_pointing', anonymous=True)
hebi_modules = [('arm', 'joint_0'), ('arm', 'joint_1')]
theta_bounds = [[-1.5, 1.5], [-0.15, 0.6]]
motion_bound = 0.05
# discover HEBI modules
count_found = 0
lookup = hebi.Lookup()
time.sleep(2) # Give the Lookup process 2 seconds to discover modules
print('Modules found on network:')
for entry in lookup.entrylist:
print('{0} | {1}'.format(entry.family, entry.name))
if (entry.family, entry.name) in hebi_modules:
count_found = count_found + 1
if len(hebi_modules) != count_found:
print('Some HEBI module(s) is missing (%d/%d)...' % (count_found, len(hebi_modules)))
if not MOCK_TEST:
exit()
# create a HEBI control group from a set of names
if not MOCK_TEST:
hebi_group = lookup.get_group_from_names([m[0] for m in hebi_modules], [m[1] for m in hebi_modules])
print('HEBI control group created (size=%d)' % (hebi_group.size))
print(hebi_group)
group_command = hebi.GroupCommand(hebi_group.size)
# create TF listener
tf_listener = tf.TransformListener()
# initialize HEBI actuator position control parameters
update_alpha = 0.5
theta = np.array([0, 0])
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# observe the current ring location
if not MOCK_TEST:
if not TWO_POINT_TEST:
try:
(T_ak1_ring, R_ak1_ring) = tf_listener.lookupTransform('/ak2_claw', '/ak1_ring', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
x_c = [T_ak1_ring[0], T_ak1_ring[1], T_ak1_ring[2]]
else:
if time.time() % 20 > 10:
x_c = [2,2,1]
else:
x_c = [2,-2,0.5]
else:
if time.time() % 20 > 10:
x_c = [2,2,1]
else:
x_c = [2,-2,0.5]
# loop for gradient descent
for t_gd in range(20):
# find gradient
d_theta = np.array([calc_d_theta1(theta[0], theta[1], x_c[0], x_c[1], x_c[2]),
calc_d_theta2(theta[0], theta[1], x_c[0], x_c[1], x_c[2])])
print(d_theta)
# update desired theta
for i_dim in range(len(d_theta)):
d_theta[i_dim] = max(-motion_bound, min(motion_bound, d_theta[i_dim]))
theta = theta - d_theta * update_alpha
for i_dim in range(len(theta)):
theta[i_dim] = max(theta_bounds[i_dim][0], min(theta_bounds[i_dim][1], theta[i_dim]))
# send command to HEBI control group
if not MOCK_TEST:
group_command.position = theta
hebi_group.send_command(group_command)
print('theta1 = %.4f, theta2 = %.4f, x_c = (%.2f, %.2f, %.2f)' % (
theta[0], theta[1], x_c[0], x_c[1], x_c[2]));
rate.sleep()
def main():
'''
description: step function of system
position of actuator shaft follows a sine wave path
parameters: none
returns: none
'''
# Look up actuator
lookup = hebi.Lookup()
group = lookup.get_group_from_family('*') # '*' selects all modules
# Command all modules in a group with this zero force or effort command
group_command = hebi.GroupCommand(group.size)
group_feedback = hebi.GroupFeedback(group.size)
group.feedback_frequency = 200.0 # This effectively limits the loop below to 200Hz
#group.feedback_frequency = 50.0 # This effectively limits the loop below to 200Hz
## Open-loop controller ----------------------------------------------------------------------
w = -1 * m.pi * 5.0 # frequency (to control the velocity)
#setting initial positipon to 0
group_command.position = 0
time.sleep(2)
#t0, dt, tf = 0.0, 0.1, 2.0 # initial time & step size
t0, dt, tf = 0.0, 0.03, 15.0 # initial time & step size
time_range = np.arange(t0, tf, dt)
group.command_lifetime = dt # set length of time of command (ms)
# Initialize all array
pos, vel, defl, eff, pwm = [], [], [], [], []
stop_loop = 0
t = t0
# First restart poistion
while (0):
group.command_lifetime = 200.0 # set length of time of command (ms)
group_feedback = group.get_next_feedback(reuse_fbk=group_feedback)
group_command.effort == 1.0
group.send_command(group_command)
sleep(2)
group.command_lifetime = dt # set length of time of command (ms)
sleep(10)
start_time = time.time()
# Run motor for specified time
for i in time_range:
group_feedback = group.get_next_feedback(reuse_fbk=group_feedback)
w_t = np.multiply(w, t)
#group_command.velocity = 2*np.sin(w_t)
#I am generating a random input signal (random gaussian input)
#group_command.velocity = random.gauss(0,0.6)
#group_command.velocity = random.gammavariate(0.2, 1.8)
group_command.velocity = random.uniform(-3, 3)
group.send_command(group_command)
print('input effort:', group_command.effort)
positions = group_feedback.position
velocity = group_feedback.velocity
deflection = group_feedback.deflection
effort = group_feedback.effort
pwm_command = group_feedback.pwm_command
print('Position Feedback:\n{0}'.format(positions))
pos = np.append(pos, positions)
vel = np.append(vel, velocity)
defl = np.append(defl, deflection)
eff = np.append(eff, effort)
pwm = np.append(pwm, pwm_command)
'''with open('motor.csv', 'w') as csvfile:
fieldnames = ['pos', 'vel', 'eff']
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
writer.writeheader()
writer.writerow({'pos': pos, 'vel': vel, 'eff':eff})'''
row = [pos, vel, defl, eff, pwm]
#print ('row:', row)
sleep(dt)
t = t + dt
end_time = time.time()
print('Time: ', end_time - start_time)
print('pos', pos)
print('vel', vel)
print('deflection', defl)
print('eff', eff)
print('pwm', pwm)
with open('motor.csv', 'w') as csvfile:
fieldnames = ['pos', 'vel', 'defl', 'eff', 'pwm']
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
writer.writeheader()
writer.writerow({
'pos': pos,
'vel': vel,
'defl': defl,
'eff': eff,
'pwm': pwm
})
group_command.velocity = np.nan #0.001*np.cos(w_t)h
group.send_command(group_command)
import matplotlib.pyplot as plt
# row = []
#I am also importing the deflection data to look for any correlations.
plt.subplot(511)
plt.plot(pos)
plt.ylabel('position')
plt.subplot(512)
plt.plot(vel)
plt.ylabel('velocity')
plt.subplot(513)
plt.plot(eff)
plt.ylabel('torque (N m)')
plt.subplot(514)
plt.plot(defl)
plt.ylabel('deflection')
plt.subplot(515)
plt.plot(pwm)
plt.ylabel('pwm')
plt.xlabel('time')
plt.show()
import pandas as pd
import matplotlib.pyplot as plt
from scipy import signal
######################################################################
######################################################################
#####
##### 歩行器を押したら動いて引いたら止まる
#####
######################################################################
######################################################################
###################################################################
#### HEBIの初期設定など
###################################################################
lookup = hebi.Lookup()
#group = lookup.get_group_from_names(['Wheel'], ['Right', 'Left'])
group = lookup.get_group_from_names(['Wheel'], ['Right_light', 'Left_light'])
if group is None:
print(
'Group not found: Did you forget to set the module family and names above?'
)
exit(1)
for entry in lookup.entrylist:
print(entry)
group_feedback = hebi.GroupFeedback(group.size)
group_command = hebi.GroupCommand(group.size)
group_info = hebi.GroupInfo(group.size)
def timeDiff(end, start):
return end - start
fuse = Fusion(timeDiff)
# Module Names on SUPERball V2
numModules = 24
SBModuleNames = (['M' + str(i + 1).zfill(2) for i in xrange(numModules)])
# Need to look into XML formatting for Hebi Gains
# sio.loadmat('defaultGains.mat')
lookup = hebi.Lookup() # Get table of all Hebi motors
sleep(2) # gives the Lookup process time to discover modules
# Displays the Hebi modules found on the network
print('Modules found on the network:')
for entry in lookup.entrylist:
print('{0} | {1}'.format(entry.family, entry.name))
# print('\n')
var = raw_input('Were any modules found? [y/N]: \n')
if var == 'y':
print('\nYay!\n')
elif var == 'Y':
print('\nYay!\n')
def getTheArmToMove():
lookup = hebi.Lookup()
sleep(2)
points = convertObjects()
positionsElb = []
positionsSho = []
for x, y in points:
positionsElb.append(y)
positionsSho.append(x)
group1 = lookup.get_group_from_names(["Elbow"], ["elbow"])
group2 = lookup.get_group_from_names(["Sholder"], ["sholder rot"])
if group1 is None or group2 is None:
print(
'Group not found! Check that the family and name of a module on the network'
)
print('matches what is given in the source file.')
exit(1)
num_joints = group1.size + group2.size
group_feedback1 = hebi.GroupFeedback(group1.size)
group_feedback2 = hebi.GroupFeedback(group2.size)
if group1.get_next_feedback(
reuse_fbk=group_feedback1) is None or group2.get_next_feedback(
reuse_fbk=group_feedback2) is None:
print('Error getting feedback.')
exit(1)
#positions = np.zeros((num_joints, 2), dtype=np.float64)
offset = [math.pi] * num_joints
current_pos1 = group_feedback1.position
current_pos2 = group_feedback2.position
time_vector1 = []
starttime = 0
steps = len(positionsElb) / 60.0
for pt in positionsElb:
time_vector1.append((starttime))
starttime = starttime + steps
time_vector2 = []
steps = len(positionsSho) / 60.0
for pt in positionsElb:
time_vector2.append((starttime))
starttime = starttime + steps
trajectory1 = hebi.trajectory.create_trajectory(time_vector1, positionsElb)
trajectory2 = hebi.trajectory.create_trajectory(time_vector2, positionsSho)
# Start logging in the background
group1.start_log('logs1')
group2.start_log('logs2')
group_command1 = hebi.GroupCommand(group1.size)
group_command2 = hebi.GroupCommand(group2.size)
duration1 = trajectory1.duration
start = time()
t = time() - start
while t < duration1:
# Serves to rate limit the loop without calling sleep
group1.get_next_feedback(reuse_fbk=group_feedback1)
t = time() - start
pos, vel, acc = trajectory1.get_state(t)
group_command1.position = pos
group_command1.velocity = vel
group1.send_command(group_command1)
# Serves to rate limit the loop without calling sleep
group2.get_next_feedback(reuse_fbk=group_feedback2)
t = time() - start
pos, vel, acc = trajectory2.get_state(t)
group_command2.position = pos
group_command2.velocity = vel
group2.send_command(group_command2)
group1.stop_log()
group2.stop_log()
pi = np.pi
names = []
for i in range(0, 6):
names.append('base' + str(i + 1))
names.append('shoulder' + str(i + 1))
names.append('elbow' + str(i + 1))
names = np.array(names)
np.save("names", names)
base = names[range(0, len(names), 3)]
Hebi = hebi.Lookup()
imu = Hebi.get_group_from_names("*", names)
if imu is None:
print(
"Group 'imu' not found! Check that the family and name of a module on the network"
)
print('matches what is given in the source file.')
exit(1)
imu.feedback_frequency = 5.0
group_feedback = hebi.GroupFeedback(imu.size)
while True:
