Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Teleop.yaml with strings specifying xbox mapping #694

Open
wants to merge 4 commits into
base: integration
Choose a base branch
from
Open

Teleop.yaml with strings specifying xbox mapping #694

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3cc21de
updated teleop.yml xbox index to strings
Ericmzhang Apr 23, 2024
fcbda76
merged integration, works how it used to
neuenfeldttj Apr 27, 2024
a287fcb
pulled integration
neuenfeldttj Apr 29, 2024
638a639
format
neuenfeldttj Apr 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 20 additions & 29 deletions config/teleop.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,49 +5,52 @@ teleop:
ra_controls:
joint_a:
multiplier: -1
slow_mode_multiplier: 0.5
xbox_index: "left_x"
joint_b:
multiplier: 1
slow_mode_multiplier: 1
xbox_index: "left_y"
joint_c:
multiplier: 1
slow_mode_multiplier: 0.6
xbox_index: "right_y"
joint_de_pitch:
multiplier: -1
slow_mode_multiplier: 1
xbox_index_pos: "right_trigger"
xbox_index_neg: "left_trigger"
joint_de_roll:
multiplier: 1
slow_mode_multiplier: 1
xbox_index_pos: "right_bumper"
xbox_index_neg: "left_bumper"
allen_key:
multiplier: 1
slow_mode_multiplier: 1
xbox_index_pos: "y"
xbox_index_neg: "a"
gripper:
multiplier: 1
slow_mode_multiplier: 1
xbox_index_pos: "b"
xbox_index_neg: "x"

sa_controls:
sa_x:
multiplier: 1
slow_mode_multiplier: 1.0 # 0.5
xbox_index: 1
xbox_index: "left_y"
sa_y:
multiplier: -1
slow_mode_multiplier: 1.0 # 0.5
xbox_index: 0
xbox_index: "left_x"
sa_z:
multiplier: 1
slow_mode_multiplier: 0.5
xbox_index: 4
xbox_index: "right_y"
sampler:
multiplier: 1
slow_mode_multiplier: 1.0
xbox_index: 5
xbox_index_pos: "right_trigger"
xbox_index_neg: "left_trigger"
sensor_actuator:
multiplier: 1
slow_mode_multiplier: 0.5
xbox_index_pos: "right_bumper"
xbox_index_neg: "left_bumper"
cache:
multiplier: 1
xbox_index: 4
xbox_index_pos: "right_bumper"
xbox_index_neg: "left_bumper"

drive_controls:
forward_back:
Expand Down Expand Up @@ -109,18 +112,6 @@ teleop:
joint_d: 1.57
joint_e: 1.57

# Mappings from site name to index in which values will be sent
# for each site, mostly for heaters and which syringe servos we want to use
site_mappings:
A: 0
B: 1
C: 2

echo_presets:
blah: ["/tf", "/tf_static", "/rosout"]
foo: ["/rosout", "/tf"]
bar: ["/tf_static", "/rosout", "/tf"]

ik_multipliers:
x: 0.1
y: 0.1
Expand Down
123 changes: 85 additions & 38 deletions src/teleoperation/backend/consumers.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ def connect(self):
except Exception as e:
rospy.logerr(e)

def disconnect(self, close_code):
def disconnect(self, close_code) -> None:
self.pdb_sub.unregister()
self.arm_moteus_sub.unregister()
self.drive_moteus_sub.unregister()
Expand Down Expand Up @@ -197,8 +197,6 @@ def receive(self, text_data):
self.change_cameras(message)
elif message["type"] == "takePanorama":
self.capture_panorama()
elif message["type"] == "capturePhoto":
self.capture_photo()
elif message["type"] == "heaterEnable":
self.heater_enable_service(message)
elif message["type"] == "autoShutoff":
Expand All @@ -220,12 +218,16 @@ def receive(self, text_data):
except Exception as e:
rospy.logerr(e)

@staticmethod
def filter_xbox_axis(
value: float,
self,
axes: "List[float]",
axis: str,
deadzone_threshold: float = DEFAULT_ARM_DEADZONE,
quad_control: bool = False,
) -> float:
index = self.xbox_mappings[axis] # takes the specified yaml str (e.g. "left_y") and gets index
value = axes[index]

value = deadzone(value, deadzone_threshold)
if quad_control:
value = quadratic(value)
Expand Down Expand Up @@ -263,8 +265,8 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl
def publish_ik(self, axes: list[float], buttons: list[float]) -> None:
ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_d_link")

ee_in_map.position[0] += self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]])
ee_in_map.position[1] += self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]])
ee_in_map.position[0] += self.ik_names["x"] * self.filter_xbox_axis(axes, "left_x")
ee_in_map.position[1] += self.ik_names["y"] * self.filter_xbox_axis(axes, "left_y")
ee_in_map.position[2] += self.ik_names["z"] * self.filter_xbox_button(buttons, "right_trigger", "left_trigger")

self.send(
Expand Down Expand Up @@ -304,8 +306,6 @@ def publish_position(self, type, names, positions):
self.cache_position_cmd_pub.publish(position_cmd)

def publish_velocity(self, type, names, axes, buttons):
left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]])
right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]])
velocity_cmd = Velocity()
velocity_cmd.names = names
if type == "cache_values":
Expand All @@ -315,68 +315,115 @@ def publish_velocity(self, type, names, axes, buttons):
self.cache_velocity_cmd_pub.publish(velocity_cmd)
elif type == "sa_arm_values":
velocity_cmd.velocities = [
self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False),
self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False),
self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True),
self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger),
self.to_velocity(self.filter_xbox_axis(axes, self.sa_config["sa_x"]["xbox_index"]), "sa_x", False),
self.to_velocity(self.filter_xbox_axis(axes, self.sa_config["sa_y"]["xbox_index"]), "sa_y", False),
self.to_velocity(self.filter_xbox_axis(axes, self.sa_config["sa_z"]["xbox_index"]), "sa_z", True),
self.sa_config["sampler"]["multiplier"]
* self.filter_xbox_button(
buttons, self.sa_config["sampler"]["xbox_index_pos"], self.sa_config["sampler"]["xbox_index_neg"]
),
self.sa_config["sensor_actuator"]["multiplier"]
* self.filter_xbox_button(buttons, "right_bumper", "left_bumper"),
* self.filter_xbox_button(
buttons,
self.sa_config["sensor_actuator"]["xbox_index_pos"],
self.sa_config["sensor_actuator"]["xbox_index_neg"],
),
]
self.sa_velocity_cmd_pub.publish(velocity_cmd)
elif type == "arm_values":
velocity_cmd.velocities = [
self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), "joint_a"),
self.to_velocity(self.filter_xbox_axis(axes, self.ra_config["joint_a"]["xbox_index"]), "joint_a"),
self.to_velocity(
self.filter_xbox_axis(axes[self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False
self.filter_xbox_axis(axes, self.ra_config["joint_b"]["xbox_index"]), "joint_b", False
),
self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), "joint_c"),
self.to_velocity(self.filter_xbox_axis(axes, self.ra_config["joint_c"]["xbox_index"]), "joint_c"),
self.to_velocity(
self.filter_xbox_axis(axes[self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0"
self.filter_xbox_button(
buttons,
self.ra_config["joint_de_pitch"]["xbox_index_pos"],
self.ra_config["joint_de_pitch"]["xbox_index_neg"],
),
"joint_de_0",
),
self.to_velocity(
self.filter_xbox_axis(axes[self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1"
self.filter_xbox_button(
buttons,
self.ra_config["joint_de_roll"]["xbox_index_pos"],
self.ra_config["joint_de_roll"]["xbox_index_neg"],
),
"joint_de_1",
),
self.ra_config["allen_key"]["multiplier"]
* self.filter_xbox_button(
buttons,
self.ra_config["allen_key"]["xbox_index_pos"],
self.ra_config["allen_key"]["xbox_index_neg"],
),
self.ra_config["gripper"]["multiplier"]
* self.filter_xbox_button(
buttons, self.ra_config["gripper"]["xbox_index_pos"], self.ra_config["gripper"]["xbox_index_neg"]
),
self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"),
self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"),
]
self.arm_velocity_cmd_pub.publish(velocity_cmd)

def publish_throttle(self, type, names, axes, buttons):
left_trigger = self.filter_xbox_axis(buttons[self.xbox_mappings["left_trigger"]])
right_trigger = self.filter_xbox_axis(buttons[self.xbox_mappings["right_trigger"]])
throttle_cmd = Throttle()
throttle_cmd.names = names
if type == "cache_values":
throttle_cmd.throttles = [
self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper")
self.sa_config["cache"]["multiplier"]
* self.filter_xbox_button(
buttons, self.sa_config["cache"]["xbox_index_pos"], self.sa_config["cache"]["xbox_index_neg"]
)
]
self.cache_throttle_cmd_pub.publish(throttle_cmd)
elif type == "arm_values":
throttle_cmd.throttles = [
self.ra_config["joint_a"]["multiplier"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_x"]]),
self.ra_config["joint_b"]["multiplier"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_y"]]),
self.ra_config["joint_a"]["multiplier"]
* self.filter_xbox_axis(axes, self.ra_config["joint_a"]["xbox_index"]),
self.ra_config["joint_b"]["multiplier"]
* self.filter_xbox_axis(axes, self.ra_config["joint_b"]["xbox_index"]),
self.ra_config["joint_c"]["multiplier"]
* quadratic(self.filter_xbox_axis(axes[self.xbox_mappings["right_y"]])),
* self.filter_xbox_axis(axes, self.ra_config["joint_c"]["xbox_index"], quad_control=True),
self.ra_config["joint_de_pitch"]["multiplier"]
* self.filter_xbox_axis(
buttons[self.xbox_mappings["right_trigger"]] - buttons[self.xbox_mappings["left_trigger"]]
* self.filter_xbox_button(
buttons,
self.ra_config["joint_de_pitch"]["xbox_index_pos"],
self.ra_config["joint_de_pitch"]["xbox_index_neg"],
),
self.ra_config["joint_de_roll"]["multiplier"]
* self.filter_xbox_axis(
buttons[self.xbox_mappings["right_bumper"]] - buttons[self.xbox_mappings["left_bumper"]]
* self.filter_xbox_button(
buttons,
self.ra_config["joint_de_roll"]["xbox_index_pos"],
self.ra_config["joint_de_roll"]["xbox_index_neg"],
),
self.ra_config["allen_key"]["multiplier"]
* self.filter_xbox_button(
buttons,
self.ra_config["allen_key"]["xbox_index_pos"],
self.ra_config["allen_key"]["xbox_index_neg"],
),
self.ra_config["gripper"]["multiplier"]
* self.filter_xbox_button(
buttons, self.ra_config["gripper"]["xbox_index_pos"], self.ra_config["gripper"]["xbox_index_neg"]
),
self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"),
self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"),
]
self.arm_throttle_cmd_pub.publish(throttle_cmd)
elif type == "sa_arm_values":
throttle_cmd.throttles = [
self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]),
self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]),
self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]),
self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger),
self.filter_xbox_axis(axes, self.sa_config["sa_x"]["xbox_index"]),
self.filter_xbox_axis(axes, self.sa_config["sa_y"]["xbox_index"]),
self.filter_xbox_axis(axes, self.sa_config["sa_z"]["xbox_index"]),
self.sa_config["sampler"]["multiplier"]
* self.filter_xbox_button(
buttons, self.sa_config["sampler"]["xbox_index_pos"], self.sa_config["sampler"]["xbox_index_neg"]
),
self.sa_config["sensor_actuator"]["multiplier"]
* self.filter_xbox_button(buttons, "right_bumper", "left_bumper"),
* self.filter_xbox_button(
buttons,
self.sa_config["sensor_actuator"]["xbox_index_pos"],
self.sa_config["sensor_actuator"]["xbox_index_neg"],
),
]
self.sa_throttle_cmd_pub.publish(throttle_cmd)

Expand Down