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, 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 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 __init__(self, group):
if group.size != 1:
raise RuntimeError('Group must be of a single module')
self._group = group
group.add_feedback_handler(self.__fbk_handler)
self._event_handlers = list()
self._feedback_data = FeedbackData()
self._current_feedback = hebi.GroupFeedback(group.size)
from threading import Lock
self._current_feedback_lock = Lock()
def __init__(self, group: 'Group', offsets):
self.group = group
self.group_fbk = hebi.GroupFeedback(group.size)
self.angle_offsets: 'npt.NDArray[np.float64]' = np.array(offsets)
self.prev_angles: 'npt.NDArray[np.float64]' = np.zeros(
group.size, dtype=np.float64)
self.group.feedback_frequency = 200.0
base_dir, _ = os.path.split(__file__)
hrdf_file = os.path.join(base_dir, 'hrdf/mapsArm_7DoF_standard.hrdf')
self.input_model = hebi.robot_model.import_from_hrdf(hrdf_file)
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 command_actuator(group, w, command_time, feedback_freq, tf, step):
"""
Send a command to the motor for a specified time
param group: actuators being commanded
param w: system frequency
return position, velocity and effort
"""
# set feedback
group.command_lifetime = command_time # set length of time of command (ms)
group.feedback_frequency = feedback_freq # This effectively limits the loop below to 200Hz
group_command = hebi.GroupCommand(
group.size) # Commands all modules in a group
group_feedback = hebi.GroupFeedback(group.size) # Reads feedback
# Open-loop controller commanding sine waves with different frequencies
t0 = 0.0
dt = 0.1
t = t0
# Initialize all array
pos = np.empty(0)
vel = np.empty(0)
eff = np.empty(0)
pwm = np.empty(0)
start = time()
# Run motor for specified time
while (t < tf - 2.0):
# measure variables
group_feedback = group.get_next_feedback(reuse_fbk=group_feedback)
#w_t = np.multiply(w,t)
#print('input effort:', group_command.effort)
#print('Position Feedback:\n{0}'.format(group_feedback.position))
pos = np.append(pos, group_feedback.position)
vel = np.append(vel, group_feedback.velocity)
eff = np.append(eff, group_feedback.effort)
pwm = np.append(pwm, group_feedback.pwm_command)
#sleep(dt)
t = time() - start
# reset all the pid gains
#group_command.control_strategy = 'directpwm'
print('strategy', group_command.control_strategy)
# send step response of torque
group_command.effort = step #0.001*np.cos(w_t)
#group_command.pwm_command = step #0.001*np.cos(w_t)
group.send_command(group_command)
# Run motor for specified time
while (t < tf):
# measure variables
group_feedback = group.get_next_feedback(reuse_fbk=group_feedback)
#w_t = np.multiply(w,t)
#print('input effort:', group_command.effort)
#print('Position Feedback:\n{0}'.format(group_feedback.position))
pos = np.append(pos, group_feedback.position)
vel = np.append(vel, group_feedback.velocity)
eff = np.append(eff, group_feedback.effort)
pwm = np.append(pwm, group_feedback.pwm_command)
#sleep(dt)
t = time() - start
group_command.effort = np.nan #0.001*np.cos(w_t)h
group.send_command(group_command)
return pos, vel, eff, pwm
sleep(2.0)
family_name = "Test Family"
module_name = "Test Actuator"
group = lookup.get_group_from_names([family_name], [module_name])
if group 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 = group.size
group_feedback = hebi.GroupFeedback(num_joints)
if group.get_next_feedback(reuse_fbk=group_feedback) is None:
print('Error getting feedback.')
exit(1)
positions = np.zeros((num_joints, 3), dtype=np.float64)
offset = [pi] * num_joints
current_pos = group_feedback.position
positions[:, 0] = current_pos
positions[:, 1] = current_pos + pi
positions[:, 2] = current_pos
time_vector = [0, 3, 6]
collide_fix_chop = np.argwhere(
dist_to_chop1 <= sphere_sizes[:-4] + THRESHOLD).ravel().tolist()
collide_moving_chop = np.argwhere(
dist_to_chop2 <= sphere_sizes[:-4] + THRESHOLD).ravel().tolist()
return col_pairs, collide_fix_chop, collide_moving_chop
# Use n to select the next sphere
if __name__ == '__main__':
my_bullet = Bullet(gui=True)
my_bullet.load_robot(URDF_PATH)
if USE_REAL_ROBOT:
import hebi
my_bullet.arm = tycho_env.create_robot(dof=7)
my_bullet.feedback = hebi.GroupFeedback(7)
my_bullet.current_position = np.zeros(7)
refinement = 0.01
my_bullet.load_spheres(SPHERES_CONFIG)
my_bullet.marionette(MOVING_POSITION)
cursor = 0
keys = ''
while True:
keys = p.getKeyboardEvents()
p.stepSimulation()
time.sleep(0.01)
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"
sleep(2.0)
family_name = "arm"
module_name = "J1"
group = lookup.get_group_from_names([family_name], [module_name])
group1 = lookup.get_group_from_names(["arm"], ["J2"])
group2 = lookup.get_group_from_names(["arm"], ["J3"])
if group 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)
group_command = hebi.GroupCommand(group.size)
group_feedback = hebi.GroupFeedback(group.size)
group_command1 = hebi.GroupCommand(group1.size)
group_feedback1 = hebi.GroupFeedback(group1.size)
group_command2 = hebi.GroupCommand(group2.size)
group_feedback2 = hebi.GroupFeedback(group2.size)
# Start logging in the background
#group.start_log('logs')
freq_hz = 0.5 # [Hz]
freq = freq_hz * 2.0 * pi # [rad / sec]
amp = pi * 0.25 # [rad] (45 degrees)
sleep(1)
duration = 2.0 # [sec]
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()
def initialize_hebi():
group = get_group()
feedback = hebi.GroupFeedback(group.size)
num_joints = group.size
command = hebi.GroupCommand(num_joints)
return group, feedback, command
def start(self):
with self._state_lock:
if self._started:
return
config = self._config
group = create_group(config)
group.feedback_frequency = config.robot_feedback_frequency
group.command_lifetime = config.robot_command_lifetime
num_modules = group.size
self._group = group
self._auxilary_group_command = hebi.GroupCommand(num_modules)
self._group_command = hebi.GroupCommand(num_modules)
self._group_feedback = hebi.GroupFeedback(num_modules)
self._group_info = hebi.GroupInfo(num_modules)
# Construction of legs
from math import radians
leg_angles = [
radians(entry)
for entry in [30.0, -30.0, 90.0, -90.0, 150.0, -150.0]
]
leg_distances = [0.2375, 0.2375, 0.1875, 0.1875, 0.2375, 0.2375]
leg_configs = ['left', 'right'] * 3
num_joints_in_leg = Leg.joint_count
parameters = self._params
# HACK: The mass is hardcoded as 21Kg. This hardcodes the weight to be the force here (which will be accurate as long as we are on Earth).
weight = 21.0 * 9.8
# The legs need to start with valid feedback, so we must wait for a feedback here
self._group.get_next_feedback(reuse_fbk=self._group_feedback)
legs = list()
for i, angle, distance, leg_configuration in zip(
range(num_modules), leg_angles, leg_distances,
leg_configs):
start_idx = num_joints_in_leg * i
indices = [start_idx, start_idx + 1, start_idx + 2]
leg = Leg(i, angle, distance, parameters, leg_configuration,
self._group_command.create_view(indices),
self._group_feedback.create_view(indices))
legs.append(leg)
self._legs = legs
self._weight = weight
joystick_selector = self._config.joystick_selector
while True:
try:
joy = joystick_selector()
if joy is None:
raise RuntimeError
self._joy = joy
break
except:
pass
load_gains(self)
from threading import Condition, Lock, Thread
start_condition = Condition(Lock())
def start_routine(start_condition):
with start_condition:
# 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()
self._start()
self._proc_thread = Thread(target=start_routine,
name='Hexapod 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
with start_condition:
self._proc_thread.start()
start_condition.wait()
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()
print('')
# Set command stuctures for SB motors
CmdSB = hebi.GroupCommand(GroupSB.size)
var = raw_input('Press y to continue, q to exit: \n')
if var == 'y':
print('\nYay!\n')
elif var == 'Y':
print('\nYay!\n')
else:
print('\nBye!\n')
sys.exit()
## Feedback Setup
fbk = hebi.GroupFeedback(GroupSB.size)
GroupSB.feedback_frequency = 200.0
#x = -cos(yaw)sin(pitch)sin(roll)-sin(yaw)cos(roll)
#y = -sin(yaw)sin(pitch)sin(roll)+cos(yaw)cos(roll)
#z = cos(pitch)sin(roll)
tStart = 0
tStop = 0
count = 0
while (1):
fbk = GroupSB.get_next_feedback(reuse_fbk=fbk)
tStop = timeit.default_timer()
#ts = tStop - tStart
fuse.update_nomag(fbk.accelerometer[0], fbk.gyro[0], tStop)
tStart = timeit.default_timer()
#### 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)
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
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:
feedback = imu.get_next_feedback(reuse_fbk=group_feedback)
if feedback is not None:
gyros = group_feedback.gyro
accels = group_feedback.accelerometer
efforts = group_feedback.effort
break
gyros = gyros[range(0, len(names), 3)]
accels = accels[range(0, len(names), 3)]
# gyroOffsets = [[np.mean(gyros[:,0])], [np.mean(gyros[:,1])], [np.mean(gyros[:,2])]]
# accelOffsets =[[np.mean(accels[:,0])], [np.mean(accels[:,1])], [np.mean(accels[:,2])]]
# efforts = np.reshape(efforts,[6,3])
