├── .gitattributes ├── .gitignore ├── LICENSE ├── README.adoc ├── advanced ├── 7DoFArmKit │ ├── ex_grav_comp.m │ ├── ex_teach_repeat.m │ ├── ex_trochar_impedance_control.m │ ├── setupArm.m │ ├── startup.m │ ├── teachRepeatGains.xml │ ├── trocharManipulationGains.xml │ └── trocharManipulationGains_w_Integral.xml ├── control_input.m ├── ex1_custom_pid_1_controller.m ├── ex1_custom_pid_2_setter.m ├── feedingArm │ ├── defaultDrinkingGains.xml │ ├── defaultFeedingGains.xml │ ├── defaultFeedingWaypoints.mat │ ├── feedingDemo.m │ ├── setupDrinkingArm.m │ ├── setupFeedingArm.m │ └── startup.m ├── kinematics │ ├── 3-DoF_arm_example.hrdf │ └── ik_edge_cases.m ├── startup.m └── youtube │ ├── io_board │ ├── demo1_live_feedback.m │ ├── demo2_live_commands.m │ ├── demo3_actuator.m │ └── demo4_logging.m │ ├── teleop_taxi │ ├── startup.m │ ├── teleop_taxi_full.m │ └── teleop_taxi_simple.m │ └── x5_teaser │ ├── startup.m │ ├── x5_teaser_demo2_stable_output.m │ ├── x5_teaser_demo3a_follow_imu.m │ ├── x5_teaser_demo3b_zero_torque.m │ └── x5_teaser_demo3c_ping_pong.m ├── basic ├── README.md ├── io_board │ ├── ex1a_lookup.m │ ├── ex1b_newGroup_single_module.m │ ├── ex1c_newGroup_multiple_modules.m │ ├── ex2a_feedback.m │ ├── ex2b_feedback_io.m │ └── startup.m ├── mobile_io │ ├── ex1a_lookup.m │ ├── ex1b_newGroup_single_module.m │ ├── ex1c_newGroup_multiple_modules.m │ ├── ex2a_feedback.m │ ├── ex2b_feedback_io_w_logging.m │ ├── ex2c_feedback_orientation.m │ ├── ex2d_feedback_full_pose.m │ ├── ex2e_feedback_pose_w_magnetometer.m │ ├── ex3a_feedback_io_and_mobile.m │ └── startup.m └── x-series_actuator │ ├── ex1a_lookup.m │ ├── ex1b_newGroup_single_module.m │ ├── ex1c_newGroup_multiple_modules.m │ ├── ex2a_feedback.m │ ├── ex2b_feedback_w_logging.m │ ├── ex2c_feedback_loading_log.m │ ├── ex2d_feedback_loading_log_ui.m │ ├── ex3a_command_position.m │ ├── ex3b_command_velocity.m │ ├── ex3c_command_effort.m │ ├── ex3d_command_pos_vel.m │ ├── ex3e_command_pos_vel_effort.m │ ├── ex3f_command_vel_w_feedback.m │ ├── ex4a_gains_positionKp.m │ ├── ex4b_gains_saving_xml.m │ ├── ex4c_gains_loading_xml.m │ ├── ex5a_trajectory_blocking.m │ ├── ex5b_trajectory_non_blocking.m │ ├── ex6a_kinematics_setup.m │ ├── ex6b_kinematics_fwd_kinematics.m │ ├── ex6c_kinematics_inv_kinematics.m │ ├── ex7a_robot_3_dof_arm.m │ ├── ex7b_robot_6_dof_arm.m │ ├── gains │ ├── 3-DoF_arm_gains.xml │ ├── 6-DoF_arm_gains.xml │ └── exampleGains.XML │ ├── hrdf │ ├── 3-DoF_arm_example.hrdf │ └── 6-DoF_arm_example.hrdf │ ├── logs │ └── exampleLog.hebilog │ └── startup.m ├── include ├── exchange │ └── SpinCalc │ │ ├── SpinCalc.m │ │ └── license.txt ├── experimental │ └── experimental_apis_may_change.txt ├── hebi │ ├── +HebiArmPlugins │ │ ├── DoubledJoint.m │ │ ├── DynamicsCompensation.m │ │ ├── EffortOffset.m │ │ ├── GravityCompensation.m │ │ └── ImpedanceController.m │ ├── CommandStruct.m │ ├── FeedbackStruct.m │ ├── FocCommandStruct.m │ ├── GainStruct.m │ ├── HebiArm.m │ ├── HebiArmPlugin.m │ ├── HebiEnum.m │ ├── HebiGripper.m │ ├── HebiGroup.m │ ├── HebiKinematics.m │ ├── HebiLookup.m │ ├── HebiMaps.m │ ├── HebiMobileIO.m │ ├── HebiPoseFilter.m │ ├── HebiTrajectory.m │ ├── HebiTrajectoryGenerator.m │ ├── HebiUtils.m │ ├── IoCommandStruct.m │ ├── LocalStateStruct.m │ ├── MapsGateConfig.m │ ├── SafetyParamsStruct.m │ ├── hebi-matlab-2.2.1.jar │ ├── hebi_config.m │ └── hebi_load.p ├── include.m ├── input │ ├── HebiJoystick.m │ ├── HebiKeyboard.m │ ├── joystickTest.m │ ├── lib │ │ ├── jinput-dx8.dll │ │ ├── jinput-dx8_64.dll │ │ ├── jinput-raw.dll │ │ ├── jinput-raw_64.dll │ │ ├── jinput-wintab.dll │ │ ├── libjinput-linux64.so │ │ └── libjinput-osx.jnilib │ └── matlab-input-1.2.jar ├── kinematics │ ├── FrameDisplay.m │ ├── R_x.m │ ├── R_y.m │ ├── R_z.m │ └── pinv_damped.m └── util │ ├── DemoUtils.m │ ├── SharedData.m │ ├── SharedTransform.m │ └── SonyPS4Gamepad.m └── kits ├── arms ├── config │ ├── A-2084-01.cfg.yaml │ ├── A-2084-01G.cfg.yaml │ ├── A-2085-03.cfg.yaml │ ├── A-2085-04.cfg.yaml │ ├── A-2085-05.cfg.yaml │ ├── A-2085-05G.cfg.yaml │ ├── A-2085-06.cfg.yaml │ ├── A-2085-06G.cfg.yaml │ ├── A-2099-07.cfg.yaml │ ├── A-2099-07G.cfg.yaml │ ├── A-2240-04.cfg.yaml │ ├── A-2240-05.cfg.yaml │ ├── A-2240-05G.cfg.yaml │ ├── A-2240-06.cfg.yaml │ ├── A-2240-06G.cfg.yaml │ ├── A-2302-01.cfg.yaml │ ├── A-2302-01G.cfg.yaml │ ├── A-2303-01.cfg.yaml │ ├── A-2303-01G.cfg.yaml │ ├── A-2580-04.cfg.yaml │ ├── A-2580-05.cfg.yaml │ ├── A-2580-05G.cfg.yaml │ ├── A-2580-06.cfg.yaml │ ├── A-2580-06G.cfg.yaml │ ├── A-2582-07.cfg.yaml │ ├── A-2582-07G.cfg.yaml │ ├── A-2590-01.cfg.yaml │ ├── A-2590-01G.cfg.yaml │ ├── ex_AR_kit.cfg.yaml │ ├── ex_AR_kit_w_gripper.cfg.yaml │ ├── ex_gravity_compensation.cfg.yaml │ ├── ex_gravity_compensation_toggle.cfg.yaml │ ├── ex_impedance_control_cartesian.cfg.yaml │ ├── ex_impedance_control_damping.cfg.yaml │ ├── ex_impedance_control_fixed.cfg.yaml │ ├── ex_impedance_control_gimbal.cfg.yaml │ ├── ex_mobile_io_control.cfg.yaml │ ├── ex_teach_repeat.cfg.yaml │ ├── ex_teach_repeat_w_gripper.cfg.yaml │ ├── gains │ │ ├── A-2080-01.xml │ │ ├── A-2084-01.xml │ │ ├── A-2085-03.xml │ │ ├── A-2085-04.xml │ │ ├── A-2085-05.xml │ │ ├── A-2085-06.xml │ │ ├── A-2099-07.xml │ │ ├── A-2240-04.xml │ │ ├── A-2240-05.xml │ │ ├── A-2240-06.xml │ │ ├── A-2255-01.xml │ │ ├── A-2302-01.xml │ │ ├── A-2303-01.xml │ │ ├── A-2580-04.xml │ │ ├── A-2580-05.xml │ │ ├── A-2580-06.xml │ │ ├── A-2582-07.xml │ │ ├── A-2590-01.xml │ │ ├── gripper_spool_gains.xml │ │ └── mirror_shoulder.xml │ ├── hrdf │ │ ├── A-2084-01.hrdf │ │ ├── A-2084-01G.hrdf │ │ ├── A-2085-03.hrdf │ │ ├── A-2085-04.hrdf │ │ ├── A-2085-05.hrdf │ │ ├── A-2085-05G.hrdf │ │ ├── A-2085-06.hrdf │ │ ├── A-2085-06G.hrdf │ │ ├── A-2099-07.hrdf │ │ ├── A-2099-07G.hrdf │ │ ├── A-2240-04.hrdf │ │ ├── A-2240-05.hrdf │ │ ├── A-2240-05G.hrdf │ │ ├── A-2240-06.hrdf │ │ ├── A-2240-06G.hrdf │ │ ├── A-2302-01.hrdf │ │ ├── A-2302-01G.hrdf │ │ ├── A-2303-01.hrdf │ │ ├── A-2303-01G.hrdf │ │ ├── A-2580-04.hrdf │ │ ├── A-2580-05.hrdf │ │ ├── A-2580-05G.hrdf │ │ ├── A-2580-06.hrdf │ │ ├── A-2580-06G.hrdf │ │ ├── A-2582-07.hrdf │ │ ├── A-2582-07G.hrdf │ │ ├── A-2590-01.hrdf │ │ └── A-2590-01G.hrdf │ └── layouts │ │ ├── ar_control_sm.json │ │ ├── ex_AR_kit.json │ │ ├── ex_AR_kit_w_gripper.json │ │ ├── ex_gravity_compensation_toggle.json │ │ ├── ex_impedance_control_cartesian.json │ │ ├── ex_impedance_control_damping.json │ │ ├── ex_impedance_control_fixed.json │ │ ├── ex_impedance_control_floor.json │ │ ├── ex_impedance_control_gimbal.json │ │ ├── ex_mobile_io_control.json │ │ ├── ex_teach_repeat.json │ │ ├── ex_teach_repeat_w_gripper.json │ │ └── joystick_control_sm.json ├── createGripperFromConfig.m ├── createMobileIOFromConfig.m ├── ex_AR_kit.m ├── ex_AR_kit_w_gripper.m ├── ex_gravity_compensation.m ├── ex_gravity_compensation_toggle.m ├── ex_impedance_control_cartesian.m ├── ex_impedance_control_damping.m ├── ex_impedance_control_fixed.m ├── ex_impedance_control_gimbal.m ├── ex_kinematics_log_analysis.m ├── ex_mobile_io_control.m ├── ex_teach_repeat.m ├── ex_teach_repeat_w_gripper.m ├── kinematics_analysis.m ├── readme.adoc └── startup.m ├── daisy ├── README.md ├── config │ └── A-2240-06_X-Daisy.yaml ├── daisyStart.sh ├── gains │ └── daisyLeg-Gains.xml ├── getBodyPoseFromFeet.m ├── getJoyCommands.m ├── getMSIJoyCommands.m ├── hrdf │ ├── Daisy.hrdf │ ├── daisyLeg-Left.hrdf │ └── daisyLeg-Right.hrdf ├── images │ ├── controller.png │ ├── daisy_coordinates.png │ ├── daisy_coordinates_small.png │ ├── daisy_labeled.png │ └── daisy_render.png ├── install.sh ├── joystickTest.m ├── makeDaisySafetyLimits.m ├── runDaisy.m ├── setDaisyParams.m ├── setupDaisyController.m └── startup.m ├── edward ├── edwardDemo.m ├── edwardDemoRunner.m ├── edwardGains.xml ├── edwardStart.sh ├── readme.adoc ├── setupEdward.m └── startup.m ├── igor ├── README.adoc ├── gains │ └── igorGains.xml ├── hrdf │ ├── igorArm-Left.hrdf │ ├── igorArm-Right.hrdf │ ├── igorChassis.hrdf │ └── igorLeg.hrdf ├── igor2DemoIntegrated.m ├── igor2StartupIntegrated.m ├── igorStart.bat ├── igorStart.sh ├── makeIgorKinematics.m ├── resources │ └── mobile_io_igor.png ├── startup.m └── tools │ ├── FramesDisplay.m │ ├── cropLog.m │ ├── downSampleLog.m │ ├── input │ ├── HebiJoystick.m │ ├── HebiKeyboard.m │ ├── joystickTest.m │ ├── lib │ │ ├── jinput-dx8.dll │ │ ├── jinput-dx8_64.dll │ │ ├── jinput-raw.dll │ │ ├── jinput-raw_64.dll │ │ ├── jinput-wintab.dll │ │ ├── libjinput-linux.so │ │ ├── libjinput-linux64.so │ │ └── libjinput-osx.jnilib │ └── matlab-input-1.2.jar │ ├── kinematics │ ├── DCM_Error.m │ ├── R_x.m │ ├── R_y.m │ ├── R_z.m │ ├── SpinCalc.m │ ├── drawAxes.m │ ├── quat2dcm.m │ └── quat_rotate_better.m │ ├── loadBurnInLogs.m │ └── loadHebiLogs.m ├── lily ├── README.md ├── gains │ └── lilyLeg-Gains.xml ├── getBodyPoseFromFeet.m ├── getJoyCommands.m ├── getMSIJoyCommands.m ├── hrdf │ ├── lilyChassis.hrdf │ ├── lilyLeg-Left.hrdf │ └── lilyLeg-Right.hrdf ├── images │ ├── controller.png │ ├── lily_Labeled.pptx │ ├── lily_axis.pptx │ ├── lily_coordinates.png │ ├── lily_labeled.png │ └── lily_render.png ├── install.sh ├── joystickTest.m ├── lilyStart.sh ├── makeLilyKinematics.m ├── makeLilySafetyLimits.m ├── runLily.m ├── setLilyParams.m ├── setupLilyController.m └── startup.m └── rosie ├── README.md ├── config └── A-2160-01_X-Rosie.yaml ├── gains ├── 6-dof-arm-gains-rosie.xml ├── diff-drive-wheel-gains.xml ├── gripper-gains.xml ├── mecanum-wheel-gains.xml └── omni-drive-wheel-gains.xml ├── hrdf ├── 6-DoF_arm_w_gripper.hrdf └── Rosie.hrdf ├── install.sh ├── rosieDemo.m ├── rosieDemoRunner.m ├── rosieStart.sh ├── setupArmWithGripper.m ├── setupDiffDriveBase.m ├── setupMecanumBase.m ├── setupOmniBase.m └── startup.m /.gitattributes: -------------------------------------------------------------------------------- 1 | * text=auto 2 | 3 | *.m text 4 | 5 | *.jar binary 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.m~ 2 | *.asv 3 | 4 | *.hebilog 5 | !**/exampleLog.hebilog 6 | 7 | *.DS_Store 8 | -------------------------------------------------------------------------------- /advanced/7DoFArmKit/ex_grav_comp.m: -------------------------------------------------------------------------------- 1 | % ------------------------------------------------------------------------- 2 | % NOTE 3 | % Controlling only torques (or forces) will always exhibit some amount of 4 | % drift due to noise in the sensors and a non-perfect model of the robot. 5 | % This can be mitigated by adding an extra controller that can add torques 6 | % to remain at a position when the robot is not actively beind held. 7 | % ------------------------------------------------------------------------- 8 | 9 | %% Setup 10 | % Robot specific setup. Edit as needed. 11 | [group, kin, gravityVec] = setupArm(); 12 | 13 | gains = HebiUtils.loadGains('teachRepeatGains.XML'); 14 | group.send('gains',gains); 15 | 16 | % Select the duration in seconds 17 | demoDuration = 30; 18 | 19 | % Setup Visualization 20 | framesDisplay = FrameDisplay(); 21 | 22 | % Start Logging 23 | % group.startLog; 24 | 25 | %% Gravity compensated mode 26 | cmd = CommandStruct(); 27 | demoTimer = tic(); 28 | while toc(demoTimer) < demoDuration 29 | 30 | % Gather sensor data 31 | fbk = group.getNextFeedback(); 32 | 33 | % Calculate required torques to negate gravity at current position 34 | cmd.effort = kin.getGravCompEfforts( fbk.position, gravityVec ); 35 | 36 | % Get Feedback to create a Visualization 37 | positions = fbk.position; 38 | frames = kin.getFK('output', positions); 39 | framesDisplay.setFrames(frames) 40 | drawnow; 41 | axis equal 42 | 43 | % Send to robot 44 | group.send(cmd); 45 | 46 | end 47 | 48 | % Stop Logging 49 | log = group.stopLogFull(); -------------------------------------------------------------------------------- /advanced/7DoFArmKit/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup sets up libraries and should be run once on startup. 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 6 | run(includeScript); 7 | 8 | end 9 | -------------------------------------------------------------------------------- /advanced/ex1_custom_pid_2_setter.m: -------------------------------------------------------------------------------- 1 | %% Custom PID - Target Setter 2 | % This example shows how the target in pid_1_controller.m can be set 3 | % from another instance. Needs to be run in a separate MATLAB instance. 4 | % 5 | %% 6 | % 7 | % 8 | % 9 | % 10 | % 11 | % 12 | % 13 | %
CreatedSept21, 2017
Last UpdateSept 21, 2017
API Versionhebi-matlab-1.0
RequirementsMATLAB 2013b or higher
14 | % 15 | % 16 | % Copyright 2017 HEBI Robotics 17 | 18 | %% Create shared memory to communicate with other instance 19 | % This creates a cmdPosition() function that returns a target 20 | % set point that can be modified via shared memory. Note that the 21 | % dimensions need to be the same for both instances 22 | numModules = 1; % MODIFY TO MATCH CONTROLLER ! 23 | target = SharedData('target', 'double', [1 numModules]); 24 | 25 | %% Step Input (open-loop) 26 | % This example shows an open-loop controller commanding alternating 27 | % step inputs of +/- amplitude to position 28 | amplitude = 1 * ones(1,numModules); % [rad] 29 | duration = 2; % [s] 30 | 31 | cmd = CommandStruct(); 32 | direction = 1; 33 | tNext = duration; 34 | t0 = tic(); 35 | while toc(t0) < 20 36 | 37 | % Flip direction 38 | if toc(t0) >= tNext 39 | tNext = tNext + duration; 40 | direction = -direction; 41 | end 42 | 43 | % Command position via shared memory 44 | target.data = amplitude * direction; 45 | pause(0.01); 46 | 47 | end 48 | 49 | -------------------------------------------------------------------------------- /advanced/feedingArm/defaultFeedingWaypoints.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/advanced/feedingArm/defaultFeedingWaypoints.mat -------------------------------------------------------------------------------- /advanced/feedingArm/setupDrinkingArm.m: -------------------------------------------------------------------------------- 1 | function [ group, kin, gravityVec ] = setupDrinkingArm() 2 | 3 | % NOTE: CHANGE AS NEEDED TO MATCH YOUR CONFIGURATION 4 | 5 | % Communications 6 | group = HebiLookup.newGroupFromNames( 'Arm', { 'Base' 7 | 'Shoulder' 8 | 'Elbow' 9 | 'Wrist1' 10 | 'Wrist2' } ); 11 | 12 | gains = HebiUtils.loadGains('defaultDrinkingGains.xml'); 13 | group.send('gains',gains); 14 | 15 | % End Effector Kinematics 16 | xyz_endEff = [.030 0 .01]; % Bottle opener 17 | rot_endEff = R_y(pi/2); 18 | 19 | % 4x4 Homogeneous transform of end effector output frame 20 | output_EndEff = eye(4); 21 | output_EndEff(1:3,4) = xyz_endEff; 22 | output_EndEff(1:3,1:3) = rot_endEff; 23 | 24 | comEndEff = xyz_endEff / 2; 25 | massEndEff = .2; % kg .2 default 26 | 27 | % Arm Kinematics 28 | kin = HebiKinematics(); 29 | kin.addBody('X5-4'); 30 | kin.addBody('X5-HeavyBracket', 'mount', 'right-outside'); 31 | kin.addBody('X5-9'); 32 | kin.addBody('X5-Link', 'extension', .325, 'twist', pi, 'mass', .300); 33 | kin.addBody('X5-9'); 34 | kin.addBody('X5-Link', 'extension', .325, 'twist', pi, 'mass', .300); 35 | kin.addBody('X5-1'); 36 | kin.addBody('X5-LightBracket', 'mount', 'right'); 37 | kin.addBody('X5-1'); 38 | kin.addBody('GenericLink', 'com', comEndEff, ... 39 | 'out', output_EndEff, ... 40 | 'mass', massEndEff ); 41 | 42 | baseFrame = eye(4); 43 | baseFrame(1:3,1:3) = R_z(pi); 44 | kin.setBaseFrame( baseFrame ); 45 | 46 | % Determine gravity vector (assumes fixed base frame) 47 | fbk = group.getNextFeedback(); 48 | gravityVec = -[fbk.accelX(1); fbk.accelY(1); fbk.accelZ(1)]; 49 | baseFrame = kin.getBaseFrame; 50 | gravityVec = baseFrame(1:3,1:3) * gravityVec; 51 | 52 | 53 | 54 | end 55 | 56 | -------------------------------------------------------------------------------- /advanced/feedingArm/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup sets up libraries and should be run once on startup. 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 6 | run(includeScript); 7 | 8 | end 9 | -------------------------------------------------------------------------------- /advanced/kinematics/3-DoF_arm_example.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /advanced/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup sets up libraries and should be run once on startup. 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | includeScript = fullfile(localDir, '..', 'include', 'include.m'); 6 | run(includeScript); 7 | 8 | end 9 | -------------------------------------------------------------------------------- /advanced/youtube/io_board/demo1_live_feedback.m: -------------------------------------------------------------------------------- 1 | %% Setup Figure Handles 2 | lineWidth = 2; 3 | figure(101); 4 | 5 | hold off; 6 | handle1 = plot(1, nan, 'ro', 'LineWidth', lineWidth); 7 | hold on; 8 | handle2 = plot(2, nan, 'go', 'LineWidth', lineWidth); 9 | handle3 = plot(3, nan, 'bo', 'LineWidth', lineWidth); 10 | hold off; 11 | 12 | ylim([-0.1 1.1]); 13 | xlim([0.9 3.1]); 14 | ylabel('value [0-1]'); 15 | 16 | legend button potentiometer loadCell; 17 | 18 | %% I/O Board Pin Mapping 19 | potentiometer = 'a1'; % analog input (0-5 volts) 20 | loadCell = 'a8'; % analog input (0-5 volts) 21 | inputButton = 'b1'; % digital input (0 or 1) 22 | 23 | %% Setup Groups 24 | ioBoard = HebiLookup.newGroupFromNames('*', 'IO_basic'); 25 | 26 | %% Live Display of Input Values 27 | while true 28 | 29 | % Read feedback 30 | fbk = ioBoard.getNextFeedbackIO(); 31 | 32 | % Update values 33 | handle1.YData = fbk.(inputButton); 34 | handle2.YData = fbk.(potentiometer); 35 | handle3.YData = fbk.(loadCell); 36 | 37 | % Force re-draw 38 | drawnow; 39 | 40 | end 41 | -------------------------------------------------------------------------------- /advanced/youtube/io_board/demo2_live_commands.m: -------------------------------------------------------------------------------- 1 | %% I/O Board Pin Mapping 2 | potentiometer = 'a1'; % analog input (0-5 volts) 3 | loadCell = 'a8'; % analog input (0-5 volts) 4 | inputButton = 'b1'; % digital input (0 or 1) 5 | led_r = 'e6'; % digital output (0 or 1) 6 | led_g = 'e7'; % digital output (0 or 1) 7 | led_b = 'e8'; % digital output (0 or 1) 8 | 9 | %% Setup Groups 10 | ioBoard = HebiLookup.newGroupFromNames('*', 'IO_basic'); 11 | 12 | %% Live LED Commands 13 | cmd = IOCommandStruct(); 14 | 15 | loadCellVoltageThreshold = 0.2; 16 | potentiometerVoltageThreshold = 0.5; 17 | 18 | while true 19 | 20 | % Read feedback 21 | fbk = ioBoard.getNextFeedbackIO(); 22 | 23 | % Update values 24 | cmd.(led_r) = fbk.(inputButton); 25 | cmd.(led_g) = fbk.(loadCell) > loadCellVoltageThreshold; 26 | cmd.(led_b) = fbk.(potentiometer) > potentiometerVoltageThreshold; 27 | 28 | % Send commands 29 | ioBoard.send(cmd); 30 | 31 | end 32 | -------------------------------------------------------------------------------- /advanced/youtube/io_board/demo3_actuator.m: -------------------------------------------------------------------------------- 1 | %% I/O Board Pin Mapping 2 | potentiometer = 'a1'; % analog input (0-5 volts) 3 | 4 | %% Setup Groups 5 | ioBoard = HebiLookup.newGroupFromNames('*', 'IO_basic'); 6 | actuator = HebiLookup.newGroupFromNames('*', 'X5-1_00004'); 7 | 8 | %% Command Actuator Position based on Ananlog Input 9 | tmpFbk = ioBoard.getNextFeedbackIO(); 10 | cmd = CommandStruct(); 11 | 12 | positionCmdScale = 2*pi; % Scales voltage to radians 13 | 14 | while true 15 | 16 | % Map analog sensor feedback to position command 17 | fbk = ioBoard.getNextFeedback(tmpFbk); 18 | cmd.position = fbk.(potentiometer) * positionCmdScale; 19 | actuator.send(cmd); 20 | 21 | end 22 | -------------------------------------------------------------------------------- /advanced/youtube/io_board/demo4_logging.m: -------------------------------------------------------------------------------- 1 | %% I/O Box Pin Mapping 2 | potentiometer = 'a1'; % analog input (0-5 volts) 3 | led_r = 'b6'; % digital input (0 or 1) 4 | 5 | %% Setup Groups 6 | ioBoard = HebiLookup.newGroupFromNames('*', 'IO_basic'); 7 | 8 | %% Start Background Logging 9 | ioBoard.startLog(); 10 | 11 | %% Flash red LED while data is recording 12 | cmd = IoCommandStruct(); 13 | cmd.(led_r) = 1; 14 | t = tic(); 15 | 16 | blinkPeriod = 0.1; 17 | 18 | while toc(t) < 5 19 | cmd.(led_r) = ~cmd.(led_r); 20 | ioBoard.send(cmd); 21 | pause(blinkPeriod); 22 | end 23 | 24 | cmd.(led_r) = 0; 25 | ioBoard.set(cmd); 26 | 27 | %% Stop Logging and Retreive Data 28 | log = ioBoard.stopLogIO(); 29 | 30 | %% Visualize Logged Data 31 | figure(101); 32 | hold off; 33 | plot(log.time, log.(potentiometer), 'LineWidth', 2); 34 | xlim( [0 log.time(end)] ); 35 | ylim( [0 1] ); 36 | xlabel('time (s)'); 37 | ylabel('potentiometer (V)'); 38 | -------------------------------------------------------------------------------- /advanced/youtube/teleop_taxi/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup sets up libraries and should be run once on startup. 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | includeScript = fullfile(localDir, '..', '..', '..', 'include', 'include.m'); 6 | run(includeScript); 7 | 8 | end 9 | -------------------------------------------------------------------------------- /advanced/youtube/x5_teaser/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup sets up libraries and should be run once on startup. 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | includeScript = fullfile(localDir, '..', '..', '..', 'include', 'include.m'); 6 | run(includeScript); 7 | 8 | end 9 | -------------------------------------------------------------------------------- /advanced/youtube/x5_teaser/x5_teaser_demo2_stable_output.m: -------------------------------------------------------------------------------- 1 | % Source code for X5 teaser video 2 | % 3 | % YouTube: X-Series Industrial Smart Actuator - HEBI Robotics 4 | % https://youtu.be/oHAddCWBobs?t=30s 5 | % 6 | % Requirements: MATLAB 2013b or higher 7 | % 8 | % Author: Florian Enner 9 | % Date: 23 March, 2016 10 | % Last Update: 04 July, 2017 11 | % API: hebi-matlab-1.0 12 | % 13 | % Copyright 2016 HEBI Robotics 14 | 15 | %% Setup 16 | group = HebiLookup.newGroupFromFamily('Demo2'); 17 | 18 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | % Demo 2) Keep output stable by cancelling IMU 20 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | cmd = CommandStruct(); 22 | fbk = group.getNextFeedback(); 23 | while true 24 | 25 | % Read feedback and counter IMU 26 | fbk = group.getNextFeedback(fbk); % call reuses memory 27 | cmd.velocity = -fbk.gyroZ; % gyro feedback is already unbiased 28 | group.send(cmd); 29 | 30 | end -------------------------------------------------------------------------------- /advanced/youtube/x5_teaser/x5_teaser_demo3b_zero_torque.m: -------------------------------------------------------------------------------- 1 | % Source code for X5 teaser video 2 | % 3 | % YouTube: X-Series Industrial Smart Actuator - HEBI Robotics 4 | % https://youtu.be/oHAddCWBobs?t=1m9s 5 | % 6 | % Requirements: MATLAB 2013b or higher 7 | % 8 | % Author: Florian Enner 9 | % Date: 23 March, 2016 10 | % Last Update: 04 July, 2017 11 | % API: hebi-matlab-1.0 12 | % 13 | % Copyright 2016 HEBI Robotics 14 | 15 | %% Setup 16 | group = HebiLookup.newGroupFromNames('Demo3', 'X5-4_003'); 17 | 18 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | % Demo 3) b) Zero Torque Mode 20 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | cmd = CommandStruct(); 22 | cmd.torque = 0; 23 | 24 | % Note that the zero torque commands needs to be sent in a loop to 25 | % prevent the command lifetime from turning off the actuator. While the 26 | % command lifetime could be deactivated for this particular demo, we prefer 27 | % to always keep it enabled. Aside from the safety benefits it also 28 | % prevents anyone else from commanding the actuator during the demo. 29 | while true 30 | 31 | % send command to actuator 32 | group.send(cmd); 33 | 34 | % pause some time to keep this loop from busy-spinning 35 | % and overloading the network 36 | pause(0.01); 37 | 38 | end -------------------------------------------------------------------------------- /basic/README.md: -------------------------------------------------------------------------------- 1 | # HEBI MATLAB API - Basic Examples 2 | 3 | This folder contains examples that help you get started with the HEBI 4 | Robotics APIs for MATLAB. There are separate examples for each of our 5 | different products: 6 | - X-Series Actuator 7 | - I/O Board 8 | - Mobile I/O 9 | 10 | The examples provided here work progressively through the following concepts: 11 | - Lookup / Groups 12 | - Feedback 13 | - Commands 14 | - Gains 15 | - Trajectories 16 | - Kinematics 17 | - Example - Robot Arm 18 | 19 | These API features and concepts are documented in more detail at: 20 | - http://docs.hebi.us/tools.html#matlab-api 21 | - http://docs.hebi.us/core_concepts.html 22 | 23 | 24 | # Setup 25 | 26 | To begin, run the 'startup.m' script. 27 | 28 | This will add the various folder paths and libraries to the MATLAB search 29 | path so that the API works correctly. 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /basic/io_board/ex1a_lookup.m: -------------------------------------------------------------------------------- 1 | % Initialize the lookup of modules and display information for any modules 2 | % on the network. A 'module' can be an actuator, and I/O board, or a 3 | % mobile device running the Mobile I/O app. 4 | % 5 | % For more information type: 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | HebiLookup.initialize(); 18 | 19 | disp(HebiLookup); 20 | 21 | disp(' NOTE:'); 22 | disp(' The table above should show the information for all the modules'); 23 | disp(' on the local network. If this table is empty make sure that the'); 24 | disp(' modules are connected, powered on, and that the status LEDs are'); 25 | disp(' displaying a green soft-fade.'); 26 | disp(' '); 27 | -------------------------------------------------------------------------------- /basic/io_board/ex1b_newGroup_single_module.m: -------------------------------------------------------------------------------- 1 | % Make a group consisting of a single module. A 'module' can be an 2 | % actuator, and I/O board, or a mobile device running the Mobile I/O app. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | % Only needed once per session, but it doesn't hurt to do this every time 18 | % we run a new script, just in case something changed on the network. 19 | HebiLookup.initialize(); 20 | 21 | % Use Scope to change select a module and change the name and family to 22 | % match the names below. Following examples will use the same names. 23 | familyName = 'Test Family'; 24 | moduleNames = 'Test IO Board'; 25 | 26 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ) -------------------------------------------------------------------------------- /basic/io_board/ex1c_newGroup_multiple_modules.m: -------------------------------------------------------------------------------- 1 | % Make a group consisting of multiple modules. A 'module' can be an 2 | % actuator, and I/O board, or a mobile device running the Mobile I/O app. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | % Only needed once per session, but it doesn't hurt to do this every time 18 | % we run a new script, just in case something changed on the network. 19 | HebiLookup.initialize(); 20 | 21 | familyName = 'Test Family'; 22 | moduleNames = {'Module1','Module2','Module3'}; 23 | 24 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ) 25 | -------------------------------------------------------------------------------- /basic/io_board/ex2a_feedback.m: -------------------------------------------------------------------------------- 1 | % Get feedback from a module and plot it live. 2 | % 3 | % Assumes that you have a group created with at least 1 module in it. 4 | % 5 | % HEBI Robotics 6 | % July 2018 7 | 8 | %% Setup 9 | clear *; 10 | close all; 11 | HebiLookup.initialize(); 12 | 13 | % Use Scope to change select a module and change the name and family to 14 | % match the names below. Following examples will use the same names. 15 | familyName = 'Test Family'; 16 | moduleNames = 'Test IO Board'; 17 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 18 | 19 | %% Visualize Gyro Feedback 20 | disp(' Plotting gyro data from the mobile device IMU.'); 21 | disp(' Move it around to make the feedback interesting...'); 22 | 23 | % Starts logging in the background 24 | group.startLog( 'dir', 'logs' ); 25 | 26 | figure(1); 27 | clf; 28 | 29 | duration = 10; % [sec] 30 | timer = tic(); 31 | while toc(timer) < duration 32 | 33 | % read a struct of sensor data 34 | fbk = group.getNextFeedback(); 35 | 36 | % visualize gyroscope data 37 | bar( [fbk.gyroX fbk.gyroY fbk.gyroZ] ); 38 | 39 | % format plot 40 | yAxisMaxLim = 15; 41 | yAxisMinLim = -15; 42 | ylim([yAxisMinLim yAxisMaxLim]); 43 | title( 'Module Gyro Feedback' ); 44 | xlabel( 'Axis' ); 45 | ylabel( 'Angular Velocity (rad/sec)'); 46 | grid on; 47 | drawnow; 48 | 49 | end 50 | 51 | disp(' All Done!'); 52 | 53 | % Stops background logging 54 | log = group.stopLog(); 55 | 56 | %% Plot the logged gyro feedback 57 | figure(101); 58 | plot( log.time, log.gyroX ); 59 | hold on; 60 | plot( log.time, log.gyroY ); 61 | plot( log.time, log.gyroZ ); 62 | hold off; 63 | title('3-Axis Gyro'); 64 | xlabel('time (sec)'); 65 | ylabel('angular velocity (rad/sec)'); 66 | legend gyroX gyroY gyroZ; 67 | grid on; 68 | -------------------------------------------------------------------------------- /basic/io_board/ex2b_feedback_io.m: -------------------------------------------------------------------------------- 1 | % Get feedback from a module, log in the background, and plot offline. 2 | % 3 | % Assumes that you have a group created with at least 1 module in it. 4 | % 5 | % HEBI Robotics 6 | % June 2018 7 | 8 | %% Setup 9 | clear *; 10 | close all; 11 | HebiLookup.initialize(); 12 | 13 | % Use Scope to change select a module and change the name and family to 14 | % match the names below. Following examples will use the same names. 15 | familyName = 'Test Family'; 16 | moduleNames = 'Test IO Board'; 17 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 18 | 19 | %% Visualize analog input a1 20 | disp(' We need to come up with something fun for the I/O Board...'); 21 | 22 | % Start logging in the background 23 | group.startLog( 'dir', 'logs' ); 24 | 25 | figure(1); 26 | clf; 27 | 28 | duration = 10; % [sec] 29 | timer = tic(); 30 | while toc(timer) < duration 31 | 32 | % Read feedback 33 | fbk = group.getNextFeedbackIO(); 34 | 35 | % Display 36 | bar( fbk.a1 ); 37 | 38 | % Format plot 39 | yAxisMaxLim = 5; 40 | yAxisMinLim = -5; 41 | ylim([yAxisMinLim yAxisMaxLim]); 42 | title( 'Module Output Velocity' ); 43 | ylabel( 'Angular Velocity (rad/sec)'); 44 | grid on; 45 | drawnow; 46 | 47 | end 48 | 49 | disp(' All done!'); 50 | 51 | % Stop background logging 52 | log = group.stopLogIO(); 53 | 54 | %% Plot the logged feedback 55 | figure(101); 56 | plot(log.time,log.a1); 57 | title('Pin a1'); 58 | xlabel('time (sec)'); 59 | ylabel('value'); 60 | grid on; 61 | -------------------------------------------------------------------------------- /basic/io_board/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /basic/mobile_io/ex1a_lookup.m: -------------------------------------------------------------------------------- 1 | % Initialize the lookup of modules and display information for any modules 2 | % on the network. A 'module' can be an actuator, and I/O board, or a 3 | % mobile device running the Mobile I/O app. 4 | % 5 | % For more information type: 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | HebiLookup.initialize(); 18 | 19 | disp(HebiLookup); 20 | 21 | disp(' NOTE:'); 22 | disp(' The table above should show the information for all the modules'); 23 | disp(' on the local network. If this table is empty make sure that the'); 24 | disp(' modules are connected, powered on, and that the status LEDs are'); 25 | disp(' displaying a green soft-fade.'); 26 | disp(' '); 27 | -------------------------------------------------------------------------------- /basic/mobile_io/ex1b_newGroup_single_module.m: -------------------------------------------------------------------------------- 1 | % Make a group consisting of a single module. A 'module' can be an 2 | % actuator, and I/O board, or a mobile device running the Mobile I/O app. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | % Only needed once per session, but it doesn't hurt to do this every time 18 | % we run a new script, just in case something changed on the network. 19 | HebiLookup.initialize(); 20 | 21 | % Use Scope to change select a module and change the name and family to 22 | % match the names below. Following examples will use the same names. 23 | familyName = 'HEBI'; 24 | moduleNames = 'mobileIO'; 25 | 26 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ) 27 | 28 | -------------------------------------------------------------------------------- /basic/mobile_io/ex1c_newGroup_multiple_modules.m: -------------------------------------------------------------------------------- 1 | % Make a group consisting of multiple modules. A 'module' can be an 2 | % actuator, and I/O board, or a mobile device running the Mobile I/O app. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | % Only needed once per session, but it doesn't hurt to do this every time 18 | % we run a new script, just in case something changed on the network. 19 | HebiLookup.initialize(); 20 | 21 | familyName = 'Test Family'; 22 | moduleNames = {'Module1','Module2','Module3'}; 23 | 24 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ) 25 | -------------------------------------------------------------------------------- /basic/mobile_io/ex2a_feedback.m: -------------------------------------------------------------------------------- 1 | % Get feedback from a module and plot it live. 2 | % 3 | % This script assumes that you have run 'startup.m' in this folder. 4 | % 5 | % HEBI Robotics 6 | % July 2018 7 | 8 | %% Setup 9 | clear *; 10 | close all; 11 | HebiLookup.initialize(); 12 | 13 | % Use Scope to change select a module and change the name and family to 14 | % match the names below. Following examples will use the same names. 15 | familyName = 'HEBI'; 16 | deviceName = 'mobileIO'; 17 | 18 | % The HebiMobileIO utility wrapper makes it easier to work with the 19 | % relevant parts of the group API. 20 | mobileIO = HebiMobileIO.findDevice(familyName, deviceName); 21 | mobileIO.initializeUI(); 22 | 23 | %% Visualize Gyro Feedback 24 | disp(' Plotting gyro data from the mobile device IMU.'); 25 | disp(' Move it around to make the feedback interesting...'); 26 | 27 | figure(1); 28 | clf; 29 | 30 | duration = 20; % [sec] 31 | timer = tic(); 32 | while toc(timer) < duration 33 | 34 | % Get Feedback 35 | mobileIO.update(); 36 | fbkMobile = mobileIO.getFeedbackMobile(); 37 | 38 | % Visualize gyroscope data 39 | bar( [fbkMobile.gyroX fbkMobile.gyroY fbkMobile.gyroZ] ); 40 | 41 | % Format plot 42 | yAxisMaxLim = 15; 43 | yAxisMinLim = -15; 44 | ylim([yAxisMinLim yAxisMaxLim]); 45 | title( 'Module Gyro Feedback' ); 46 | xlabel( 'Axis' ); 47 | ylabel( 'Angular Velocity (rad/sec)'); 48 | grid on; 49 | drawnow; 50 | 51 | end -------------------------------------------------------------------------------- /basic/mobile_io/ex2c_feedback_orientation.m: -------------------------------------------------------------------------------- 1 | % Get orientation feedback from a mobile device and visualize online 2 | % 3 | % This script assumes that you have run 'startup.m' in this folder. 4 | % 5 | % HEBI Robotics 6 | % July 2018 7 | 8 | %% Setup 9 | clear *; 10 | close all; 11 | HebiLookup.initialize(); 12 | 13 | % Use Scope to change select a module and change the name and family to 14 | % match the names below. Following examples will use the same names. 15 | familyName = 'HEBI'; 16 | deviceName = 'mobileIO'; 17 | 18 | % The HebiMobileIO utility wrapper makes it easier to work with the 19 | % relevant parts of the group API. 20 | mobileIO = HebiMobileIO.findDevice(familyName, deviceName); 21 | mobileIO.initializeUI(); 22 | 23 | mobileIO.group.startLog('dir','logs'); 24 | 25 | %% Visualize Device Orientation 26 | disp(' Visualizing orientation estimate from the mobile device.'); 27 | disp(' Move it around to make the feedback interesting...'); 28 | 29 | % Setup helper function to visualize the orientation 30 | xyzLimits = [ -0.05 0.05; 31 | -0.05 0.05; 32 | -0.05 0.05 ]; 33 | frameDisplay = FrameDisplay( [], [], xyzLimits ); 34 | 35 | duration = 30; % [sec] 36 | timer = tic(); 37 | while toc(timer) < duration 38 | 39 | % Get the orientation feedback 40 | mobileIO.update(); 41 | mobileRotMat = mobileIO.getOrientation(); 42 | 43 | % Visualize result 44 | frame = eye(4); 45 | frame(1:3,1:3) = mobileRotMat; 46 | frameDisplay.setFrames(frame) 47 | drawnow; 48 | 49 | end 50 | 51 | disp(' All done!'); 52 | 53 | log = mobileIO.group.stopLogMobile(); 54 | 55 | -------------------------------------------------------------------------------- /basic/mobile_io/ex2d_feedback_full_pose.m: -------------------------------------------------------------------------------- 1 | % Get full pose feedback from a mobile device and visualize online 2 | % 3 | % This script assumes that you have run 'startup.m' in this folder. 4 | % 5 | % HEBI Robotics 6 | % July 2018 7 | 8 | %% Setup 9 | clear *; 10 | close all; 11 | HebiLookup.initialize(); 12 | 13 | % Use Scope to change select a module and change the name and family to 14 | % match the names below. Following examples will use the same names. 15 | familyName = 'HEBI'; 16 | deviceName = 'mobileIO'; 17 | 18 | % The HebiMobileIO utility wrapper makes it easier to work with the 19 | % relevant parts of the group API. 20 | mobileIO = HebiMobileIO.findDevice(familyName, deviceName); 21 | mobileIO.initializeUI(); 22 | 23 | mobileIO.group.startLog('dir','logs'); 24 | 25 | %% Visualize Full Pose 26 | disp(' Visualizing 6-DoF pose estimate from the mobile device.'); 27 | disp(' Move it around to make the feedback interesting...'); 28 | 29 | % Setup helper function to visualize the pose 30 | frameDisplay = FrameDisplay(); 31 | 32 | duration = 30; % [sec] 33 | timer = tic(); 34 | while toc(timer) < duration 35 | 36 | % Update Feedback 37 | mobileIO.update(); 38 | [arTransform, arQuality] = mobileIO.getArPose(); 39 | 40 | frameDisplay.setFrames( arTransform ); 41 | title( ['AR Quality: ' num2str(arQuality)] ); 42 | drawnow; 43 | 44 | end 45 | 46 | disp(' All done!'); 47 | 48 | log = mobileIO.group.stopLogMobile(); 49 | -------------------------------------------------------------------------------- /basic/mobile_io/ex3a_feedback_io_and_mobile.m: -------------------------------------------------------------------------------- 1 | % Simultaneously read analog, digital inputs, and gyro feedback, 2 | % and visualize online. 3 | % 4 | % This script assumes that you have run 'startup.m' in this folder. 5 | % 6 | % HEBI Robotics 7 | % July 2018 8 | 9 | %% Setup 10 | clear *; 11 | close all; 12 | HebiLookup.initialize(); 13 | mobileIO = HebiMobileIO.findDevice('HEBI', 'mobileIO'); 14 | mobileIO.initializeUI(); 15 | 16 | %% Gather data w/ visualization 17 | disp(' Drag the sliders on the app screen and move the device...'); 18 | 19 | % Orientation visualization 20 | frameDisplay = FrameDisplay(); 21 | figure(2); 22 | 23 | t0 = tic(); 24 | while toc(t0) < 30 25 | 26 | % Read new sensor data 27 | mobileIO.update(); 28 | [mobileFbk, ioFbk] = mobileIO.getFeedback(); 29 | 30 | % Visualize digital (buttons) and analog (sliders) feedback 31 | subplot(2,1,1); 32 | bar( [ioFbk.b1 ioFbk.b2 ioFbk.b3 ioFbk.b4 ioFbk.b5 ioFbk.b6 ioFbk.b7 ioFbk.b8], 'r' ); 33 | hold on; 34 | bar( [ioFbk.a1 ioFbk.a2 ioFbk.a3 ioFbk.a4 ioFbk.a5 ioFbk.a6 ioFbk.a7 ioFbk.a8], 'b' ); 35 | hold off; 36 | 37 | ylim([-1 1]); 38 | title('Digital Inputs (red) and Analog Inputs (blue)'); 39 | ylabel('[-1 to 1]'); 40 | grid on; 41 | 42 | % Visualize gyroscope data 43 | subplot(2,1,2); 44 | bar( [mobileFbk.gyroX mobileFbk.gyroY mobileFbk.gyroZ] ); 45 | 46 | yAxisMaxLim = 15; 47 | yAxisMinLim = -15; 48 | ylim([yAxisMinLim yAxisMaxLim]); 49 | title( 'Module Gyro Feedback' ); 50 | xlabel( 'Axis' ); 51 | ylabel( 'Angular Velocity (rad/sec)'); 52 | grid on; 53 | drawnow; 54 | 55 | end 56 | 57 | disp(' All done!'); -------------------------------------------------------------------------------- /basic/mobile_io/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex1a_lookup.m: -------------------------------------------------------------------------------- 1 | % Initialize the lookup of modules and display information for any modules 2 | % on the network. A 'module' can be an actuator, an I/O board, or a 3 | % mobile device running the Mobile I/O app. 4 | % 5 | % For more information type: 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | HebiLookup.initialize(); 18 | 19 | disp(HebiLookup); 20 | 21 | disp(' NOTE:'); 22 | disp(' The table above should show the information for all the modules'); 23 | disp(' on the local network. If this table is empty make sure that the'); 24 | disp(' modules are connected, powered on, and that the status LEDs are'); 25 | disp(' displaying a green soft-fade.'); 26 | disp(' '); 27 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex1b_newGroup_single_module.m: -------------------------------------------------------------------------------- 1 | % Make a group consisting of a single module. A 'module' can be an 2 | % actuator, an I/O board, or a mobile device running the Mobile I/O app. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | % Only needed once per session, but it doesn't hurt to do this every time 18 | % we run a new script, just in case something changed on the network. 19 | HebiLookup.initialize(); 20 | 21 | % Use Scope to change select a module and change the name and family to 22 | % match the names below. Following examples will use the same names. 23 | familyName = 'Test Family'; 24 | moduleName = 'Test Actuator'; 25 | 26 | group = HebiLookup.newGroupFromNames( familyName, moduleName ) 27 | 28 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex1c_newGroup_multiple_modules.m: -------------------------------------------------------------------------------- 1 | % Make a group consisting of multiple modules. A 'module' can be an 2 | % actuator, an I/O board, or a mobile device running the Mobile I/O app. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % help HebiLookup 7 | % 8 | % This script assumes that you have run 'startup.m' in this folder. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | 17 | % Only needed once per session, but it doesn't hurt to do this every time 18 | % we run a new script, just in case something changed on the network. 19 | HebiLookup.initialize(); 20 | 21 | familyName = 'Test Family'; 22 | moduleNames = {'Actuator1','Actuator2','Actuator3'}; 23 | 24 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ) 25 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex2a_feedback.m: -------------------------------------------------------------------------------- 1 | % This example does a live visualization of sensor data gathered from 2 | % an actuator's gyroscope. 3 | % 4 | % For more information type: 5 | % help HebiGroup 6 | % 7 | % HEBI Robotics 8 | % June 2018 9 | 10 | %% Setup group 11 | clear *; 12 | close all; 13 | HebiLookup.initialize(); 14 | 15 | % Use Scope to change select a module and change the name and family to 16 | % match the names below. Following examples will use the same names. 17 | familyName = 'Test Family'; 18 | moduleNames = 'Test Actuator'; 19 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 20 | 21 | %% Visualize Gyro Feedback 22 | disp(' Plotting gyro data from the module IMU.'); 23 | disp(' Move the module around to make the feedback interesting...'); 24 | 25 | figure(1); 26 | clf; 27 | 28 | duration = 10; % [sec] 29 | timer = tic(); 30 | while toc(timer) < duration 31 | 32 | % read a struct of sensor data 33 | fbk = group.getNextFeedback(); 34 | 35 | % visualize gyroscope data 36 | bar( [fbk.gyroX fbk.gyroY fbk.gyroZ] ); 37 | 38 | % format plot 39 | yAxisMaxLim = 15; 40 | yAxisMinLim = -15; 41 | ylim([yAxisMinLim yAxisMaxLim]); 42 | title('Actuator Gyro Feedback'); 43 | xlabel('Axis'); 44 | ylabel('Angular Velocity (rad/sec)'); 45 | grid on; 46 | drawnow; 47 | 48 | end 49 | 50 | disp(' All Done!'); 51 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex2c_feedback_loading_log.m: -------------------------------------------------------------------------------- 1 | % Load a saved log file with feedback from an actuator and plot some of 2 | % the data. 3 | % 4 | % For more information type: 5 | % help HebiUtils 6 | % help HebiGroup 7 | % 8 | % HEBI Robotics 9 | % July 2018 10 | 11 | %% 12 | clear *; 13 | close all; 14 | 15 | logFileFolder = './logs/'; 16 | logFileName = 'exampleLog.hebilog'; % the '.hebilog' is optional 17 | 18 | log = HebiUtils.loadGroupLog( [logFileFolder logFileName] ); 19 | 20 | % Plot using some handy helper functions 21 | HebiUtils.plotLogs( log, 'position', 'figNum', 101 ); 22 | HebiUtils.plotLogs( log, 'velocity', 'figNum', 102 ); 23 | HebiUtils.plotLogs( log, 'effort', 'figNum', 103 ); 24 | 25 | 26 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex2d_feedback_loading_log_ui.m: -------------------------------------------------------------------------------- 1 | % Load a saved log file with feedback from an actuator and plot some of 2 | % the data. 3 | % 4 | % For more information type: 5 | % help HebiUtils 6 | % help HebiGroup 7 | % 8 | % HEBI Robotics 9 | % July 2018 10 | 11 | %% 12 | clear *; 13 | close all; 14 | 15 | % Bring up a dialog box to interactively choose log file(s) to load. Note 16 | % that when using the UI to select logs they get returned as a cell array, 17 | % even if only one log is selected. 18 | logs = HebiUtils.loadGroupLogsUI(); 19 | 20 | % Plot using some handy helper functions 21 | HebiUtils.plotLogs( logs, 'position', 'figNum', 101 ); 22 | HebiUtils.plotLogs( logs, 'velocity', 'figNum', 102 ); 23 | HebiUtils.plotLogs( logs, 'effort', 'figNum', 103 ); 24 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex3a_command_position.m: -------------------------------------------------------------------------------- 1 | % Send position commands, log in the background, and plot offline. 2 | % 3 | % For more information type: 4 | % help CommandStruct 5 | % help HebiGroup 6 | % 7 | % This script assumes you can create a group with 1 module. 8 | % 9 | % HEBI Robotics 10 | % June 2018 11 | 12 | %% Setup 13 | clear *; 14 | close all; 15 | HebiLookup.initialize(); 16 | 17 | familyName = 'Test Family'; 18 | moduleNames = 'Test Actuator'; 19 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 20 | 21 | %% Open-Loop Controller (Position) 22 | % The command struct has fields for position, velocity, and effort. 23 | % Fields that are empty [] or NaN will be ignored when sending. 24 | cmd = CommandStruct(); 25 | 26 | % Starts logging in the background 27 | group.startLog( 'dir', 'logs' ); 28 | 29 | % Parameters for sin/cos function 30 | freqHz = 1.0; % [Hz] 31 | freq = freqHz * 2*pi; % [rad / sec] 32 | amp = deg2rad( 45 ); % [rad] 33 | 34 | duration = 10; % [sec] 35 | timer = tic(); 36 | while toc(timer) < duration 37 | 38 | % Even though we don't use the feedback, getting feedback conveniently 39 | % limits the loop rate to the feedback frequency 40 | fbk = group.getNextFeedback(); 41 | 42 | % Update position set point 43 | cmd.position = amp * sin( freq * toc(timer) ); 44 | group.send(cmd); 45 | 46 | end 47 | 48 | % Stop logging and plot the position data using helper functions 49 | log = group.stopLog(); 50 | HebiUtils.plotLogs( log, 'position' ); 51 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex3b_command_velocity.m: -------------------------------------------------------------------------------- 1 | % Send velocity commands, log in the background, and plot offline. 2 | % 3 | % For more information type: 4 | % help CommandStruct 5 | % help HebiGroup 6 | % 7 | % This script assumes you can create a group with 1 module. 8 | % 9 | % HEBI Robotics 10 | % June 2018 11 | 12 | %% Setup 13 | clear *; 14 | close all; 15 | HebiLookup.initialize(); 16 | 17 | familyName = 'Test Family'; 18 | moduleNames = 'Test Actuator'; 19 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 20 | 21 | %% Open-Loop Controller (Velocity) 22 | % The command struct has fields for position, velocity, and effort. 23 | % Fields that are empty [] or NaN will be ignored when sending. 24 | cmd = CommandStruct(); 25 | 26 | % Starts logging in the background 27 | group.startLog( 'dir', 'logs' ); 28 | 29 | % Parameters for sin/cos function 30 | freqHz = 0.5; % [Hz] 31 | freq = freqHz * 2*pi; % [rad / sec] 32 | amp = 1.0; % [rad / sec] 33 | 34 | duration = 10; % [sec] 35 | timer = tic(); 36 | while toc(timer) < duration 37 | 38 | % Even though we don't use the feedback, getting feedback conveniently 39 | % limits the loop rate to the feedback frequency 40 | fbk = group.getNextFeedback(); 41 | 42 | % Update velocity set point 43 | cmd.velocity = amp * sin( freq * toc(timer) ); 44 | group.send(cmd); 45 | 46 | end 47 | 48 | % Stop logging and plot the velocity data using helper functions 49 | log = group.stopLog(); 50 | HebiUtils.plotLogs( log, 'velocity' ); 51 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex3c_command_effort.m: -------------------------------------------------------------------------------- 1 | % Send effort commands, log in the background, and plot offline. 2 | % 3 | % For more information type: 4 | % help CommandStruct 5 | % help HebiGroup 6 | % 7 | % This script assumes you can create a group with 1 module. 8 | % 9 | % HEBI Robotics 10 | % June 2018 11 | 12 | %% Setup 13 | clear *; 14 | close all; 15 | HebiLookup.initialize(); 16 | 17 | familyName = 'Test Family'; 18 | moduleNames = 'Test Actuator'; 19 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 20 | 21 | %% Command Loop (Effort) 22 | % The commmand struct has fields for position, velocity, and effort. 23 | % Fields that are empty [] or NaN will be ignored when sending. 24 | cmd = CommandStruct(); 25 | 26 | % Starts logging in the background 27 | group.startLog( 'dir', 'logs' ); 28 | 29 | % Parameters for sin/cos function 30 | freqHz = 0.5; % [Hz] 31 | freq = freqHz * 2*pi; % [rad / sec] 32 | amp = 1; % [Nm] 33 | 34 | duration = 10; % [sec] 35 | timer = tic(); 36 | while toc(timer) < duration 37 | 38 | % Even though we don't use the feedback, getting feedback conveniently 39 | % limits the loop rate to the feedback frequency 40 | fbk = group.getNextFeedback(); 41 | 42 | % Update effort set point 43 | cmd.effort = amp * sin( freq * toc(timer) ); 44 | group.send(cmd); 45 | 46 | end 47 | 48 | % Stop logging and plot the effort data using helper functions 49 | log = group.stopLog(); 50 | HebiUtils.plotLogs( log, 'effort' ); -------------------------------------------------------------------------------- /basic/x-series_actuator/ex3d_command_pos_vel.m: -------------------------------------------------------------------------------- 1 | % Send simultaneous position and velocity commands, log in the background, 2 | % and plot offline. 3 | % 4 | % For more information type: 5 | % help CommandStruct 6 | % help HebiGroup 7 | % 8 | % This script assumes you can create a group with 1 module. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% Setup 14 | clear *; 15 | close all; 16 | HebiLookup.initialize(); 17 | 18 | familyName = 'Test Family'; 19 | moduleNames = 'Test Actuator'; 20 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 21 | 22 | %% Command Loop (Position + Velocity) 23 | cmd = CommandStruct(); 24 | group.startLog( 'dir', 'logs' ); 25 | 26 | % Parameters for sin/cos function 27 | freqHz = 1.0; % [Hz] 28 | freq = freqHz * 2*pi; % [rad / sec] 29 | amp = deg2rad( 45 ); % [rad] 30 | 31 | duration = 10; % [sec] 32 | timer = tic(); 33 | while toc(timer) < duration 34 | 35 | % Even though we don't use the feedback, getting feedback conveniently 36 | % limits the loop rate to the feedback frequency 37 | fbk = group.getNextFeedback(); 38 | t = toc(timer); 39 | 40 | % Position command 41 | cmdPosition = amp * sin( freq * t ); 42 | 43 | % Velocity command (time-derivative of position) 44 | cmdVelocity = freq * amp * cos( freq * t ); 45 | 46 | % Update set points 47 | cmd.position = cmdPosition; 48 | cmd.velocity = cmdVelocity; 49 | group.send(cmd); 50 | 51 | end 52 | 53 | % Stop logging and plot the commands using helper functions 54 | log = group.stopLog(); 55 | HebiUtils.plotLogs( log, 'position', 'figNum', 101 ); 56 | HebiUtils.plotLogs( log, 'velocity', 'figNum', 102 ); 57 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex3f_command_vel_w_feedback.m: -------------------------------------------------------------------------------- 1 | % Send velocity commands based on feedback from a module's internal gyro, 2 | % log in the background, and plot offline. 3 | % 4 | % For more information type: 5 | % help CommandStruct 6 | % help HebiGroup 7 | % 8 | % This script assumes you can create a group with 1 module. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% Setup 14 | clear *; 15 | close all; 16 | HebiLookup.initialize(); 17 | 18 | familyName = 'Test Family'; 19 | moduleNames = 'Test Actuator'; 20 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 21 | 22 | %% Closed-Loop Controller (Velocity) 23 | cmd = CommandStruct(); 24 | group.startLog( 'dir', 'logs' ); 25 | 26 | disp(' Move the module to make the output move...'); 27 | duration = 15; % [sec] 28 | timer = tic(); 29 | while toc(timer) < duration 30 | 31 | fbk = group.getNextFeedback(); 32 | 33 | % Command a velocity that counters the measured angular velocity 34 | % around the z-axis (same axis as the output). 35 | cmd.velocity = -fbk.gyroZ; 36 | 37 | group.send(cmd); 38 | 39 | end 40 | 41 | disp(' All done!'); 42 | 43 | % Stop background logging and plot data 44 | log = group.stopLog(); 45 | HebiUtils.plotLogs( log, 'velocity', 'figNum', 101 ); 46 | HebiUtils.plotLogs( log, 'gyroZ', 'figNum', 102 ); 47 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex4b_gains_saving_xml.m: -------------------------------------------------------------------------------- 1 | % Save and load gains using the cross-API gains XML format. 2 | % 3 | % For more information type: 4 | % help GainStruct 5 | % help HebiUtils 6 | % help HebiGroup 7 | % 8 | % This script assumes you can create a group with 1 module. 9 | % 10 | % HEBI Robotics 11 | % July 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | HebiLookup.initialize(); 17 | 18 | familyName = 'Test Family'; 19 | moduleNames = 'Test Actuator'; 20 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 21 | 22 | % Get the gains that are currently active on the actuator and save them 23 | gains = group.getGains(); 24 | HebiUtils.saveGains( gains, './gains/myActuatorGains.xml' ); 25 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex4c_gains_loading_xml.m: -------------------------------------------------------------------------------- 1 | % Save and load gains using the cross-API gains XML format. 2 | % 3 | % For more information type: 4 | % help GainStruct 5 | % help HebiUtils 6 | % help HebiGroup 7 | % 8 | % This script assumes you can create a group with 1 module. 9 | % 10 | % HEBI Robotics 11 | % July 2018 12 | 13 | %% 14 | clear *; 15 | close all; 16 | HebiLookup.initialize(); 17 | 18 | familyName = 'Test Family'; 19 | moduleNames = 'Test Actuator'; 20 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 21 | 22 | % Load the gains from a saved file 23 | newGains = HebiUtils.loadGains( './gains/exampleGains.xml' ); 24 | disp(newGains); % display 25 | 26 | % Send the new gains to the actuator 27 | group.send( 'gains', newGains ); 28 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex5a_trajectory_blocking.m: -------------------------------------------------------------------------------- 1 | % Generate a trajectory and execute it using the blocking API. This code 2 | % does the exact same commands as the next non-blocking API example. 3 | % 4 | % For more information type: 5 | % help HebiTrajectoryGenerator 6 | % help HebiTrajectory 7 | % 8 | % This script assumes you can create a group with 1 module. 9 | % 10 | % HEBI Robotics 11 | % June 2018 12 | 13 | %% Setup Group 14 | clear *; 15 | close all; 16 | HebiLookup.initialize(); 17 | 18 | familyName = 'Test Family'; 19 | moduleNames = 'Test Actuator'; 20 | group = HebiLookup.newGroupFromNames( familyName, moduleNames ); 21 | 22 | %% Blocking Trajectory 23 | trajGen = HebiTrajectoryGenerator(); 24 | group.startLog( 'dir', 'logs' ); 25 | 26 | % Go from 0 to 180-degrees in 3 seconds 27 | waypoints = [ 28 | 0; 29 | pi ]; 30 | time = [ 0 3 ]; 31 | 32 | % This function generates smooth minimum jerk trajectories 33 | trajectory = trajGen.newJointMove( waypoints, 'time', time ); 34 | 35 | % Visualize the trajectory 36 | HebiUtils.plotTrajectory(trajectory); 37 | drawnow; 38 | 39 | % This function executes the trajectory using the 'blocking' API. This 40 | % means that your program will go into this function and 'block' the rest 41 | % of the code from running until it is done. This makes the high-level 42 | % code convenient, but it makes it more difficult to grab and analyze 43 | % feedback while the motion is being executed. 44 | trajGen.executeTrajectory( group, trajectory ); 45 | 46 | % Reverse the waypoints and to go back to the first position 47 | waypoints = flipud(waypoints); 48 | trajectory = trajGen.newJointMove( waypoints, 'time', time ); 49 | trajGen.executeTrajectory( group, trajectory ); 50 | 51 | % Stop logging and plot the velocity data using helper functions 52 | log = group.stopLog(); 53 | HebiUtils.plotLogs( log, 'position', 'figNum', 101 ); 54 | HebiUtils.plotLogs( log, 'velocity', 'figNum', 102 ); 55 | -------------------------------------------------------------------------------- /basic/x-series_actuator/ex6a_kinematics_setup.m: -------------------------------------------------------------------------------- 1 | % Setup robot kinematics based on HEBI Robot Definition Format (HRDF) file. 2 | % 3 | % For more information type: 4 | % help HebiKinematics 5 | % 6 | % HEBI Robotics 7 | % July 2018 8 | 9 | %% 10 | clear *; 11 | close all; 12 | 13 | % Load the kinematics from HRDF file 14 | kin = HebiUtils.loadHRDF('./hrdf/3-DoF_arm_example.hrdf'); 15 | 16 | % Display the basic kinematics information 17 | disp(kin); -------------------------------------------------------------------------------- /basic/x-series_actuator/gains/3-DoF_arm_gains.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 4 4 | 5 | 15 30 10 6 | 0 0 0 7 | 0 0 0 8 | 0 0 0 9 | 0 0 0 10 | 1 1 1 11 | 0 0 0 12 | -inf -inf -inf 13 | inf inf inf 14 | 1 1 1 15 | -20 -20 -10 16 | 20 20 10 17 | 1 1 1 18 | 1 1 1 19 | 20 | 21 | 0.05 0.05 0.05 22 | 0 0 0 23 | 0 0 0 24 | 1 1 1 25 | 0 0 0 26 | 0.25 0.25 0.25 27 | 0 0 0 28 | -1.502675 -1.502675 -3.434687 29 | 1.502675 1.502675 3.434687 30 | 1 1 1 31 | -1 -1 -1 32 | 1 1 1 33 | 0.75 0.75 0.75 34 | 1 1 1 35 | 36 | 37 | 0.25 0.25 0.25 38 | 0 0 0 39 | 0.001 0.001 0.001 40 | 1 1 1 41 | 0 0 0 42 | 0.25 0.25 0.25 43 | 0 0 0 44 | -20 -20 -10 45 | 20 20 10 46 | 1 1 1 47 | -1 -1 -1 48 | 1 1 1 49 | 0.9 0.9 0.9 50 | 0 0 0 51 | 52 | 53 | -------------------------------------------------------------------------------- /basic/x-series_actuator/gains/exampleGains.XML: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 4 7 | 8 | 15 9 | 0 10 | 0 11 | 0 12 | 0 13 | 1 14 | 0 15 | -inf 16 | inf 17 | 1 18 | -4 19 | 4 20 | 1 21 | 1 22 | 23 | 24 | 0.05 25 | 0 26 | 0 27 | 1 28 | 0 29 | 0.25 30 | 0 31 | -9.617128 32 | 9.617128 33 | 1 34 | -1 35 | 1 36 | 0.75 37 | 1 38 | 39 | 40 | 0.25 41 | 0 42 | 0.001 43 | 1 44 | 0 45 | 0.25 46 | -0 47 | -4 48 | 4 49 | 1 50 | -1 51 | 1 52 | 0.9 53 | 0 54 | 55 | -------------------------------------------------------------------------------- /basic/x-series_actuator/hrdf/3-DoF_arm_example.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /basic/x-series_actuator/hrdf/6-DoF_arm_example.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /basic/x-series_actuator/logs/exampleLog.hebilog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/basic/x-series_actuator/logs/exampleLog.hebilog -------------------------------------------------------------------------------- /basic/x-series_actuator/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /include/exchange/SpinCalc/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2011, John Fuller 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /include/experimental/experimental_apis_may_change.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/experimental/experimental_apis_may_change.txt -------------------------------------------------------------------------------- /include/hebi/+HebiArmPlugins/DynamicsCompensation.m: -------------------------------------------------------------------------------- 1 | classdef DynamicsCompensation < HebiArmPlugin 2 | % DynamicsCompensation arm plugin 3 | % 4 | % Compensates for the mass moved by joint accelerations. 5 | 6 | methods 7 | 8 | function this = DynamicsCompensation() 9 | end 10 | 11 | function [] = update(this, arm) 12 | 13 | % Only apply if there is a trajectory 14 | if isempty(arm.state.cmdPos) 15 | return; 16 | end 17 | 18 | % Add dynamics comp efforts 19 | dynamicsCompEfforts = arm.kin.getDynamicCompEfforts(... 20 | arm.state.fbk.position, ... 21 | arm.state.cmdPos, ... 22 | arm.state.cmdVel, ... 23 | arm.state.cmdAccel); 24 | 25 | % Add to efforts 26 | rampScale = this.getRampScale(arm.state.time); 27 | arm.state.cmdEffort = arm.state.cmdEffort + dynamicsCompEfforts .* rampScale; 28 | 29 | end 30 | 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /include/hebi/+HebiArmPlugins/EffortOffset.m: -------------------------------------------------------------------------------- 1 | classdef EffortOffset < HebiArmPlugin 2 | % EffortOffsetPlugin adds constant offsets to the 3 | 4 | properties 5 | offset double; 6 | end 7 | 8 | methods 9 | 10 | function this = EffortOffset(effortOffset) 11 | this.offset = effortOffset; 12 | end 13 | 14 | function [] = update(this, arm) 15 | rampScale = this.getRampScale(arm.state.time); 16 | arm.state.cmdEffort = arm.state.cmdEffort + this.offset .* rampScale; 17 | end 18 | 19 | end 20 | 21 | end 22 | 23 | -------------------------------------------------------------------------------- /include/hebi/FocCommandStruct.m: -------------------------------------------------------------------------------- 1 | function struct = FocCommandStruct() 2 | % FocCommandStruct is used to send commands to groups of modules. 3 | % 4 | % The struct created by this function is used to command FOC 5 | % (Field Oriented Control) values on motor drivers. 6 | % 7 | % A FocCommandStruct only has to be initialized once, and the fields can 8 | % be overwritten and reused multiple times in a loop. When setting 9 | % in a loop it is important to limit the loop rate using either a 10 | % pause or a feedback request. 11 | % 12 | % See also CommandStruct, IoCommandStruct, HebiGroup, HebiGroup.send 13 | 14 | % Copyright 2022-2023 HEBI Robotics, Inc. 15 | struct = javaObject(hebi_load('FocCommandStruct')); 16 | end 17 | -------------------------------------------------------------------------------- /include/hebi/IoCommandStruct.m: -------------------------------------------------------------------------------- 1 | function struct = IoCommandStruct() 2 | % IoCommandStruct can be used to send IO commands to groups 3 | % 4 | % The struct created by this function can be used to set pins on 5 | % an IO board. NaN and Inf values are ignored. Setting values 6 | % to pins that are not configured to be output pins will be ignored 7 | % by the hardware. 8 | % 9 | % Example 10 | % io = IoCommandStruct(); 11 | % io.a1 = 1; 12 | % io.b3 = 0.3; 13 | % group.send('io', io); 14 | % 15 | % See also HebiLookup, HebiGroup 16 | 17 | % Copyright 2014-2018 HEBI Robotics, LLC. 18 | struct = javaObject(hebi_load('IoCommandStruct')); 19 | end -------------------------------------------------------------------------------- /include/hebi/LocalStateStruct.m: -------------------------------------------------------------------------------- 1 | function struct = LocalStateStruct() 2 | % LocalStateStruct is used to set local state for logging purposes. 3 | % 4 | % See also HebiGroup.setLocalState, HebiGroup.getLocalState 5 | 6 | % Copyright 2014-2023 HEBI Robotics, Inc. 7 | struct = javaObject(hebi_load('LocalStateStruct')); 8 | end 9 | -------------------------------------------------------------------------------- /include/hebi/MapsGateConfig.m: -------------------------------------------------------------------------------- 1 | function struct = MapsCommandStruct() 2 | struct = javaObject(hebi_load('MapsGateConfig')); 3 | end 4 | -------------------------------------------------------------------------------- /include/hebi/hebi-matlab-2.2.1.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/hebi/hebi-matlab-2.2.1.jar -------------------------------------------------------------------------------- /include/hebi/hebi_load.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/hebi/hebi_load.p -------------------------------------------------------------------------------- /include/include.m: -------------------------------------------------------------------------------- 1 | function [] = include() 2 | % include adds paths required to run demos 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | addpath(fullfile(localDir)); 6 | 7 | % Main API 8 | addpath(fullfile(localDir, 'hebi')); 9 | hebi_load(); 10 | 11 | % Experimental API 12 | addpath(fullfile(localDir, 'experimental')); 13 | 14 | % Joystick / Keyboard input 15 | addpath(fullfile(localDir, 'input')); 16 | HebiJoystick.loadLibs(); 17 | HebiKeyboard.loadLibs(); 18 | 19 | % MATLAB File Exchange 20 | addpath(fullfile(localDir, 'exchange', 'SpinCalc')); 21 | 22 | % Utilities 23 | addpath(fullfile(localDir, 'kinematics')); 24 | addpath(fullfile(localDir, 'util')); 25 | 26 | end -------------------------------------------------------------------------------- /include/input/joystickTest.m: -------------------------------------------------------------------------------- 1 | % Display joystick data 2 | % 3 | % Dave Rollinson 4 | % Feb 2017 5 | 6 | HebiJoystick.loadLibs(); 7 | joy = HebiJoystick(1) 8 | 9 | disp('Displaying button states, ctrl-C to quit...'); 10 | 11 | figure(345); 12 | while true 13 | 14 | [axes, buttons, povs] = read(joy); 15 | 16 | subplot(3,1,1); 17 | plot(1:length(axes),axes,'o'); 18 | title('Axes'); 19 | ylim([-1 1]); 20 | xlim([.5 length(axes)+.5]); 21 | set(gca,'xTick',1:length(axes)); 22 | grid on; 23 | 24 | subplot(3,1,2); 25 | plot(1:length(buttons),buttons,'*'); 26 | title('Buttons'); 27 | ylim([0 1]); 28 | xlim([.5 length(buttons)+.5]); 29 | set(gca,'xTick',1:length(buttons)); 30 | grid on; 31 | 32 | subplot(3,1,3); 33 | if povs == -1 34 | plot(0,0,'o'); 35 | else 36 | plot([0 sin(deg2rad(povs))],[0 cos(deg2rad(povs))]); 37 | end 38 | title('POVs'); 39 | text(.5,.75,['Val: ' num2str(povs)]) 40 | axis equal; 41 | ylim([-1 1]); 42 | xlim([-1 1]); 43 | grid on; 44 | 45 | drawnow; 46 | end -------------------------------------------------------------------------------- /include/input/lib/jinput-dx8.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/jinput-dx8.dll -------------------------------------------------------------------------------- /include/input/lib/jinput-dx8_64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/jinput-dx8_64.dll -------------------------------------------------------------------------------- /include/input/lib/jinput-raw.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/jinput-raw.dll -------------------------------------------------------------------------------- /include/input/lib/jinput-raw_64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/jinput-raw_64.dll -------------------------------------------------------------------------------- /include/input/lib/jinput-wintab.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/jinput-wintab.dll -------------------------------------------------------------------------------- /include/input/lib/libjinput-linux64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/libjinput-linux64.so -------------------------------------------------------------------------------- /include/input/lib/libjinput-osx.jnilib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/lib/libjinput-osx.jnilib -------------------------------------------------------------------------------- /include/input/matlab-input-1.2.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/include/input/matlab-input-1.2.jar -------------------------------------------------------------------------------- /include/kinematics/R_x.m: -------------------------------------------------------------------------------- 1 | function DCM = R_x( angle ) 2 | % SO(3) Rotation matrix about x-axis by an angle in radians 3 | 4 | DCM = [ 1 0 0; 5 | 0 cos(angle) -sin(angle); 6 | 0 sin(angle) cos(angle) ]; 7 | end 8 | -------------------------------------------------------------------------------- /include/kinematics/R_y.m: -------------------------------------------------------------------------------- 1 | function DCM = R_y( angle ) 2 | % SO(3) Rotation matrix about y-axis by an angle in radians 3 | 4 | DCM = [ cos(angle) 0 sin(angle); 5 | 0 1 0; 6 | -sin(angle) 0 cos(angle) ]; 7 | 8 | end 9 | -------------------------------------------------------------------------------- /include/kinematics/R_z.m: -------------------------------------------------------------------------------- 1 | function DCM = R_z( angle ) 2 | % SO(3) Rotation matrix about z-axis by an angle in radians 3 | 4 | DCM = [ cos(angle) -sin(angle) 0; 5 | sin(angle) cos(angle) 0; 6 | 0 0 1 ]; 7 | 8 | end 9 | -------------------------------------------------------------------------------- /include/kinematics/pinv_damped.m: -------------------------------------------------------------------------------- 1 | function [ pinv_J_damp ] = pinv_damped( J ) 2 | 3 | if size(J,1) > size(J,2) 4 | w = det(J'*J); 5 | I = eye(size(J'*J)); 6 | else 7 | w = det(J*J'); 8 | I = eye(size(J*J')); 9 | end 10 | 11 | % DAMPING TERMS 12 | % Based on manipulability, roughly following the methods of: 13 | % "Robust Inverse Kinematics Using Damped Least Squares 14 | % with Dynamic Weighting" - Schinstock et al, NASA 1994 15 | 16 | % These defaults seem to work well. Tune them if needed. 17 | a0 = .01; % Max damping factor (no damping = 0) 18 | w0 = .001; % Manipulability threshold for when to start damping 19 | 20 | if w < w0 21 | % If manipulability is less than some threshold, ramp up damping 22 | damping = a0 * (1 - w/w0)^2; 23 | else 24 | % Otherwise, no damping. 25 | damping = 0; 26 | end 27 | 28 | 29 | if size(J,1) > size(J,2) 30 | pinv_J_damp = (J'*J + + damping*I) \ J'; 31 | else 32 | pinv_J_damp = J' / (J*J' + damping*I); 33 | end 34 | 35 | end -------------------------------------------------------------------------------- /kits/arms/config/A-2084-01.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 4-DoF SCARA Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2084-01.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2084-01.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2084-01G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 4-DoF SCARA Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2084-01G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2084-01.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2085-03.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 3-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow"] 5 | hrdf: "hrdf/A-2085-03.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-03.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2085-04.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 4-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2085-04.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-04.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2085-05.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 5-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2"] 5 | hrdf: "hrdf/A-2085-05.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-05.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2085-05G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 5-DoF Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2"] 5 | hrdf: "hrdf/A-2085-05G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-05.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2085-06.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5, 0.01] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # An impedance controller adds a virtual spring to the 26 | # end-effector and can improve tracking. It can be enabled 27 | # by setting 'enabled' to true. The gains are in the form of 28 | # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates 29 | # the corresponding degree of translation or rotation. 30 | - type: ImpedanceController 31 | name: impedanceController 32 | enabled: false 33 | ramp_time: 5 34 | gains_in_end_effector_frame: true 35 | kp: [50, 50, 50, 0, 0, 0] # (N/m) or (Nm/rad) 36 | kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) 37 | ki: [0, 0, 1, 0, 0, 0] 38 | i_clamp: [10, 10, 10, 10, 10, 10] # max value 39 | 40 | # Kits with a gas spring need to add a shoulder compensation torque. 41 | # It should be around -7 Nm for most kits, but it may need to be tuned 42 | # for your specific setup. 43 | - type: EffortOffset 44 | name: gasSpringCompensation 45 | enabled: false # disabled by default as the default configuration ships without one 46 | ramp_time: 5 47 | offset: [0, -7, 0, 0, 0, 0] 48 | -------------------------------------------------------------------------------- /kits/arms/config/A-2085-06G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 6-DoF Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5, 0.01] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # An impedance controller adds a virtual spring to the 32 | # end-effector and can improve tracking. It can be enabled 33 | # by setting 'enabled' to true. The gains are in the form of 34 | # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates 35 | # the corresponding degree of translation or rotation. 36 | - type: ImpedanceController 37 | name: impedanceController 38 | enabled: false 39 | ramp_time: 5 40 | gains_in_end_effector_frame: true 41 | kp: [50, 50, 50, 0, 0, 0] # (N/m) or (Nm/rad) 42 | kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) 43 | ki: [0, 0, 1, 0, 0, 0] 44 | i_clamp: [10, 10, 10, 10, 10, 10] # max value 45 | 46 | # Kits with a gas spring need to add a shoulder compensation torque. 47 | # It should be around -7 Nm for most kits, but it may need to be tuned 48 | # for your specific setup. 49 | - type: EffortOffset 50 | name: gasSpringCompensation 51 | enabled: false # disabled by default as the default configuration ships without one 52 | ramp_time: 5 53 | offset: [0, -7, 0, 0, 0, 0] 54 | -------------------------------------------------------------------------------- /kits/arms/config/A-2099-07.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 7-DoF Double Shoulder Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", J2A_shoulder1", "J3_shoulder2", "J4_elbow1", "J5_elbow2", "J6_wrist1", "J7_wrist2"] 5 | hrdf: "hrdf/A-2099-07.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2099-07.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, -1, 0, 1.5, 0.01, 0.01, 0.01] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | - name: 'DoubleShoulder' 26 | type: DoubledJointMirror 27 | enabled: true 28 | group_family: ["Arm"] 29 | group_name: [J2B_shoulder1"] 30 | index: 1 31 | -------------------------------------------------------------------------------- /kits/arms/config/A-2099-07G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # X-Series 7-DoF Double Shoulder Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", J2A_shoulder1", "J3_shoulder2", "J4_elbow1", "J5_elbow2", "J6_wrist1", "J7_wrist2"] 5 | hrdf: "hrdf/A-2099-07G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2099-01-7.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1, 0.01, 0.01, -1.5, 0.01, 0.01] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | - name: 'DoubleShoulder' 32 | type: DoubledJointMirror 33 | enabled: true 34 | group_family: ["Arm"] 35 | group_name: [J2B_shoulder1"] 36 | index: 1 37 | -------------------------------------------------------------------------------- /kits/arms/config/A-2240-04.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 4-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2240-04.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-04.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2240-05.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 5-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2"] 5 | hrdf: "hrdf/A-2240-05.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-05.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2240-05G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 5-DoF Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2"] 5 | hrdf: "hrdf/A-2240-05G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-05.xml" 9 | gripper: "gains/A-2255-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2240-06.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2240-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-06.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5, 0.01] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2240-06G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 6-DoF Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2240-06G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-06.xml" 9 | gripper: "gains/A-2255-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5, 0.01] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2302-01.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 4-DoF SCARA Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2302-01.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2302-01.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2302-01G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 4-DoF SCARA Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2302-01G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2302-01.xml" 9 | gripper: "gains/A-2255-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2303-01.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 7-DoF Double Shoulder Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", J2A_shoulder1", "J3_shoulder2", "J4_elbow1", "J5_elbow2", "J6_wrist1", "J7_wrist2"] 5 | hrdf: "hrdf/A-2303-01.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2303-01.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, -1, 0.01, 0.01, -1.5, 0.01, 0.01] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | - name: 'DoubleShoulder' 26 | type: DoubledJointMirror 27 | enabled: true 28 | group_family: ["Arm"] 29 | group_name: [J2B_shoulder1"] 30 | index: 1 31 | -------------------------------------------------------------------------------- /kits/arms/config/A-2303-01G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 7-DoF Double Shoulder Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", J2A_shoulder1", "J3_shoulder2", "J4_elbow1", "J5_elbow2", "J6_wrist1", "J7_wrist2"] 5 | hrdf: "hrdf/A-2303-01G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2303-01.xml" 9 | gripper: "gains/A-2255-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, -1, 0.01, 0.01, -1.5, 0.01, 0.01] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | - name: 'DoubleShoulder' 32 | type: DoubledJointMirror 33 | enabled: true 34 | group_family: ["Arm"] 35 | group_name: [J2B_shoulder1"] 36 | index: 1 37 | -------------------------------------------------------------------------------- /kits/arms/config/A-2580-04.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 4-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2580-04.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2580-04.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2580-05.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 5-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2"] 5 | hrdf: "hrdf/A-2580-05.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2580-05.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2580-05G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 5-DoF Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2"] 5 | hrdf: "hrdf/A-2580-05G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2580-05.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2580-06.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2580-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2580-06.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5, 0.01] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2580-06G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 6-DoF Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2580-06G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2580-06.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5, -1.5, 0.01] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/A-2582-07.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 7-DoF Double Shoulder Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", J2A_shoulder1", "J3_shoulder2", "J4_elbow1", "J5_elbow2", "J6_wrist1", "J7_wrist2"] 5 | hrdf: "hrdf/A-2582-07.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2582-07.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, -1, 0.01, 0.01, -1.5, 0.01, 0.01] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | - name: 'DoubleShoulder' 26 | type: DoubledJointMirror 27 | enabled: true 28 | group_family: ["Arm"] 29 | group_name: [J2B_shoulder1"] 30 | index: 1 31 | -------------------------------------------------------------------------------- /kits/arms/config/A-2582-07G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 7-DoF Double Shoulder Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", J2A_shoulder1", "J3_shoulder2", "J4_elbow1", "J5_elbow2", "J6_wrist1", "J7_wrist2"] 5 | hrdf: "hrdf/A-2582-07G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2582-07.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, -1, 0.01, 0.01, -1.5, 0.01, 0.01] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | - name: 'DoubleShoulder' 32 | type: DoubledJointMirror 33 | enabled: true 34 | group_family: ["Arm"] 35 | group_name: [J2B_shoulder1"] 36 | index: 1 37 | -------------------------------------------------------------------------------- /kits/arms/config/A-2590-01.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 4-DoF SCARA Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2590-01.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2590-01.xml" 9 | 10 | user_data: 11 | # Default seed positions for doing inverse kinematics 12 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 13 | 14 | plugins: 15 | - type: GravityCompensationEffort 16 | name: gravComp 17 | enabled: true 18 | ramp_time: 5 19 | 20 | - type: DynamicsCompensationEffort 21 | name: dynamicsComp 22 | enabled: true 23 | ramp_time: 5 24 | 25 | # Kits with a gas spring need to add a shoulder compensation torque. 26 | # It should be around -7 Nm for most kits, but it may need to be tuned 27 | # for your specific setup. 28 | - name: 'gasSpringCompensation' 29 | type: EffortOffset 30 | enabled: false 31 | ramp_time: 5 32 | offset: [0, -7, 0, 0] 33 | -------------------------------------------------------------------------------- /kits/arms/config/A-2590-01G.cfg.yaml: -------------------------------------------------------------------------------- 1 | # T-Series 4-DoF SCARA Arm with Gripper 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist"] 5 | hrdf: "hrdf/A-2590-01G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2590-01.xml" 9 | gripper: "gains/A-2080-01.xml" 10 | 11 | user_data: 12 | # Default seed positions for doing inverse kinematics 13 | ik_seed_pos: [0.01, 1.0, 2.5, 1.5] 14 | 15 | # Gripper specific settings 16 | has_gripper: true 17 | gripper_open_effort: 1 18 | gripper_close_effort: -5 19 | 20 | plugins: 21 | - type: GravityCompensationEffort 22 | name: gravComp 23 | enabled: true 24 | ramp_time: 5 25 | 26 | - type: DynamicsCompensationEffort 27 | name: dynamicsComp 28 | enabled: true 29 | ramp_time: 5 30 | 31 | # Kits with a gas spring need to add a shoulder compensation torque. 32 | # It should be around -7 Nm for most kits, but it may need to be tuned 33 | # for your specific setup. 34 | - name: 'gasSpringCompensation' 35 | type: EffortOffset 36 | enabled: false 37 | ramp_time: 5 38 | offset: [0, -7, 0, 0] 39 | -------------------------------------------------------------------------------- /kits/arms/config/ex_AR_kit.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | user_data: 22 | # Joint angles at home for AR demo: [0, pi/3, 2*pi/3, 5*pi/6, -pi/2, 0] 23 | home_position: [0.001, 1.047, 2.0943, 2.618, -1.571, 0.001] # radians 24 | 25 | # Time taken for a steady motion to the home position 26 | homing_duration: 5 # seconds 27 | 28 | # Online trajectory delay to smooth out controller motion. A shorter 29 | # delay will make a trajectory more response, and longer delay will make 30 | # the trajectory smooother. 31 | delay_time: 0.5 # seconds 32 | 33 | # Displacements of the mobile device are scaled by these value to give 34 | # displacement of the end-effector 35 | xyz_scale: [1.0, 1.0, 2.0] # [ x, y, z ] 36 | 37 | mobile_io_family: "Arm" 38 | mobile_io_name: "mobileIO" 39 | mobile_io_layout: "layouts/ex_AR_kit.json" 40 | -------------------------------------------------------------------------------- /kits/arms/config/ex_AR_kit_w_gripper.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2240-06G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | user_data: 22 | # Joint angles at home for AR demo: [0, pi/3, pi/2, 2*pi/3, -pi/2, 0] 23 | home_position: [0.001, 1.047, 2.0943, 2.618, -1.571, 0.001] # radians 24 | 25 | # Time taken for a steady motion to the home position 26 | homing_duration: 5 # seconds 27 | 28 | # Online trajectory delay to smooth out controller motion. A shorter 29 | # delay will make a trajectory more response, and longer delay will make 30 | # the trajectory smooother. 31 | delay_time: 0.5 # seconds 32 | 33 | # Displacements of the mobile device are scaled by these value to give 34 | # displacement of the end-effector 35 | xyz_scale: [1.0, 1.0, 2.0] # [ x, y, z ] 36 | 37 | gripper_family: "Arm" 38 | gripper_name: "gripperSpool" 39 | gripper_gains: "gains/gripper_spool_gains.xml" 40 | gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. 41 | gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. 42 | 43 | mobile_io_family: "Arm" 44 | mobile_io_name: "mobileIO" 45 | mobile_io_layout: "layouts/ex_AR_kit_w_gripper.json" 46 | -------------------------------------------------------------------------------- /kits/arms/config/ex_gravity_compensation.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | -------------------------------------------------------------------------------- /kits/arms/config/ex_gravity_compensation_toggle.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: false 14 | ramp_time: 0.5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 0.5 20 | 21 | user_data: 22 | mobile_io_family: "Arm" 23 | mobile_io_name: "mobileIO" 24 | mobile_io_layout: "layouts/ex_gravity_compensation_toggle.json" 25 | -------------------------------------------------------------------------------- /kits/arms/config/ex_impedance_control_cartesian.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | # An impedance controller adds a virtual spring to the 22 | # end-effector and can improve tracking. It can be enabled 23 | # by setting 'enabled' to true. The gains are in the form of 24 | # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates 25 | # the corresponding degree of translation or rotation. 26 | # These gains correspond to a translational spring which maintains position but not orientation 27 | - type: ImpedanceController 28 | name: impedanceController 29 | enabled: true 30 | ramp_time: 5 31 | gains_in_end_effector_frame: true 32 | kp: [300, 300, 300, 0, 0, 0] # (N/m) or (Nm/rad) 33 | kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) 34 | ki: [20, 20, 20, 0, 0, 0] 35 | i_clamp: [10, 10, 10, 0, 0, 0] # max value 36 | 37 | user_data: 38 | mobile_io_family: "Arm" 39 | mobile_io_name: "mobileIO" 40 | mobile_io_layout: "layouts/ex_impedance_control_cartesian.json" 41 | -------------------------------------------------------------------------------- /kits/arms/config/ex_impedance_control_fixed.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | # An impedance controller adds a virtual spring to the 22 | # end-effector and can improve tracking. It can be enabled 23 | # by setting 'enabled' to true. The gains are in the form of 24 | # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates 25 | # the corresponding degree of translation or rotation. 26 | # These gains correspond to a controller that rigidly maintains a fixed pose 27 | - type: ImpedanceController 28 | name: impedanceController 29 | enabled: true 30 | ramp_time: 5 31 | gains_in_end_effector_frame: true 32 | kp: [300, 300, 300, 5, 5, 1] # (N/m) or (Nm/rad) 33 | kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) 34 | ki: [20, 20, 20, 0.5, 0.5, 0.5] 35 | i_clamp: [10, 10, 10, 1, 1, 1] # max value 36 | 37 | user_data: 38 | mobile_io_family: "Arm" 39 | mobile_io_name: "mobileIO" 40 | mobile_io_layout: "layouts/ex_impedance_control_fixed.json" 41 | -------------------------------------------------------------------------------- /kits/arms/config/ex_impedance_control_gimbal.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | # An impedance controller adds a virtual spring to the 22 | # end-effector and can improve tracking. It can be enabled 23 | # by setting 'enabled' to true. The gains are in the form of 24 | # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates 25 | # the corresponding degree of translation or rotation. 26 | # These gains correspond to a rotational spring emulating a gimbal 27 | - type: ImpedanceController 28 | name: impedanceController 29 | enabled: true 30 | ramp_time: 5 31 | gains_in_end_effector_frame: true 32 | kp: [0, 0, 0, 8, 8, 1] # (N/m) or (Nm/rad) 33 | kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) 34 | ki: [0, 0, 0, 0.5, 0.5, 0.5] 35 | i_clamp: [0, 0, 0, 1, 1, 1] # max value 36 | 37 | user_data: 38 | mobile_io_family: "Arm" 39 | mobile_io_name: "mobileIO" 40 | mobile_io_layout: "layouts/ex_impedance_control_gimbal.json" 41 | -------------------------------------------------------------------------------- /kits/arms/config/ex_mobile_io_control.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | user_data: 22 | # Three waypoints for Buttons - B1, B2, B3 23 | waypoint_1: [0, 0, 0, 0, 0, 0] 24 | waypoint_2: [0.7854, 1.0472, 2.0944, 1.0472, 0.7854, 0] # [pi/4, pi/3, 2*pi/3, pi/3, pi/4, 0] 25 | waypoint_3: [-0.7854, 1.0472, 2.0944, 1.0472, 2.356194490192345, 0] # [-pi/4, pi/3, 2*pi/3, pi/3, 3*pi/4, 0] 26 | 27 | # Time taken to travel to a waypoint 28 | travel_time: 3 # seconds 29 | 30 | mobile_io_family: "Arm" 31 | mobile_io_name: "mobileIO" 32 | mobile_io_layout: "layouts/ex_mobile_io_control.json" 33 | -------------------------------------------------------------------------------- /kits/arms/config/ex_teach_repeat.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2085-06.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2085-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | user_data: 22 | # Travel time is calculated using 23 | # base_travel_time + slider * (base_travel_time - min_travel_time) 24 | # Since slider goes from -1 to 1, travel time goes from min_travel_time to 2 * base_travel_time - min_travel_time 25 | 26 | # Minimum travel time value 27 | min_travel_time: 0.5 28 | 29 | # The default travel time value 30 | base_travel_time: 3 # seconds 31 | 32 | mobile_io_family: "Arm" 33 | mobile_io_name: "mobileIO" 34 | mobile_io_layout: "layouts/ex_teach_repeat.json" 35 | -------------------------------------------------------------------------------- /kits/arms/config/ex_teach_repeat_w_gripper.cfg.yaml: -------------------------------------------------------------------------------- 1 | # 6-DoF Arm 2 | version: 1.0 3 | families: ["Arm"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] 5 | hrdf: "hrdf/A-2240-06G.hrdf" 6 | 7 | gains: 8 | default: "gains/A-2240-06.xml" 9 | 10 | plugins: 11 | - type: GravityCompensationEffort 12 | name: gravComp 13 | enabled: true 14 | ramp_time: 5 15 | 16 | - type: DynamicsCompensationEffort 17 | name: dynamicsComp 18 | enabled: true 19 | ramp_time: 5 20 | 21 | user_data: 22 | # Travel time is calculated using 23 | # base_travel_time + slider * (base_travel_time - min_travel_time) 24 | # Since slider goes from -1 to 1, travel time goes from min_travel_time to 2 * base_travel_time - min_travel_time 25 | 26 | # Minimum travel time value 27 | min_travel_time: 0.5 28 | 29 | # The default travel time value 30 | base_travel_time: 3 # seconds 31 | 32 | gripper_family: "Arm" 33 | gripper_name: "gripperSpool" 34 | gripper_gains: "gains/gripper_spool_gains.xml" 35 | gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. 36 | gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. 37 | 38 | mobile_io_family: "Arm" 39 | mobile_io_name: "mobileIO" 40 | mobile_io_layout: "layouts/ex_teach_repeat_w_gripper.json" 41 | -------------------------------------------------------------------------------- /kits/arms/config/gains/A-2080-01.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 6 | 15 7 | 0 8 | 0 9 | 0 10 | 0 11 | 1 12 | 0 13 | -inf 14 | inf 15 | 1 16 | -20 17 | 20 18 | 1 19 | 1 20 | 21 | 22 | 0.05 23 | 0 24 | 0 25 | 1 26 | 0 27 | 0.25 28 | 0 29 | -1.502675 30 | 1.502675 31 | 1 32 | -1 33 | 1 34 | 0.75 35 | 1 36 | 37 | 38 | 0.25 39 | 0 40 | 0.001 41 | 1 42 | 0 43 | 0.25 44 | 0 45 | -20 46 | 20 47 | 1 48 | -1 49 | 1 50 | 0.9 51 | 0 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /kits/arms/config/gains/A-2085-03.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 4 4 | 5 | 15 30 10 6 | 0 0 0 7 | 0 0 0 8 | 0 0 0 9 | 0 0 0 10 | 1 1 1 11 | 0 0 0 12 | -inf -inf -inf 13 | inf inf inf 14 | 1 1 1 15 | -20 -20 -10 16 | 20 20 10 17 | 1 1 1 18 | 1 1 1 19 | 20 | 21 | 0.05 0.05 0.05 22 | 0 0 0 23 | 0 0 0 24 | 1 1 1 25 | 0 0 0 26 | 0.25 0.25 0.25 27 | 0 0 0 28 | -3.4 -1.5 -3.4 29 | 3.4 1.5 3.4 30 | 1 1 1 31 | -1 -1 -1 32 | 1 1 1 33 | 0.75 0.75 0.75 34 | 1 1 1 35 | 36 | 37 | 0.25 0.25 0.25 38 | 0 0 0 39 | 0.001 0.001 0.001 40 | 1 1 1 41 | 0 0 0 42 | 0.25 0.25 0.25 43 | 0 0 0 44 | -20 -20 -10 45 | 20 20 10 46 | 1 1 1 47 | -1 -1 -1 48 | 1 1 1 49 | 0.9 0.9 0.9 50 | 0 0 0 51 | 52 | 53 | -------------------------------------------------------------------------------- /kits/arms/config/gains/A-2255-01.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 6 | 15 7 | 0 8 | 0 9 | 0 10 | 0 11 | 1 12 | 0 13 | -inf 14 | inf 15 | 1 16 | -20 17 | 20 18 | 1 19 | 1 20 | 21 | 22 | 0.05 23 | 0 24 | 0 25 | 1 26 | 0 27 | 0.25 28 | 0 29 | -1.502675 30 | 1.502675 31 | 1 32 | -1 33 | 1 34 | 0.75 35 | 1 36 | 37 | 38 | 0.25 39 | 0 40 | 0.001 41 | 1 42 | 0 43 | 0.25 44 | 0 45 | -20 46 | 20 47 | 1 48 | -1 49 | 1 50 | 0.9 51 | 0 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /kits/arms/config/gains/gripper_spool_gains.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 | 5 | 15 6 | 0 7 | 0 8 | 0 9 | 0 10 | 1 11 | 0 12 | -inf 13 | inf 14 | 1 15 | -20 16 | 20 17 | 1 18 | 1 19 | 20 | 21 | 0.05 22 | 0 23 | 0 24 | 1 25 | 0 26 | 0.25 27 | 0 28 | -1.502675 29 | 1.502675 30 | 1 31 | -1 32 | 1 33 | 0.75 34 | 1 35 | 36 | 37 | 0.25 38 | 0 39 | 0.001 40 | 1 41 | 0 42 | 0.25 43 | 0 44 | -20 45 | 20 46 | 1 47 | -1 48 | 1 49 | 0.9 50 | 0 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /kits/arms/config/gains/mirror_shoulder.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 | 5 | 50 6 | 0 7 | 0 8 | 0 9 | 0 10 | 1 11 | 0 12 | -inf 13 | inf 14 | 1 15 | -40 16 | 40 17 | 1 18 | 1 19 | 20 | 21 | 0.1 22 | 0 23 | 0 24 | 1 25 | 0 26 | 0.25 27 | 0 28 | -1.790422 29 | 1.790422 30 | 1 31 | -1 32 | 1 33 | 0.5 34 | 1 35 | 36 | 37 | 0.3 38 | 0 39 | 0.0001 40 | 1 41 | 0 42 | 0.25 43 | 0 44 | -40 45 | 40 46 | 1 47 | -1 48 | 1 49 | 0.5 50 | 0 51 | 52 | 53 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2084-01.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2084-01G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2085-03.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2085-04.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2085-05.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2085-05G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2085-06.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2085-06G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2099-07.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2099-07G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2240-04.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2240-05.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2240-05G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2240-06.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2240-06G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2302-01.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2302-01G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2303-01.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2303-01G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2580-04.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2580-05.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2580-05G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2580-06.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2580-06G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2582-07.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2582-07G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2590-01.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /kits/arms/config/hrdf/A-2590-01G.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kits/arms/config/layouts/ar_control_sm.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.5, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "Home" 15 | ], 16 | "mode": 0 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "Arm Control" 32 | ], 33 | "mode": 0 34 | } 35 | }, 36 | { 37 | "id": "b3", 38 | "channels": [ 39 | "b3" 40 | ], 41 | "type": "button", 42 | "x": 0.5, 43 | "y": -0.73, 44 | "width": 0.15, 45 | "height": 0.15, 46 | "parameters": { 47 | "text": [ 48 | "Gripper" 49 | ], 50 | "mode": 0 51 | } 52 | } 53 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_AR_kit.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.75, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "🏠" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b3", 21 | "channels": [ 22 | "b3" 23 | ], 24 | "type": "button", 25 | "x": -0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "📲" 32 | ], 33 | "mode": "momentary" 34 | } 35 | }, 36 | { 37 | "id": "b6", 38 | "channels": [ 39 | "b6" 40 | ], 41 | "type": "button", 42 | "x": 0.25, 43 | "y": -0.73, 44 | "width": 0.15, 45 | "height": 0.15, 46 | "parameters": { 47 | "text": [ 48 | "🌍" 49 | ], 50 | "mode": "momentary" 51 | } 52 | }, 53 | { 54 | "id": "b8", 55 | "channels": [ 56 | "b8" 57 | ], 58 | "type": "button", 59 | "x": 0.75, 60 | "y": -0.73, 61 | "width": 0.15, 62 | "height": 0.15, 63 | "parameters": { 64 | "text": [ 65 | "❌" 66 | ], 67 | "mode": "momentary" 68 | } 69 | } 70 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_gravity_compensation_toggle.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.25, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "❌/📈" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "🌍" 32 | ], 33 | "mode": "toggle" 34 | } 35 | } 36 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_impedance_control_cartesian.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.25, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "❌/📈" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "📌" 32 | ], 33 | "mode": "toggle" 34 | } 35 | } 36 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_impedance_control_damping.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.25, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "❌/📈" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "💪" 32 | ], 33 | "mode": "toggle" 34 | } 35 | } 36 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_impedance_control_fixed.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.25, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "❌/📈" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "🛑" 32 | ], 33 | "mode": "toggle" 34 | } 35 | } 36 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_impedance_control_floor.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.25, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "❌/📈" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "🧱" 32 | ], 33 | "mode": "toggle" 34 | } 35 | } 36 | ] -------------------------------------------------------------------------------- /kits/arms/config/layouts/ex_impedance_control_gimbal.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "id": "b1", 4 | "channels": [ 5 | "b1" 6 | ], 7 | "type": "button", 8 | "x": -0.25, 9 | "y": -0.73, 10 | "width": 0.15, 11 | "height": 0.15, 12 | "parameters": { 13 | "text": [ 14 | "❌/📈" 15 | ], 16 | "mode": "momentary" 17 | } 18 | }, 19 | { 20 | "id": "b2", 21 | "channels": [ 22 | "b2" 23 | ], 24 | "type": "button", 25 | "x": 0.25, 26 | "y": -0.73, 27 | "width": 0.15, 28 | "height": 0.15, 29 | "parameters": { 30 | "text": [ 31 | "🤳" 32 | ], 33 | "mode": "toggle" 34 | } 35 | } 36 | ] -------------------------------------------------------------------------------- /kits/arms/createGripperFromConfig.m: -------------------------------------------------------------------------------- 1 | function gripper = createGripperFromConfig( config ) 2 | userData = config.userData; 3 | 4 | % Required keys for the gripper configuration 5 | requiredKeys = {'gripper_family', 'gripper_name', 'gripper_gains', 'gripper_close_effort', 'gripper_open_effort'}; 6 | requiredKeysString = {'gripper_family', 'gripper_name', 'gripper_gains'}; 7 | requiredKeysFloat = {'gripper_close_effort', 'gripper_open_effort'}; 8 | 9 | % Validate the user data configuration 10 | if ~all(isfield(userData, requiredKeys)) 11 | error('HEBI config "user_data" field must contain the keys: %s', strjoin(requiredKeys, ', ')); 12 | end 13 | if ~all(cellfun(@(key) ischar(userData.(key)), requiredKeysString)) 14 | error('HEBI config "user_data" fields %s must contain strings', strjoin(requiredKeysString, ', ')); 15 | end 16 | if ~all(cellfun(@(key) isnumeric(userData.(key)), requiredKeysFloat)) 17 | error('HEBI config "user_data" fields %s must contain numbers', strjoin(requiredKeysFloat, ', ')); 18 | end 19 | 20 | % Load gains relative to the config file 21 | [cfgDir, ~, ~] = fileparts(config.configLocation); 22 | gainsFile = fullfile(cfgDir, userData.gripper_gains); 23 | gains = HebiUtils.loadGains(gainsFile); 24 | 25 | % Create the backing group comms 26 | group = HebiLookup.newGroupFromNames(userData.gripper_family, userData.gripper_name); 27 | HebiUtils.sendWithRetry(group, 'gains', gains); 28 | 29 | % Setup gripper object 30 | gripper = HebiGripper(group); 31 | gripper.openEffort = userData.gripper_open_effort; 32 | gripper.closeEffort = userData.gripper_close_effort; 33 | 34 | end -------------------------------------------------------------------------------- /kits/arms/createMobileIOFromConfig.m: -------------------------------------------------------------------------------- 1 | function mobileIO = createMobileIOFromConfig( config ) 2 | userData = config.userData; 3 | 4 | % Required keys for the mobile IO configuration 5 | requiredKeys = {'mobile_io_family', 'mobile_io_name', 'mobile_io_layout'}; 6 | 7 | % Validate the user data configuration 8 | if ~all(isfield(userData, requiredKeys)) 9 | error('HEBI config "user_data" field must contain the keys: %s', strjoin(requiredKeys, ', ')); 10 | end 11 | if ~all(cellfun(@(key) ischar(userData.(key)), requiredKeys)) 12 | error('HEBI config "user_data" fields %s must contain strings', strjoin(requiredKeys, ', ')); 13 | end 14 | 15 | % Load layout relative to the config file 16 | [cfgDir, ~, ~] = fileparts(config.configLocation); 17 | layoutFile = fullfile(cfgDir, userData.mobile_io_layout); 18 | 19 | % Validate the mobile_io configuration 20 | mobileIO = HebiMobileIO.findDevice(userData.mobile_io_family, userData.mobile_io_name); 21 | mobileIO.sendLayout(layoutFile); 22 | mobileIO.update(); 23 | 24 | end -------------------------------------------------------------------------------- /kits/arms/ex_kinematics_log_analysis.m: -------------------------------------------------------------------------------- 1 | % Kinematics Logged Data Analysis 2 | % 3 | % Features: Loads an example log file and kinematics description for 4 | % plotting end-effector error and tracking. 5 | % 6 | % Requirements: MATLAB 2013b or higher 7 | % 8 | % Author: Dave Rollinson 9 | % 10 | % Date: Oct 2018 11 | 12 | % Copyright 2017-2018 HEBI Robotics 13 | 14 | % The local directory of this file to make sure that the XML / HRDF files 15 | % load correctly. 16 | % 17 | % NOTE: 18 | % If you run this script using Evaluate Selection (F9) or Evaluate Current 19 | % Section (CTRL-ENTER) this will not work unless you are working from the 20 | % directory that has this script. 21 | localDir = fileparts(mfilename('fullpath')); 22 | if isempty(localDir) && (isunix || ismac) 23 | localDir = '.'; 24 | end 25 | 26 | 27 | % Load example log file and kinematics. You can replace these with data 28 | % from your own arm. You want to make sure that the HRDF file matches the 29 | % arm that used in the logged data :-). 30 | hebilog = HebiUtils.loadGroupLog( [localDir '/logs/A-2085-03_example_log'] ); 31 | kin = HebiUtils.loadHRDF( [localDir '/hrdf/A-2085-03'] ); 32 | 33 | kinematics_analysis( hebilog, kin ); 34 | 35 | -------------------------------------------------------------------------------- /kits/arms/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /kits/daisy/config/A-2240-06_X-Daisy.yaml: -------------------------------------------------------------------------------- 1 | # R-Series 6-DoF Arm 2 | version: 1.0 3 | families: ["Daisy"] 4 | names: ["L1_J1_base", "L1_J2_shoulder", "L1_J3_elbow", "L2_J1_base", "L2_J2_shoulder", "L2_J3_elbow", "L3_J1_base", "L3_J2_shoulder", "L3_J3_elbow", "L4_J1_base", "L4_J2_shoulder", "L4_J3_elbow", "L5_J1_base", "L5_J2_shoulder", "L5_J3_elbow", "L6_J1_base", "L6_J2_shoulder", "L6_J3_elbow"] 5 | hrdf: "../hrdf/Daisy.hrdf" 6 | 7 | gains: 8 | leg: "../gains/daisyLeg-gains.xml" 9 | 10 | user_data: 11 | controller_family: "HEBI" 12 | controller_name: "mobileIO" 13 | -------------------------------------------------------------------------------- /kits/daisy/daisyStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | #This script launches the daisy matlab script with the correct path. 3 | cd $(dirname $(readlink -f $0)) 4 | x-terminal-emulator -e "matlab -r \"runDaisy\"" 5 | -------------------------------------------------------------------------------- /kits/daisy/getBodyPoseFromFeet.m: -------------------------------------------------------------------------------- 1 | function chassisPose = getBodyPoseFromFeet( feetXYZ, baseXYZ ) 2 | % GETBODYPOSEFROMFEET Calculates the translation and rotation of the 3 | % chassis based on the XYZ locations of the feet from some "baseXYZ" 4 | % position. 5 | % 6 | % INPUT: feetXYZ = 3x1x6 feedback position of feet 7 | % baseXYZ = 3x1x6 'home' position of feet 8 | % 9 | % Dave Rollinson 10 | % Dec 2014 11 | 12 | numFeet = size(feetXYZ,2); 13 | 14 | % Zero mean the points 15 | feetXYZ_CoM = mean(feetXYZ,2); 16 | baseXYZ_CoM = mean(baseXYZ,2); 17 | 18 | feetXYZ = feetXYZ - repmat( feetXYZ_CoM, 1, numFeet ); 19 | baseXYZ = baseXYZ - repmat( baseXYZ_CoM, 1, numFeet ); 20 | 21 | % Build the weighted correlation matrix 22 | xyzCorr = baseXYZ * feetXYZ'; 23 | 24 | % Take the SVD 25 | [U,~,V] = svd( xyzCorr ); 26 | 27 | % Modify the scaling matrix to account for reflections 28 | S = eye(3); 29 | S(3,3) = sign(det(U*V)); 30 | 31 | % Make the final transform 32 | chassisPose = eye(4); 33 | R = V*S*U'; 34 | t = feetXYZ_CoM - baseXYZ_CoM; 35 | 36 | chassisPose(1:3,1:3) = R; 37 | chassisPose(1:3,4) = t; 38 | 39 | 40 | end -------------------------------------------------------------------------------- /kits/daisy/getJoyCommands.m: -------------------------------------------------------------------------------- 1 | function [xyzVel, rotVel, auxCmd] = getJoyCommands( fbkIO ) 2 | 3 | % Assumes you're using the HEBI Mobile I/O App 4 | 5 | xyzVel = zeros(3,1); 6 | rotVel = zeros(3,1); 7 | auxCmd = struct(); 8 | 9 | QUIT_BUTTON = 'b8'; 10 | STANCE_MODE = 'b1'; 11 | 12 | % PRESS DOWN B8 TO QUIT 13 | if fbkIO.(QUIT_BUTTON) 14 | auxCmd.quit = true; 15 | else 16 | auxCmd.quit = false; 17 | end 18 | 19 | % PRESS A TO TOGGLE STEPPING ON AND OFF 20 | if fbkIO.(STANCE_MODE) 21 | auxCmd.steppingMode = false; 22 | else 23 | auxCmd.steppingMode = true; 24 | end 25 | 26 | joyXYZScale = .3; 27 | joyRotScale = .6; 28 | joyAxisDeadZone = .05; 29 | 30 | % LINEAR VELOCITIES 31 | % X-Axis 32 | if abs(fbkIO.a8) > joyAxisDeadZone 33 | xyzVel(1) = -joyXYZScale * fbkIO.a8; 34 | end 35 | % Y-Axis 36 | if abs(fbkIO.a7) > joyAxisDeadZone 37 | xyzVel(2) = joyXYZScale * fbkIO.a7; 38 | end 39 | % Z-Axis 40 | if abs(fbkIO.a3) > joyAxisDeadZone 41 | xyzVel(3) = -joyXYZScale * fbkIO.a3; 42 | end 43 | 44 | % Make sure max velocity magnitude is maintained (L2-Norm) 45 | if norm(xyzVel) > joyXYZScale 46 | xyzVel = xyzVel * (joyXYZScale/norm(xyzVel)); 47 | end 48 | 49 | % ROTATIONAL VELOCITIES 50 | % X-Axis Rotation 51 | 52 | % % Y-Axis Rotation 53 | % if abs(axes(2)) > joyAxisDeadZone 54 | % rotVel(2) = joyRotScale * axes(2); 55 | % end 56 | 57 | % Z-Axis Rotation 58 | if abs(fbkIO.a1) > joyAxisDeadZone 59 | rotVel(3) = joyRotScale * fbkIO.a1; 60 | end 61 | 62 | 63 | end -------------------------------------------------------------------------------- /kits/daisy/getMSIJoyCommands.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/daisy/getMSIJoyCommands.m -------------------------------------------------------------------------------- /kits/daisy/hrdf/daisyLeg-Left.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kits/daisy/hrdf/daisyLeg-Right.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kits/daisy/images/controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/daisy/images/controller.png -------------------------------------------------------------------------------- /kits/daisy/images/daisy_coordinates.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/daisy/images/daisy_coordinates.png -------------------------------------------------------------------------------- /kits/daisy/images/daisy_coordinates_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/daisy/images/daisy_coordinates_small.png -------------------------------------------------------------------------------- /kits/daisy/images/daisy_labeled.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/daisy/images/daisy_labeled.png -------------------------------------------------------------------------------- /kits/daisy/images/daisy_render.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/daisy/images/daisy_render.png -------------------------------------------------------------------------------- /kits/daisy/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | #Run this script to create a dynamic link to the daisyStart script. 3 | #Once this has executed, you can run the program anywhere by typing "daisyStart" 4 | DAISY_FILE="/daisyStart.sh" 5 | sudo ln -s $(dirname $(readlink -f $0))$DAISY_FILE /usr/bin/daisyStart 6 | chmod +x daisyStart.sh 7 | echo "you can now run the program by typing: daisyStart" 8 | 9 | -------------------------------------------------------------------------------- /kits/daisy/joystickTest.m: -------------------------------------------------------------------------------- 1 | % Display joystick data 2 | % 3 | % Dave Rollinson 4 | % Feb 2017 5 | 6 | HebiJoystick.loadLibs(); 7 | joy = HebiJoystick(1) 8 | 9 | disp('Displaying button states, ctrl-C to quit...'); 10 | 11 | figure(345); 12 | while true 13 | 14 | [axes, buttons, povs] = read(joy); 15 | 16 | subplot(3,1,1); 17 | plot(1:length(axes),axes,'o'); 18 | title('Axes'); 19 | ylim([-1 1]); 20 | xlim([.5 length(axes)+.5]); 21 | set(gca,'xTick',1:length(axes)); 22 | grid on; 23 | 24 | subplot(3,1,2); 25 | plot(1:length(buttons),buttons,'*'); 26 | title('Buttons'); 27 | ylim([0 1]); 28 | xlim([.5 length(buttons)+.5]); 29 | set(gca,'xTick',1:length(buttons)); 30 | grid on; 31 | 32 | subplot(3,1,3); 33 | if povs == -1 34 | plot(0,0,'o'); 35 | else 36 | plot([0 sin(deg2rad(povs))],[0 cos(deg2rad(povs))]); 37 | end 38 | title('POVs'); 39 | text(.5,.75,['Val: ' num2str(povs)]) 40 | axis equal; 41 | ylim([-1 1]); 42 | xlim([-1 1]); 43 | grid on; 44 | 45 | drawnow; 46 | end -------------------------------------------------------------------------------- /kits/daisy/setDaisyParams.m: -------------------------------------------------------------------------------- 1 | % Robot-Specific Parameters 2 | % This is called in runHexapod.m. In the future it would be good to 3 | % combine all the setup into a function, setupDaisy.m, like we do with the 4 | % Rosie and Florence kits. 5 | % 6 | % Dave Rollinson 7 | % Apr 2019 8 | 9 | % Masses / Weights. Assume all the legs weigh the same 10 | legMass = sum( legKin{1}.getBodyMasses() ); 11 | chassisMass = sum(chassisKin.getBodyMasses()); 12 | robotMass = chassisMass + 6*legMass; % kg 13 | robotWeight = 9.8 * robotMass; 14 | 15 | numLegs = length(legKin); 16 | allLegs = 1:numLegs; 17 | 18 | % Stance Parameters 19 | bodyHeight = .21; % meters 20 | stanceRadius = .55; % meters 21 | for leg=1:numLegs 22 | baseFrame = legKin{leg}.getFirstJointFrame(); 23 | homeStanceXYZ(:,leg) = baseFrame(1:3,1:3) * ... 24 | [stanceRadius; 0; -bodyHeight]; 25 | end 26 | 27 | levelHomeStanceXYZ = homeStanceXYZ; 28 | 29 | % Step Parameters 30 | stepHeight = .040; % meters (.040 default) 31 | stepOverShoot = 0.35; % factor of step to overshoot 32 | stepPeriod = 0.7; % seconds (.7 default) 33 | stepPhase = [0 .5 1]; 34 | 35 | isStance = ones(1,6)==true; % Boolean mask to determine stance feet 36 | stepping = false; % track state of stepping 37 | legStepState = 0; % Alternates set A and B in tripod gait 38 | 39 | % Thresholds: sets how far COM can be from 'home' position before stepping 40 | shiftThresh = .02; % meters (.025 default) 41 | rotThresh = .05; % rad .1 42 | 43 | -------------------------------------------------------------------------------- /kits/daisy/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /kits/edward/edwardDemoRunner.m: -------------------------------------------------------------------------------- 1 | %% 2 | %run_edward 3 | function edwardDemoRunner() 4 | 5 | % Close any previously opened figures 6 | close all; 7 | 8 | % Note: 'clear *' was removed as functions have a separate 9 | % workspace and it wouldn't do anything. 10 | 11 | % Make sure libraries and paths are loaded appropriately 12 | localDir = fileparts(mfilename('fullpath')); 13 | run(fullfile(localDir, 'startup.m')); 14 | 15 | % Start the demo 16 | edwardDemo(); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /kits/edward/edwardStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | matlab -nodisplay -nosplash -r "run('./edwardDemoRunner.m')" 3 | 4 | -------------------------------------------------------------------------------- /kits/edward/readme.adoc: -------------------------------------------------------------------------------- 1 | # HEBI Robotics Kits - Edward 2 | 3 | This folder contains source code for running the Edward kit (link:http://docs.hebi.us/resources/kits/assyInstructions/X-Series_Edward.pdf[Assembly Instructions]): 4 | 5 | image::http://docs.hebi.us//resources/kits/images/Edward_img.PNG[width=50%] 6 | 7 | ## Contents 8 | 9 | * *startup.m* loads the required libraries 10 | * *edwardDemo.m* contains the main demo including the startup procedure and stop/restart functionality 11 | * *setupEdward.m* creates various static parameters as well as kinematics and trajectory generator objects for each limb 12 | * *edwardDemoRunner.m* is a single script that executes startup and edwardDemo 13 | * *edwardStart.sh* is a shell script that starts MATLAB and executes edwardDemoRunner.m 14 | 15 | ## Notes 16 | 17 | * The gains file (edwardGains.xml) file gets loaded automatically and assumes that you are using the default configuration of modules. 18 | 19 | ## Instructions 20 | 21 | TODO: 22 | 23 | * general overview 24 | * joystick mappings 25 | * code overview 26 | * link to assembly docs, pictures, etc. 27 | -------------------------------------------------------------------------------- /kits/edward/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup adds paths required to run demos 3 | 4 | localDir = fileparts(mfilename('fullpath')); 5 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 6 | run(includeScript); 7 | 8 | end 9 | -------------------------------------------------------------------------------- /kits/igor/hrdf/igorArm-Left.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /kits/igor/hrdf/igorArm-Right.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /kits/igor/hrdf/igorChassis.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | -------------------------------------------------------------------------------- /kits/igor/hrdf/igorLeg.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /kits/igor/igor2StartupIntegrated.m: -------------------------------------------------------------------------------- 1 | %% 2 | % run_igor 3 | 4 | % Run as a function 5 | function igor2StartupIntegrated() 6 | 7 | hasCamera = false; 8 | igor2DemoIntegrated( hasCamera ); 9 | 10 | end 11 | 12 | % % Run as a script 13 | % clear *; 14 | % close all; 15 | % 16 | % cam_module = true; 17 | % igor2DemoIntegrated; -------------------------------------------------------------------------------- /kits/igor/igorStart.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | "C:\Program Files\MATLAB\R2016a\bin\matlab.exe" -r "run('C:\Users\hebi\Desktop\igor_svn\igor2StartupIntegrated.m')" -------------------------------------------------------------------------------- /kits/igor/igorStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | matlab -r "run('/home/igor/hebi-matlab-examples/kits/igor/igor2StartupIntegrated.m')" 3 | 4 | -------------------------------------------------------------------------------- /kits/igor/makeIgorKinematics.m: -------------------------------------------------------------------------------- 1 | function [ legKin, armKin, chassisKin ] = makeIgorKinematics(numLegs, numArms) 2 | %MAKEIGORKINEMATICS returns kinematics for the legs, arms, and chassis of 3 | % the X-Series Igor Robot 4 | % 5 | % Andrew Willig 6 | % Nov 2020 7 | % 8 | % Left Arm and Leg = 1 9 | % Right Arm and Leg = 2 10 | % Forward = X 11 | % Left = Y 12 | % Up = Z 13 | 14 | % Get relative path 15 | localDir = fileparts(mfilename('fullpath')); 16 | 17 | %%%%%%%%%%%%%%%%%% 18 | % Leg Kinematics % 19 | %%%%%%%%%%%%%%%%%% 20 | R_hip = R_x(pi/2); 21 | xyz_hip = [0; .0225; .055]; 22 | T_hip = eye(4); 23 | T_hip(1:3,1:3) = R_hip; 24 | T_hip(1:3,4) = xyz_hip; 25 | 26 | legBaseFrames(:,:,1) = eye(4); 27 | legBaseFrames(:,:,2) = eye(4); 28 | 29 | legBaseFrames(1:3,4,1) = [0; .15; 0]; 30 | legBaseFrames(1:3,4,2) = [0; -.15; 0]; 31 | 32 | legBaseFrames(1:3,1:3,1) = R_x(-pi/2); 33 | legBaseFrames(1:3,1:3,2) = R_x(pi/2); 34 | 35 | for leg = 1:numLegs 36 | legKin{leg} = HebiUtils.loadHRDF([localDir '/hrdf/igorLeg.hrdf']); 37 | 38 | legKin{leg}.setBaseFrame(legBaseFrames(:,:,leg)); 39 | end 40 | 41 | 42 | %%%%%%%%%%%%%%%%%% 43 | % Arm Kinematics % 44 | %%%%%%%%%%%%%%%%%% 45 | % 1 = Left, 2 = Right 46 | armBaseXYZ(:,1) = [0; .10; .20]; 47 | armBaseXYZ(:,2) = [0; -.10; .20]; 48 | 49 | armKin{1} = HebiUtils.loadHRDF([localDir '/hrdf/igorArm-Left.hrdf']); 50 | armKin{2} = HebiUtils.loadHRDF([localDir '/hrdf/igorArm-Right.hrdf']); 51 | 52 | for arm = 1:numArms 53 | armTransform = eye(4); 54 | armTransform(1:3,4) = armBaseXYZ(:,arm); 55 | armKin{arm}.setBaseFrame(armTransform); 56 | end 57 | 58 | %%%%%%%%%%%%%%%%%%%%%% 59 | % Chassis Kinematics % 60 | %%%%%%%%%%%%%%%%%%%%%% 61 | chassiskin = HebiUtils.loadHRDF([localDir '/hrdf/igorChassis.hrdf']); -------------------------------------------------------------------------------- /kits/igor/resources/mobile_io_igor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/igor/resources/mobile_io_igor.png -------------------------------------------------------------------------------- /kits/igor/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /kits/igor/tools/cropLog.m: -------------------------------------------------------------------------------- 1 | function [ log ] = cropLog( log, startTime, endTime ) 2 | %CROPLOG trims a log based on a starting and end time in seconds. 3 | % 4 | % log = cropLog( log, startTime, endTime) 5 | % 6 | % If you hand in empty [] for either time it will trim to the end of the 7 | % log. For example, CROPLOG( log, [], 10 ) will return a log of all the 8 | % data from the first 10 seconds of the log. 9 | % 10 | % Dave Rollinson 11 | % Feb 2015 12 | 13 | logFields = fields(log); 14 | 15 | if ~isempty(startTime) 16 | startIndex = find(log.time>startTime,1,'first'); 17 | else 18 | startIndex = 1; 19 | end 20 | 21 | if ~isempty(endTime) 22 | endIndex = find(log.time= 3 19 | latestLogNums = latestLogNums(end-numLatestLogs+1:end); 20 | end 21 | 22 | for i=1:length(latestLogNums) 23 | disp([ 'Loading ' num2str(latestLogNums(i)) ' of ' ... 24 | num2str(numLogs) ': ' logFiles{latestLogNums(i)} ' - ' ... 25 | num2str(fileInfo(latestLogNums(i)).bytes/1E6,'%0.1f') ' MB']); 26 | 27 | if nargin==4 && strcmp(logView,'full') 28 | hebiLogs{i} = struct( HebiUtils.convertGroupLog( ... 29 | [logFilePath logFiles{latestLogNums(i)}], ... 30 | 'view', 'full' ) ); 31 | else 32 | hebiLogs{i} = struct( HebiUtils.convertGroupLog( ... 33 | [logFilePath logFiles{latestLogNums(i)}] ) ); 34 | end 35 | end 36 | 37 | end 38 | 39 | -------------------------------------------------------------------------------- /kits/lily/getBodyPoseFromFeet.m: -------------------------------------------------------------------------------- 1 | function chassisPose = getBodyPoseFromFeet( feetXYZ, baseXYZ ) 2 | % GETBODYPOSEFROMFEET Calculates the translation and rotation of the 3 | % chassis based on the XYZ locations of the feet from some "baseXYZ" 4 | % position. 5 | % 6 | % INPUT: feetXYZ = 3x1x6 feedback position of feet 7 | % baseXYZ = 3x1x6 'home' position of feet 8 | % 9 | % Dave Rollinson 10 | % Dec 2014 11 | 12 | numFeet = size(feetXYZ,2); 13 | 14 | % Zero mean the points 15 | feetXYZ_CoM = mean(feetXYZ,2); 16 | baseXYZ_CoM = mean(baseXYZ,2); 17 | 18 | feetXYZ = feetXYZ - repmat( feetXYZ_CoM, 1, numFeet ); 19 | baseXYZ = baseXYZ - repmat( baseXYZ_CoM, 1, numFeet ); 20 | 21 | % Build the weighted correlation matrix 22 | xyzCorr = baseXYZ * feetXYZ'; 23 | 24 | % Take the SVD 25 | [U,~,V] = svd( xyzCorr ); 26 | 27 | % Modify the scaling matrix to account for reflections 28 | S = eye(3); 29 | S(3,3) = sign(det(U*V)); 30 | 31 | % Make the final transform 32 | chassisPose = eye(4); 33 | R = V*S*U'; 34 | t = feetXYZ_CoM - baseXYZ_CoM; 35 | 36 | chassisPose(1:3,1:3) = R; 37 | chassisPose(1:3,4) = t; 38 | 39 | 40 | end -------------------------------------------------------------------------------- /kits/lily/getJoyCommands.m: -------------------------------------------------------------------------------- 1 | function [xyzVel, rotVel, auxCmd] = getJoyCommands( fbkIO ) 2 | 3 | % Assumes you're using the HEBI Mobile I/O App 4 | 5 | xyzVel = zeros(3,1); 6 | rotVel = zeros(3,1); 7 | auxCmd = struct(); 8 | 9 | QUIT_BUTTON = 'b8'; 10 | STANCE_MODE = 'b1'; 11 | 12 | % PRESS DOWN B8 TO QUIT 13 | if fbkIO.(QUIT_BUTTON) 14 | auxCmd.quit = true; 15 | else 16 | auxCmd.quit = false; 17 | end 18 | 19 | % PRESS A TO TOGGLE STEPPING ON AND OFF 20 | if fbkIO.(STANCE_MODE) 21 | auxCmd.steppingMode = false; 22 | else 23 | auxCmd.steppingMode = true; 24 | end 25 | 26 | joyXYZScale = .3; 27 | joyRotScale = .6; 28 | joyAxisDeadZone = .05; 29 | 30 | % LINEAR VELOCITIES 31 | % X-Axis 32 | if abs(fbkIO.a8) > joyAxisDeadZone 33 | xyzVel(1) = -joyXYZScale * fbkIO.a8; 34 | end 35 | % Y-Axis 36 | if abs(fbkIO.a7) > joyAxisDeadZone 37 | xyzVel(2) = joyXYZScale * fbkIO.a7; 38 | end 39 | % Z-Axis 40 | if abs(fbkIO.a3) > joyAxisDeadZone 41 | xyzVel(3) = -joyXYZScale * fbkIO.a3; 42 | end 43 | 44 | % Make sure max velocity magnitude is maintained (L2-Norm) 45 | if norm(xyzVel) > joyXYZScale 46 | xyzVel = xyzVel * (joyXYZScale/norm(xyzVel)); 47 | end 48 | 49 | % ROTATIONAL VELOCITIES 50 | % X-Axis Rotation 51 | 52 | % % Y-Axis Rotation 53 | % if abs(axes(2)) > joyAxisDeadZone 54 | % rotVel(2) = joyRotScale * axes(2); 55 | % end 56 | 57 | % Z-Axis Rotation 58 | if abs(fbkIO.a1) > joyAxisDeadZone 59 | rotVel(3) = joyRotScale * fbkIO.a1; 60 | end 61 | 62 | 63 | end -------------------------------------------------------------------------------- /kits/lily/getMSIJoyCommands.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/getMSIJoyCommands.m -------------------------------------------------------------------------------- /kits/lily/hrdf/lilyChassis.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | -------------------------------------------------------------------------------- /kits/lily/hrdf/lilyLeg-Left.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kits/lily/hrdf/lilyLeg-Right.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kits/lily/images/controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/images/controller.png -------------------------------------------------------------------------------- /kits/lily/images/lily_Labeled.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/images/lily_Labeled.pptx -------------------------------------------------------------------------------- /kits/lily/images/lily_axis.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/images/lily_axis.pptx -------------------------------------------------------------------------------- /kits/lily/images/lily_coordinates.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/images/lily_coordinates.png -------------------------------------------------------------------------------- /kits/lily/images/lily_labeled.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/images/lily_labeled.png -------------------------------------------------------------------------------- /kits/lily/images/lily_render.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HebiRobotics/hebi-matlab-examples/0e5e84d72687995018dc0cdb9cfef17f41381c21/kits/lily/images/lily_render.png -------------------------------------------------------------------------------- /kits/lily/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | #Run this script to create a dynamic link to the daisyStart script. 3 | #Once this has executed, you can run the program anywhere by typing "daisyStart" 4 | DAISY_FILE="/daisyStart.sh" 5 | sudo ln -s $(dirname $(readlink -f $0))$DAISY_FILE /usr/bin/daisyStart 6 | chmod +x daisyStart.sh 7 | echo "you can now run the program by typing: daisyStart" 8 | 9 | -------------------------------------------------------------------------------- /kits/lily/joystickTest.m: -------------------------------------------------------------------------------- 1 | % Display joystick data 2 | % 3 | % Dave Rollinson 4 | % Feb 2017 5 | 6 | HebiJoystick.loadLibs(); 7 | joy = HebiJoystick(1) 8 | 9 | disp('Displaying button states, ctrl-C to quit...'); 10 | 11 | figure(345); 12 | while true 13 | 14 | [axes, buttons, povs] = read(joy); 15 | 16 | subplot(3,1,1); 17 | plot(1:length(axes),axes,'o'); 18 | title('Axes'); 19 | ylim([-1 1]); 20 | xlim([.5 length(axes)+.5]); 21 | set(gca,'xTick',1:length(axes)); 22 | grid on; 23 | 24 | subplot(3,1,2); 25 | plot(1:length(buttons),buttons,'*'); 26 | title('Buttons'); 27 | ylim([0 1]); 28 | xlim([.5 length(buttons)+.5]); 29 | set(gca,'xTick',1:length(buttons)); 30 | grid on; 31 | 32 | subplot(3,1,3); 33 | if povs == -1 34 | plot(0,0,'o'); 35 | else 36 | plot([0 sin(deg2rad(povs))],[0 cos(deg2rad(povs))]); 37 | end 38 | title('POVs'); 39 | text(.5,.75,['Val: ' num2str(povs)]) 40 | axis equal; 41 | ylim([-1 1]); 42 | xlim([-1 1]); 43 | grid on; 44 | 45 | drawnow; 46 | end -------------------------------------------------------------------------------- /kits/lily/lilyStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | #This script launches the daisy matlab script with the correct path. 3 | cd $(dirname $(readlink -f $0)) 4 | x-terminal-emulator -e "matlab -r \"runLily\"" 5 | -------------------------------------------------------------------------------- /kits/lily/makeLilyKinematics.m: -------------------------------------------------------------------------------- 1 | function [ legKin, chassisKin ] = makeLilyKinematics( ) 2 | % MAKERDAISYKINEMATICS returns kinematics for each leg of the R-Series Hexapod 3 | % 4 | % Andrew Willig 5 | % Dec 2019 6 | % 7 | % Leg Numbering / Chassis Coordinate convention: 8 | % This should match ROS wheeled vehicle convention 9 | % 10 | % 1 --- 2 +x 11 | % | ^ 12 | % 3 ----- 4 | 13 | % | +y <--o 14 | % 5 --- 6 +z 15 | 16 | 17 | % Get relative path 18 | localDir = fileparts(mfilename('fullpath')); 19 | 20 | % Distances to each leg 21 | chassisRadii = [0.3625 0.3625 0.2625 0.2625 0.3625 0.3625]; % m 22 | chassisAngles = deg2rad( [30 -30 90 -90 150 -150] ); 23 | 24 | chassiskin = HebiUtils.loadHRDF([localDir '/hrdf/LilyChassis.hrdf']); 25 | 26 | baseLims = [-pi/3 pi/3]; 27 | shoulderLims = [-pi/2 pi/3]; 28 | elbowLims = [-pi pi/4]; 29 | 30 | for leg=1:6 31 | if rem(leg,2)==1 32 | legKin{leg} = HebiUtils.loadHRDF([localDir '/hrdf/lilyLeg-Left.hrdf']); 33 | else 34 | legKin{leg} = HebiUtils.loadHRDF([localDir '/hrdf/lilyLeg-Right.hrdf']); 35 | end 36 | 37 | legTransform = eye(4); 38 | legTransform(1:3,1:3) = R_z(chassisAngles(leg)); 39 | legTransform(1:3,4) = ... 40 | R_z(chassisAngles(leg)) * [chassisRadii(leg); 0; 0]; 41 | legKin{leg}.setBaseFrame(legTransform); 42 | end 43 | 44 | -------------------------------------------------------------------------------- /kits/lily/setLilyParams.m: -------------------------------------------------------------------------------- 1 | % Robot-Specific Parameters 2 | % This is called in runHexapod.m. In the future it would be good to 3 | % combine all the setup into a function, setupLily.m, like we do with the 4 | % Rosie and Florence kits. 5 | % 6 | % Andrew Willig 7 | % Dec 2019 8 | 9 | % Masses / Weights. Assume all the legs weigh the same 10 | legMass = sum( legKin{1}.getBodyMasses() ); 11 | chassisMass = chassisKin.getBodyMasses(); 12 | robotMass = chassisMass + 6*legMass; % kg 13 | robotWeight = 9.8 * robotMass; 14 | 15 | numLegs = length(legKin); 16 | allLegs = 1:numLegs; 17 | 18 | % Leg Indices 19 | jointInds = 1:(3*numLegs); 20 | jointInds = reshape(jointInds,3,6)'; 21 | 22 | % Stance Parameters 23 | bodyHeight = .21; % meters 24 | stanceRadius = .60; % meters 25 | for leg=1:numLegs 26 | baseFrame = legKin{leg}.getBaseFrame(); 27 | homeStanceXYZ(:,leg) = baseFrame(1:3,1:3) * ... 28 | [stanceRadius; 0; -bodyHeight]; 29 | end 30 | 31 | levelHomeStanceXYZ = homeStanceXYZ; 32 | 33 | % Step Parameters 34 | stepHeight = .040; % meters (.040 default) 35 | stepOverShoot = 0.35; % factor of step to overshoot 36 | stepPeriod = 1.0; % seconds (.7 default) 37 | stepPhase = [0 .5 1]; 38 | 39 | isStance = ones(1,6)==true; % Boolean mask to determine stance feet 40 | stepping = false; % track state of stepping 41 | legStepState = 0; % Alternates set A and B in tripod gait 42 | 43 | % Thresholds: sets how far COM can be from 'home' position before stepping 44 | shiftThresh = .02; % meters (.025 default) 45 | rotThresh = .05; % rad .1 46 | 47 | -------------------------------------------------------------------------------- /kits/lily/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /kits/rosie/config/A-2160-01_X-Rosie.yaml: -------------------------------------------------------------------------------- 1 | # X-Series Rosie with default omniwheel configuration 2 | version: 1.0 3 | families: ["Rosie"] 4 | names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3", "W1", "W2", "W3"] 5 | hrdf: "../hrdf/Rosie.hrdf" 6 | 7 | gains: 8 | omni_wheels: "../gains/omni-drive-wheel-gains.xml" 9 | mecanum_wheels: "../gains/mecanum-wheel-gains.xml" 10 | diff_drive: "../gains/diff-drive-wheel-gains.xml" 11 | arm: "../gains/6-dof-arm-gains-rosie.xml" 12 | gripper: "../gains/gripper-gains.xml" 13 | 14 | user_data: 15 | controller_family: "HEBI" 16 | controller_name: "mobileIO" 17 | -------------------------------------------------------------------------------- /kits/rosie/gains/diff-drive-wheel-gains.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 4 | 5 | 3 3 6 | 0 0 7 | 0 0 8 | 0 0 9 | 0 0 10 | 1 1 11 | 0 0 12 | -inf -inf 13 | inf inf 14 | 1 1 15 | -10 -10 16 | 10 10 17 | 1 1 18 | 1 1 19 | 20 | 21 | 0.03 0.03 22 | 0 0 23 | 0 0 24 | 1 1 25 | 0 0 26 | 0.25 0.25 27 | 0 0 28 | -9.617128 -9.617128 29 | 9.617128 9.617128 30 | 1 1 31 | -1 -1 32 | 1 1 33 | 0.75 0.75 34 | 1 1 35 | 36 | 37 | 0.02 0.02 38 | 0 0 39 | 0.0001 0.0001 40 | 1 1 41 | 0 0 42 | 0.25 0.25 43 | 0 0 44 | -10 -10 45 | 10 10 46 | 1 1 47 | -1 -1 48 | 1 1 49 | 0.9 0.9 50 | 0 0 51 | 52 | 53 | -------------------------------------------------------------------------------- /kits/rosie/gains/gripper-gains.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 | 5 | 15 6 | 0 7 | 0 8 | 0 9 | 0 10 | 1 11 | 0 12 | -inf 13 | inf 14 | 1 15 | -20 16 | 20 17 | 1 18 | 1 19 | 20 | 21 | 0.05 22 | 0 23 | 0 24 | 1 25 | 0 26 | 0.25 27 | 0 28 | -1.502675 29 | 1.502675 30 | 1 31 | -1 32 | 1 33 | 0.75 34 | 1 35 | 36 | 37 | 0.25 38 | 0 39 | 0.001 40 | 1 41 | 0 42 | 0.25 43 | -0 44 | -20 45 | 20 46 | 1 47 | -1 48 | 1 49 | 0.9 50 | 0 51 | 52 | 53 | -------------------------------------------------------------------------------- /kits/rosie/gains/omni-drive-wheel-gains.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 4 4 4 | 5 | 3 3 3 6 | 0 0 0 7 | 0 0 0 8 | 0 0 0 9 | 0 0 0 10 | 1 1 1 11 | 0 0 0 12 | -inf -inf -inf 13 | inf inf inf 14 | 1 1 1 15 | -10 -10 -10 16 | 10 10 10 17 | 1 1 1 18 | 1 1 1 19 | 20 | 21 | 0.03 0.03 0.03 22 | 0 0 0 23 | 0 0 0 24 | 1 1 1 25 | 0 0 0 26 | 0.25 0.25 0.25 27 | 0 0 0 28 | -9.617128 -9.617128 -9.617128 29 | 9.617128 9.617128 9.617128 30 | 1 1 1 31 | -1 -1 -1 32 | 1 1 1 33 | 0.75 0.75 0.75 34 | 1 1 1 35 | 36 | 37 | 0.02 0.02 0.02 38 | 0 0 0 39 | 0.0001 0.0001 0.0001 40 | 1 1 1 41 | 0 0 0 42 | 0.25 0.25 0.25 43 | 0 0 0 44 | -10 -10 -10 45 | 10 10 10 46 | 1 1 1 47 | -1 -1 -1 48 | 1 1 1 49 | 0.9 0.9 0.9 50 | 0 0 0 51 | 52 | 53 | -------------------------------------------------------------------------------- /kits/rosie/hrdf/6-DoF_arm_w_gripper.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /kits/rosie/hrdf/Rosie.hrdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /kits/rosie/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | #Run this script to create a dynamic link to the rosieStart script. 3 | #Once this has executed, you can run the program anywhere by typing "rosieStart" 4 | ROSIE_FILE="/rosieStart.sh" 5 | sudo ln -s $(dirname $(readlink -f $0))$ROSIE_FILE /usr/bin/rosieStart 6 | chmod +x rosieStart.sh 7 | echo "you can now run the program by typing: rosieStart" 8 | 9 | -------------------------------------------------------------------------------- /kits/rosie/rosieDemoRunner.m: -------------------------------------------------------------------------------- 1 | function rosieDemoRunner( chassisType ) 2 | 3 | % Close any previously opened figures 4 | close all; 5 | 6 | % Make sure libraries and paths are loaded appropriately 7 | localDir = fileparts(mfilename('fullpath')); 8 | run(fullfile(localDir, 'startup.m')); 9 | 10 | % Start the demo 11 | while true 12 | try 13 | rosieDemo( chassisType ); 14 | catch me 15 | disp(me.message); 16 | end 17 | end 18 | 19 | end 20 | -------------------------------------------------------------------------------- /kits/rosie/rosieStart.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | #This script launches the rosie matlab script with the correct path. 3 | cd $(dirname $(readlink -f $0)) 4 | x-terminal-emulator -e "matlab -r \"rosieDemoRunner('omni')\"" 5 | -------------------------------------------------------------------------------- /kits/rosie/setupArmWithGripper.m: -------------------------------------------------------------------------------- 1 | function [ arm, params, gripper ] = setupArmWithGripper(family) 2 | 3 | % Arm Module Names 4 | group = HebiLookup.newGroupFromNames(family, { 5 | 'J1_base' 6 | 'J2_shoulder' 7 | 'J3_elbow' 8 | 'J4_wrist1' 9 | 'J5_wrist2' 10 | 'J6_wrist3' }); 11 | 12 | % Load and send Gains 13 | params.gains = HebiUtils.loadGains('gains/6-dof-arm-gains-rosie'); 14 | HebiUtils.sendWithRetry(group, 'gains', params.gains); 15 | 16 | % Kinematic Model 17 | kin = HebiUtils.loadHRDF('hrdf/6-DoF_arm_w_gripper'); 18 | 19 | % Gripper 20 | gripperGroup = HebiLookup.newGroupFromNames(family, 'Spool'); 21 | params.gripperGains = HebiUtils.loadGains('gains/gripper-gains'); 22 | HebiUtils.sendWithRetry(gripperGroup, 'gains', params.gripperGains); 23 | 24 | % API Wrappers 25 | arm = HebiArm(group, kin); 26 | gripper = HebiGripper(gripperGroup); 27 | 28 | % Default seed positions for doing inverse kinematics 29 | params.ikSeedPos = [0 1 2.5 1.5 -1.5 1]; 30 | 31 | % Trajectory generator parameters 32 | params.minTrajDuration = 0.33; % [sec] 33 | params.defaultSpeedFactor = 0.9; 34 | arm.trajGen.setMinDuration(params.minTrajDuration); 35 | arm.trajGen.setSpeedFactor(params.defaultSpeedFactor); 36 | 37 | % (Optional) Compensation to joint efforts due to a gas spring (if present) 38 | shoulderJointComp = 0; % Nm <--- Change this if you add a gas spring 39 | params.effortOffset = [0 shoulderJointComp 0 0 0 0]; 40 | 41 | % Default plugins 42 | arm.plugins = { 43 | HebiArmPlugins.EffortOffset(params.effortOffset) 44 | }; 45 | 46 | end 47 | -------------------------------------------------------------------------------- /kits/rosie/startup.m: -------------------------------------------------------------------------------- 1 | function [] = startup() 2 | % startup.m sets up libraries and paths and should be run once on startup. 3 | % 4 | % HEBI Robotics 5 | % Jun 2018 6 | 7 | localDir = fileparts(mfilename('fullpath')); 8 | 9 | % Run the include script from the top level of the examples 10 | includeScript = fullfile(localDir, '..', '..', 'include', 'include.m'); 11 | run(includeScript); 12 | 13 | % Add this folder and all its subfolders 14 | addpath(genpath(localDir)); 15 | 16 | end 17 | --------------------------------------------------------------------------------