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 execute_trajectory(group, model, trajectory, feedback):
# Set up command object, timing variables, and other necessary variables
num_joints = group.size
command = hebi.GroupCommand(num_joints)
duration = trajectory.duration
start = time()
t = time() - start
while t < duration:
# Get feedback and update the timer
group.get_next_feedback(reuse_fbk=feedback)
t = time() - start
# Get new commands from the trajectory
pos_cmd, vel_cmd, acc_cmd = trajectory.get_state(t)
# Calculate commanded efforts to assist with tracking the trajectory.
# Gravity Compensation uses knowledge of the arm's kinematics and mass to
# compensate for the weight of the arm. Dynamic Compensation uses the
# kinematics and mass to compensate for the commanded accelerations of the arm.
eff_cmd = math_utils.get_grav_comp_efforts(model, feedback.position,
[0, 0, 1])
# NOTE: dynamic compensation effort computation has not yet been added to the APIs
# Fill in the command and send commands to the arm
command.position = pos_cmd
command.velocity = vel_cmd
command.effort = eff_cmd
group.send_command(command)
def __init__(self,
robot_name,
effort_offset,
ik_seed_pos,
has_gripper=False,
gripper_open_effort=None,
gripper_close_effort=None,
gripper_gains=None):
self._robot_name = robot_name
gains_path = os.path.join(_arm_resource_local_dir, "gains",
robot_name + "_gains.xml")
gains = hebi.GroupCommand(len(ik_seed_pos))
try:
gains.read_gains(gains_path)
except Exception as e:
import sys
sys.stderr.write(
"Could not load gains from file {0}\n".format(gains_path))
raise e
self._gains = gains
self._has_gripper = has_gripper
self._gripper_open_effort = gripper_open_effort
self._gripper_close_effort = gripper_close_effort
self._gripper_gains = gripper_gains
self._effort_offset = np.asarray(effort_offset)
self._ik_seed_pos = ik_seed_pos
self._gravity_vec = np.zeros(3, dtype=np.float32)
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 __init__(self, group: 'Group', chassis_ramp_time: float,
flipper_ramp_time: float):
self.group = group
self.fbk = self.group.get_next_feedback()
while self.fbk == None:
self.fbk = self.group.get_next_feedback()
self.wheel_fbk = self.fbk.create_view([0, 1, 2, 3])
self.flipper_fbk = self.fbk.create_view([4, 5, 6, 7])
self.cmd = hebi.GroupCommand(group.size)
self.wheel_cmd = self.cmd.create_view([0, 1, 2, 3])
self.flipper_cmd = self.cmd.create_view([4, 5, 6, 7])
self.chassis_ramp_time = chassis_ramp_time
self.flipper_ramp_time = flipper_ramp_time
self.flipper_cmd.position = self.flipper_fbk.position
self.wheel_cmd.position = np.nan
self.t_prev: float = time()
self._aligned_flipper_mode = False
self._is_aligning = False
self.chassis_traj = None
self.flipper_traj = None
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 __init__(self, group: 'Group'):
self.group = group
self.fbk = self.group.get_next_feedback()
while self.fbk is None:
self.fbk = group.get_next_feedback()
self.roll: float = self.ea_zxz[0]
self.cmd = hebi.GroupCommand(self.group.size)
def __init__(self, group):
self.group = group
self.base_command = hebi.GroupCommand(group.size)
self.base_feedback = hebi.GroupFeedback(group.size)
self.trajectory = None
self.trajectory_start_time = 0
self.start_wheel_pos = np.array([0, 0])
self.color = hebi.Color(0, 0, 0)
def update_gains(self):
group = self._group
size = group.size
gains = hebi.GroupCommand(size)
# TODO: gains_file = "gains{0}.xml".format(size)
# TODO: gains.read_gains(gains_file)
return group.send_command_with_acknowledgement(gains)
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")
M = group.size
cmd = hebi.GroupCommand(M)
return [group, model, cmd, M]
def loadGains(self, gains_file):
try:
gains_cmd = hebi.GroupCommand(1) # only 1 module in the group
gains_cmd.read_gains(gains_file)
if not self.group.send_command_with_acknowledgement(gains_cmd):
raise RuntimeError('Did not receive acknowledgement from group.')
print('Successfully read gains from file and sent to gripper.')
except Exception as e:
print('Problem reading gains from file or sending to module: {0}'.format(e))
def start(self):
"""
Start running Igor with the provided configuration.
This can safely be called multiple times
but will do nothing after the first invocation.
All processing will be done on a background thread which is spawned
in this method.
"""
self._state_lock.acquire()
if self._started:
self._state_lock.release()
return
group = create_group(self._config, self._has_camera)
group.command_lifetime = 500
group.feedback_frequency = 100.0
self._group = group
self._group_command = hebi.GroupCommand(group.size)
self._group_feedback = hebi.GroupFeedback(group.size)
self._group_info = hebi.GroupInfo(group.size)
joystick_selector = self._config.joystick_selector
self._joy = joystick_selector()
load_gains(self)
from threading import Condition, Lock, Thread
start_condition = Condition(Lock())
def start_routine(start_condition):
start_condition.acquire()
# Calling thread holds `__state_lock` AND
# is also blocked until we call `start_condition.notify_all()`
# So we can safely modify this here.
self._started = True
# Allow the calling thread to continue
start_condition.notify_all()
start_condition.release()
self._start()
self._proc_thread = Thread(target=start_routine,
name='Igor II Controller',
args=(start_condition, ))
# We will have started the thread before returning,
# but make sure the function has begun running before
# we release the `_state_lock` and return
start_condition.acquire()
self._proc_thread.start()
start_condition.wait()
start_condition.release()
self._state_lock.release()
def load_gains(group: 'Group', gains_file: str):
gains_command = hebi.GroupCommand(group.size)
sleep(0.1)
try:
gains_command.read_gains(gains_file)
except Exception as e:
print(f'Warning - Could not load gains: {e}')
return
# Send gains multiple times
for _ in range(3):
group.send_command(gains_command)
sleep(0.1)
def command_proc(state):
# The background thread in which communication with the modules occur
group = state.arm.group
group.feedback_frequency = 100.0
num_modules = group.size
command = hebi.GroupCommand(num_modules)
feedback = hebi.GroupFeedback(num_modules)
prev_mode = state.mode
start_time = time()
trajectory = None
while True:
if group.get_next_feedback(reuse_fbk=feedback) is None:
print('Did not receive feedback')
continue
state.lock()
if state.quit:
state.unlock()
break
feedback.get_position(state.current_position)
command.effort = state.arm.get_efforts(feedback)
current_mode = state.mode
if current_mode == 'playback':
if prev_mode != 'playback':
# First time
trajectory = build_trajectory(state)
start_time = time()
time_in_seconds = time() - start_time
if time_in_seconds > trajectory.duration:
start_time = time()
time_in_seconds = 0
pos, vel, acc = trajectory.get_state(time_in_seconds)
command.position = pos
command.velocity = vel
elif current_mode == 'training' and prev_mode != 'training':
# Clear old position commands
command.position = None
command.velocity = None
group.send_command(command)
state.unlock()
prev_mode = current_mode
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 __init__(self,
pan_tilt_group: 'Group',
hrdf: 'RobotModel | str' = os.path.join(
os.path.dirname(__file__), 'hrdf', 'pan_tilt_mast.hrdf')):
self.group = pan_tilt_group
self.fbk = self.group.get_next_feedback()
while self.fbk is None:
self.fbk = self.group.get_next_feedback()
self.cmd = hebi.GroupCommand(self.group.size)
self.trajectory: 'Optional[Trajectory]' = None
if isinstance(hrdf, str):
self.robot_model = hebi.robot_model.import_from_hrdf(hrdf)
else:
self.robot_model = hrdf
self.output_frame = np.empty((4, 4), dtype=np.float64)
def play_trajectory(group, model, trajectory, fraction=1.0):
t = 0.0
period = 0.01
cmd = hebi.GroupCommand(5)
spring_offset = numpy.zeros((1, 5))
duration = trajectory.duration
while (t < duration * fraction):
fbk = get_fbk(group)
pos_cmd, vel_cmd, acc_cmd = trajectory.get_state(t)
cmd.position = pos_cmd
cmd.velocity = vel_cmd
cmd.effort = numpy.matrix(
get_grav_comp_efforts(model, fbk.position, [0.0, 0.0, 1.0]) +
spring_offset).A1
group.send_command(cmd)
t = t + period # TODO: do this better!
def reset_mobile_io_gui_state():
cmd = hebi.GroupCommand(1)
io = cmd.io
a_bank = io.a
b_bank = io.b
e_bank = io.e
# set UI back to original state
body_height_pin = _get_pin_number_from_name(controller_mapping.body_height_velocity)
toggle_mode_pin = _get_pin_number_from_name(controller_mapping.mode_selection)
quit_pin = _get_pin_number_from_name(controller_mapping.quit)
a_bank.set_float(body_height_pin, None)
b_bank.set_int(toggle_mode_pin, 0)
e_bank.set_int(toggle_mode_pin, 0)
e_bank.set_int(quit_pin, 0)
hexapod.joystick.group.send_command(cmd)
def _set_mobile_io_gui_state(hexapod, controller_mapping): cmd = hebi.GroupCommand(1) io = cmd.io a_bank = io.a b_bank = io.b e_bank = io.e body_height_pin = _get_pin_number_from_name(controller_mapping.body_height_velocity) toggle_mode_pin = _get_pin_number_from_name(controller_mapping.mode_selection) quit_pin = _get_pin_number_from_name(controller_mapping.quit) a_bank.set_float(body_height_pin, 0.001) # Use a small value as opposed to 0.0 to work around bug in older versions of Mobile IO app b_bank.set_int(toggle_mode_pin, 1) # Set to toggle mode e_bank.set_int(toggle_mode_pin, 1) # Highlight e_bank.set_int(quit_pin, 1) # Highlight hexapod.joystick.group.send_command(cmd)
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 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 load_gains(hexapod):
group = hexapod.group
# Bail out if group is imitation
if hexapod.config.is_imitation:
return
gains_command = hebi.GroupCommand(group.size)
sleep(0.1)
import os
gains_xml = os.path.join(hexapod.config.resources_directory, 'gains18.xml')
try:
gains_command.read_gains(gains_xml)
except Exception as e:
print(f'Warning - Could not load gains: {e}')
return
# Send gains multiple times
for i in range(3):
group.send_command(gains_command)
sleep(0.1)
def load_gains(igor):
group = igor.group
# Bail out if group is imitation
if igor.config.is_imitation:
return
gains_command = hebi.GroupCommand(group.size)
sleep(0.1)
try:
if igor.has_camera:
gains_command.read_gains(igor.config.gains_xml)
else:
gains_command.read_gains(igor.config.gains_no_camera_xml)
except Exception as e:
print('Warning - Could not load gains: {0}'.format(e))
return
# Send gains multiple times
for i in range(3):
group.send_command(gains_command)
sleep(0.1)
def update(self):
self.clear_canvas()
group_command = hebi.GroupCommand(self.robot.group.size)
p = []
for w in self.sliders:
p.append(w[0].get())
group_command.position = p
self.robot.group.send_command(group_command)
self.virtual_robot.generate_3d_mesh(
VirtualFeedback(p, self.virtual_robot.base_rotation))
self.draw_mesh_wireframe(
self.virtual_robot.mesh,
colors=['yellow', 'yellow', 'yellow', 'yellow'],
horizon=400,
offsets=[250, 250, 0],
camera=self.camera,
camera_angle=self.camera_angle)
self.draw_mesh_solid(self.robot.mesh,
mesh_colors=[[0, 255, 0], [0, 0, 255]],
horizon=400,
offsets=[250, 250, 0],
camera=self.camera,
camera_angle=self.camera_angle,
light=[0, 10, 10])
###################################################################
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)
group.feedback_frequency = 100
def Setting():
group_command.reference_effort = 0.0000
#group_command.reference_position = 0.0000
new_velocity_kp_gains = [0.01, 0.01]
new_effort_kp_gains = [0.1, 0.1]
for new_gain in new_velocity_kp_gains:
for i in range(group.size):
group_command.velocity_kp = new_gain
if not group.send_command_with_acknowledgement(group_command):
def setCommand(model, fbk, group, t):
cmd = hebi.GroupCommand(5)
cmd.position = np.array(
[pi / 2 + pi / 6 * sin(2 * pi * 0.05 * t), pi / 2, 0, pi / 2, 0])
group.send_command(cmd)
print('Could not get feedback')
raise RuntimeError('Could not get feedback')
return fbk
def setCommand(model, fbk, group, t):
cmd = hebi.GroupCommand(5)
cmd.position = np.array(
[pi / 2 + pi / 6 * sin(2 * pi * 0.05 * t), pi / 2, 0, pi / 2, 0])
group.send_command(cmd)
[group, model] = setup()
cmd = hebi.GroupCommand(5)
#cmd.control_strategy = 'Strategy3'
#group_info = group.request_info()
#group_info.write_gains("saved_gains.xml")
#cmd.read_gains("saved_gains.xml")
#group.send_command_with_acknowledgement(cmd)
N_states = 6
pos = np.zeros([5, N_states])
vel = np.zeros([5, N_states])
def clear_color(self):
color_cmd = hebi.GroupCommand(self.group.size)
color_cmd.led.clear()
self.group.send_command(color_cmd)
def set_color(self, color: 'hebi.Color | str'):
color_cmd = hebi.GroupCommand(self.group.size)
color_cmd.led.color = color
self.group.send_command(color_cmd)
hrdf_file='hrdf/A-2303-01.hrdf',
lookup=lookup)
mirror_group = lookup.get_group_from_names(['Arm'], ['J2B_shoulder1'])
while mirror_group is None:
print('Still looking for mirror group...')
sleep(1)
mirror_group = lookup.get_group_from_names(['Arm'], ['J2B_shoulder1'])
# mirror the position/velocity/effort of module 1 ('J2A_shoulder1') to the module
# in the mirror group ('J2B_shoulder1')
# Keeps the two modules in the double shoulder bracket in sync
output_arm.add_plugin(hebi.arm.DoubledJointMirror(1, mirror_group))
output_arm.load_gains('gains/A-2303-01.xml')
# need to update the gains for the mirror group also
gains_cmd = hebi.GroupCommand(1)
gains_cmd.read_gains('gains/mirror_shoulder.xml')
mirror_group.send_command_with_acknowledgement(gains_cmd)
output_arm.cancel_goal()
input_arm.update()
output_arm.update()
output_joints_home = [0.0, -1.0, 0.0, -0.6, -np.pi / 2, 1.0, 0.0]
# allowed angular difference (°) per joint before starting align
allowed_diff = np.array([30.0, 20.0, 30.0, 20.0, 45.0, 45.0, 360.0])
leader_follower_control = LeaderFollowerControl(input_arm, output_arm,
output_joints_home,
allowed_diff)
