diff --git a/kits/arm/config/ex_AR_kit.cfg.yaml b/kits/arm/config/ex_AR_kit.cfg.yaml new file mode 100644 index 00000000..a9a7ab37 --- /dev/null +++ b/kits/arm/config/ex_AR_kit.cfg.yaml @@ -0,0 +1,38 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Joint angles at home for AR demo: [0, pi/3, 2*pi/3, 5*pi/6, -pi/2, 0] + home_position: [0, 1.0471975511965976, 2.0943951023931953, 2.6179938779914944, -1.5707963267948966, 0] # radians + + # Time taken for a steady motion to the home position + homing_duration: 4 # seconds + + # Latency in tracking the AR pose. Increase this to avoid jerks + latency: 0.3 # seconds + + # Displacements of the phone are scaled by this value to give displacement of the end-effector + xyz_scale: 0.75 + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_AR_kit.json" \ No newline at end of file diff --git a/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml new file mode 100644 index 00000000..fd065071 --- /dev/null +++ b/kits/arm/config/ex_AR_kit_w_gripper.cfg.yaml @@ -0,0 +1,44 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2240-06G.hrdf" + +gains: + default: "gains/A-2240-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Joint angles at home for AR demo: [0, pi/3, pi/2, 2*pi/3, -pi/2, 0] + home_position: [0, 1.0471975511965976, 1.5707963267948966, 2.0943951023931953, -1.5707963267948966, 0] # radians + + # Time taken for a steady motion to the home position + homing_duration: 4 # seconds + + # Latency in tracking the AR pose. Increase this to avoid jerks + latency: 0.3 # seconds + + # Displacements of the phone are scaled by this value to give displacement of the end-effector + xyz_scale: 0.75 + + gripper_family: "Arm" + gripper_name: "gripperSpool" + gripper_gains: "gains/gripper_spool_gains.xml" + gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. + gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file diff --git a/kits/arm/config/ex_gravity_compensation.cfg.yaml b/kits/arm/config/ex_gravity_compensation.cfg.yaml new file mode 100644 index 00000000..504ce76e --- /dev/null +++ b/kits/arm/config/ex_gravity_compensation.cfg.yaml @@ -0,0 +1,20 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 \ No newline at end of file diff --git a/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml b/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml new file mode 100644 index 00000000..cc92451a --- /dev/null +++ b/kits/arm/config/ex_gravity_compensation_toggle.cfg.yaml @@ -0,0 +1,26 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: false + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_gravity_compensation_toggle.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml new file mode 100644 index 00000000..5dd000a2 --- /dev/null +++ b/kits/arm/config/ex_impedance_control_cartesian.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a translational spring which maintains position but not orientation + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [300, 300, 300, 0, 0, 0] # (N/m) or (Nm/rad) + kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [20, 20, 20, 0, 0, 0] + i_clamp: [10, 10, 10, 0, 0, 0] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_impedance_control_cartesian.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_damping.cfg.yaml b/kits/arm/config/ex_impedance_control_damping.cfg.yaml new file mode 100644 index 00000000..3e54d2a2 --- /dev/null +++ b/kits/arm/config/ex_impedance_control_damping.cfg.yaml @@ -0,0 +1,53 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains are initialized to zero as the will be changed over the course of the damping example + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: false + kp: [0, 0, 0, 0, 0, 0] # (N/m) or (Nm/rad) + kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + +# Any extra configuration data is stored here +user_data: + + # Distance above the base for overdamped, critically damped, and underdamped cases respectively + lower_limits: [0.0, 0.15, 0.3] # (m) + + overdamped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) + overdamped_kd: [15, 15, 1, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + + critically_damped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) + critically_damped_kd: [5, 5, 1, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + + underdamped_kp: [100, 100, 0, 5, 5, 1] # (N/m) or (Nm/rad) + underdamped_kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_impedance_control_damping.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_fixed.cfg.yaml b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml new file mode 100644 index 00000000..2b4ea4ab --- /dev/null +++ b/kits/arm/config/ex_impedance_control_fixed.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a controller that rigidly maintains a fixed pose + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [300, 300, 300, 5, 5, 1] # (N/m) or (Nm/rad) + kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [20, 20, 20, 0.5, 0.5, 0.5] + i_clamp: [10, 10, 10, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_impedance_control_fixed.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_floor.cfg.yaml b/kits/arm/config/ex_impedance_control_floor.cfg.yaml new file mode 100644 index 00000000..9e5a89d5 --- /dev/null +++ b/kits/arm/config/ex_impedance_control_floor.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to the stiffness of the virtual floor + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [500, 500, 500, 5, 5, 1] # (N/m) or (Nm/rad) + kd: [5, 5, 5, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [20, 20, 20, 0.5, 0.5, 0.5] + i_clamp: [10, 10, 10, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_impedance_control_floor.json" \ No newline at end of file diff --git a/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml new file mode 100644 index 00000000..210ab69a --- /dev/null +++ b/kits/arm/config/ex_impedance_control_gimbal.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + + # An impedance controller adds a virtual spring to the + # end-effector and can improve tracking. It can be enabled + # by setting 'enabled' to true. The gains are in the form of + # [x, y, z, rx, ry, rz]. Setting gains of zero deactivates + # the corresponding degree of translation or rotation. + # These gains correspond to a rotational spring emulating a gimbal + - type: ImpedanceController + name: impedanceController + enabled: true + ramp_time: 0 + gains_in_end_effector_frame: true + kp: [0, 0, 0, 8, 8, 1] # (N/m) or (Nm/rad) + kd: [0, 0, 0, 0, 0, 0] # (N/(m/sec)) or (Nm/(rad/sec)) + ki: [0, 0, 0, 0.5, 0.5, 0.5] + i_clamp: [0, 0, 0, 1, 1, 1] # max value + +user_data: + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_impedance_control_gimbal.json" \ No newline at end of file diff --git a/kits/arm/config/ex_mobile_io_control.cfg.yaml b/kits/arm/config/ex_mobile_io_control.cfg.yaml new file mode 100644 index 00000000..401f7ee2 --- /dev/null +++ b/kits/arm/config/ex_mobile_io_control.cfg.yaml @@ -0,0 +1,34 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Three waypoints for Buttons - B1, B2, B3 + waypoint_1: [0, 0, 0, 0, 0, 0] + waypoint_2: [0.7853981633974483, 1.0471975511965976, 2.0943951023931953, 1.0471975511965976, 0.7853981633974483, 0] # [pi/4, pi/3, 2*pi/3, pi/3, pi/4, 0] + waypoint_3: [-0.7853981633974483, 1.0471975511965976, 2.0943951023931953, 1.0471975511965976, 2.356194490192345, 0] # [-pi/4, pi/3, 2*pi/3, pi/3, 3*pi/4, 0] + + # Time taken to travel to a waypoint + travel_time: 3 # seconds + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_mobile_io_control.json" \ No newline at end of file diff --git a/kits/arm/config/ex_teach_repeat.cfg.yaml b/kits/arm/config/ex_teach_repeat.cfg.yaml new file mode 100644 index 00000000..1eedfc45 --- /dev/null +++ b/kits/arm/config/ex_teach_repeat.cfg.yaml @@ -0,0 +1,36 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2085-06.hrdf" + +gains: + default: "gains/A-2085-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Travel time is calculated using + # base_travel_time + slider * (base_travel_time - min_travel_time) + # Since slider goes from -1 to 1, travel time goes from min_travel_time to 2 * base_travel_time - min_travel_time + + # Minimum travel time value + min_travel_time: 0.5 + + # The default travel time value + base_travel_time: 3 # seconds + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_teach_repeat.json" \ No newline at end of file diff --git a/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml b/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml new file mode 100644 index 00000000..5e30eaad --- /dev/null +++ b/kits/arm/config/ex_teach_repeat_w_gripper.cfg.yaml @@ -0,0 +1,42 @@ +# 6-DoF Arm +version: 1.0 +families: ["Arm"] +names: ["J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"] +hrdf: "hrdf/A-2240-06G.hrdf" + +gains: + default: "gains/A-2240-06.xml" + +plugins: + + - type: GravityCompensationEffort + name: gravComp + enabled: true + ramp_time: 0 + + - type: DynamicsCompensationEffort + name: dynamicsComp + enabled: true + ramp_time: 0 + +user_data: + + # Travel time is calculated using + # base_travel_time + slider * (base_travel_time - min_travel_time) + # Since slider goes from -1 to 1, travel time goes from min_travel_time to 2 * base_travel_time - min_travel_time + + # Minimum travel time value + min_travel_time: 0.5 + + # The default travel time value + base_travel_time: 3 # seconds + + gripper_family: "Arm" + gripper_name: "gripperSpool" + gripper_gains: "gains/gripper_spool_gains.xml" + gripper_close_effort: -5.0 # (Nm) Effort applied to close the gripper. More negative effort will pinch the gripper harder. + gripper_open_effort: 1.0 # (Nm) Effort applied to open the gripper. More positive effort will NOT make the gripper harder to close. + + mobile_io_family: "Arm" + mobile_io_name: "mobileIO" + mobile_io_layout: "layouts/ex_AR_kit_w_gripper.json" \ No newline at end of file diff --git a/kits/arm/gains/A-2080-01.xml b/kits/arm/config/gains/A-2080-01.xml similarity index 100% rename from kits/arm/gains/A-2080-01.xml rename to kits/arm/config/gains/A-2080-01.xml diff --git a/kits/arm/gains/A-2084-01.xml b/kits/arm/config/gains/A-2084-01.xml similarity index 100% rename from kits/arm/gains/A-2084-01.xml rename to kits/arm/config/gains/A-2084-01.xml diff --git a/kits/arm/gains/A-2085-03.xml b/kits/arm/config/gains/A-2085-03.xml similarity index 100% rename from kits/arm/gains/A-2085-03.xml rename to kits/arm/config/gains/A-2085-03.xml diff --git a/kits/arm/gains/A-2085-04.xml b/kits/arm/config/gains/A-2085-04.xml similarity index 100% rename from kits/arm/gains/A-2085-04.xml rename to kits/arm/config/gains/A-2085-04.xml diff --git a/kits/arm/gains/A-2085-05.xml b/kits/arm/config/gains/A-2085-05.xml similarity index 100% rename from kits/arm/gains/A-2085-05.xml rename to kits/arm/config/gains/A-2085-05.xml diff --git a/kits/arm/gains/A-2085-06.xml b/kits/arm/config/gains/A-2085-06.xml similarity index 100% rename from kits/arm/gains/A-2085-06.xml rename to kits/arm/config/gains/A-2085-06.xml diff --git a/kits/arm/gains/A-2240-04.xml b/kits/arm/config/gains/A-2240-04.xml similarity index 100% rename from kits/arm/gains/A-2240-04.xml rename to kits/arm/config/gains/A-2240-04.xml diff --git a/kits/arm/gains/A-2240-05.xml b/kits/arm/config/gains/A-2240-05.xml similarity index 100% rename from kits/arm/gains/A-2240-05.xml rename to kits/arm/config/gains/A-2240-05.xml diff --git a/kits/arm/gains/A-2240-06.xml b/kits/arm/config/gains/A-2240-06.xml similarity index 100% rename from kits/arm/gains/A-2240-06.xml rename to kits/arm/config/gains/A-2240-06.xml diff --git a/kits/arm/gains/A-2255-01.xml b/kits/arm/config/gains/A-2255-01.xml similarity index 100% rename from kits/arm/gains/A-2255-01.xml rename to kits/arm/config/gains/A-2255-01.xml diff --git a/kits/arm/gains/A-2302-01.xml b/kits/arm/config/gains/A-2302-01.xml similarity index 100% rename from kits/arm/gains/A-2302-01.xml rename to kits/arm/config/gains/A-2302-01.xml diff --git a/kits/arm/gains/gripper_spool_gains.xml b/kits/arm/config/gains/gripper_spool_gains.xml similarity index 100% rename from kits/arm/gains/gripper_spool_gains.xml rename to kits/arm/config/gains/gripper_spool_gains.xml diff --git a/kits/arm/hrdf/A-2084-01.hrdf b/kits/arm/config/hrdf/A-2084-01.hrdf similarity index 100% rename from kits/arm/hrdf/A-2084-01.hrdf rename to kits/arm/config/hrdf/A-2084-01.hrdf diff --git a/kits/arm/hrdf/A-2084-01G.hrdf b/kits/arm/config/hrdf/A-2084-01G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2084-01G.hrdf rename to kits/arm/config/hrdf/A-2084-01G.hrdf diff --git a/kits/arm/hrdf/A-2085-03.hrdf b/kits/arm/config/hrdf/A-2085-03.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-03.hrdf rename to kits/arm/config/hrdf/A-2085-03.hrdf diff --git a/kits/arm/hrdf/A-2085-04.hrdf b/kits/arm/config/hrdf/A-2085-04.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-04.hrdf rename to kits/arm/config/hrdf/A-2085-04.hrdf diff --git a/kits/arm/hrdf/A-2085-05.hrdf b/kits/arm/config/hrdf/A-2085-05.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-05.hrdf rename to kits/arm/config/hrdf/A-2085-05.hrdf diff --git a/kits/arm/hrdf/A-2085-05G.hrdf b/kits/arm/config/hrdf/A-2085-05G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-05G.hrdf rename to kits/arm/config/hrdf/A-2085-05G.hrdf diff --git a/kits/arm/hrdf/A-2085-06.hrdf b/kits/arm/config/hrdf/A-2085-06.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-06.hrdf rename to kits/arm/config/hrdf/A-2085-06.hrdf diff --git a/kits/arm/hrdf/A-2085-06G.hrdf b/kits/arm/config/hrdf/A-2085-06G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2085-06G.hrdf rename to kits/arm/config/hrdf/A-2085-06G.hrdf diff --git a/kits/arm/hrdf/A-2240-04.hrdf b/kits/arm/config/hrdf/A-2240-04.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-04.hrdf rename to kits/arm/config/hrdf/A-2240-04.hrdf diff --git a/kits/arm/hrdf/A-2240-05.hrdf b/kits/arm/config/hrdf/A-2240-05.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-05.hrdf rename to kits/arm/config/hrdf/A-2240-05.hrdf diff --git a/kits/arm/hrdf/A-2240-05G.hrdf b/kits/arm/config/hrdf/A-2240-05G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-05G.hrdf rename to kits/arm/config/hrdf/A-2240-05G.hrdf diff --git a/kits/arm/hrdf/A-2240-06.hrdf b/kits/arm/config/hrdf/A-2240-06.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-06.hrdf rename to kits/arm/config/hrdf/A-2240-06.hrdf diff --git a/kits/arm/hrdf/A-2240-06G.hrdf b/kits/arm/config/hrdf/A-2240-06G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2240-06G.hrdf rename to kits/arm/config/hrdf/A-2240-06G.hrdf diff --git a/kits/arm/hrdf/A-2302-01.hrdf b/kits/arm/config/hrdf/A-2302-01.hrdf similarity index 100% rename from kits/arm/hrdf/A-2302-01.hrdf rename to kits/arm/config/hrdf/A-2302-01.hrdf diff --git a/kits/arm/hrdf/A-2302-01G.hrdf b/kits/arm/config/hrdf/A-2302-01G.hrdf similarity index 100% rename from kits/arm/hrdf/A-2302-01G.hrdf rename to kits/arm/config/hrdf/A-2302-01G.hrdf diff --git a/kits/arm/config/layouts/ar_control_sm.json b/kits/arm/config/layouts/ar_control_sm.json new file mode 100644 index 00000000..41401bbc --- /dev/null +++ b/kits/arm/config/layouts/ar_control_sm.json @@ -0,0 +1,49 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.5, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Home", + "mode": 0 + } + }, + { + "id": "b2", + "type": "button", + "x": 0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Arm Control", + "mode": 0 + } + }, + { + "id": "b3", + "type": "button", + "x": 0.5, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Gripper", + "mode": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_AR_kit.json b/kits/arm/config/layouts/ex_AR_kit.json new file mode 100755 index 00000000..1c03a393 --- /dev/null +++ b/kits/arm/config/layouts/ex_AR_kit.json @@ -0,0 +1,61 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🏠", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📲", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] diff --git a/kits/arm/config/layouts/ex_AR_kit_w_gripper.json b/kits/arm/config/layouts/ex_AR_kit_w_gripper.json new file mode 100755 index 00000000..19b03251 --- /dev/null +++ b/kits/arm/config/layouts/ex_AR_kit_w_gripper.json @@ -0,0 +1,76 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🏠", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📳", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "🤌", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_gravity_compensation_toggle.json b/kits/arm/config/layouts/ex_gravity_compensation_toggle.json new file mode 100755 index 00000000..9057c8c4 --- /dev/null +++ b/kits/arm/config/layouts/ex_gravity_compensation_toggle.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_cartesian.json b/kits/arm/config/layouts/ex_impedance_control_cartesian.json new file mode 100755 index 00000000..acb02d8c --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_cartesian.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📌", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_damping.json b/kits/arm/config/layouts/ex_impedance_control_damping.json new file mode 100755 index 00000000..43f34114 --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_damping.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "💪", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_fixed.json b/kits/arm/config/layouts/ex_impedance_control_fixed.json new file mode 100755 index 00000000..09b7e359 --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_fixed.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🛑", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_floor.json b/kits/arm/config/layouts/ex_impedance_control_floor.json new file mode 100755 index 00000000..fc04783b --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_floor.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🧱", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_impedance_control_gimbal.json b/kits/arm/config/layouts/ex_impedance_control_gimbal.json new file mode 100755 index 00000000..e066913f --- /dev/null +++ b/kits/arm/config/layouts/ex_impedance_control_gimbal.json @@ -0,0 +1,37 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📈", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": 0.25, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🤳", + "mode": "toggle" + } + }, + { + "id": "led", + "type": "led", + "x": 0, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_mobile_io_control.json b/kits/arm/config/layouts/ex_mobile_io_control.json new file mode 100644 index 00000000..c531668a --- /dev/null +++ b/kits/arm/config/layouts/ex_mobile_io_control.json @@ -0,0 +1,73 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "1", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": -0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "2", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "3", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🌍", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "led", + "type": "led", + "x": -0, + "y": -0.15, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_teach_repeat.json b/kits/arm/config/layouts/ex_teach_repeat.json new file mode 100755 index 00000000..063c3ff4 --- /dev/null +++ b/kits/arm/config/layouts/ex_teach_repeat.json @@ -0,0 +1,88 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📌", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": -0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🚏", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🔄", + "mode": "momentary" + } + }, + { + "id": "b4", + "type": "button", + "x": 0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🗑️", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "⏱️", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/ex_teach_repeat_w_gripper.json b/kits/arm/config/layouts/ex_teach_repeat_w_gripper.json new file mode 100644 index 00000000..20660c98 --- /dev/null +++ b/kits/arm/config/layouts/ex_teach_repeat_w_gripper.json @@ -0,0 +1,100 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "📌", + "mode": "momentary" + } + }, + { + "id": "b2", + "type": "button", + "x": -0.45, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🤌", + "mode": "momentary" + } + }, + { + "id": "b3", + "type": "button", + "x": -0.15, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🚏", + "mode": "momentary" + } + }, + { + "id": "b5", + "type": "button", + "x": 0.15, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🔄", + "mode": "momentary" + } + }, + { + "id": "b6", + "type": "button", + "x": 0.45, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "🗑️", + "mode": "momentary" + } + }, + { + "id": "b8", + "type": "button", + "x": 0.75, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "❌", + "mode": "momentary" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "Time to Waypoint", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + } +] \ No newline at end of file diff --git a/kits/arm/config/layouts/joystick_control_sm.json b/kits/arm/config/layouts/joystick_control_sm.json new file mode 100644 index 00000000..d95b7bcb --- /dev/null +++ b/kits/arm/config/layouts/joystick_control_sm.json @@ -0,0 +1,112 @@ +[ + { + "id": "b1", + "type": "button", + "x": -0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Home", + "mode": 0 + } + }, + { + "id": "b4", + "type": "button", + "x": -0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Quit", + "mode": 0 + } + }, + { + "id": "b6", + "type": "button", + "x": -0, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "\u21E7", + "mode": 0 + } + }, + { + "id": "b7", + "type": "button", + "x": 0.4, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "Grip", + "mode": 1 + } + }, + { + "id": "b8", + "type": "button", + "x": 0.8, + "y": -0.73, + "width": 0.15, + "height": 0.15, + "parameters": { + "text": "\u21E9", + "mode": 0 + } + }, + { + "id": "a1,a2", + "type": "joystick", + "x": -0.9, + "y": 0.88, + "width": 0.18, + "height": 0.22, + "parameters": { + "text1": "", + "text2": "Rotate" + } + }, + { + "id": "a3", + "type": "rotslider", + "x": -0.25, + "y": 0.6, + "width": 0.05, + "height": 0.5, + "parameters": { + "text": "Wrist", + "steps": 10, + "min": -1, + "max": 1, + "rest": 0 + } + }, + { + "id": "led", + "type": "led", + "x": 0.25, + "y": 0, + "width": 0.2, + "height": 0.2, + "parameters": { + "text": "led" + } + }, + { + "id": "a7,a8", + "type": "joystick", + "x": 0.9, + "y": 0.86, + "width": 0.18, + "height": 0.22, + "parameters": { + "text1": "", + "text2": "Translate" + } + } +] \ No newline at end of file diff --git a/kits/arm/ex_AR_kit.cpp b/kits/arm/ex_AR_kit.cpp new file mode 100644 index 00000000..da3cb4d6 --- /dev/null +++ b/kits/arm/ex_AR_kit.cpp @@ -0,0 +1,207 @@ +/* + * AR Kit Example + * Currently assumes 6 degrees of freedom in you arm! + * This demo sets up your arm's end effector to mimick the changes in + * position and orientation of your mobile IO device. + */ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; // For all things mobileIO + + + +Eigen::Matrix3d makeRotationMatrix (hebi::Quaternionf phone_orientation) { + Eigen::Quaterniond q; + q.w() = phone_orientation.getW(); + q.x() = phone_orientation.getX(); + q.y() = phone_orientation.getY(); + q.z() = phone_orientation.getZ(); + return q.toRotationMatrix(); +} + + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_AR_kit.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + // Different modes it can be in (when in none, then automatic grav_comp) + bool softstart = true; + bool ar_mode = false; + + // Load user data from config + Eigen::VectorXd home_position(arm -> robotModel().getDoFCount()); + home_position = Eigen::Map(example_config->getUserData().getFloatList("home_position").data(), example_config->getUserData().getFloatList("home_position").size()); + double soft_start_time = example_config->getUserData().getFloat("homing_duration"); + double xyz_scale = example_config->getUserData().getFloat("xyz_scale"); + + // Command the softstart to home position + arm -> update(); + arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); // take 4 seconds + arm -> send(); + + // Get the cartesian position and rotation matrix @ home position + Eigen::Vector3d xyz_home; + Eigen::Matrix3d rot_home; + arm -> FK(home_position, xyz_home, rot_home); + + // Set up states for the mobile device + Eigen::Vector3d xyz_phone_init; + Eigen::Matrix3d rot_phone_init; + + // Target variables + Eigen::VectorXd target_joints(arm -> robotModel().getDoFCount()); + + while(arm->update()) + { + + if (softstart) { + // End softstart when arm reaches its homePosition + if (arm -> atGoal()){ + mobile_io->appendText("Softstart Complete!"); + softstart = false; + continue; + } + arm -> send(); + + // Stay in softstart, don't do any other behavior + continue; + } + + // Get latest mobile_state + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + // Button B1 - Return to home position + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + ar_mode = false; + arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); + } + + // Button B3 - Start AR Control + if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) { + xyz_phone_init << mobile_io -> getLastFeedback().mobile().arPosition().get().getX(), + mobile_io -> getLastFeedback().mobile().arPosition().get().getY(), + mobile_io -> getLastFeedback().mobile().arPosition().get().getZ(); + rot_phone_init = makeRotationMatrix(mobile_io -> getLastFeedback().mobile().arOrientation().get()); + ar_mode = true; + } + + // Button B6 - Grav Comp Mode + if (mobile_io->getButtonDiff(6) == hebi::util::MobileIO::ButtonState::ToOn) { + arm -> cancelGoal(); + ar_mode = false; + } + + // Button B8 - End Demo + if (mobile_io->getButtonDiff(8) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + } + + if (ar_mode) { + // Get the latest mobile position and orientation + Eigen::Vector3d xyz_phone; + xyz_phone << mobile_io->getLastFeedback().mobile().arPosition().get().getX(), + mobile_io->getLastFeedback().mobile().arPosition().get().getY(), + mobile_io->getLastFeedback().mobile().arPosition().get().getZ(); + auto rot_phone = makeRotationMatrix(mobile_io->getLastFeedback().mobile().arOrientation().get()); + + // Calculate new targets + Eigen::Vector3d xyz_scale_vec; + xyz_scale_vec << 1, 1, 2; + Eigen::Vector3d xyz_target = xyz_home + (xyz_scale * xyz_scale_vec.array() * + (rot_phone_init.transpose() * (xyz_phone - xyz_phone_init)).array()).matrix(); + Eigen::Matrix3d rot_target = rot_phone_init.transpose() * rot_phone * rot_home; + + // Calculate new arm joint angle targets + target_joints = arm -> solveIK(arm -> lastFeedback().getPosition(), + xyz_target, + rot_target); + + // Create and send new goal to the arm + arm -> setGoal(arm::Goal::createFromPosition(example_config->getUserData().getFloat("latency"), target_joints)); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->resetUI(); + + return 0; +} diff --git a/kits/arm/example_double_arm_teach_repeat.cpp b/kits/arm/ex_double_arm_teach_repeat.cpp similarity index 100% rename from kits/arm/example_double_arm_teach_repeat.cpp rename to kits/arm/ex_double_arm_teach_repeat.cpp diff --git a/kits/arm/example_gravity_compensation.cpp b/kits/arm/ex_gravity_compensation.cpp similarity index 56% rename from kits/arm/example_gravity_compensation.cpp rename to kits/arm/ex_gravity_compensation.cpp index be1d0103..c6121fad 100644 --- a/kits/arm/example_gravity_compensation.cpp +++ b/kits/arm/ex_gravity_compensation.cpp @@ -8,13 +8,13 @@ * demo. */ -// #include "lookup.hpp" -// #include "group.hpp" #include "group_command.hpp" #include "group_feedback.hpp" #include "robot_model.hpp" #include "arm/arm.hpp" +#include "util/mobile_io.hpp" #include +#include "hebi_util.hpp" using namespace hebi; using namespace experimental; @@ -22,26 +22,41 @@ using namespace experimental; int main(int argc, char* argv[]) { ////////////////////////// - ///// Arm Setup ////////// + ///// Config Setup /////// ////////////////////////// - arm::Arm::Params params; + // Config file path + const std::string example_config_file = "config/ex_gravity_compensation.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } - // Setup Module Family and Module Names - params.families_ = {"Arm"}; //[change back] - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; + // For this demo, we need just the arm + std::unique_ptr arm; - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); - // Create the Arm Object - auto arm = arm::Arm::create(params); + // Keep retrying if arm not found while (!arm) { - arm = arm::Arm::create(params); - } + std::cerr << "Failed to create arm, retrying..." << std::endl; - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; ////////////////////////// //// Main Control Loop /// diff --git a/kits/arm/ex_gravity_compensation_toggle.cpp b/kits/arm/ex_gravity_compensation_toggle.cpp new file mode 100644 index 00000000..cc2656c1 --- /dev/null +++ b/kits/arm/ex_gravity_compensation_toggle.cpp @@ -0,0 +1,146 @@ +/** + * This file is a barebones skeleton of how to setup an arm for use. + * It demonstrates gravity compensation behavior by commanding torques + * equal to the force from gravity on the links and joints of an arm. + * Note that this only approximately balances out gravity, as imperfections in + * the torque sensing and modeled system can lead to "drift". Also, the + * particular choice of PID control gains can affect the performance of this + * demo. + */ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_gravity_compensation_toggle.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Retrieve the gravcomp plugin from the arm + + // Get a weak_ptr from the arm API, lock it as a shared_ptr, and then downcast it from a general plugin pointer to a specific plugin pointer + auto plugin_shared_ptr = arm->getPluginByType().lock(); + if (!plugin_shared_ptr) { + std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl; + return -1; + } + auto gravcomp_plugin_ptr = std::dynamic_pointer_cast(plugin_shared_ptr); + if (!gravcomp_plugin_ptr) { + std::cerr << "Failed to cast plugin to GravityCompensationEffort." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + std::cout << "Commanded gravity-compensated zero force to the arm.\n" + << " 🌍 (B2) - Toggles the gravity compensation on/off:\n" + << " ON - Apply controller \n" + << " OFF - Disable controller\n" + << " ❌ (B1) - Exits the demo.\n"; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + + // Button B2 - Set and unset gravcomp mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + // Enable gravcomp + gravcomp_plugin_ptr->setEnabled(true); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + // Disable gravcomp + gravcomp_plugin_ptr->setEnabled(false); + } + } + // Send latest commands to the arm + arm->send(); + } + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_cartesian.cpp b/kits/arm/ex_impedance_control_cartesian.cpp new file mode 100644 index 00000000..a8940e09 --- /dev/null +++ b/kits/arm/ex_impedance_control_cartesian.cpp @@ -0,0 +1,181 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Cartesian" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_impedance_control_cartesian.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + std::cout << "Commanded gravity-compensated zero force to the arm.\n" + << " 📌 (B2) - Toggles an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + // NOTE: Angle wraparound is an unresolved issue which can lead to unstable behaviour for any case involving rotational positional control. + // Make sure that the rotational gains are either all zero, or are high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // Interacting with the end-effector in these examples is perfectly safe. + // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_damping.cpp b/kits/arm/ex_impedance_control_damping.cpp new file mode 100644 index 00000000..6acdf0be --- /dev/null +++ b/kits/arm/ex_impedance_control_damping.cpp @@ -0,0 +1,239 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Damping" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_impedance_control_damping.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + // Retrieve the impedance controller plugin from the arm + + // Get a weak_ptr from the arm API, lock it as a shared_ptr, and then downcast it from a general plugin pointer to a specific plugin pointer + auto plugin_shared_ptr = arm->getPluginByType().lock(); + if (!plugin_shared_ptr) { + std::cerr << "Failed to lock plugin shared_ptr. The plugin may have been destroyed." << std::endl; + return -1; + } + auto impedance_plugin_ptr = std::dynamic_pointer_cast(plugin_shared_ptr); + if (!impedance_plugin_ptr) { + std::cerr << "Failed to cast plugin to ImpedanceController." << std::endl; + return -1; + } + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + std::cout << "Commanded gravity-compensated zero force to the arm.\n" + << " 💪 (B2) - Toggles an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Meters above the base for overdamped, critically damped, and underdamped cases respectively + std::vector lower_limits = example_config->getUserData().getFloatList("lower_limits"); + + // State variable for current mode: 0 for overdamped, 1 for crtically damped, 2 for underdamped, -1 for free + int mode = -1; + int prevmode = -1; + + // Kp and Kd in different modes + std::vector damping_kp, damping_kd; + + // 0: Overdamped + damping_kp.push_back(Eigen::Map(example_config->getUserData().getFloatList("overdamped_kp").data(), example_config->getUserData().getFloatList("overdamped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().getFloatList("overdamped_kd").data(), example_config->getUserData().getFloatList("overdamped_kd").size())); + + // 1: Critically damped + damping_kp.push_back(Eigen::Map(example_config->getUserData().getFloatList("critically_damped_kp").data(), example_config->getUserData().getFloatList("critically_damped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().getFloatList("critically_damped_kd").data(), example_config->getUserData().getFloatList("critically_damped_kd").size())); + + // 2: Underdamped + damping_kp.push_back(Eigen::Map(example_config->getUserData().getFloatList("underdamped_kp").data(), example_config->getUserData().getFloatList("underdamped_kp").size())); + damping_kd.push_back(Eigen::Map(example_config->getUserData().getFloatList("underdamped_kd").data(), example_config->getUserData().getFloatList("underdamped_kd").size())); + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + mode = -1; // Free + prevmode = -1; + } + else + { + // Use forward kinematics to calculate pose of end-effector + Eigen::Vector3d ee_position_curr; + Eigen::Matrix3d ee_orientation_curr; + arm->FK(arm->lastFeedback().getPosition(), ee_position_curr, ee_orientation_curr); + + // Assign mode based on current position + for (int i = 0; i < static_cast(lower_limits.size()); i++) + { + if (ee_position_curr(2) > lower_limits[i]) + { + mode = i; + } + } + + // Change gains only upon mode switches + if (mode != prevmode && mode >= 0) + { + impedance_plugin_ptr->setKp(damping_kp.at(mode)); + impedance_plugin_ptr->setKd(damping_kd.at(mode)); + std::cout << "Mode: " << mode << std::endl; + } + prevmode = mode; + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_fixed.cpp b/kits/arm/ex_impedance_control_fixed.cpp new file mode 100644 index 00000000..0408b4ee --- /dev/null +++ b/kits/arm/ex_impedance_control_fixed.cpp @@ -0,0 +1,192 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Fixed" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_impedance_control_fixed.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + std::cout << "Commanded gravity-compensated zero force to the arm.\n" + << " 🛑 (B2) - Toggles an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + // NOTE: Angle wraparound is an unresolved issue which can lead to unstable behaviour for any case involving rotational positional control. + // Make sure that the rotational gains are either all zero, or are high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // Interacting with the end-effector in these examples is perfectly safe. + // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_floor.cpp b/kits/arm/ex_impedance_control_floor.cpp new file mode 100644 index 00000000..ef567ce8 --- /dev/null +++ b/kits/arm/ex_impedance_control_floor.cpp @@ -0,0 +1,237 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Floor" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_impedance_control_floor.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + std::cout << "Commanded gravity-compensated zero force to the arm.\n" + << " 🧱 (B2) - Toggles an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Initialize floor demo variables + double floor_level = 0.0; + double floor_buffer = 0.01; // 1cm + + // Initialize floor demo flags + bool floor_command_flag = false; // Indicates whether or not to command floor stiffness goals + bool cancel_command_flag = false; // Indicates whether or not to cancel goals + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + + // Store current height as floor level, for floor demo + + // Use forward kinematics to find end-effector pose + Eigen::Vector3d ee_position0; + Eigen::Matrix3d ee_orientation0; + arm->FK(arm->lastFeedback().getPosition(), ee_position0, ee_orientation0); + + // Give a little margin to floor level + floor_level = ee_position0(2) - floor_buffer; + + // Update flags to indicate having left the floor + cancel_command_flag = true; + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + else + { + // Use forward kinematics to calculate pose of end-effector + Eigen::Vector3d ee_position_curr, ee_position_floor; + Eigen::VectorXd joint_position_floor(num_joints); + Eigen::Matrix3d ee_orientation_curr; + arm->FK(arm->lastFeedback().getPosition(), ee_position_curr, ee_orientation_curr); + + // Snap goal to floor if end-effector is at or below floor, only when it first reaches the floor + if(ee_position_curr(2) <= floor_level && floor_command_flag) + { + // Snap current pose to floor + ee_position_floor = ee_position_curr; + ee_position_floor(2) = floor_level; + + // Use inverse kinematics to calculate appropriate joint positions + joint_position_floor = arm->solveIK(arm->lastFeedback().getPosition(), ee_position_floor, ee_orientation_curr); + + // Set snapped pose as goal + arm->setGoal(arm::Goal::createFromPosition(0.001, joint_position_floor)); // Time is very small to make less complex trajectories + + std::cout << "Hit floor!\n"; + + // Update flags to indicate being in contact with the floor + floor_command_flag = false; + cancel_command_flag = true; + } + // Cancel goal if end-effector is above the floor, only when it leaves the floor + else if (ee_position_curr(2) > floor_level and cancel_command_flag) + { + // Cancel goal to move freely + arm->cancelGoal(); + + std::cout << "Left floor!\n"; + + // Update flags to indicate having left the floor + cancel_command_flag = false; + floor_command_flag = true; + } + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_impedance_control_gimbal.cpp b/kits/arm/ex_impedance_control_gimbal.cpp new file mode 100644 index 00000000..2a304995 --- /dev/null +++ b/kits/arm/ex_impedance_control_gimbal.cpp @@ -0,0 +1,181 @@ +/** +In these examples we will implement various hybrid motion-force controllers using the impedance control plugin, which can be used for a wide variety of +applications. +Impedance control is BEST SUITED for assigning free, rigid and springy behaviour, along/about each different axis. +While this is perfectly useful for: +- Having a selectively compliant end-effector, +- Switching between fixed and free behaviour to simulate (mostly) rigid constraints, and +- Allowing human intervention for automated operations by separating controls across different axes, +any applications involving more salient control of the forces (as more complex functions with flexible inputs) should use our force control plugin. See ex_force_control_demoname.cpp. + +This comprises the following demos: +- Fixed: A task-space pose controller implemented entirely using force control via the (PID) impedance controller. +- Cartesian: Locks onto a particular end-effector position while having some compliant orientation. +- Gimbal: A gimbal that locks a specific end-effector orientation, while keeping the rest of the arm compliant. +- Floor: The end-effector is free to move but can't travel below a virtual floor. To further simulate sliding on the floor, see force_control example. +- Damping: The end-effector behaves as 3-different damped systems (overdamped, critically damped, and underdamped), at 3 different heights. + +The following example is for the "Gimbal" demo: +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_impedance_control_gimbal.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + // Ideally, in the impedance control demos, positions and velocities must not be commanded + + // Get the pending command pointer + hebi::GroupCommand& command = arm->pendingCommand(); + + // Clear gains for all position and velocity commands + auto num_joints = arm->robotModel().getDoFCount(); + for (size_t i = 0; i < num_joints; ++i) + { + command[i].settings().actuator().positionGains().kP().set(0); + command[i].settings().actuator().velocityGains().kP().set(0); + } + while (!arm->send()) + { + std::cerr << "Couldn't set actuator position gains to zero!" << std::endl; + } + command.clear(); + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + std::cout << "Commanded gravity-compensated zero force to the arm.\n" + << " 🤳 (B2) - Toggles an impedance controller on/off:\n" + << " ON - Apply controller based on current position\n" + << " OFF - Go back to gravity-compensated mode\n" + << " ❌ (B1) - Exits the demo.\n"; + + // NOTE: Angle wraparound is an unresolved issue which can lead to unstable behaviour for any case involving rotational positional control. + // Make sure that the rotational gains are either all zero, or are high enough to prevent large angular errors (greater than pi/2). The gains provided in these examples are well behaved. + // Interacting with the end-effector in these examples is perfectly safe. + // However, ensure that nothing prevents the wrist's actuators from moving, and DO NOT place your fingers between them. + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Flag to indicate when impedance controller is on + bool controller_on = false; + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // Buttton B1 - End demo + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + + // Button B2 - Set and unset impedance mode when button is pressed and released, respectively + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + + controller_on = true; + + arm->setGoal(arm::Goal::createFromPosition(arm->lastFeedback().getPosition())); + } + else if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOff){ + + controller_on = false; + } + } + + if (!controller_on) + { + arm->cancelGoal(); + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_mobile_io_control.cpp b/kits/arm/ex_mobile_io_control.cpp new file mode 100644 index 00000000..82ea83d3 --- /dev/null +++ b/kits/arm/ex_mobile_io_control.cpp @@ -0,0 +1,148 @@ +/** + * Mobile IO Control + * An example for setting up your arm for simple control from a mobile io devoce + * to pre-programmed waypoints. + */ + +/* +CAUTION: +This example uses waypoints containing fixed joint angles, which is a bad idea if your actuators have large wind-up. +The correct way to store waypoints is by using se3 coordinates, and converting them to joint positions using our IK functions. +*/ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_mobile_io_control.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io->clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Waypoints + auto num_joints = arm->robotModel().getDoFCount(); + std::vector waypoints; + waypoints.push_back(Eigen::Map(example_config->getUserData().getFloatList("waypoint_1").data(), example_config->getUserData().getFloatList("waypoint_1").size())); + waypoints.push_back(Eigen::Map(example_config->getUserData().getFloatList("waypoint_2").data(), example_config->getUserData().getFloatList("waypoint_2").size())); + waypoints.push_back(Eigen::Map(example_config->getUserData().getFloatList("waypoint_3").data(), example_config->getUserData().getFloatList("waypoint_3").size())); + + // Travel time + double travel_time = example_config->getUserData().getFloat("travel_time"); + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + auto updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + ///////////////// + // Button Presses + ///////////////// + + // BN - Waypoint N (N = 1, 2 , 3) + for (int button = 1; button <= 3; button++) + { + if (mobile_io->getButtonDiff(button) == hebi::util::MobileIO::ButtonState::ToOn) { + arm -> setGoal(arm::Goal::createFromPosition(travel_time, waypoints.at(button-1))); + } + } + + // Button B6 - Grav Comp Mode + if (mobile_io->getButtonDiff(6) == hebi::util::MobileIO::ButtonState::ToOn) { + // cancel any goal that is set, returning arm into gravComp mode + arm -> cancelGoal(); + } + + // Button B8 - End Demo + if (mobile_io->getButtonDiff(8) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->resetUI(); + return 1; + } + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} + + + diff --git a/kits/arm/ex_teach_repeat.cpp b/kits/arm/ex_teach_repeat.cpp new file mode 100644 index 00000000..ae330179 --- /dev/null +++ b/kits/arm/ex_teach_repeat.cpp @@ -0,0 +1,235 @@ +/* + * This file runs a teach_repeat program with a 6-DoF Arm, allowing you to + * physically set waypoints for the arm to then move through when you enter + * playback mode. This is an example of the arm's ability to get accurate + * feedback about its position and replay that for the user. Take note, the + * particular choice of PID control gains can affect the performance of this + * demo. + */ + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include "hebi_util.hpp" + +using namespace hebi; +using namespace experimental; + +struct Waypoint +{ + Eigen::VectorXd positions; + Eigen::VectorXd vels; + Eigen::VectorXd accels; + double time; +}; + +struct State +{ + int num_modules; + std::vector waypoints; +}; + +void addWaypoint (State& state, double wp_time, const GroupFeedback& feedback, bool stop) { + printf("Adding a Waypoint.\n"); + + if (stop) { // stop waypoint + state.waypoints.push_back(Waypoint {feedback.getPosition(), + VectorXd::Constant(state.num_modules, 0), + VectorXd::Constant(state.num_modules, 0), + wp_time}); + } + else { // through waypoint + state.waypoints.push_back(Waypoint {feedback.getPosition(), + VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN()), + VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN()), + wp_time}); + } +} + +arm::Goal playWaypoints (State& state) { + + // Set up required variables + Eigen::MatrixXd target_pos(state.num_modules, state.waypoints.size()); + Eigen::MatrixXd target_vels(state.num_modules, state.waypoints.size()); + Eigen::MatrixXd target_accels(state.num_modules, state.waypoints.size()); + Eigen::VectorXd times(state.waypoints.size()); + + // Fill up matrices appropriately + for (int i = 0; i < state.waypoints.size(); i++) + { + target_pos.col(i) << state.waypoints[i].positions; + target_vels.col(i) << state.waypoints[i].vels; + target_accels.col(i) << state.waypoints[i].accels; + times[i] = state.waypoints[i].time + (i > 0 ? times[i-1] : 0.0); + } + return arm::Goal::createFromWaypoints(times, target_pos, target_vels, target_accels); +} + +int main(int argc, char* argv[]) +{ + ////////////////////////// + ///// Config Setup /////// + ////////////////////////// + + // Config file path + const std::string example_config_file = "config/ex_teach_repeat.cfg.yaml"; + std::vector errors; + + // Load the config + const auto example_config = RobotConfig::loadConfig(example_config_file, errors); + for (const auto& error : errors) { + std::cerr << error << std::endl; + } + if (!example_config) { + std::cerr << "Failed to load configuration from: " << example_config_file << std::endl; + return -1; + } + + // For this demo, we need the arm and mobile_io + std::unique_ptr arm; + std::unique_ptr mobile_io; + + ////////////////////////// + ///// Arm Setup ////////// + ////////////////////////// + + // Create the arm object from the configuration + arm = arm::Arm::create(*example_config); + + // Keep retrying if arm not found + while (!arm) { + std::cerr << "Failed to create arm, retrying..." << std::endl; + + // Retry + arm = arm::Arm::create(*example_config); + } + std::cout << "Arm connected." << std::endl; + + ////////////////////////// + //// MobileIO Setup ////// + ////////////////////////// + + // Create the mobile_io object from the configuration + std::cout << "Waiting for Mobile IO device to come online..." << std::endl; + mobile_io = createMobileIOFromConfig(*example_config); + + // Keep retrying if Mobile IO not found + while (mobile_io == nullptr) { + std::cout << "Couldn't find Mobile IO. Check name, family, or device status..." << std::endl; + + // Retry + mobile_io = createMobileIOFromConfig(*example_config); + } + std::cout << "Mobile IO connected." << std::endl; + + // Clear any garbage on screen + mobile_io -> clearText(); + + // Refresh mobile_io + auto last_state = mobile_io->update(); + + ///////////////////////////// + // Control Variables Setup // + ///////////////////////////// + + // Teach Repeat Variables + State state; + state.num_modules = arm->robotModel().getDoFCount(); + + // Run mode is either "training" or "playback" + std::string run_mode = "training"; + + // Variable to hold slider value for A3 + double slider3 = 0.0; + + // Load travel times from config + double base_travel_time = example_config->getUserData().getFloat("base_travel_time"); + double min_travel_time = example_config->getUserData().getFloat("min_travel_time"); + + ////////////////////////// + //// Main Control Loop /// + ////////////////////////// + + while(arm->update()) + { + // Get latest mobile_state + bool updated_mobile_io = mobile_io->update(0); + + if (updated_mobile_io) + { + if (run_mode == "training") + { + // Axis A3 state + slider3 = mobile_io->getAxis(3); + + // Buttton B1 - Add Stop Waypoint + if (mobile_io->getButtonDiff(1) == hebi::util::MobileIO::ButtonState::ToOn) { + addWaypoint(state, + base_travel_time + slider3 * (base_travel_time - min_travel_time), + arm -> lastFeedback(), + true); + } + + // Button B2 - Add Through Waypoint + if (mobile_io->getButtonDiff(2) == hebi::util::MobileIO::ButtonState::ToOn) { + addWaypoint(state, + base_travel_time + slider3 * (base_travel_time - min_travel_time), + arm -> lastFeedback(), + false); + } + + // Button B3 - Toggle to Playback Waypoints + if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) { + if (state.waypoints.size() <= 1){ + std::cout << "You have not added enough waypoints! You need at least two.\n" << std::endl; + } + else { + std::cout << "Entering playback mode.\n" << std::endl; + const arm::Goal playback = playWaypoints(state); + arm->setGoal(playback); + run_mode = "playback"; + } + } + + // Button B4 - Clear Waypoints + if (mobile_io->getButtonDiff(4) == hebi::util::MobileIO::ButtonState::ToOn) { + std::cout << "Discarding waypoints.\n" << std::endl; + state.waypoints.clear(); + } + } + else if (run_mode == "playback") + { + // Button B3 - Toggle to Training Mode + if (mobile_io->getButtonDiff(3) == hebi::util::MobileIO::ButtonState::ToOn) { + std::cout << "Entering training mode.\n" << std::endl; + arm->cancelGoal(); + run_mode = "training"; + } + + // Replay goal + if (arm -> atGoal()) { + const arm::Goal playback = playWaypoints(state); + arm -> setGoal(playback); + } + } + + // Button B8 - End Demo + if (mobile_io->getButtonDiff(8) == hebi::util::MobileIO::ButtonState::ToOn) { + // Clear MobileIO text + mobile_io->clearText(); + return 1; + } + } + + // Send latest commands to the arm + arm->send(); + } + + // Clear MobileIO text + mobile_io->clearText(); + + return 0; +} diff --git a/kits/arm/example_teach_repeat_with_gripper.cpp b/kits/arm/ex_teach_repeat_w_gripper.cpp similarity index 100% rename from kits/arm/example_teach_repeat_with_gripper.cpp rename to kits/arm/ex_teach_repeat_w_gripper.cpp diff --git a/kits/arm/example_AR_kit.cpp b/kits/arm/example_AR_kit.cpp deleted file mode 100644 index 4ef76df0..00000000 --- a/kits/arm/example_AR_kit.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/* - * AR Kit Example - * Currently assumes 6 degrees of freedom in you arm! - * This demo sets up your arm's end effector to mimick the changes in - * position and orientation of your mobile IO device. - */ - -#include "group_command.hpp" -#include "group_feedback.hpp" -#include "robot_model.hpp" -#include "arm/arm.hpp" -#include "util/mobile_io.hpp" -#include -#include - -using namespace hebi; -using namespace experimental; // For all things mobileIO - - - -Eigen::Matrix3d makeRotationMatrix (hebi::Quaternionf phone_orientation) { - Eigen::Quaterniond q; - q.w() = phone_orientation.getW(); - q.x() = phone_orientation.getX(); - q.y() = phone_orientation.getY(); - q.z() = phone_orientation.getZ(); - return q.toRotationMatrix(); -} - - -int main(int argc, char* argv[]) -{ - ////////////////////////// - ///// Arm Setup ////////// - ////////////////////////// - - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"Arm"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to setup a RobotModel object for the 6-DoF Arm - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; - - // Setup Gripper - // std::shared_ptr> gripper(arm::EffortEndEffector<1>::create(params.families_[0], "gripperSpool").release()); - // params.end_effector_ = gripper; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); - } - - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); - - ///////////////////////// - //// MobileIO Setup ///// - ///////////////////////// - - // Create the MobileIO object - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - std::cout << "couldn't find mobile IO device!\n"; - return 1; - } - - // Clear any garbage on screen - mobile->resetUI(); - - // Setup instructions for display - std::string instructions; - instructions = ("B1 - Home Position\nB3 - AR Control Mode\n" - "B6 - Grav Comp Mode\nB8 - Quit\n"); - - // Display instructions on screen - mobile->appendText(instructions); - - // Setup state variable for mobile device - auto last_mobile_state = mobile->update(); - - ////////////////////////// - //// Main Control Loop /// - ////////////////////////// - - // Different modes it can be in (when in none, then automatic grav_comp) - bool softstart = true; - bool ar_mode = false; - - // Make sure we softstart the arm first. - Eigen::VectorXd home_position(arm -> robotModel().getDoFCount()); - home_position << 0, M_PI/3, 2*M_PI/3, 5*M_PI/6, -M_PI/2, 0; // Adjust depending on your DoFs - - // Command the softstart to home position - arm -> update(); - arm -> setGoal(arm::Goal::createFromPosition(4, home_position)); // take 4 seconds - arm -> send(); - - // Get the cartesian position and rotation matrix @ home position - Eigen::Vector3d xyz_home; - Eigen::Matrix3d rot_home; - arm -> FK(home_position, xyz_home, rot_home); - - // Set up states for the mobile device - Eigen::Vector3d xyz_phone_init; - Eigen::Matrix3d rot_phone_init; - - // Target variables - Eigen::VectorXd target_joints(arm -> robotModel().getDoFCount()); - - while(arm->update()) - { - - if (softstart) { - // End softstart when arm reaches its homePosition - if (arm -> atGoal()){ - mobile->appendText("Softstart Complete!"); - softstart = false; - continue; - } - arm -> send(); - - // Stay in softstart, don't do any other behavior - continue; - } - - // Get latest mobile_state - auto updated_mobile = mobile->update(0); - - if (!updated_mobile) - std::cout << "Failed to get feedback from mobile I/O; check connection!\n"; - else - { - // Button B1 - Return to home position - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { - ar_mode = false; - arm -> setGoal(arm::Goal::createFromPosition(4, home_position)); - } - - // Button B3 - Start AR Control - if (mobile->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - xyz_phone_init << mobile -> getLastFeedback().mobile().arPosition().get().getX(), - mobile -> getLastFeedback().mobile().arPosition().get().getY(), - mobile -> getLastFeedback().mobile().arPosition().get().getZ(); - std::cout << xyz_phone_init << std::endl; - rot_phone_init = makeRotationMatrix(mobile -> getLastFeedback().mobile().arOrientation().get()); - ar_mode = true; - } - - // Button B6 - Grav Comp Mode - if (mobile->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { - arm -> cancelGoal(); - ar_mode = false; - } - - // Button B8 - End Demo - if (mobile->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { - // Clear MobileIO text - mobile->resetUI(); - return 1; - } - } - - if (ar_mode) { - // Get the latest mobile position and orientation - Eigen::Vector3d xyz_phone; - xyz_phone << mobile->getLastFeedback().mobile().arPosition().get().getX(), - mobile->getLastFeedback().mobile().arPosition().get().getY(), - mobile->getLastFeedback().mobile().arPosition().get().getZ(); - auto rot_phone = makeRotationMatrix(mobile->getLastFeedback().mobile().arOrientation().get()); - - // Calculate new targets - Eigen::Vector3d xyz_scale; - xyz_scale << 1, 1, 2; - Eigen::Vector3d xyz_target = xyz_home + (0.75 * xyz_scale.array() * - (rot_phone_init.transpose() * (xyz_phone - xyz_phone_init)).array()).matrix(); - Eigen::Matrix3d rot_target = rot_phone_init.transpose() * rot_phone * rot_home; - - // Calculate new arm joint angle targets - target_joints = arm -> solveIK(arm -> lastFeedback().getPosition(), - xyz_target, - rot_target); - - // Create and send new goal to the arm - arm -> setGoal(arm::Goal::createFromPosition(target_joints)); - } - - // Send latest commands to the arm - arm->send(); - } - - // Clear MobileIO text - mobile->resetUI(); - - return 0; -} diff --git a/kits/arm/example_mobile_io_control.cpp b/kits/arm/example_mobile_io_control.cpp deleted file mode 100644 index b09a7590..00000000 --- a/kits/arm/example_mobile_io_control.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/** - * Mobile IO Control - * An example for setting up your arm for simple control from a mobile io devoce - * to pre-programmed waypoints. - */ - -// #include "lookup.hpp" -// #include "group.hpp" -#include "group_command.hpp" -#include "group_feedback.hpp" -#include "robot_model.hpp" -#include "arm/arm.hpp" -#include "util/mobile_io.hpp" -#include - -using namespace hebi; -using namespace experimental; - -int main(int argc, char* argv[]) -{ - ////////////////////////// - ///// Arm Setup ////////// - ////////////////////////// - - arm::Arm::Params params; - - // Setup Module Family and Module Names - params.families_ = {"Arm"}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to seutp a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); - } - - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); - - ////////////////////////// - //// MobileIO Setup ////// - ////////////////////////// - - // Create the MobileIO object - std::unique_ptr mobile = util::MobileIO::create(params.families_[0], "mobileIO"); - if (!mobile) - { - std::cout << "couldn't find mobile IO device!\n"; - return 1; - } - - std::string instructions; - instructions = ("B1 - Waypoint 1\nB2 - Waypoint 2\n" - "B3 - Waypoint 3\n" - "B6 - Grav comp mode\nB8 - Quit\n"); - // Clear any garbage on screen - mobile->clearText(); - - // Display instructions on screen - mobile->appendText(instructions); - - // Setup instructions - auto last_state = mobile->update(); - - ///////////////////////////// - // Control Variables Setup // - ///////////////////////////// - - // Single Waypoint Vectors - auto num_joints = arm->robotModel().getDoFCount(); - Eigen::VectorXd positions(num_joints); - double single_time; - single_time = 3; - - ////////////////////////// - //// Main Control Loop /// - ////////////////////////// - - while(arm->update()) - { - auto updated_mobile = mobile->update(0); - - if (!updated_mobile) - std::cout << "Failed to get feedback from mobile I/O; check connection!\n"; - else - { - ///////////////// - // Button Presses - ///////////////// - - // Buttton B1 - Home Position - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { - positions << 0, 0, 0, 0, 0, 0; - arm -> setGoal(arm::Goal::createFromPosition(single_time, positions)); - } - - // Button B2 - Waypoint 1 - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { - positions << M_PI/4, M_PI/3, 2*M_PI/3, M_PI/3, M_PI/4, 0; - arm -> setGoal(arm::Goal::createFromPosition(single_time, positions)); - } - - // Button B3 - Waypoint 2 - if (mobile->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - positions << -M_PI/4, M_PI/3, 2*M_PI/3, M_PI/3, 3*M_PI/4, 0; - arm -> setGoal(arm::Goal::createFromPosition(single_time, positions)); - - } - - // Button B6 - Grav Comp Mode - if (mobile->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { - // cancel any goal that is set, returning arm into gravComp mode - arm -> cancelGoal(); - } - - // Button B8 - End Demo - if (mobile->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { - // Clear MobileIO text - mobile->resetUI(); - return 1; - } - } - - // Send latest commands to the arm - arm->send(); - } - - // Clear MobileIO text - mobile->clearText(); - - return 0; -} - - - diff --git a/kits/arm/example_teach_repeat.cpp b/kits/arm/example_teach_repeat.cpp deleted file mode 100644 index 82d9b530..00000000 --- a/kits/arm/example_teach_repeat.cpp +++ /dev/null @@ -1,188 +0,0 @@ -/* - * This file runs a teach_repeat program with a 6-DoF Arm, allowing you to - * physically set waypoints for the arm to then move through when you enter - * playback mode. This is an example of the arm's ability to get accurate - * feedback about its position and replay that for the user. Take note, the - * particular choice of PID control gains can affect the performance of this - * demo. - */ - -#include "group_command.hpp" -#include "group_feedback.hpp" -#include "robot_model.hpp" -#include "arm/arm.hpp" -#include "util/mobile_io.hpp" -#include -#include - -using namespace hebi; -using namespace experimental; - -struct Waypoint -{ - Eigen::VectorXd positions; - Eigen::VectorXd vels; - Eigen::VectorXd accels; -}; - -struct State -{ - int num_modules; - std::vector waypoints; -}; - -void addWaypoint (State& state, const GroupFeedback& feedback, bool stop) { - printf("Adding a Waypoint.\n"); - - if (stop) { // stop waypoint - state.waypoints.push_back(Waypoint {feedback.getPosition(), - VectorXd::Constant(state.num_modules, 0), - VectorXd::Constant(state.num_modules, 0)}); - } - else { // through waypoint - state.waypoints.push_back(Waypoint {feedback.getPosition(), - VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN()), - VectorXd::Constant(state.num_modules, std::numeric_limits::quiet_NaN())}); - } -} - -arm::Goal playWaypoints (State& state, double wp_time) { - - // Set up required variables - Eigen::VectorXd times(state.waypoints.size()); - Eigen::MatrixXd target_pos(state.num_modules, state.waypoints.size()); - Eigen::MatrixXd target_vels(state.num_modules, state.waypoints.size()); - Eigen::MatrixXd target_accels(state.num_modules, state.waypoints.size()); - - // Fill up matrices appropriately - for (int i = 0; i < state.waypoints.size(); i++) - { - times[i] = (i+1) * wp_time; - target_pos.col(i) << state.waypoints[i].positions; - target_vels.col(i) << state.waypoints[i].vels; - target_accels.col(i) << state.waypoints[i].accels; - } - return arm::Goal::createFromWaypoints(times, target_pos, target_vels, target_accels); -} - - -int main(int argc, char* argv[]) -{ - ////////////////////////// - ///// Arm Setup ////////// - ////////////////////////// - - arm::Arm::Params params; - - // Setup Module Family and Module Names - std::string family = "Arm"; - params.families_ = {family}; - params.names_ = {"J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3"}; - - // Read HRDF file to setup a RobotModel object for the 6-DoF Arm - // Make sure you are running this from the correct directory! - params.hrdf_file_ = "kits/arm/hrdf/A-2085-06.hrdf"; - - // Create the Arm Object - auto arm = arm::Arm::create(params); - while (!arm) { - arm = arm::Arm::create(params); - } - - // Load the gains file that is approriate to the arm - arm -> loadGains("kits/arm/gains/A-2085-06.xml"); - - ///////////////////////// - //// MobileIO Setup ///// - ///////////////////////// - - // Create the MobileIO object - std::unique_ptr mobile = util::MobileIO::create(family, "mobileIO"); - if (!mobile) - { - std::cout << "couldn't find mobile IO device!\n"; - return 1; - } - - // Clear any garbage on screen - mobile -> clearText(); - - // Setup instructions for display - std::string instructions; - instructions = ("B1 - Add stop WP\nB2 - Clear waypoints\n" - "B3 - Add through WP\nB5 - Playback mode\n" - "B6 - Grav comp mode\nB8 - Quit\n"); - - // Display instructions on screen - mobile->appendText(instructions); - - // Setup state variable for mobile device - auto last_mobile_state = mobile->update(); - - - ////////////////////////// - //// Main Control Loop /// - ////////////////////////// - - // Teach Repeat Variables - State state; - state.num_modules = arm->robotModel().getDoFCount(); - - while(arm->update()) - { - // Get latest mobile_state - bool updated_mobile = mobile->update(0); - - if (!updated_mobile) - std::cout << "Failed to get feedback from mobile I/O; check connection!\n"; - else - { - // Buttton B1 - Add Stop Waypoint - if (mobile->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { - addWaypoint(state, arm -> lastFeedback(), true); - } - - // Button B2 - Clear Waypoints - if (mobile->getButtonDiff(2) == util::MobileIO::ButtonState::ToOn) { - state.waypoints.clear(); - } - - // Button B3 - Add Through Waypoint - if (mobile->getButtonDiff(3) == util::MobileIO::ButtonState::ToOn) { - addWaypoint(state, arm -> lastFeedback(), false); - } - - // Button B5 - Playback Waypoints - if (mobile->getButtonDiff(5) == util::MobileIO::ButtonState::ToOn) { - if (state.waypoints.size() <= 1){ - printf("You have not added enough waypoints!\n"); - } - else { - const arm::Goal playback = playWaypoints(state, 2.5); - arm->setGoal(playback); - } - } - - // Button B6 - Grav Comp Mode - if (mobile->getButtonDiff(6) == util::MobileIO::ButtonState::ToOn) { - // Cancel any goal that is set, returning arm into gravComp mode - arm->cancelGoal(); - } - - // Button B8 - End Demo - if (mobile->getButtonDiff(8) == util::MobileIO::ButtonState::ToOn) { - // Clear MobileIO text - mobile->clearText(); - return 1; - } - } - - // Send latest commands to the arm - arm->send(); - } - - // Clear MobileIO text - mobile->clearText(); - - return 0; -} diff --git a/kits/arm/hebi_util.cpp b/kits/arm/hebi_util.cpp new file mode 100644 index 00000000..3145db5f --- /dev/null +++ b/kits/arm/hebi_util.cpp @@ -0,0 +1,80 @@ +// hebi_util.cpp + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" +#include "util/file.hpp" + +#include +#include +#include + +#include "hebi_util.hpp" + +std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& config) { + std::map mobile_io_dict; + std::vector errors; + + auto parent_dir_absolute = config.getParentDirectory(); + + // Lambda function to check file paths + auto check_file = [&errors, &parent_dir_absolute](const std::string& type, const std::string& relative_filename) { + // Ensure file is relative: + if (hebi::util::file::File(relative_filename).isAbsolute()) { + errors.push_back("'" + type + "' exists but provides an absolute path."); + return std::string(); + } + // Ensure file exists + hebi::util::file::File absolute_filename(parent_dir_absolute); + absolute_filename.append(relative_filename); + if (!absolute_filename.exists()) { + errors.push_back("'" + type + "' exists but does not point to a valid file."); + return std::string(); + } + // Return success! + return absolute_filename.getAbsolutePath(); + }; + + // Validate the mobile_io configuration + if (config.getUserData().strings_.count("mobile_io_family") && + config.getUserData().strings_.count("mobile_io_name") && + config.getUserData().strings_.count("mobile_io_layout")) { + + // Check that all required fields are present and are strings + if (config.getUserData().getString("mobile_io_family").empty() || + config.getUserData().getString("mobile_io_name").empty()) { + errors.push_back("HEBI config \"user_data\"'s \"mobile_io_...\" fields must be non-empty strings."); + } + else { + // Populate the dictionary + mobile_io_dict["family"] = config.getUserData().getString("mobile_io_family"); + mobile_io_dict["name"] = config.getUserData().getString("mobile_io_name"); + + // Use check_file to validate and convert layout to absolute path + mobile_io_dict["layout"] = check_file("mobile_io_layout", config.getUserData().getString("mobile_io_layout")); + if (mobile_io_dict["layout"].empty()) { + errors.push_back("HEBI config \"user_data\"'s \"mobile_io_layout\" file is invalid."); + } + } + } else { + errors.push_back("HEBI config \"user_data\" must contain the keys: \"mobile_family\", \"mobile_name\", and \"mobile_layout\"."); + } + + // If there are errors, print them and throw an exception + if (!errors.empty()) { + for (const auto& error : errors) { + std::cerr << "Error: " << error << std::endl; + } + throw std::runtime_error("Failed to create MobileIO due to configuration errors."); + } + + // Create Mobile IO based on validated config + std::unique_ptr mobile_io = hebi::util::MobileIO::create(mobile_io_dict["family"], mobile_io_dict["name"]); + + if(mobile_io != nullptr) + { + mobile_io->sendLayout(mobile_io_dict["layout"]); + } + return mobile_io; +} \ No newline at end of file diff --git a/kits/arm/hebi_util.hpp b/kits/arm/hebi_util.hpp new file mode 100644 index 00000000..34d93fb4 --- /dev/null +++ b/kits/arm/hebi_util.hpp @@ -0,0 +1,29 @@ +// hebi_util.hpp + +#ifndef HEBI_UTIL_HPP +#define HEBI_UTIL_HPP + +#include "group_command.hpp" +#include "group_feedback.hpp" +#include "robot_model.hpp" +#include "arm/arm.hpp" + +#include +#include +#include + +#include "robot_config.hpp" +#include "arm/arm.hpp" +#include "util/mobile_io.hpp" +#include +#include + +// hebi_util stores helper functions that may be rolled into the API under hebi::util + +// Function to create Mobile IO from the config +std::unique_ptr createMobileIOFromConfig(const hebi::RobotConfig& example_config); + +// TODO: Function to create gripper from the config +// std::unique_ptr createGripperFromConfig(const hebi::RobotConfig& example_config, const std::string& config_file_path); + +#endif // HEBI_UTIL_HPP diff --git a/projects/cmake/CMakeLists.txt b/projects/cmake/CMakeLists.txt index 82e3e516..f3864993 100644 --- a/projects/cmake/CMakeLists.txt +++ b/projects/cmake/CMakeLists.txt @@ -274,12 +274,19 @@ endforeach (EXAMPLE ${ADVANCED_SOURCES}) # Kit Examples SET(ARM_SOURCES - ${ROOT_DIR}/kits/arm/example_gravity_compensation.cpp - ${ROOT_DIR}/kits/arm/example_mobile_io_control.cpp - ${ROOT_DIR}/kits/arm/example_teach_repeat.cpp - ${ROOT_DIR}/kits/arm/example_AR_kit.cpp - ${ROOT_DIR}/kits/arm/example_teach_repeat_with_gripper.cpp - ${ROOT_DIR}/kits/arm/example_double_arm_teach_repeat.cpp) + ${ROOT_DIR}/kits/arm/ex_gravity_compensation.cpp + ${ROOT_DIR}/kits/arm/ex_gravity_compensation_toggle.cpp + ${ROOT_DIR}/kits/arm/ex_mobile_io_control.cpp + ${ROOT_DIR}/kits/arm/ex_teach_repeat.cpp + ${ROOT_DIR}/kits/arm/ex_AR_kit.cpp + ${ROOT_DIR}/kits/arm/ex_teach_repeat_w_gripper.cpp + ${ROOT_DIR}/kits/arm/ex_double_arm_teach_repeat.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_fixed.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_cartesian.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_gimbal.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_floor.cpp + ${ROOT_DIR}/kits/arm/ex_impedance_control_damping.cpp + ) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY kits/arm) foreach (EXAMPLE ${ARM_SOURCES}) @@ -288,9 +295,9 @@ foreach (EXAMPLE ${ARM_SOURCES}) get_filename_component(EX_NAME ${EXAMPLE} NAME_WE) if(WIN32) - add_executable(${EX_NAME} ${EXAMPLE} $) + add_executable(${EX_NAME} ${EXAMPLE} ${ROOT_DIR}/kits/arm/hebi_util.cpp $) else() - add_executable(${EX_NAME} ${EXAMPLE}) + add_executable(${EX_NAME} ${EXAMPLE} ${ROOT_DIR}/kits/arm/hebi_util.cpp) endif() add_dependencies(examples ${EX_NAME}) target_include_directories(${EX_NAME} PRIVATE ${ROOT_DIR}) @@ -317,9 +324,8 @@ foreach (EXAMPLE ${ARM_SOURCES}) endforeach (EXAMPLE ${KIT_SOURCES}) -# Copy the HRDF files for kits into the build directory -file(COPY ${ROOT_DIR}/kits/arm/hrdf DESTINATION ${CMAKE_BINARY_DIR}/kits/arm) -file(COPY ${ROOT_DIR}/kits/arm/gains DESTINATION ${CMAKE_BINARY_DIR}/kits/arm) +# Copy the config directory for kits into the build directory +file(COPY ${ROOT_DIR}/kits/arm/config DESTINATION ${CMAKE_BINARY_DIR}/kits/arm) diff --git a/projects/cmake/DownloadHebiCpp.cmake b/projects/cmake/DownloadHebiCpp.cmake index 5f1ee048..c426cfae 100644 --- a/projects/cmake/DownloadHebiCpp.cmake +++ b/projects/cmake/DownloadHebiCpp.cmake @@ -1,9 +1,9 @@ # Used to download the C++ API - this should not be used directly. cmake_minimum_required(VERSION 3.0) -set(HEBI_CPP_VERSION "3.9.0") +set(HEBI_CPP_VERSION "3.11.1") set(HEBI_CPP_FILE_NAME "hebi-cpp-${HEBI_CPP_VERSION}.tar.gz") -set(HEBI_CPP_LIB_SHA256 "9ad5752448117fb4f5e0f0822c0c596205e4eb33a97aa94e47e81d4d9bc77695") +set(HEBI_CPP_LIB_SHA256 "26454863b4f4d9dd7902c8d7530fc70f766353b0aed97c33781f897874c3d7dd") set(HEBI_CPP_URL "https://files.hebi.us/download/cpp/${HEBI_CPP_FILE_NAME}") # If the CMakeLists.txt is not found, then redownload the C++ API