From ccba81360f369dd1158965e697207e7881cd855b Mon Sep 17 00:00:00 2001 From: Austin Wang Date: Thu, 15 Sep 2022 13:48:08 -0700 Subject: [PATCH 01/34] [Hello] Add continuous trajectory tracking & teleop (#1371) * Velocity control * Add keyboard teleop script * Debug * Hack * Debug controller * debug controller * Disable stow * Fix teleop script * Debug teleop * Wheel control test * tmp * Fix teleop once and for all * Clean up, adapt ros version * Fix * Pub to cmd_vel * Debug * debug * remove debug print * remove debug print * Clean up teleop API * Expose vel and rvel * Clean up teleop API * Initial goto controller impl * Add params * Yaw tracking modes * typing * Set tolerances * Make static methods * Add tolerances to control * Debug * Tune heading tracking * Remove acceleration and tol * Debug * Tune tol * Add turn rate limit and update tol * Tune controller * Update comments * Improve teleop * Add comment * Add docstrings and input checks * Debug docstring * Improve yaw tracking performance * Add option to pause controller * Tune teleop * Tune rate * Tune hz --- .../lowlevel/hello_robot/keyboard_teleop.py | 99 +++++++++++ .../hello_robot/remote/goto_controller.py | 157 ++++++++++++++++++ .../hello_robot/remote/remote_hello_robot.py | 6 +- .../remote/remote_hello_robot_ros.py | 45 ++++- .../remote/stretch_ros_move_api.py | 14 +- 5 files changed, 318 insertions(+), 3 deletions(-) create mode 100644 droidlet/lowlevel/hello_robot/keyboard_teleop.py create mode 100644 droidlet/lowlevel/hello_robot/remote/goto_controller.py diff --git a/droidlet/lowlevel/hello_robot/keyboard_teleop.py b/droidlet/lowlevel/hello_robot/keyboard_teleop.py new file mode 100644 index 0000000000..ee66da57f1 --- /dev/null +++ b/droidlet/lowlevel/hello_robot/keyboard_teleop.py @@ -0,0 +1,99 @@ +import sys +import time + +from pynput import keyboard +import numpy as np + + +UP = keyboard.Key.up +DOWN = keyboard.Key.down +LEFT = keyboard.Key.left +RIGHT = keyboard.Key.right +ESC = keyboard.Key.esc + +HZ_DEFAULT = 15 + +# 6 * v_max + w_max <= 1.8 (computed from max wheel vel & vel diff required for w) +VEL_MAX_DEFAULT = 0.20 +RVEL_MAX_DEFAULT = 0.45 + + +class RobotController: + def __init__( + self, + mover, + vel_max=None, + rvel_max=None, + hz=HZ_DEFAULT, + ): + # Params + self.dt = 1.0 / hz + self.vel_max = vel_max or VEL_MAX_DEFAULT + self.rvel_max = rvel_max or RVEL_MAX_DEFAULT + + # Robot + print("Connecting to robot...") + self.robot = mover.bot + print("Connected.") + + # Keyboard + self.key_states = {key: 0 for key in [UP, DOWN, LEFT, RIGHT]} + + # Controller states + self.alive = True + self.vel = 0 + self.rvel = 0 + + def on_press(self, key): + if key in self.key_states: + self.key_states[key] = 1 + elif key == ESC: + self.alive = False + return False # returning False from a callback stops listener + + def on_release(self, key): + if key in self.key_states: + self.key_states[key] = 0 + + def run(self): + print( + "(+[__]o) Teleoperation started. Use arrow keys to control robot, press ESC to exit. ^o^" + ) + while self.alive: + # Map keystrokes + vert_sign = self.key_states[UP] - self.key_states[DOWN] + hori_sign = self.key_states[LEFT] - self.key_states[RIGHT] + + # Compute velocity commands + self.vel = self.vel_max * vert_sign + self.rvel = self.rvel_max * hori_sign + + # Command robot + self.robot.set_velocity(self.vel, self.rvel) + + # Spin + time.sleep(self.dt) + + +def run_teleop(mover, vel=None, rvel=None): + robot_controller = RobotController(mover, vel, rvel) + listener = keyboard.Listener( + on_press=robot_controller.on_press, + on_release=robot_controller.on_release, + suppress=True, # suppress terminal outputs + ) + + # Start teleop + listener.start() + robot_controller.run() + + # Cleanup + listener.join() + print("(+[__]o) Teleoperation ended. =_=") + + +if __name__ == "__main__": + from droidlet.lowlevel.hello_robot.hello_robot_mover import HelloRobotMover + + mover = HelloRobotMover(ip=sys.argv[1]) + run_teleop(mover) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py new file mode 100644 index 0000000000..9645d8a173 --- /dev/null +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -0,0 +1,157 @@ +from typing import List, Optional +import time +import threading + +import numpy as np +import rospy + +V_MAX_DEFAULT = 0.15 # base.params["motion"]["default"]["vel_m"] +W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m + + +class GotoVelocityController: + def __init__( + self, + robot, + hz: float, + v_max: Optional[float] = None, + w_max: Optional[float] = None, + ): + self.robot = robot + self.hz = hz + self.dt = 1.0 / self.hz + + # Params + self.v_max = v_max or V_MAX_DEFAULT + self.w_max = w_max or W_MAX_DEFAULT + self.lin_error_tol = self.v_max / hz + self.ang_error_tol = self.w_max / hz + + # Initialize + self.loop_thr = None + self.control_lock = threading.Lock() + self.active = False + + self.xyt_err = np.zeros(3) + self.track_yaw = True + + @staticmethod + def _error_velocity_multiplier(x_err, tol=0.0): + """ + Computes velocity multiplier based on distance from target. + Used for both linear and angular motion. + + Current implementation: Simple thresholding + Output = 1 if linear error is larger than the tolerance, 0 otherwise. + """ + assert x_err >= 0.0 + return float(x_err - tol > 0) + + @staticmethod + def _projection_velocity_multiplier(theta_err, tol=0.0): + """ + Compute velocity muliplier based on yaw (faster if facing towards target). + Used to control linear motion. + + Current implementation: + Output = 1 when facing target, gradually decreases to 0 when angle to target is pi/3. + """ + assert theta_err >= 0.0 + return 1.0 - np.sin(min(max(theta_err - tol, 0.0) * 2.0, np.pi / 3.0)) + + @staticmethod + def _turn_rate_limit(w_max, lin_err, heading_err): + """ + Computed velocity limit based on the turning radius required to reach goal. + """ + assert lin_err >= 0.0 + assert heading_err >= 0.0 + return w_max * lin_err / np.sin(heading_err) + 1e-5 / 2.0 + + def _integrate_state(self, v, w): + """ + Predict error in the next timestep with current commanded velocity + """ + dx = v * self.dt + dtheta = w * self.dt + + x_err_f0 = self.xyt_err[0] - dx * np.cos(dtheta / 2.0) + y_err_f0 = self.xyt_err[1] - dx * np.sin(dtheta / 2.0) + ct = np.cos(-dtheta) + st = np.sin(-dtheta) + + self.xyt_err[0] = ct * x_err_f0 - st * y_err_f0 + self.xyt_err[1] = st * x_err_f0 + ct * y_err_f0 + self.xyt_err[2] = self.xyt_err[2] - dtheta if self.track_yaw else 0.0 + + def _run(self): + rate = rospy.Rate(self.hz) + + while True: + v_cmd = w_cmd = 0 + + lin_err_abs = np.linalg.norm(self.xyt_err[0:2]) + ang_err = self.xyt_err[2] + ang_err_abs = abs(ang_err) + + # Go to goal XY position if not there yet + if lin_err_abs > self.lin_error_tol: + heading_err = np.arctan2(self.xyt_err[1], self.xyt_err[0]) + heading_err_abs = abs(heading_err) + + # Compute linear velocity + k_t = self._error_velocity_multiplier(lin_err_abs, tol=self.lin_error_tol) + k_p = self._projection_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) + v_limit = self._turn_rate_limit(self.w_max, lin_err_abs, heading_err_abs) + v_cmd = min(k_t * k_p * self.v_max, v_limit) + + # Compute angular velocity + k_t_ang = self._error_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) + w_cmd = np.sign(heading_err) * k_t_ang * self.w_max + + # Rotate to correct yaw if yaw tracking is on and XY position is at goal + elif ang_err_abs > self.ang_error_tol: + # Compute angular velocity + k_t_ang = self._error_velocity_multiplier(ang_err_abs, tol=self.ang_error_tol) + w_cmd = np.sign(ang_err) * k_t_ang * self.w_max + + # Command robot + with self.control_lock: + self.robot.set_velocity(v_cmd, w_cmd) + + # Update odometry prediction + self._integrate_state(v_cmd, w_cmd) + + # Spin + rate.sleep() + + def check_at_goal(self) -> bool: + xy_fulfilled = np.linalg.norm(self.xyt_err[0:2]) <= self.lin_error_tol + + t_fulfilled = True + if self.track_yaw: + t_fulfilled = abs(self.xyt_err[2]) <= self.ang_error_tol + + return xy_fulfilled and t_fulfilled + + def set_goal( + self, + xyt_position: List[float], + ): + self.xyt_err = xyt_position + if not self.track_yaw: + self.xyt_err[2] = 0.0 + + def enable_yaw_tracking(self, value: bool = True): + self.track_yaw = value + + def start(self): + if self.loop_thr is None: + self.loop_thr = threading.Thread(target=self._run) + self.loop_thr.start() + self.active = True + + def pause(self): + self.active = False + with self.control_lock: + self.robot.set_velocity(0.0, 0.0) diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py index e29a988beb..34119522ae 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot.py @@ -63,7 +63,7 @@ def __init__(self, ip): self._robot.startup() if not self._robot.is_calibrated(): self._robot.home() - self._robot.stow() + # self._robot.stow() # HACK: not working currently, robot runs fine without this line self._done = True self.cam = None # Read battery maintenance guide https://docs.hello-robot.com/battery_maintenance_guide/ @@ -248,6 +248,10 @@ def obstacle_fn(): self._done = True return status + def set_velocity(self, v_m, w_r): + self._robot.base.set_velocity(v_m, w_r) + self._robot.push_command() + def is_base_moving(self): robot = self._robot left_wheel_moving = ( diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py index 37ff41869e..1b5931a395 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py @@ -13,13 +13,15 @@ from rich import print import Pyro4 import numpy as np + from droidlet.lowlevel.hello_robot.remote.utils import ( goto_trackback, transform_global_to_base, goto, ) -from stretch_ros_move_api import MoveNode as Robot from droidlet.lowlevel.pyro_utils import safe_call +from stretch_ros_move_api import MoveNode as Robot +from goto_controller import GotoVelocityController import traceback @@ -27,6 +29,7 @@ Pyro4.config.SERIALIZERS_ACCEPTED.add("pickle") Pyro4.config.ITER_STREAMING = True +VEL_CONTROL_HZ = 15 # ##################################################### @Pyro4.expose @@ -43,6 +46,8 @@ def __init__(self, ip): self._load_urdf() self.tilt_correction = 0.0 + self._goto_controller = GotoVelocityController(robot=self._robot, hz=VEL_CONTROL_HZ) + def _load_urdf(self): import os @@ -260,6 +265,44 @@ def obstacle_fn(): raise e return status + def set_velocity(self, v_m, w_r): + """Directly sets the forward and yaw velocity of the robot.""" + self._robot.set_velocity(v_m, w_r) + + def set_relative_position_goal(self, xy_position): + """Moves the robot base to the given goal position relative to its current + pose. The robot does not have a yaw goal and will simply turn & move towards + the desired position. + + :param xy_position: The relative goal position of the form (x,y) + """ + assert ( + len(xy_position) == 2 + ), f"Input goal should be of length 2 (xy), got {len(xy_position)} instead." + + xyt_position = list(xy_position) + [0.0] + + self._goto_controller.start() + self._goto_controller.enable_yaw_tracking(False) + self._goto_controller.set_goal(xyt_position) + + def set_relative_goal(self, xyt_position): + """Moves the robot base to the given goal state relative to its current + pose. + + :param xyt_position: The relative goal state of the form (x,y,yaw) + """ + assert ( + len(xyt_position) == 3 + ), f"Input goal should be of length 3 (xyt), got {len(xyt_position)} instead." + + self._goto_controller.start() + self._goto_controller.enable_yaw_tracking(True) + self._goto_controller.set_goal(xyt_position) + + def stop_continuous_control(self): + self._goto_controller.pause() + def is_moving(self): return not self._done diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 5dbf21a8d2..feed42b113 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -6,7 +6,7 @@ from sensor_msgs.msg import JointState from control_msgs.msg import FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectoryPoint -from geometry_msgs.msg import PoseStamped, Pose2D, PoseWithCovarianceStamped +from geometry_msgs.msg import PoseStamped, Pose2D, PoseWithCovarianceStamped, Twist from nav_msgs.msg import Odometry import hello_helpers.hello_misc as hm from tf.transformations import euler_from_quaternion @@ -27,6 +27,12 @@ def __init__(self): self._scan_matched_pose = None self._lock = threading.Lock() + self._nav_mode = rospy.ServiceProxy("/switch_to_navigation_mode", Trigger) + s_request = TriggerRequest() + self._nav_mode(s_request) + + self._vel_command_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) + def _joint_states_callback(self, joint_state): with self._lock: self._joint_state = joint_state @@ -47,6 +53,12 @@ def _odom_callback(self, pose): ) self._angular_movement.append(abs(pose.twist.twist.angular.z)) + def set_velocity(self, v_m, w_r): + cmd = Twist() + cmd.linear.x = v_m + cmd.angular.z = w_r + self._vel_command_pub.publish(cmd) + def is_moving(self): with self._lock: lm, am = self._linear_movement, self._angular_movement From 6931eb006d72939ae064a86f34d07cfedbdb618a Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Thu, 15 Sep 2022 16:04:39 -0700 Subject: [PATCH 02/34] Made go to async --- .../locobot/remote/navigation_service.py | 84 ++++++++++++++++--- 1 file changed, 72 insertions(+), 12 deletions(-) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 4af1a07ed8..258e6df499 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -2,8 +2,11 @@ import sys import random import math -from threading import local +import threading import time +from dataclasses import dataclass +from typing import Optional + import torch import numpy as np import Pyro4 @@ -72,6 +75,24 @@ def get_loc(self, cur_loc): return ans +@dataclass +class GoalParams: + is_multi_goal: bool + goal: Optional[np.ndarray] + goal_map: Optional[np.ndarray] + distance_threshold: Optional[np.ndarray] + angle_threshold: Optional[np.ndarray] + steps: float + visualize: bool + + +@dataclass +class NavigationStatus: + valid: bool + path_found: bool + goal_reached: bool + + @Pyro4.expose class Navigation(object): def __init__(self, planner, slam, robot): @@ -82,6 +103,14 @@ def __init__(self, planner, slam, robot): num_sem_categories = len(coco_categories) self.map_size, self.local_map_size = self.slam.get_map_sizes() + + # Async navigator + self.goto_thr = None + self.goal_data = None + self.nav_status = NavigationStatus(False, False, False) + self.nav_init = threading.Event() + + # ObjectNav policy self.goal_policy = GoalPolicy( map_features_shape=(num_sem_categories + 8, self.local_map_size, self.local_map_size), num_outputs=2, @@ -198,18 +227,44 @@ def go_to_absolute( steps=100000000, visualize=True, ): - print("[navigation] Starting a go_to_absolute") - # specify exactly one of goal or goal_map + # Specify exactly one of goal or goal_map assert (goal is not None and goal_map is None) or (goal is None and goal_map is not None) + is_multi_goal = goal is None + self.goal_data = GoalParams( + is_multi_goal, goal, goal_map, distance_threshold, angle_threshold, steps, visualize + ) + + # Reset params + self.nav_status = NavigationStatus(True, False, False) + self.nav_init.clear() + + # Start navigator thread + if self.goto_thr is None: + self.goto_thr = threading.Thread(target=self._navigate_to_absolute) + self.goto_thr.start() + self._stop = False + + # Wait for navigator to finish first loop + self.nav_init.wait() + + def _navigate_to_absolute(self): + print("[navigation] Starting a go_to_absolute") self._busy = True self._stop = False robot_loc = self.robot.get_base_state() initial_robot_loc = robot_loc - goal_reached = False - path_found = True - while not goal_reached and steps > 0 and self._stop is False: + while not self.nav_status.goal_reached and steps > 0 and self._stop is False: + # Load goal data + goal = self.goal_data.goal + goal_map = self.goal_data.goal_map + distance_threshold = self.goal_data.distance_threshold + angle_threshold = self.goal_data.angle_threshold + steps = self.goal_data.steps + visualize = self.goal_data.visualize + + # Plan & execute stg = self.planner.get_short_term_goal( robot_loc, goal=goal, @@ -223,7 +278,7 @@ def go_to_absolute( goal, robot_loc ) ) - path_found = False + self.nav_status.path_found = False break robot_loc = self.robot.get_base_state() status, action = self.robot.go_to_absolute(stg) @@ -238,7 +293,7 @@ def go_to_absolute( print(" Short-term goal: {}, Reached Location: {}".format(stg, robot_loc)) print(" Robot Status: {}".format(status)) if status == "SUCCEEDED": - goal_reached = self.planner.goal_within_threshold( + self.nav_status.goal_reached = self.planner.goal_within_threshold( robot_loc, goal=goal, goal_map=goal_map, @@ -293,8 +348,9 @@ def go_to_absolute( self.vis.update_last_position_vis_info(self.slam.get_last_position_vis_info()) self.vis.snapshot() + self.nav_init.set() + self._busy = False - return path_found, goal_reached def go_to_object( self, @@ -357,13 +413,14 @@ def go_to_object( if visualize: self.vis.set_location_goal(goal_map) - _, goal_reached = self.go_to_absolute( + self.go_to_absolute( goal_map=goal_map, distance_threshold=0.5, angle_threshold=30, steps=1, visualize=visualize, ) + goal_reached = self.nav_status.goal_reached continue elif (cat_frame == 1).sum() > 0: @@ -559,8 +616,8 @@ def explore(self, far_away_goal): self._done_exploring = False if not self._done_exploring: print("exploring 1 step") - path_found, _ = self.go_to_absolute(far_away_goal, steps=1) - if path_found == False: + self.go_to_absolute(far_away_goal, steps=1) + if self.nav_status.path_found == False: # couldn't reach far_away_goal # and don't seem to have any unexplored # paths to attempt to get there @@ -578,6 +635,9 @@ def reset_explore(self): def stop(self): self._stop = True + if self.goto_thr is not None: + self.goto_thr.join() + self.goto_thr = None robot_ip = os.getenv("LOCOBOT_IP") From 4b8ba4beb134b12e6aed039671af2c5a0c87b14a Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Thu, 15 Sep 2022 17:49:47 -0700 Subject: [PATCH 03/34] Use continuous goto for navigation --- .../remote/remote_hello_robot_ros.py | 27 +++++++++++++------ .../locobot/remote/navigation_service.py | 9 +++++-- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py index 1b5931a395..b7afb8bd87 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py @@ -269,33 +269,44 @@ def set_velocity(self, v_m, w_r): """Directly sets the forward and yaw velocity of the robot.""" self._robot.set_velocity(v_m, w_r) - def set_relative_position_goal(self, xy_position): - """Moves the robot base to the given goal position relative to its current - pose. The robot does not have a yaw goal and will simply turn & move towards + def set_position_goal(self, xy_position, absolute=False): + """Moves the robot base to the given goal position. + The robot does not have a yaw goal and will simply turn & move towards the desired position. - :param xy_position: The relative goal position of the form (x,y) + :param xy_position: The goal position of the form (x,y) """ assert ( len(xy_position) == 2 ), f"Input goal should be of length 2 (xy), got {len(xy_position)} instead." + # Convert abs to rel xyt_position = list(xy_position) + [0.0] + if absolute: + base_state = self.get_base_state() + xyt_position = transform_global_to_base(xyt_position, base_state) + xyt_position[2] = 0.0 + # Set motion goal self._goto_controller.start() self._goto_controller.enable_yaw_tracking(False) self._goto_controller.set_goal(xyt_position) - def set_relative_goal(self, xyt_position): - """Moves the robot base to the given goal state relative to its current - pose. + def set_goal(self, xyt_position, absolute=False): + """Moves the robot base to the given goal state - :param xyt_position: The relative goal state of the form (x,y,yaw) + :param xyt_position: The goal state of the form (x,y,yaw) """ assert ( len(xyt_position) == 3 ), f"Input goal should be of length 3 (xyt), got {len(xyt_position)} instead." + # Convert abs to rel + if absolute: + base_state = self.get_base_state() + xyt_position = transform_global_to_base(xyt_position, base_state) + + # Set motion goal self._goto_controller.start() self._goto_controller.enable_yaw_tracking(True) self._goto_controller.set_goal(xyt_position) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 258e6df499..1a622041a9 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -281,9 +281,13 @@ def _navigate_to_absolute(self): self.nav_status.path_found = False break robot_loc = self.robot.get_base_state() - status, action = self.robot.go_to_absolute(stg) - robot_loc = self.robot.get_base_state() + # status, action = self.robot.go_to_absolute(stg) + self.robot.set_goal(stg, absolute=True) + status = "SUCCEEDED" + action = "move_to" + robot_loc = self.robot.get_base_state() + """ print("[navigation] Finished a go_to_absolute") print( " Initial location: {} Final goal: {}".format( @@ -292,6 +296,7 @@ def _navigate_to_absolute(self): ) print(" Short-term goal: {}, Reached Location: {}".format(stg, robot_loc)) print(" Robot Status: {}".format(status)) + """ if status == "SUCCEEDED": self.nav_status.goal_reached = self.planner.goal_within_threshold( robot_loc, From 55560b706ed1ac24d14fc72d14e1c8864523aaad Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 20 Sep 2022 12:14:15 -0700 Subject: [PATCH 04/34] Always running nav thread --- .../locobot/remote/navigation_service.py | 46 +++++++------------ 1 file changed, 17 insertions(+), 29 deletions(-) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 1a622041a9..f5f06c5bcd 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -88,9 +88,10 @@ class GoalParams: @dataclass class NavigationStatus: - valid: bool path_found: bool goal_reached: bool + goal: Optional[GoalParams] + goal_update: threading.Event @Pyro4.expose @@ -105,10 +106,9 @@ def __init__(self, planner, slam, robot): self.map_size, self.local_map_size = self.slam.get_map_sizes() # Async navigator - self.goto_thr = None - self.goal_data = None - self.nav_status = NavigationStatus(False, False, False) - self.nav_init = threading.Event() + self.nav_status = NavigationStatus(False, False, None, threading.Event()) + self.goto_thr = threading.Thread(target=self._navigate_to_absolute) + self.goto_thr.start() # ObjectNav policy self.goal_policy = GoalPolicy( @@ -230,41 +230,33 @@ def go_to_absolute( # Specify exactly one of goal or goal_map assert (goal is not None and goal_map is None) or (goal is None and goal_map is not None) is_multi_goal = goal is None - self.goal_data = GoalParams( + goal_data = GoalParams( is_multi_goal, goal, goal_map, distance_threshold, angle_threshold, steps, visualize ) # Reset params - self.nav_status = NavigationStatus(True, False, False) - self.nav_init.clear() + self.nav_status.goal = goal_data + self.nav_status.goal_update.set() - # Start navigator thread - if self.goto_thr is None: - self.goto_thr = threading.Thread(target=self._navigate_to_absolute) - self.goto_thr.start() - self._stop = False - - # Wait for navigator to finish first loop - self.nav_init.wait() - - def _navigate_to_absolute(self): - print("[navigation] Starting a go_to_absolute") - - self._busy = True + def _navigation_loop(self): self._stop = False - robot_loc = self.robot.get_base_state() - initial_robot_loc = robot_loc - while not self.nav_status.goal_reached and steps > 0 and self._stop is False: + while self._stop is False: + if self.nav_status.goal_reached or not self.nav_status.path_found: + self.nav_status.goal_update.clear() + self.nav_status.goal_update.wait() + # Load goal data goal = self.goal_data.goal goal_map = self.goal_data.goal_map distance_threshold = self.goal_data.distance_threshold angle_threshold = self.goal_data.angle_threshold - steps = self.goal_data.steps visualize = self.goal_data.visualize # Plan & execute + self.nav_status.path_found = True + + robot_loc = self.robot.get_base_state() stg = self.planner.get_short_term_goal( robot_loc, goal=goal, @@ -353,10 +345,6 @@ def _navigate_to_absolute(self): self.vis.update_last_position_vis_info(self.slam.get_last_position_vis_info()) self.vis.snapshot() - self.nav_init.set() - - self._busy = False - def go_to_object( self, object_goal: str, From 592d9c880fa700c0c1d640b2913ef7ae2b4e4c5f Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 20 Sep 2022 12:20:03 -0700 Subject: [PATCH 05/34] Add locking --- .../locobot/remote/navigation_service.py | 58 ++++++++++--------- 1 file changed, 31 insertions(+), 27 deletions(-) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index f5f06c5bcd..2f9011b3e6 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -92,6 +92,7 @@ class NavigationStatus: goal_reached: bool goal: Optional[GoalParams] goal_update: threading.Event + goal_lock: threading.Lock @Pyro4.expose @@ -106,7 +107,7 @@ def __init__(self, planner, slam, robot): self.map_size, self.local_map_size = self.slam.get_map_sizes() # Async navigator - self.nav_status = NavigationStatus(False, False, None, threading.Event()) + self.nav_status = NavigationStatus(False, False, None, threading.Event(), threading.Lock()) self.goto_thr = threading.Thread(target=self._navigate_to_absolute) self.goto_thr.start() @@ -235,8 +236,9 @@ def go_to_absolute( ) # Reset params - self.nav_status.goal = goal_data - self.nav_status.goal_update.set() + with self.nav_status.goal_lock: + self.nav_status.goal = goal_data + self.nav_status.goal_update.set() def _navigation_loop(self): self._stop = False @@ -246,33 +248,35 @@ def _navigation_loop(self): self.nav_status.goal_update.clear() self.nav_status.goal_update.wait() - # Load goal data - goal = self.goal_data.goal - goal_map = self.goal_data.goal_map - distance_threshold = self.goal_data.distance_threshold - angle_threshold = self.goal_data.angle_threshold - visualize = self.goal_data.visualize + with self.nav_status.goal_lock: + # Load goal data + goal = self.goal_data.goal + goal_map = self.goal_data.goal_map + distance_threshold = self.goal_data.distance_threshold + angle_threshold = self.goal_data.angle_threshold + visualize = self.goal_data.visualize - # Plan & execute - self.nav_status.path_found = True + # Plan + self.nav_status.path_found = True - robot_loc = self.robot.get_base_state() - stg = self.planner.get_short_term_goal( - robot_loc, - goal=goal, - goal_map=goal_map, - vis_path=f"{self.vis.path}/planner/step{self.vis.snapshot_idx}.png", - ) - if stg == False: - # no path to end-goal - print( - "Could not find a path to the end goal {} from current robot location {}, aborting move".format( - goal, robot_loc - ) + robot_loc = self.robot.get_base_state() + stg = self.planner.get_short_term_goal( + robot_loc, + goal=goal, + goal_map=goal_map, + vis_path=f"{self.vis.path}/planner/step{self.vis.snapshot_idx}.png", ) - self.nav_status.path_found = False - break - robot_loc = self.robot.get_base_state() + if stg == False: + # no path to end-goal + print( + "Could not find a path to the end goal {} from current robot location {}, aborting move".format( + goal, robot_loc + ) + ) + self.nav_status.path_found = False + break + + # Execute plan # status, action = self.robot.go_to_absolute(stg) self.robot.set_goal(stg, absolute=True) status = "SUCCEEDED" From 94f7d549009314ecca9e5eb0a76bd00f64005332 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 20 Sep 2022 12:22:12 -0700 Subject: [PATCH 06/34] debug --- .../locobot/remote/navigation_service.py | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 2f9011b3e6..7c6eb5a525 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -90,7 +90,7 @@ class GoalParams: class NavigationStatus: path_found: bool goal_reached: bool - goal: Optional[GoalParams] + goal_data: Optional[GoalParams] goal_update: threading.Event goal_lock: threading.Lock @@ -108,7 +108,7 @@ def __init__(self, planner, slam, robot): # Async navigator self.nav_status = NavigationStatus(False, False, None, threading.Event(), threading.Lock()) - self.goto_thr = threading.Thread(target=self._navigate_to_absolute) + self.goto_thr = threading.Thread(target=self._navigation_loop) self.goto_thr.start() # ObjectNav policy @@ -122,7 +122,6 @@ def __init__(self, planner, slam, robot): self.goal_policy.load_state_dict(state_dict, strict=False) self._busy = False - self._stop = True self._done_exploring = False self.vis = ObjectGoalNavigationVisualization() @@ -237,7 +236,7 @@ def go_to_absolute( # Reset params with self.nav_status.goal_lock: - self.nav_status.goal = goal_data + self.nav_status.goal_data = goal_data self.nav_status.goal_update.set() def _navigation_loop(self): @@ -250,11 +249,11 @@ def _navigation_loop(self): with self.nav_status.goal_lock: # Load goal data - goal = self.goal_data.goal - goal_map = self.goal_data.goal_map - distance_threshold = self.goal_data.distance_threshold - angle_threshold = self.goal_data.angle_threshold - visualize = self.goal_data.visualize + goal = self.nav_status.goal_data.goal + goal_map = self.nav_status.goal_data.goal_map + distance_threshold = self.nav_status.goal_data.distance_threshold + angle_threshold = self.nav_status.goal_data.angle_threshold + visualize = self.nav_status.goal_data.visualize # Plan self.nav_status.path_found = True @@ -340,8 +339,6 @@ def _navigation_loop(self): # TODO: if the trackback fails, we're screwed. Handle this robustly. - steps = steps - 1 - if visualize: self.vis.set_action_and_collision( {"action": action, "collision": status != "SUCCEEDED"} @@ -634,7 +631,6 @@ def stop(self): self._stop = True if self.goto_thr is not None: self.goto_thr.join() - self.goto_thr = None robot_ip = os.getenv("LOCOBOT_IP") From 283e6110c88377049ee25ce9994e96d33690fc90 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 20 Sep 2022 15:13:03 -0700 Subject: [PATCH 07/34] Add monitoring through ros topics --- .../remote/stretch_ros_move_api.py | 37 ++++++++++++++----- .../locobot/remote/navigation_service.py | 26 +++++++++++++ 2 files changed, 53 insertions(+), 10 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index feed42b113..1aed41843d 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -13,6 +13,9 @@ import collections import numpy as np +import rospy +from geometry_msgs.msg import Twist + class MoveNode(hm.HelloNode): def __init__(self): @@ -33,6 +36,9 @@ def __init__(self): self._vel_command_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) + # ROS monitoring + self._pose_pub = rospy.Publisher("robot/slam_pose", Twist) + def _joint_states_callback(self, joint_state): with self._lock: self._joint_state = joint_state @@ -41,6 +47,13 @@ def _slam_pose_callback(self, pose): with self._lock: self._slam_pose = pose + xyt_pose = self.slam_pose_to_xyt_pose(pose) + msg = Twist() + msg.linear.x = xyt_pose[0] + msg.linear.y = xyt_pose[1] + msg.angular.z = xyt_pose[2] + self._pose_pub.publish(msg) + def _scan_matched_pose_callback(self, pose): with self._lock: self._scan_matched_pose = (pose.x, pose.y, pose.theta) @@ -53,6 +66,19 @@ def _odom_callback(self, pose): ) self._angular_movement.append(abs(pose.twist.twist.angular.z)) + @staticmethod + def slam_pose_to_xyt_pose(pose): + quat = np.array( + [ + pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z, + pose.pose.pose.orientation.w, + ] + ) + euler = euler_from_quaternion(quat) + return (pose.pose.pose.position.x, pose.pose.pose.position.y, euler[2]) + def set_velocity(self, v_m, w_r): cmd = Twist() cmd.linear.x = v_m @@ -70,16 +96,7 @@ def get_slam_pose(self): with self._lock: pose = self._slam_pose if pose is not None: - quat = np.array( - [ - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z, - pose.pose.pose.orientation.w, - ] - ) - euler = euler_from_quaternion(quat) - return (pose.pose.pose.position.x, pose.pose.pose.position.y, euler[2]) + return self.slam_pose_to_xyt_pose(pose) else: return (0.0, 0.0, 0.0) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 7c6eb5a525..5bb0659258 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -14,6 +14,8 @@ from droidlet.lowlevel.pyro_utils import safe_call import skimage.morphology import cv2 +import rospy +from geometry_msgs.msg import Twist from slam_pkg.utils import depth_util as du from visualization.ogn_vis import ObjectGoalNavigationVisualization @@ -106,6 +108,12 @@ def __init__(self, planner, slam, robot): num_sem_categories = len(coco_categories) self.map_size, self.local_map_size = self.slam.get_map_sizes() + # ROS publishers for monitoring + self._loc_pub = rospy.Publisher("nav/robot_pose", Twist) + self._stg_pub = rospy.Publisher("nav/st_goal_pose", Twist) + self._goal_pub = rospy.Publisher("nav/goal_pose", Twist) + rospy.init_node("navigation_service") + # Async navigator self.nav_status = NavigationStatus(False, False, None, threading.Event(), threading.Lock()) self.goto_thr = threading.Thread(target=self._navigation_loop) @@ -126,6 +134,14 @@ def __init__(self, planner, slam, robot): self.vis = ObjectGoalNavigationVisualization() + @staticmethod + def pose_msg(x, y, theta): + msg = Twist() + msg.linear.x = x + msg.linear.y = y + msg.angular.z = theta + return msg + def go_to_relative(self, goal, distance_threshold=None, angle_threshold=None): robot_loc = self.robot.get_base_state() abs_goal = du.get_relative_state(goal, (0.0, 0.0, -robot_loc[2])) @@ -239,6 +255,10 @@ def go_to_absolute( self.nav_status.goal_data = goal_data self.nav_status.goal_update.set() + # Log + if goal is not None: + self._goal_pub.publish(self.pose_msg(*goal)) + def _navigation_loop(self): self._stop = False @@ -259,6 +279,8 @@ def _navigation_loop(self): self.nav_status.path_found = True robot_loc = self.robot.get_base_state() + self._loc_pub.publish(self.pose_msg(*robot_loc)) + stg = self.planner.get_short_term_goal( robot_loc, goal=goal, @@ -275,6 +297,10 @@ def _navigation_loop(self): self.nav_status.path_found = False break + # Log + if goal is not None: + self._stg_pub.publish(self.pose_msg(*stg)) + # Execute plan # status, action = self.robot.go_to_absolute(stg) self.robot.set_goal(stg, absolute=True) From a56e149a3df4980cf20834240b4daade6b729e33 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 20 Sep 2022 16:43:41 -0700 Subject: [PATCH 08/34] Publish state to pose instead of twist --- .../remote/stretch_ros_move_api.py | 34 +++++------------ .../locobot/remote/navigation_service.py | 37 ++++++++++++------- 2 files changed, 33 insertions(+), 38 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 1aed41843d..881b997af0 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -36,9 +36,6 @@ def __init__(self): self._vel_command_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) - # ROS monitoring - self._pose_pub = rospy.Publisher("robot/slam_pose", Twist) - def _joint_states_callback(self, joint_state): with self._lock: self._joint_state = joint_state @@ -47,13 +44,6 @@ def _slam_pose_callback(self, pose): with self._lock: self._slam_pose = pose - xyt_pose = self.slam_pose_to_xyt_pose(pose) - msg = Twist() - msg.linear.x = xyt_pose[0] - msg.linear.y = xyt_pose[1] - msg.angular.z = xyt_pose[2] - self._pose_pub.publish(msg) - def _scan_matched_pose_callback(self, pose): with self._lock: self._scan_matched_pose = (pose.x, pose.y, pose.theta) @@ -66,19 +56,6 @@ def _odom_callback(self, pose): ) self._angular_movement.append(abs(pose.twist.twist.angular.z)) - @staticmethod - def slam_pose_to_xyt_pose(pose): - quat = np.array( - [ - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z, - pose.pose.pose.orientation.w, - ] - ) - euler = euler_from_quaternion(quat) - return (pose.pose.pose.position.x, pose.pose.pose.position.y, euler[2]) - def set_velocity(self, v_m, w_r): cmd = Twist() cmd.linear.x = v_m @@ -96,7 +73,16 @@ def get_slam_pose(self): with self._lock: pose = self._slam_pose if pose is not None: - return self.slam_pose_to_xyt_pose(pose) + quat = np.array( + [ + pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z, + pose.pose.pose.orientation.w, + ] + ) + euler = euler_from_quaternion(quat) + return (pose.pose.pose.position.x, pose.pose.pose.position.y, euler[2]) else: return (0.0, 0.0, 0.0) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 5bb0659258..797ed07ffd 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -22,6 +22,9 @@ from policy.goal_policy import GoalPolicy from segmentation.constants import coco_categories +from geometry_msgs.msg import Pose +from tf.transformations import euler_from_quaternion, quaternion_from_euler + random.seed(0) torch.manual_seed(0) np.random.seed(0) @@ -41,6 +44,20 @@ def draw_line(start, end, mat, steps=25, w=1): return mat +def xyt2pose(xyt): + quat = quaternion_from_euler(0.0, 0.0, xyt[2]) + + pose = Pose() + pose.position.x = xyt[0] + pose.position.y = xyt[1] + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + + return pose + + class Trackback(object): def __init__(self, planner): self.locs = set() @@ -109,9 +126,9 @@ def __init__(self, planner, slam, robot): self.map_size, self.local_map_size = self.slam.get_map_sizes() # ROS publishers for monitoring - self._loc_pub = rospy.Publisher("nav/robot_pose", Twist) - self._stg_pub = rospy.Publisher("nav/st_goal_pose", Twist) - self._goal_pub = rospy.Publisher("nav/goal_pose", Twist) + self._loc_pub = rospy.Publisher("nav/robot_pose", Pose, queue_size=1) + self._stg_pub = rospy.Publisher("nav/st_goal_pose", Pose, queue_size=1) + self._goal_pub = rospy.Publisher("nav/goal_pose", Pose, queue_size=1) rospy.init_node("navigation_service") # Async navigator @@ -134,14 +151,6 @@ def __init__(self, planner, slam, robot): self.vis = ObjectGoalNavigationVisualization() - @staticmethod - def pose_msg(x, y, theta): - msg = Twist() - msg.linear.x = x - msg.linear.y = y - msg.angular.z = theta - return msg - def go_to_relative(self, goal, distance_threshold=None, angle_threshold=None): robot_loc = self.robot.get_base_state() abs_goal = du.get_relative_state(goal, (0.0, 0.0, -robot_loc[2])) @@ -257,7 +266,7 @@ def go_to_absolute( # Log if goal is not None: - self._goal_pub.publish(self.pose_msg(*goal)) + self._goal_pub.publish(xyt2pose(goal)) def _navigation_loop(self): self._stop = False @@ -279,7 +288,7 @@ def _navigation_loop(self): self.nav_status.path_found = True robot_loc = self.robot.get_base_state() - self._loc_pub.publish(self.pose_msg(*robot_loc)) + self._loc_pub.publish(xyt2pose(robot_loc)) stg = self.planner.get_short_term_goal( robot_loc, @@ -299,7 +308,7 @@ def _navigation_loop(self): # Log if goal is not None: - self._stg_pub.publish(self.pose_msg(*stg)) + self._stg_pub.publish(xyt2pose(stg)) # Execute plan # status, action = self.robot.go_to_absolute(stg) From a1278385f5aec9a82bbffd42c10e7614deb057cc Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 21 Sep 2022 14:23:38 -0700 Subject: [PATCH 09/34] Change state estimation to use odom --- .../hello_robot/remote/goto_controller.py | 31 ++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 9645d8a173..1525ea99d6 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -32,6 +32,7 @@ def __init__( self.control_lock = threading.Lock() self.active = False + self.xyt_odom = self.robot.get_odom() self.xyt_err = np.zeros(3) self.track_yaw = True @@ -71,6 +72,7 @@ def _turn_rate_limit(w_max, lin_err, heading_err): def _integrate_state(self, v, w): """ Predict error in the next timestep with current commanded velocity + Deprecated in favor of _update_error_state. """ dx = v * self.dt dtheta = w * self.dt @@ -84,6 +86,33 @@ def _integrate_state(self, v, w): self.xyt_err[1] = st * x_err_f0 + ct * y_err_f0 self.xyt_err[2] = self.xyt_err[2] - dtheta if self.track_yaw else 0.0 + def _update_error_state(self): + """ + Updates error based on odometry feedback (has drift but very low noise signal) + """ + xyt_odom_new = self.robot.get_odom() + + # Update error + ct0 = np.cos(self.xyt_odom[2]) + st0 = np.sin(self.xyt_odom[2]) + ct1 = np.cos(xyt_odom_new[2]) + st1 = np.sin(xyt_odom_new[2]) + + xyt_goal_global = np.array( + [ + self.xyt_odom[0] + ct0 * self.xyt_err[0] - st0 * self.xyt_err[1], + self.xyt_odom[1] + st0 * self.xyt_err[0] + ct0 * self.xyt_err[1], + self.xyt_odom[2] + self.xyt_err[2], + ] + ) + dxyt_global = xyt_goal_global - xyt_odom_new + self.xyt_err[0] = ct1 * dxyt_global[0] + st1 * dxyt_global[1] + self.xyt_err[1] = -st1 * dxyt_global[0] + ct1 * dxyt_global[1] + self.xyt_err[2] = dxyt_global[2] + + # Update odom state + self.xyt_odom = xyt_odom_new + def _run(self): rate = rospy.Rate(self.hz) @@ -120,7 +149,7 @@ def _run(self): self.robot.set_velocity(v_cmd, w_cmd) # Update odometry prediction - self._integrate_state(v_cmd, w_cmd) + self._update_error_state() # Spin rate.sleep() From 509e937905c5a33437ab22ad0004606c05727995 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 21 Sep 2022 14:37:44 -0700 Subject: [PATCH 10/34] Tune controller --- .../hello_robot/remote/goto_controller.py | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 1525ea99d6..5a28c943d8 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -7,6 +7,8 @@ V_MAX_DEFAULT = 0.15 # base.params["motion"]["default"]["vel_m"] W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m +ACC_LIN = 1.6 # 4 * (base.params["motion"]["max"]["accel_m"]) +ACC_ANG = 9.6 # 4 * (2 * (accel_m_max - accel_m_max) / wheel_separation_m) class GotoVelocityController: @@ -24,8 +26,8 @@ def __init__( # Params self.v_max = v_max or V_MAX_DEFAULT self.w_max = w_max or W_MAX_DEFAULT - self.lin_error_tol = self.v_max / hz - self.ang_error_tol = self.w_max / hz + self.lin_error_tol = 2 * self.v_max / hz + self.ang_error_tol = 2 * self.w_max / hz # Initialize self.loop_thr = None @@ -37,7 +39,7 @@ def __init__( self.track_yaw = True @staticmethod - def _error_velocity_multiplier(x_err, tol=0.0): + def _error_velocity_multiplier(x_err, a, tol=0.0): """ Computes velocity multiplier based on distance from target. Used for both linear and angular motion. @@ -46,7 +48,8 @@ def _error_velocity_multiplier(x_err, tol=0.0): Output = 1 if linear error is larger than the tolerance, 0 otherwise. """ assert x_err >= 0.0 - return float(x_err - tol > 0) + t = np.sqrt(2.0 * max(x_err - tol, 0.0) / a) # x_err = (1/2) * a * t^2 + return min(a * t, 1.0) @staticmethod def _projection_velocity_multiplier(theta_err, tol=0.0): @@ -129,19 +132,23 @@ def _run(self): heading_err_abs = abs(heading_err) # Compute linear velocity - k_t = self._error_velocity_multiplier(lin_err_abs, tol=self.lin_error_tol) + k_t = self._error_velocity_multiplier(lin_err_abs, ACC_LIN, tol=self.lin_error_tol) k_p = self._projection_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) v_limit = self._turn_rate_limit(self.w_max, lin_err_abs, heading_err_abs) v_cmd = min(k_t * k_p * self.v_max, v_limit) # Compute angular velocity - k_t_ang = self._error_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) + k_t_ang = self._error_velocity_multiplier( + heading_err_abs, ACC_ANG, tol=self.ang_error_tol + ) w_cmd = np.sign(heading_err) * k_t_ang * self.w_max # Rotate to correct yaw if yaw tracking is on and XY position is at goal elif ang_err_abs > self.ang_error_tol: # Compute angular velocity - k_t_ang = self._error_velocity_multiplier(ang_err_abs, tol=self.ang_error_tol) + k_t_ang = self._error_velocity_multiplier( + ang_err_abs, ACC_ANG, tol=self.ang_error_tol + ) w_cmd = np.sign(ang_err) * k_t_ang * self.w_max # Command robot From c45d281a249cb2e05e10ed93b130baed05c94673 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 21 Sep 2022 14:52:09 -0700 Subject: [PATCH 11/34] Make use odom toggleable --- .../hello_robot/remote/goto_controller.py | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 5a28c943d8..05d45b2c48 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -18,10 +18,12 @@ def __init__( hz: float, v_max: Optional[float] = None, w_max: Optional[float] = None, + use_odom: bool = True, ): self.robot = robot self.hz = hz self.dt = 1.0 / self.hz + self.use_odom = use_odom # Params self.v_max = v_max or V_MAX_DEFAULT @@ -39,7 +41,7 @@ def __init__( self.track_yaw = True @staticmethod - def _error_velocity_multiplier(x_err, a, tol=0.0): + def _error_velocity_multiplier(x_err, a, tol=0.0, use_acc=True): """ Computes velocity multiplier based on distance from target. Used for both linear and angular motion. @@ -48,8 +50,11 @@ def _error_velocity_multiplier(x_err, a, tol=0.0): Output = 1 if linear error is larger than the tolerance, 0 otherwise. """ assert x_err >= 0.0 - t = np.sqrt(2.0 * max(x_err - tol, 0.0) / a) # x_err = (1/2) * a * t^2 - return min(a * t, 1.0) + if use_acc: + t = np.sqrt(2.0 * max(x_err - tol, 0.0) / a) # x_err = (1/2) * a * t^2 + return min(a * t, 1.0) + else: + return float(x_err > tol) @staticmethod def _projection_velocity_multiplier(theta_err, tol=0.0): @@ -132,14 +137,16 @@ def _run(self): heading_err_abs = abs(heading_err) # Compute linear velocity - k_t = self._error_velocity_multiplier(lin_err_abs, ACC_LIN, tol=self.lin_error_tol) + k_t = self._error_velocity_multiplier( + lin_err_abs, ACC_LIN, tol=self.lin_error_tol, use_acc=self.use_odom + ) k_p = self._projection_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) v_limit = self._turn_rate_limit(self.w_max, lin_err_abs, heading_err_abs) v_cmd = min(k_t * k_p * self.v_max, v_limit) # Compute angular velocity k_t_ang = self._error_velocity_multiplier( - heading_err_abs, ACC_ANG, tol=self.ang_error_tol + heading_err_abs, ACC_ANG, tol=self.ang_error_tol, use_acc=self.use_odom ) w_cmd = np.sign(heading_err) * k_t_ang * self.w_max @@ -147,7 +154,7 @@ def _run(self): elif ang_err_abs > self.ang_error_tol: # Compute angular velocity k_t_ang = self._error_velocity_multiplier( - ang_err_abs, ACC_ANG, tol=self.ang_error_tol + ang_err_abs, ACC_ANG, tol=self.ang_error_tol, use_acc=self.use_odom ) w_cmd = np.sign(ang_err) * k_t_ang * self.w_max @@ -155,8 +162,11 @@ def _run(self): with self.control_lock: self.robot.set_velocity(v_cmd, w_cmd) - # Update odometry prediction - self._update_error_state() + # Update error + if self.use_odom: + self._update_error_state() + else: + self._integrate_state(v_cmd, w_cmd) # Spin rate.sleep() From d0325fd5a107df89dcd0c9e7468460c2384b1beb Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 21 Sep 2022 14:58:51 -0700 Subject: [PATCH 12/34] Use transforms from utils --- .../hello_robot/remote/goto_controller.py | 31 +++++-------------- 1 file changed, 7 insertions(+), 24 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 05d45b2c48..3709912aa3 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -5,6 +5,8 @@ import numpy as np import rospy +from utils import transform_global_to_base, transform_base_to_global + V_MAX_DEFAULT = 0.15 # base.params["motion"]["default"]["vel_m"] W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m ACC_LIN = 1.6 # 4 * (base.params["motion"]["max"]["accel_m"]) @@ -85,14 +87,9 @@ def _integrate_state(self, v, w): dx = v * self.dt dtheta = w * self.dt - x_err_f0 = self.xyt_err[0] - dx * np.cos(dtheta / 2.0) - y_err_f0 = self.xyt_err[1] - dx * np.sin(dtheta / 2.0) - ct = np.cos(-dtheta) - st = np.sin(-dtheta) - - self.xyt_err[0] = ct * x_err_f0 - st * y_err_f0 - self.xyt_err[1] = st * x_err_f0 + ct * y_err_f0 - self.xyt_err[2] = self.xyt_err[2] - dtheta if self.track_yaw else 0.0 + xyt_goal_global = transform_base_to_global(self.xyt_err, np.zeros(3)) + xyt_new = np.array([dx * np.cos(dtheta / 2.0), dx * np.sin(dtheta / 2.0), dtheta]) + self.xyt_err = transform_global_to_base(xyt_goal_global, xyt_new) def _update_error_state(self): """ @@ -101,22 +98,8 @@ def _update_error_state(self): xyt_odom_new = self.robot.get_odom() # Update error - ct0 = np.cos(self.xyt_odom[2]) - st0 = np.sin(self.xyt_odom[2]) - ct1 = np.cos(xyt_odom_new[2]) - st1 = np.sin(xyt_odom_new[2]) - - xyt_goal_global = np.array( - [ - self.xyt_odom[0] + ct0 * self.xyt_err[0] - st0 * self.xyt_err[1], - self.xyt_odom[1] + st0 * self.xyt_err[0] + ct0 * self.xyt_err[1], - self.xyt_odom[2] + self.xyt_err[2], - ] - ) - dxyt_global = xyt_goal_global - xyt_odom_new - self.xyt_err[0] = ct1 * dxyt_global[0] + st1 * dxyt_global[1] - self.xyt_err[1] = -st1 * dxyt_global[0] + ct1 * dxyt_global[1] - self.xyt_err[2] = dxyt_global[2] + xyt_goal_global = transform_base_to_global(self.xyt_err, self.xyt_odom) + self.xyt_err = transform_global_to_base(xyt_goal_global, xyt_odom_new) # Update odom state self.xyt_odom = xyt_odom_new From ec8c6ace67a835e82a3763c38ee15db608c60f67 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 21 Sep 2022 15:21:55 -0700 Subject: [PATCH 13/34] Tune on hw, turn off odom by default --- droidlet/lowlevel/hello_robot/remote/goto_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 3709912aa3..d76bb31c99 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -7,10 +7,10 @@ from utils import transform_global_to_base, transform_base_to_global -V_MAX_DEFAULT = 0.15 # base.params["motion"]["default"]["vel_m"] +V_MAX_DEFAULT = 0.2 # base.params["motion"]["default"]["vel_m"] W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m ACC_LIN = 1.6 # 4 * (base.params["motion"]["max"]["accel_m"]) -ACC_ANG = 9.6 # 4 * (2 * (accel_m_max - accel_m_max) / wheel_separation_m) +ACC_ANG = 4.8 # 2 * (2 * (accel_m_max - accel_m_max) / wheel_separation_m) class GotoVelocityController: @@ -20,7 +20,7 @@ def __init__( hz: float, v_max: Optional[float] = None, w_max: Optional[float] = None, - use_odom: bool = True, + use_odom: bool = False, ): self.robot = robot self.hz = hz @@ -129,7 +129,7 @@ def _run(self): # Compute angular velocity k_t_ang = self._error_velocity_multiplier( - heading_err_abs, ACC_ANG, tol=self.ang_error_tol, use_acc=self.use_odom + heading_err_abs, ACC_ANG, tol=self.ang_error_tol / 2.0, use_acc=self.use_odom ) w_cmd = np.sign(heading_err) * k_t_ang * self.w_max From fb7b67f93282a762c7377ba34a4a5e85ac3a2aa5 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 25 Oct 2022 15:51:24 -0700 Subject: [PATCH 14/34] Fix erroneous import --- droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py index e6090159d1..3b38ab0e68 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_realsense.py @@ -9,9 +9,6 @@ import copy import math from math import * -from droidlet.lowlevel.locobot.remote.segmentation.detectron2_segmentation import ( - Detectron2Segmentation, -) import pyrealsense2 as rs import Pyro4 From 6b419d2f294078f460f4ce85820b32d30fc9e5ad Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 25 Oct 2022 15:51:41 -0700 Subject: [PATCH 15/34] State estimator initial impl --- .../hello_robot/remote/goto_controller.py | 4 +- .../remote/stretch_ros_move_api.py | 90 ++++++++++++++++++- 2 files changed, 91 insertions(+), 3 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index d76bb31c99..4d2e92f72f 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -68,7 +68,7 @@ def _projection_velocity_multiplier(theta_err, tol=0.0): Output = 1 when facing target, gradually decreases to 0 when angle to target is pi/3. """ assert theta_err >= 0.0 - return 1.0 - np.sin(min(max(theta_err - tol, 0.0) * 2.0, np.pi / 3.0)) + return 1.0 - np.sin(max(theta_err - tol, 0.0)) @staticmethod def _turn_rate_limit(w_max, lin_err, heading_err): @@ -77,7 +77,7 @@ def _turn_rate_limit(w_max, lin_err, heading_err): """ assert lin_err >= 0.0 assert heading_err >= 0.0 - return w_max * lin_err / np.sin(heading_err) + 1e-5 / 2.0 + return w_max * lin_err / (np.sin(heading_err) + 1e-5) / 2.0 def _integrate_state(self, v, w): """ diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 881b997af0..3b553fee61 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -6,17 +6,49 @@ from sensor_msgs.msg import JointState from control_msgs.msg import FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectoryPoint -from geometry_msgs.msg import PoseStamped, Pose2D, PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import Pose, PoseStamped, Pose2D, PoseWithCovarianceStamped, Twist from nav_msgs.msg import Odometry import hello_helpers.hello_misc as hm from tf.transformations import euler_from_quaternion import collections import numpy as np +import sophus as sp +from scipy.spatial.transform import Rotation as R import rospy from geometry_msgs.msg import Twist +LOCALIZATION_TIME_CONSTANT = 1.0 + + +def pose_ros2sp(self, pose): + r_mat = R.from_quat( + (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) + ).as_matrix() + t_vec = np.array([pose.position.x, pose.position.y, pose.position.z]) + return sp.SE3(r_mat, t_vec) + + +def pose_sp2ros(self, pose_se3): + quat = R.from_matrix(pose_se3.so3().matrix()).as_quat() + + pose = Pose() + pose.position.x = pose_se3.translation()[0] + pose.position.y = pose_se3.translation()[1] + pose.position.z = pose_se3.translation()[2] + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + + return pose + + +def cutoff_angle(duration, time_constant): + return 2 * np.pi * duration / time_constant + + class MoveNode(hm.HelloNode): def __init__(self): hm.HelloNode.__init__(self) @@ -30,25 +62,51 @@ def __init__(self): self._scan_matched_pose = None self._lock = threading.Lock() + self._filtered_pose = sp.SE3() + self._pose_odom_prev = sp.SE3() + self._nav_mode = rospy.ServiceProxy("/switch_to_navigation_mode", Trigger) s_request = TriggerRequest() self._nav_mode(s_request) self._vel_command_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1) + self._estimator_pub = rospy.Publisher( + "/state_estimator/pose_filtered", PoseStamped, queue_size=1 + ) def _joint_states_callback(self, joint_state): with self._lock: self._joint_state = joint_state def _slam_pose_callback(self, pose): + t_curr = time.time() + ros_time = rospy.Time.now() with self._lock: self._slam_pose = pose + # Compute injected signals into filtered pose + w = cutoff_angle(t_curr - self._t_slam_prev, LOCALIZATION_TIME_CONSTANT) + coeff = w / (w + 1) + + # Update filtered pose + slam_pose = pose_ros2sp(pose) + with self._lock: + pose_prev = self._filtered_pose + self._filtered_pose = pose_prev * sp.SE3.exp( + coeff * (pose_prev.inverse() * slam_pose).log() + ) + + self._t_slam_prev = t_curr + + self._publish_filtered_state(ros_time) + def _scan_matched_pose_callback(self, pose): with self._lock: self._scan_matched_pose = (pose.x, pose.y, pose.theta) def _odom_callback(self, pose): + t_curr = time.time() + ros_time = rospy.Time.now() with self._lock: self._odom = pose self._linear_movement.append( @@ -56,6 +114,28 @@ def _odom_callback(self, pose): ) self._angular_movement.append(abs(pose.twist.twist.angular.z)) + # Compute injected signals into filtered pose + w = cutoff_angle(t_curr - self._t_odom_prev, LOCALIZATION_TIME_CONSTANT) + coeff = 1 / (w + 1) + pose_odom = pose_ros2sp(pose) + pose_diff_odom = self._pose_odom_prev.inverse() * pose_odom + + # Update filtered pose + with self._lock: + pose_prev = self._filtered_pose + self._filtered_pose = sp.SE3.exp(coeff * (pose_prev * pose_diff_odom).log()) + + self._pose_odom_prev = pose_odom + self._t_odom_prev = t_curr + + self._publish_filtered_state(ros_time) + + def _publish_filtered_state(self, timestamp): + pose_out = PoseStamped() + pose_out.header = timestamp + pose_out.pose = pose_sp2ros(self._filtered_pose) + self._estimator_pub.publish(pose_out) + def set_velocity(self, v_m, w_r): cmd = Twist() cmd.linear.x = v_m @@ -106,6 +186,14 @@ def get_odom(self): euler = euler_from_quaternion(quat) return (pose.pose.position.x, pose.pose.position.y, euler[2]) + def get_filtered_pose(self): + with self._lock: + pose = self._filtered_pose.copy() + + t_vec = pose.translation() + r_vec = pose.so3().log() + return (t_vec[0], t_vec[1], r_vec[2]) + def get_joint_state(self, name=None): with self._lock: joint_state = self._joint_state From 153e17b8c8466ab8fd291bedba279bea50467f31 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 25 Oct 2022 16:08:44 -0700 Subject: [PATCH 16/34] Separate lock for estimator --- .../hello_robot/remote/stretch_ros_move_api.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 3b553fee61..cc67510412 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -62,6 +62,7 @@ def __init__(self): self._scan_matched_pose = None self._lock = threading.Lock() + self._filter_lock = threading.Lock() self._filtered_pose = sp.SE3() self._pose_odom_prev = sp.SE3() @@ -90,16 +91,15 @@ def _slam_pose_callback(self, pose): # Update filtered pose slam_pose = pose_ros2sp(pose) - with self._lock: + with self._filter_lock: pose_prev = self._filtered_pose self._filtered_pose = pose_prev * sp.SE3.exp( coeff * (pose_prev.inverse() * slam_pose).log() ) + self._publish_filtered_state(ros_time) self._t_slam_prev = t_curr - self._publish_filtered_state(ros_time) - def _scan_matched_pose_callback(self, pose): with self._lock: self._scan_matched_pose = (pose.x, pose.y, pose.theta) @@ -121,15 +121,14 @@ def _odom_callback(self, pose): pose_diff_odom = self._pose_odom_prev.inverse() * pose_odom # Update filtered pose - with self._lock: + with self._filter_lock: pose_prev = self._filtered_pose self._filtered_pose = sp.SE3.exp(coeff * (pose_prev * pose_diff_odom).log()) + self._publish_filtered_state(ros_time) self._pose_odom_prev = pose_odom self._t_odom_prev = t_curr - self._publish_filtered_state(ros_time) - def _publish_filtered_state(self, timestamp): pose_out = PoseStamped() pose_out.header = timestamp From 2c6564106004d3c7976815201e0a3a423f4553da Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 25 Oct 2022 16:12:57 -0700 Subject: [PATCH 17/34] Debug --- .../hello_robot/remote/stretch_ros_move_api.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index cc67510412..061be0b283 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -22,7 +22,7 @@ LOCALIZATION_TIME_CONSTANT = 1.0 -def pose_ros2sp(self, pose): +def pose_ros2sp(pose): r_mat = R.from_quat( (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) ).as_matrix() @@ -30,7 +30,7 @@ def pose_ros2sp(self, pose): return sp.SE3(r_mat, t_vec) -def pose_sp2ros(self, pose_se3): +def pose_sp2ros(pose_se3): quat = R.from_matrix(pose_se3.so3().matrix()).as_quat() pose = Pose() @@ -64,6 +64,8 @@ def __init__(self): self._filter_lock = threading.Lock() self._filtered_pose = sp.SE3() + self._t_odom_prev = time.time() + self._t_slam_prev = time.time() self._pose_odom_prev = sp.SE3() self._nav_mode = rospy.ServiceProxy("/switch_to_navigation_mode", Trigger) @@ -90,7 +92,7 @@ def _slam_pose_callback(self, pose): coeff = w / (w + 1) # Update filtered pose - slam_pose = pose_ros2sp(pose) + slam_pose = pose_ros2sp(pose.pose.pose) with self._filter_lock: pose_prev = self._filtered_pose self._filtered_pose = pose_prev * sp.SE3.exp( @@ -117,7 +119,7 @@ def _odom_callback(self, pose): # Compute injected signals into filtered pose w = cutoff_angle(t_curr - self._t_odom_prev, LOCALIZATION_TIME_CONSTANT) coeff = 1 / (w + 1) - pose_odom = pose_ros2sp(pose) + pose_odom = pose_ros2sp(pose.pose.pose) pose_diff_odom = self._pose_odom_prev.inverse() * pose_odom # Update filtered pose @@ -131,7 +133,7 @@ def _odom_callback(self, pose): def _publish_filtered_state(self, timestamp): pose_out = PoseStamped() - pose_out.header = timestamp + pose_out.header.stamp = timestamp pose_out.pose = pose_sp2ros(self._filtered_pose) self._estimator_pub.publish(pose_out) From 92e2004d2f83503acdb93cfe1a8d2efcb9962b7f Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Tue, 25 Oct 2022 16:45:27 -0700 Subject: [PATCH 18/34] Debug filter --- droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 061be0b283..c0c4b21f1d 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -46,7 +46,7 @@ def pose_sp2ros(pose_se3): def cutoff_angle(duration, time_constant): - return 2 * np.pi * duration / time_constant + return duration / time_constant class MoveNode(hm.HelloNode): From cd28c41b7fd561e55aca6c90071b78888060e768 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 11:46:54 -0700 Subject: [PATCH 19/34] Remove odom decay and tune time constant --- .../lowlevel/hello_robot/remote/stretch_ros_move_api.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index c0c4b21f1d..5344f83eb6 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -19,7 +19,7 @@ from geometry_msgs.msg import Twist -LOCALIZATION_TIME_CONSTANT = 1.0 +LOCALIZATION_TIME_CONSTANT = 2.0 def pose_ros2sp(pose): @@ -117,15 +117,13 @@ def _odom_callback(self, pose): self._angular_movement.append(abs(pose.twist.twist.angular.z)) # Compute injected signals into filtered pose - w = cutoff_angle(t_curr - self._t_odom_prev, LOCALIZATION_TIME_CONSTANT) - coeff = 1 / (w + 1) pose_odom = pose_ros2sp(pose.pose.pose) pose_diff_odom = self._pose_odom_prev.inverse() * pose_odom # Update filtered pose with self._filter_lock: pose_prev = self._filtered_pose - self._filtered_pose = sp.SE3.exp(coeff * (pose_prev * pose_diff_odom).log()) + self._filtered_pose = pose_prev * pose_diff_odom self._publish_filtered_state(ros_time) self._pose_odom_prev = pose_odom From a2a09aafb32c5dcf922479016d6dfa734438e36d Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 12:38:46 -0700 Subject: [PATCH 20/34] Debug and pub cov --- .../lowlevel/hello_robot/remote/stretch_ros_move_api.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 5344f83eb6..b28a7c0f42 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -46,7 +46,7 @@ def pose_sp2ros(pose_se3): def cutoff_angle(duration, time_constant): - return duration / time_constant + return 2 * np.pi * duration / time_constant class MoveNode(hm.HelloNode): @@ -76,6 +76,7 @@ def __init__(self): self._estimator_pub = rospy.Publisher( "/state_estimator/pose_filtered", PoseStamped, queue_size=1 ) + self._cov_pub = rospy.Publisher("/state_estimator/slam_pose_cov", float, queue_size=1) def _joint_states_callback(self, joint_state): with self._lock: @@ -91,6 +92,9 @@ def _slam_pose_callback(self, pose): w = cutoff_angle(t_curr - self._t_slam_prev, LOCALIZATION_TIME_CONSTANT) coeff = w / (w + 1) + cov = np.linalg.det(np.array(pose.pose.covariance).reshape(6, 6)) + self._cov_pub.publish(cov) + # Update filtered pose slam_pose = pose_ros2sp(pose.pose.pose) with self._filter_lock: From 53b3990e6f5e70da30e7c5962a63d9ade72270c0 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 13:47:29 -0700 Subject: [PATCH 21/34] improved filter --- .../remote/stretch_ros_move_api.py | 35 ++++++------------- 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index b28a7c0f42..ac49b5e6e3 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -62,10 +62,9 @@ def __init__(self): self._scan_matched_pose = None self._lock = threading.Lock() - self._filter_lock = threading.Lock() self._filtered_pose = sp.SE3() + self._slam_pose_sp = sp.SE3() self._t_odom_prev = time.time() - self._t_slam_prev = time.time() self._pose_odom_prev = sp.SE3() self._nav_mode = rospy.ServiceProxy("/switch_to_navigation_mode", Trigger) @@ -76,7 +75,6 @@ def __init__(self): self._estimator_pub = rospy.Publisher( "/state_estimator/pose_filtered", PoseStamped, queue_size=1 ) - self._cov_pub = rospy.Publisher("/state_estimator/slam_pose_cov", float, queue_size=1) def _joint_states_callback(self, joint_state): with self._lock: @@ -88,23 +86,8 @@ def _slam_pose_callback(self, pose): with self._lock: self._slam_pose = pose - # Compute injected signals into filtered pose - w = cutoff_angle(t_curr - self._t_slam_prev, LOCALIZATION_TIME_CONSTANT) - coeff = w / (w + 1) - - cov = np.linalg.det(np.array(pose.pose.covariance).reshape(6, 6)) - self._cov_pub.publish(cov) - - # Update filtered pose - slam_pose = pose_ros2sp(pose.pose.pose) - with self._filter_lock: - pose_prev = self._filtered_pose - self._filtered_pose = pose_prev * sp.SE3.exp( - coeff * (pose_prev.inverse() * slam_pose).log() - ) - self._publish_filtered_state(ros_time) - - self._t_slam_prev = t_curr + # Update slam pose for filtering + self._slam_pose_sp = pose_ros2sp(pose.pose.pose) def _scan_matched_pose_callback(self, pose): with self._lock: @@ -123,13 +106,17 @@ def _odom_callback(self, pose): # Compute injected signals into filtered pose pose_odom = pose_ros2sp(pose.pose.pose) pose_diff_odom = self._pose_odom_prev.inverse() * pose_odom + pose_diff_slam = self._filtered_pose.inverse() * self._slam_pose_sp # Update filtered pose - with self._filter_lock: - pose_prev = self._filtered_pose - self._filtered_pose = pose_prev * pose_diff_odom - self._publish_filtered_state(ros_time) + w = cutoff_angle(t_curr - self._t_odom_prev, LOCALIZATION_TIME_CONSTANT) + coeff = 1 / (w + 1) + + pose_diff_log = coeff * pose_diff_odom.log() + (1 - coeff) * pose_diff_slam.log() + self._filtered_pose = self._filtered_pose * sp.SE3.exp(pose_diff_log) + self._publish_filtered_state(ros_time) + # Update variables self._pose_odom_prev = pose_odom self._t_odom_prev = t_curr From e20cbf294d303dc36b5d868e5a7fcd232df5e8a5 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 14:11:54 -0700 Subject: [PATCH 22/34] Tune cutoff freq --- .../lowlevel/hello_robot/remote/stretch_ros_move_api.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index ac49b5e6e3..70eb37197e 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -19,7 +19,7 @@ from geometry_msgs.msg import Twist -LOCALIZATION_TIME_CONSTANT = 2.0 +SLAM_CUTOFF_HZ = 0.2 def pose_ros2sp(pose): @@ -45,8 +45,8 @@ def pose_sp2ros(pose_se3): return pose -def cutoff_angle(duration, time_constant): - return 2 * np.pi * duration / time_constant +def cutoff_angle(duration, cutoff_freq): + return 2 * np.pi * duration * cutoff_freq class MoveNode(hm.HelloNode): @@ -109,7 +109,7 @@ def _odom_callback(self, pose): pose_diff_slam = self._filtered_pose.inverse() * self._slam_pose_sp # Update filtered pose - w = cutoff_angle(t_curr - self._t_odom_prev, LOCALIZATION_TIME_CONSTANT) + w = cutoff_angle(t_curr - self._t_odom_prev, SLAM_CUTOFF_HZ) coeff = 1 / (w + 1) pose_diff_log = coeff * pose_diff_odom.log() + (1 - coeff) * pose_diff_slam.log() From d0ef914a6403fcabf68364297bd607fd6c11e782 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 14:43:57 -0700 Subject: [PATCH 23/34] Have set_goal methods use estimator state --- .../hello_robot/remote/remote_hello_robot_ros.py | 10 ++++++---- .../hello_robot/remote/stretch_ros_move_api.py | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py index b7afb8bd87..4058f0b512 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py @@ -102,11 +102,13 @@ def get_camera_transform(self): return camera_transform def get_base_state(self): - # Best (from SLAM) - return self._robot.get_slam_pose() - # second Best (from lidar scanning) + # SLAM + wheel encoder (#1) + return self._robot.get_estimator_pose() + # SLAM (#2) + # return self._robot.get_slam_pose() + # lidar scanning (#3) # return self._robot.get_scan_matched_pose() - # Worst, from wheel encoder only + # Wheel encoder only (#4, but low latency & noise) # return self._robot.get_odom() def get_slam_pose(self): diff --git a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py index 70eb37197e..ccbdd43760 100755 --- a/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py +++ b/droidlet/lowlevel/hello_robot/remote/stretch_ros_move_api.py @@ -176,7 +176,7 @@ def get_odom(self): euler = euler_from_quaternion(quat) return (pose.pose.position.x, pose.pose.position.y, euler[2]) - def get_filtered_pose(self): + def get_estimator_pose(self): with self._lock: pose = self._filtered_pose.copy() From c01dc37b14544880ae49e940b278d44ed9e49e37 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 14:45:47 -0700 Subject: [PATCH 24/34] Default to abs for API --- .../lowlevel/hello_robot/remote/remote_hello_robot_ros.py | 8 ++++---- droidlet/lowlevel/locobot/remote/navigation_service.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py index 4058f0b512..ccadb09584 100644 --- a/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py +++ b/droidlet/lowlevel/hello_robot/remote/remote_hello_robot_ros.py @@ -271,7 +271,7 @@ def set_velocity(self, v_m, w_r): """Directly sets the forward and yaw velocity of the robot.""" self._robot.set_velocity(v_m, w_r) - def set_position_goal(self, xy_position, absolute=False): + def set_position_goal(self, xy_position, relative=False): """Moves the robot base to the given goal position. The robot does not have a yaw goal and will simply turn & move towards the desired position. @@ -284,7 +284,7 @@ def set_position_goal(self, xy_position, absolute=False): # Convert abs to rel xyt_position = list(xy_position) + [0.0] - if absolute: + if not relative: base_state = self.get_base_state() xyt_position = transform_global_to_base(xyt_position, base_state) xyt_position[2] = 0.0 @@ -294,7 +294,7 @@ def set_position_goal(self, xy_position, absolute=False): self._goto_controller.enable_yaw_tracking(False) self._goto_controller.set_goal(xyt_position) - def set_goal(self, xyt_position, absolute=False): + def set_goal(self, xyt_position, relative=False): """Moves the robot base to the given goal state :param xyt_position: The goal state of the form (x,y,yaw) @@ -304,7 +304,7 @@ def set_goal(self, xyt_position, absolute=False): ), f"Input goal should be of length 3 (xyt), got {len(xyt_position)} instead." # Convert abs to rel - if absolute: + if not relative: base_state = self.get_base_state() xyt_position = transform_global_to_base(xyt_position, base_state) diff --git a/droidlet/lowlevel/locobot/remote/navigation_service.py b/droidlet/lowlevel/locobot/remote/navigation_service.py index 797ed07ffd..3f69bf9f44 100644 --- a/droidlet/lowlevel/locobot/remote/navigation_service.py +++ b/droidlet/lowlevel/locobot/remote/navigation_service.py @@ -312,7 +312,7 @@ def _navigation_loop(self): # Execute plan # status, action = self.robot.go_to_absolute(stg) - self.robot.set_goal(stg, absolute=True) + self.robot.set_goal(stg) status = "SUCCEEDED" action = "move_to" From 5445d73c65caf398304b490856655572b366b4e0 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 26 Oct 2022 14:58:36 -0700 Subject: [PATCH 25/34] Use localization for goto controller --- .../hello_robot/remote/goto_controller.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 4d2e92f72f..f2d9edaf69 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -20,12 +20,12 @@ def __init__( hz: float, v_max: Optional[float] = None, w_max: Optional[float] = None, - use_odom: bool = False, + use_localization: bool = True, ): self.robot = robot self.hz = hz self.dt = 1.0 / self.hz - self.use_odom = use_odom + self.use_loc = use_localization # Params self.v_max = v_max or V_MAX_DEFAULT @@ -38,7 +38,7 @@ def __init__( self.control_lock = threading.Lock() self.active = False - self.xyt_odom = self.robot.get_odom() + self.xyt_loc = self.robot.get_base_state() self.xyt_err = np.zeros(3) self.track_yaw = True @@ -93,16 +93,16 @@ def _integrate_state(self, v, w): def _update_error_state(self): """ - Updates error based on odometry feedback (has drift but very low noise signal) + Updates error based on robot localization """ - xyt_odom_new = self.robot.get_odom() + xyt_loc_new = self.robot.get_base_state() # Update error - xyt_goal_global = transform_base_to_global(self.xyt_err, self.xyt_odom) - self.xyt_err = transform_global_to_base(xyt_goal_global, xyt_odom_new) + xyt_goal_global = transform_base_to_global(self.xyt_err, self.xyt_loc) + self.xyt_err = transform_global_to_base(xyt_goal_global, xyt_loc_new) - # Update odom state - self.xyt_odom = xyt_odom_new + # Update state + self.xyt_loc = xyt_loc_new def _run(self): rate = rospy.Rate(self.hz) @@ -121,7 +121,7 @@ def _run(self): # Compute linear velocity k_t = self._error_velocity_multiplier( - lin_err_abs, ACC_LIN, tol=self.lin_error_tol, use_acc=self.use_odom + lin_err_abs, ACC_LIN, tol=self.lin_error_tol, use_acc=self.use_loc ) k_p = self._projection_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) v_limit = self._turn_rate_limit(self.w_max, lin_err_abs, heading_err_abs) @@ -129,7 +129,7 @@ def _run(self): # Compute angular velocity k_t_ang = self._error_velocity_multiplier( - heading_err_abs, ACC_ANG, tol=self.ang_error_tol / 2.0, use_acc=self.use_odom + heading_err_abs, ACC_ANG, tol=self.ang_error_tol / 2.0, use_acc=self.use_loc ) w_cmd = np.sign(heading_err) * k_t_ang * self.w_max @@ -137,7 +137,7 @@ def _run(self): elif ang_err_abs > self.ang_error_tol: # Compute angular velocity k_t_ang = self._error_velocity_multiplier( - ang_err_abs, ACC_ANG, tol=self.ang_error_tol, use_acc=self.use_odom + ang_err_abs, ACC_ANG, tol=self.ang_error_tol, use_acc=self.use_loc ) w_cmd = np.sign(ang_err) * k_t_ang * self.w_max @@ -146,7 +146,7 @@ def _run(self): self.robot.set_velocity(v_cmd, w_cmd) # Update error - if self.use_odom: + if self.use_loc: self._update_error_state() else: self._integrate_state(v_cmd, w_cmd) From 42c481fb98f6916b5c5890a337583f615d1259bc Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Thu, 27 Oct 2022 13:15:32 -0700 Subject: [PATCH 26/34] Debug --- droidlet/lowlevel/hello_robot/remote/goto_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index f2d9edaf69..ba07642894 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -38,7 +38,7 @@ def __init__( self.control_lock = threading.Lock() self.active = False - self.xyt_loc = self.robot.get_base_state() + self.xyt_loc = self.robot.get_estimator_pose() self.xyt_err = np.zeros(3) self.track_yaw = True @@ -95,7 +95,7 @@ def _update_error_state(self): """ Updates error based on robot localization """ - xyt_loc_new = self.robot.get_base_state() + xyt_loc_new = self.robot.get_estimator_pose() # Update error xyt_goal_global = transform_base_to_global(self.xyt_err, self.xyt_loc) From a5c6ed95e7147ef4f7fe9c8e5f25bbf7d2decf48 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 03:02:49 -0700 Subject: [PATCH 27/34] Debug turn rate limit --- droidlet/lowlevel/hello_robot/remote/goto_controller.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index ba07642894..26194c2046 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -71,13 +71,14 @@ def _projection_velocity_multiplier(theta_err, tol=0.0): return 1.0 - np.sin(max(theta_err - tol, 0.0)) @staticmethod - def _turn_rate_limit(w_max, lin_err, heading_err): + def _turn_rate_limit(w_max, lin_err, heading_err, dead_zone=0.0): """ Computed velocity limit based on the turning radius required to reach goal. """ assert lin_err >= 0.0 assert heading_err >= 0.0 - return w_max * lin_err / (np.sin(heading_err) + 1e-5) / 2.0 + v_projected_max = w_max * max(lin_err - dead_zone, 0.0) + return v_projected_max * np.sin(heading_err) def _integrate_state(self, v, w): """ @@ -124,7 +125,9 @@ def _run(self): lin_err_abs, ACC_LIN, tol=self.lin_error_tol, use_acc=self.use_loc ) k_p = self._projection_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) - v_limit = self._turn_rate_limit(self.w_max, lin_err_abs, heading_err_abs) + v_limit = self._turn_rate_limit( + self.w_max / 2.0, lin_err_abs, heading_err_abs, dead_zone=self.lin_error_tol + ) v_cmd = min(k_t * k_p * self.v_max, v_limit) # Compute angular velocity From 3eca7df156dadfdf083bce34c04259773dc53a91 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 03:13:25 -0700 Subject: [PATCH 28/34] Revamp control --- .../hello_robot/remote/goto_controller.py | 55 +++++++++---------- 1 file changed, 27 insertions(+), 28 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 26194c2046..cd12b038cf 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -43,23 +43,19 @@ def __init__( self.track_yaw = True @staticmethod - def _error_velocity_multiplier(x_err, a, tol=0.0, use_acc=True): + def _velocity_feedback_control(x_err, a, v_max, tol=0.0): """ - Computes velocity multiplier based on distance from target. + Computes velocity based on distance from target. Used for both linear and angular motion. - Current implementation: Simple thresholding - Output = 1 if linear error is larger than the tolerance, 0 otherwise. + Current implementation: Trapezoidal velocity profile """ - assert x_err >= 0.0 - if use_acc: - t = np.sqrt(2.0 * max(x_err - tol, 0.0) / a) # x_err = (1/2) * a * t^2 - return min(a * t, 1.0) - else: - return float(x_err > tol) + t = np.sqrt(2.0 * max(abs(x_err) - tol, 0.0) / a) # x_err = (1/2) * a * t^2 + v = min(a * t, v_max) + return v * np.sign(x_err) @staticmethod - def _projection_velocity_multiplier(theta_err, tol=0.0): + def _projection_velocity_multiplier(theta_diff, tol=0.0): """ Compute velocity muliplier based on yaw (faster if facing towards target). Used to control linear motion. @@ -67,18 +63,18 @@ def _projection_velocity_multiplier(theta_err, tol=0.0): Current implementation: Output = 1 when facing target, gradually decreases to 0 when angle to target is pi/3. """ - assert theta_err >= 0.0 - return 1.0 - np.sin(max(theta_err - tol, 0.0)) + assert theta_diff >= 0.0 + return 1.0 - np.sin(max(theta_diff - tol, 0.0)) @staticmethod - def _turn_rate_limit(w_max, lin_err, heading_err, dead_zone=0.0): + def _turn_rate_limit(w_max, lin_err, heading_diff, dead_zone=0.0): """ Computed velocity limit based on the turning radius required to reach goal. """ assert lin_err >= 0.0 - assert heading_err >= 0.0 + assert heading_diff >= 0.0 v_projected_max = w_max * max(lin_err - dead_zone, 0.0) - return v_projected_max * np.sin(heading_err) + return v_projected_max * np.sin(heading_diff) def _integrate_state(self, v, w): """ @@ -113,7 +109,6 @@ def _run(self): lin_err_abs = np.linalg.norm(self.xyt_err[0:2]) ang_err = self.xyt_err[2] - ang_err_abs = abs(ang_err) # Go to goal XY position if not there yet if lin_err_abs > self.lin_error_tol: @@ -121,28 +116,32 @@ def _run(self): heading_err_abs = abs(heading_err) # Compute linear velocity - k_t = self._error_velocity_multiplier( - lin_err_abs, ACC_LIN, tol=self.lin_error_tol, use_acc=self.use_loc + v_raw = self._velocity_feedback_control( + lin_err_abs, ACC_LIN, self.v_max, tol=self.lin_error_tol, use_acc=self.use_loc + ) + k_proj = self._projection_velocity_multiplier( + heading_err_abs, tol=self.ang_error_tol ) - k_p = self._projection_velocity_multiplier(heading_err_abs, tol=self.ang_error_tol) v_limit = self._turn_rate_limit( self.w_max / 2.0, lin_err_abs, heading_err_abs, dead_zone=self.lin_error_tol ) - v_cmd = min(k_t * k_p * self.v_max, v_limit) + v_cmd = min(k_proj * v_raw, v_limit) # Compute angular velocity - k_t_ang = self._error_velocity_multiplier( - heading_err_abs, ACC_ANG, tol=self.ang_error_tol / 2.0, use_acc=self.use_loc + w_cmd = self._velocity_feedback_control( + heading_err, + ACC_ANG, + self.w_max, + tol=self.ang_error_tol / 2.0, + use_acc=self.use_loc, ) - w_cmd = np.sign(heading_err) * k_t_ang * self.w_max # Rotate to correct yaw if yaw tracking is on and XY position is at goal - elif ang_err_abs > self.ang_error_tol: + elif abs(ang_err) > self.ang_error_tol: # Compute angular velocity - k_t_ang = self._error_velocity_multiplier( - ang_err_abs, ACC_ANG, tol=self.ang_error_tol, use_acc=self.use_loc + w_cmd = self._velocity_feedback_control( + ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol, use_acc=self.use_loc ) - w_cmd = np.sign(ang_err) * k_t_ang * self.w_max # Command robot with self.control_lock: From 3ba9c78f5b4c6b92ccbe1c8663f1ccc2b80860a0 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 03:20:01 -0700 Subject: [PATCH 29/34] Tune hyperparams --- droidlet/lowlevel/hello_robot/remote/goto_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index cd12b038cf..8bb0e9a426 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -9,8 +9,8 @@ V_MAX_DEFAULT = 0.2 # base.params["motion"]["default"]["vel_m"] W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m -ACC_LIN = 1.6 # 4 * (base.params["motion"]["max"]["accel_m"]) -ACC_ANG = 4.8 # 2 * (2 * (accel_m_max - accel_m_max) / wheel_separation_m) +ACC_LIN = 0.4 # base.params["motion"]["max"]["accel_m"] +ACC_ANG = 1.2 # (accel_m_max - accel_m_default) / wheel_separation_m class GotoVelocityController: From 42eff4c936d730547727547a69098652e39e613a Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 10:49:51 -0700 Subject: [PATCH 30/34] Debug --- droidlet/lowlevel/hello_robot/remote/goto_controller.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 8bb0e9a426..d5486876c4 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -73,8 +73,8 @@ def _turn_rate_limit(w_max, lin_err, heading_diff, dead_zone=0.0): """ assert lin_err >= 0.0 assert heading_diff >= 0.0 - v_projected_max = w_max * max(lin_err - dead_zone, 0.0) - return v_projected_max * np.sin(heading_diff) + dist_projected = lin_err * np.sin(heading_diff) + return w_max * max(dist_projected - dead_zone, 0.0) def _integrate_state(self, v, w): """ @@ -117,7 +117,7 @@ def _run(self): # Compute linear velocity v_raw = self._velocity_feedback_control( - lin_err_abs, ACC_LIN, self.v_max, tol=self.lin_error_tol, use_acc=self.use_loc + lin_err_abs, ACC_LIN, self.v_max, tol=self.lin_error_tol ) k_proj = self._projection_velocity_multiplier( heading_err_abs, tol=self.ang_error_tol @@ -133,14 +133,13 @@ def _run(self): ACC_ANG, self.w_max, tol=self.ang_error_tol / 2.0, - use_acc=self.use_loc, ) # Rotate to correct yaw if yaw tracking is on and XY position is at goal elif abs(ang_err) > self.ang_error_tol: # Compute angular velocity w_cmd = self._velocity_feedback_control( - ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol, use_acc=self.use_loc + ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol ) # Command robot From 667e827eaf1058c89b33d658c8a5c0e6da0c228a Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 11:05:34 -0700 Subject: [PATCH 31/34] Revamp localization --- .../hello_robot/remote/goto_controller.py | 50 ++++++------------- 1 file changed, 15 insertions(+), 35 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index d5486876c4..15f78a54ed 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -20,12 +20,10 @@ def __init__( hz: float, v_max: Optional[float] = None, w_max: Optional[float] = None, - use_localization: bool = True, ): self.robot = robot self.hz = hz self.dt = 1.0 / self.hz - self.use_loc = use_localization # Params self.v_max = v_max or V_MAX_DEFAULT @@ -39,7 +37,7 @@ def __init__( self.active = False self.xyt_loc = self.robot.get_estimator_pose() - self.xyt_err = np.zeros(3) + self.xyt_goal = self.xyt_loc self.track_yaw = True @staticmethod @@ -76,43 +74,31 @@ def _turn_rate_limit(w_max, lin_err, heading_diff, dead_zone=0.0): dist_projected = lin_err * np.sin(heading_diff) return w_max * max(dist_projected - dead_zone, 0.0) - def _integrate_state(self, v, w): - """ - Predict error in the next timestep with current commanded velocity - Deprecated in favor of _update_error_state. - """ - dx = v * self.dt - dtheta = w * self.dt - - xyt_goal_global = transform_base_to_global(self.xyt_err, np.zeros(3)) - xyt_new = np.array([dx * np.cos(dtheta / 2.0), dx * np.sin(dtheta / 2.0), dtheta]) - self.xyt_err = transform_global_to_base(xyt_goal_global, xyt_new) - - def _update_error_state(self): + def _compute_error_pose(self): """ Updates error based on robot localization """ xyt_loc_new = self.robot.get_estimator_pose() - # Update error - xyt_goal_global = transform_base_to_global(self.xyt_err, self.xyt_loc) - self.xyt_err = transform_global_to_base(xyt_goal_global, xyt_loc_new) + xyt_err = transform_global_to_base(self.xyt_goal, xyt_loc_new) + if self.track_yaw: + xyt_err[2] = 0.0 - # Update state - self.xyt_loc = xyt_loc_new + return xyt_err def _run(self): rate = rospy.Rate(self.hz) while True: v_cmd = w_cmd = 0 + xyt_err = self._compute_error_pose() - lin_err_abs = np.linalg.norm(self.xyt_err[0:2]) - ang_err = self.xyt_err[2] + lin_err_abs = np.linalg.norm(xyt_err[0:2]) + ang_err = xyt_err[2] # Go to goal XY position if not there yet if lin_err_abs > self.lin_error_tol: - heading_err = np.arctan2(self.xyt_err[1], self.xyt_err[0]) + heading_err = np.arctan2(xyt_err[1], xyt_err[0]) heading_err_abs = abs(heading_err) # Compute linear velocity @@ -146,21 +132,17 @@ def _run(self): with self.control_lock: self.robot.set_velocity(v_cmd, w_cmd) - # Update error - if self.use_loc: - self._update_error_state() - else: - self._integrate_state(v_cmd, w_cmd) - # Spin rate.sleep() def check_at_goal(self) -> bool: - xy_fulfilled = np.linalg.norm(self.xyt_err[0:2]) <= self.lin_error_tol + xyt_err = self._compute_error_pose() + + xy_fulfilled = np.linalg.norm(xyt_err[0:2]) <= self.lin_error_tol t_fulfilled = True if self.track_yaw: - t_fulfilled = abs(self.xyt_err[2]) <= self.ang_error_tol + t_fulfilled = abs(xyt_err[2]) <= self.ang_error_tol return xy_fulfilled and t_fulfilled @@ -168,9 +150,7 @@ def set_goal( self, xyt_position: List[float], ): - self.xyt_err = xyt_position - if not self.track_yaw: - self.xyt_err[2] = 0.0 + self.xyt_goal = xyt_position def enable_yaw_tracking(self, value: bool = True): self.track_yaw = value From a8f8e178ab1fddc5e6098a69cb899f1440e70b8d Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 11:56:54 -0700 Subject: [PATCH 32/34] Tune controller --- .../hello_robot/remote/goto_controller.py | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 15f78a54ed..daab4c06eb 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -9,8 +9,8 @@ V_MAX_DEFAULT = 0.2 # base.params["motion"]["default"]["vel_m"] W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m -ACC_LIN = 0.4 # base.params["motion"]["max"]["accel_m"] -ACC_ANG = 1.2 # (accel_m_max - accel_m_default) / wheel_separation_m +ACC_LIN = 0.1 # 0.25 * base.params["motion"]["max"]["accel_m"] +ACC_ANG = 0.3 # 0.25 * (accel_m_max - accel_m_default) / wheel_separation_m class GotoVelocityController: @@ -41,16 +41,19 @@ def __init__( self.track_yaw = True @staticmethod - def _velocity_feedback_control(x_err, a, v_max, tol=0.0): + def _velocity_feedback_control(x_err, a, v_max, tol=0.0, use_acc=True): """ Computes velocity based on distance from target. Used for both linear and angular motion. Current implementation: Trapezoidal velocity profile """ - t = np.sqrt(2.0 * max(abs(x_err) - tol, 0.0) / a) # x_err = (1/2) * a * t^2 - v = min(a * t, v_max) - return v * np.sign(x_err) + if use_acc: + t = np.sqrt(2.0 * max(abs(x_err) - tol, 0.0) / a) # x_err = (1/2) * a * t^2 + v = min(a * t, v_max) + return v * np.sign(x_err) + else: + return np.sign(x_err) * (abs(x_err) > tol) * v_max @staticmethod def _projection_velocity_multiplier(theta_diff, tol=0.0): @@ -65,7 +68,7 @@ def _projection_velocity_multiplier(theta_diff, tol=0.0): return 1.0 - np.sin(max(theta_diff - tol, 0.0)) @staticmethod - def _turn_rate_limit(w_max, lin_err, heading_diff, dead_zone=0.0): + def _turn_rate_limit(lin_err, heading_diff, w_max, dead_zone=0.0): """ Computed velocity limit based on the turning radius required to reach goal. """ @@ -109,23 +112,23 @@ def _run(self): heading_err_abs, tol=self.ang_error_tol ) v_limit = self._turn_rate_limit( - self.w_max / 2.0, lin_err_abs, heading_err_abs, dead_zone=self.lin_error_tol + lin_err_abs, + heading_err_abs, + self.w_max / 2.0, + dead_zone=2.0 * self.lin_error_tol, ) v_cmd = min(k_proj * v_raw, v_limit) # Compute angular velocity w_cmd = self._velocity_feedback_control( - heading_err, - ACC_ANG, - self.w_max, - tol=self.ang_error_tol / 2.0, + heading_err, ACC_ANG, self.w_max, tol=self.ang_error_tol / 2.0, use_acc=False ) # Rotate to correct yaw if yaw tracking is on and XY position is at goal elif abs(ang_err) > self.ang_error_tol: # Compute angular velocity w_cmd = self._velocity_feedback_control( - ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol + ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol, use_acc=False ) # Command robot From 616f04591c98f6d0a7b24223782c209071d08e06 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Fri, 28 Oct 2022 13:26:06 -0700 Subject: [PATCH 33/34] Debug --- droidlet/lowlevel/hello_robot/remote/goto_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index daab4c06eb..1901d3b69b 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -84,7 +84,7 @@ def _compute_error_pose(self): xyt_loc_new = self.robot.get_estimator_pose() xyt_err = transform_global_to_base(self.xyt_goal, xyt_loc_new) - if self.track_yaw: + if not self.track_yaw: xyt_err[2] = 0.0 return xyt_err From 304a10e6919c79a646bab441020a5d360bf5efc1 Mon Sep 17 00:00:00 2001 From: ExhAustin Date: Wed, 2 Nov 2022 14:35:28 -0700 Subject: [PATCH 34/34] Revamp velocity controller with tested diff drive vel controller --- .../hello_robot/remote/goto_controller.py | 174 +++++++++--------- 1 file changed, 92 insertions(+), 82 deletions(-) diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py index 1901d3b69b..f9f1024c7b 100644 --- a/droidlet/lowlevel/hello_robot/remote/goto_controller.py +++ b/droidlet/lowlevel/hello_robot/remote/goto_controller.py @@ -9,27 +9,105 @@ V_MAX_DEFAULT = 0.2 # base.params["motion"]["default"]["vel_m"] W_MAX_DEFAULT = 0.45 # (vel_m_max - vel_m_default) / wheel_separation_m -ACC_LIN = 0.1 # 0.25 * base.params["motion"]["max"]["accel_m"] -ACC_ANG = 0.3 # 0.25 * (accel_m_max - accel_m_default) / wheel_separation_m +ACC_LIN = 0.4 # base.params["motion"]["max"]["accel_m"] +ACC_ANG = 1.2 # (accel_m_max - accel_m_default) / wheel_separation_m +MAX_HEADING_ANG = np.pi / 4 + + +class DiffDriveVelocityControl: + """ + Control logic for differential drive robot velocity control + """ + + def __init__(self, hz): + self.v_max = V_MAX_DEFAULT + self.w_max = W_MAX_DEFAULT + self.lin_error_tol = self.v_max / hz + self.ang_error_tol = self.w_max / hz + + @staticmethod + def _velocity_feedback_control(x_err, a, v_max): + """ + Computes velocity based on distance from target (trapezoidal velocity profile). + Used for both linear and angular motion. + """ + t = np.sqrt(2.0 * abs(x_err) / a) # x_err = (1/2) * a * t^2 + v = min(a * t, v_max) + return v * np.sign(x_err) + + @staticmethod + def _turn_rate_limit(lin_err, heading_diff, w_max, tol=0.0): + """ + Compute velocity limit that prevents path from overshooting goal + + heading error decrease rate > linear error decrease rate + (w - v * np.sin(phi) / D) / phi > v * np.cos(phi) / D + v < (w / phi) / (np.sin(phi) / D / phi + np.cos(phi) / D) + v < w * D / (np.sin(phi) + phi * np.cos(phi)) + + (D = linear error, phi = angular error) + """ + assert lin_err >= 0.0 + assert heading_diff >= 0.0 + + if heading_diff > MAX_HEADING_ANG: + return 0.0 + else: + return ( + w_max + * lin_err + / (np.sin(heading_diff) + heading_diff * np.cos(heading_diff) + 1e-5) + ) + + def __call__(self, xyt_err): + v_cmd = w_cmd = 0 + + lin_err_abs = np.linalg.norm(xyt_err[0:2]) + ang_err = xyt_err[2] + + # Go to goal XY position if not there yet + if lin_err_abs > self.lin_error_tol: + heading_err = np.arctan2(xyt_err[1], xyt_err[0]) + heading_err_abs = abs(heading_err) + + # Compute linear velocity + v_raw = self._velocity_feedback_control(lin_err_abs, ACC_LIN, self.v_max) + v_limit = self._turn_rate_limit( + lin_err_abs, + heading_err_abs, + self.w_max / 2.0, + tol=self.lin_error_tol, + ) + v_cmd = np.clip(v_raw, 0.0, v_limit) + + # Compute angular velocity + w_cmd = self._velocity_feedback_control(heading_err, ACC_ANG, self.w_max) + + # Rotate to correct yaw if XY position is at goal + elif abs(ang_err) > self.ang_error_tol: + # Compute angular velocity + w_cmd = self._velocity_feedback_control(ang_err, ACC_ANG, self.w_max) + + return v_cmd, w_cmd class GotoVelocityController: + """ + Self-contained controller module for moving a diff drive robot to a target goal. + Target goal is update-able at any given instant. + """ + def __init__( self, robot, hz: float, - v_max: Optional[float] = None, - w_max: Optional[float] = None, ): self.robot = robot self.hz = hz self.dt = 1.0 / self.hz - # Params - self.v_max = v_max or V_MAX_DEFAULT - self.w_max = w_max or W_MAX_DEFAULT - self.lin_error_tol = 2 * self.v_max / hz - self.ang_error_tol = 2 * self.w_max / hz + # Control module + self.control = DiffDriveVelocityControl(hz) # Initialize self.loop_thr = None @@ -40,43 +118,6 @@ def __init__( self.xyt_goal = self.xyt_loc self.track_yaw = True - @staticmethod - def _velocity_feedback_control(x_err, a, v_max, tol=0.0, use_acc=True): - """ - Computes velocity based on distance from target. - Used for both linear and angular motion. - - Current implementation: Trapezoidal velocity profile - """ - if use_acc: - t = np.sqrt(2.0 * max(abs(x_err) - tol, 0.0) / a) # x_err = (1/2) * a * t^2 - v = min(a * t, v_max) - return v * np.sign(x_err) - else: - return np.sign(x_err) * (abs(x_err) > tol) * v_max - - @staticmethod - def _projection_velocity_multiplier(theta_diff, tol=0.0): - """ - Compute velocity muliplier based on yaw (faster if facing towards target). - Used to control linear motion. - - Current implementation: - Output = 1 when facing target, gradually decreases to 0 when angle to target is pi/3. - """ - assert theta_diff >= 0.0 - return 1.0 - np.sin(max(theta_diff - tol, 0.0)) - - @staticmethod - def _turn_rate_limit(lin_err, heading_diff, w_max, dead_zone=0.0): - """ - Computed velocity limit based on the turning radius required to reach goal. - """ - assert lin_err >= 0.0 - assert heading_diff >= 0.0 - dist_projected = lin_err * np.sin(heading_diff) - return w_max * max(dist_projected - dead_zone, 0.0) - def _compute_error_pose(self): """ Updates error based on robot localization @@ -93,47 +134,16 @@ def _run(self): rate = rospy.Rate(self.hz) while True: - v_cmd = w_cmd = 0 + # Get state estimation xyt_err = self._compute_error_pose() - lin_err_abs = np.linalg.norm(xyt_err[0:2]) - ang_err = xyt_err[2] - - # Go to goal XY position if not there yet - if lin_err_abs > self.lin_error_tol: - heading_err = np.arctan2(xyt_err[1], xyt_err[0]) - heading_err_abs = abs(heading_err) - - # Compute linear velocity - v_raw = self._velocity_feedback_control( - lin_err_abs, ACC_LIN, self.v_max, tol=self.lin_error_tol - ) - k_proj = self._projection_velocity_multiplier( - heading_err_abs, tol=self.ang_error_tol - ) - v_limit = self._turn_rate_limit( - lin_err_abs, - heading_err_abs, - self.w_max / 2.0, - dead_zone=2.0 * self.lin_error_tol, - ) - v_cmd = min(k_proj * v_raw, v_limit) - - # Compute angular velocity - w_cmd = self._velocity_feedback_control( - heading_err, ACC_ANG, self.w_max, tol=self.ang_error_tol / 2.0, use_acc=False - ) - - # Rotate to correct yaw if yaw tracking is on and XY position is at goal - elif abs(ang_err) > self.ang_error_tol: - # Compute angular velocity - w_cmd = self._velocity_feedback_control( - ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol, use_acc=False - ) + # Compute control + v_cmd, w_cmd = self.control(xyt_err) # Command robot with self.control_lock: - self.robot.set_velocity(v_cmd, w_cmd) + if self.active: + self.robot.set_velocity(v_cmd, w_cmd) # Spin rate.sleep()