# quat2rotMat test:
#print(quat2rotMat([0.77545035, 0.4112074 , 0.24011487, -0.41464436]))
# R = [[ 0.54082968 0.84054625 0.03138466]
# [-0.44559821 0.31795693 -0.8368664 ]
# [-0.71340398 0.43861729 0.54650651]]
# Arm setup
arm_family = "Arm"
module_names = [
'J1_base', 'J2_shoulder', 'J3_elbow', 'J4_wrist1', 'J5_wrist2', 'J6_wrist3'
]
hrdf_file = "hrdf/A-2085-06.hrdf"
gains_file = "gains/A-2085-06.xml"
# Create Arm object
arm = arm_api.create([arm_family], names=module_names, hrdf_file=hrdf_file)
arm.load_gains(gains_file)
# mobileIO setup
phone_name = "mobileIO"
lookup = hebi.Lookup()
sleep(2)
# Setup MobileIO
print('Waiting for Mobile IO device to come online...')
m = create_mobile_io(lookup, arm_family, phone_name)
if m is None:
raise RuntimeError("Could not find Mobile IO device")
m.update()
# Demo Variables
sleep(2)
# Setup Mobile IO
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.set_button_mode(1, 'momentary')
m.set_button_mode(2, 'toggle')
m.update()
# Setup arm components
arm = arm_api.create([arm_family],
names=[
'J1_base', 'J2_shoulder', 'J3_elbow', 'J4_wrist1',
'J5_wrist2', 'J6_wrist3'
],
lookup=lookup,
hrdf_file=hrdf_file)
impedance_controller = arm_api.ImpedanceController()
# Configure arm components
arm.add_plugin(impedance_controller)
# hold position only (Allow rotation around end-effector position)
impedance_controller.gains_in_end_effector_frame = True
impedance_controller.set_kd(5, 5, 5, 0, 0, 0)
impedance_controller.set_kp(500, 500, 500, 0, 0, 0)
# Increase feedback frequency since we're calculating velocities at the
# high level for damping. Going faster can help reduce a little bit of