From 990aba69699eefd952e00a347af41ca8596d7068 Mon Sep 17 00:00:00 2001 From: Simon Stelter Date: Fri, 29 Nov 2024 14:06:38 +0100 Subject: [PATCH 1/2] fixed bug, where control/mpc dt couldn't be properly set --- giskardpy/debug_expression_manager.py | 2 +- giskardpy/goals/base_traj_follower.py | 16 ++-- giskardpy/goals/collision_avoidance.py | 4 +- giskardpy/model/joints.py | 32 +++---- giskardpy/model/trajectory.py | 2 +- giskardpy/model/world.py | 4 +- giskardpy/model/world_config.py | 18 ++-- giskardpy/qp/free_variable.py | 9 +- giskardpy/qp/qp_controller.py | 120 ++++++++++--------------- giskardpy/qp/qp_controller_config.py | 11 ++- giskardpy/utils/math.py | 49 ++++++++-- test/test_giskard_library.py | 12 +-- test/test_math.py | 2 +- 13 files changed, 149 insertions(+), 132 deletions(-) diff --git a/giskardpy/debug_expression_manager.py b/giskardpy/debug_expression_manager.py index 925bf9b091..355be62ba7 100644 --- a/giskardpy/debug_expression_manager.py +++ b/giskardpy/debug_expression_manager.py @@ -94,7 +94,7 @@ def evaluated_expr_to_js(self, name, last_js, next_js: JointStates, value): else: velocity = 0 next_js[name].position = value - next_js[name].velocity = velocity / god_map.qp_controller.sample_period + next_js[name].velocity = velocity / god_map.qp_controller.mpc_dt def to_pandas(self): p_debug = {} diff --git a/giskardpy/goals/base_traj_follower.py b/giskardpy/goals/base_traj_follower.py index 19ceff2f12..4c7dd6cfa8 100644 --- a/giskardpy/goals/base_traj_follower.py +++ b/giskardpy/goals/base_traj_follower.py @@ -45,7 +45,7 @@ def current_traj_point(self, free_variable_name: PrefixName, start_t: float, time = symbol_manager.time b_result_cases = [] for t in range(self.trajectory_length): - b = t * god_map.qp_controller.sample_period + b = t * god_map.qp_controller.mpc_dt eq_result = self.x_symbol(t, free_variable_name, derivative) b_result_cases.append((b, eq_result)) # FIXME if less eq cases behavior changed @@ -81,7 +81,7 @@ def trans_error_at(self, t_in_s: float): frame_P_goal = map_T_base_footprint_goal.to_position() frame_P_current = map_T_base_footprint_current.to_position() - error = (frame_P_goal - frame_P_current) / god_map.qp_controller.sample_period + error = (frame_P_goal - frame_P_current) / god_map.qp_controller.mpc_dt return error[0], error[1] @profile @@ -91,10 +91,10 @@ def add_trans_constraints(self): map_T_base_footprint = god_map.world.compose_fk_expression(god_map.world.root_link_name, self.base_footprint_link) for t in range(god_map.qp_controller.prediction_horizon): - x = self.current_traj_point(self.joint.x_vel.name, t * god_map.qp_controller.sample_period, + x = self.current_traj_point(self.joint.x_vel.name, t * god_map.qp_controller.mpc_dt, Derivatives.velocity) if isinstance(self.joint, OmniDrive): - y = self.current_traj_point(self.joint.y_vel.name, t * god_map.qp_controller.sample_period, + y = self.current_traj_point(self.joint.y_vel.name, t * god_map.qp_controller.mpc_dt, Derivatives.velocity) else: y = 0 @@ -132,14 +132,14 @@ def rot_error_at(self, t_in_s: int): rotation_goal = self.current_traj_point(self.joint.yaw.name, t_in_s) rotation_current = self.joint.yaw.get_symbol(Derivatives.position) error = cas.shortest_angular_distance(rotation_current, - rotation_goal) / god_map.qp_controller.sample_period + rotation_goal) / god_map.qp_controller.mpc_dt return error @profile def add_rot_constraints(self): errors = [] for t in range(god_map.qp_controller.prediction_horizon): - errors.append(self.current_traj_point(self.joint.yaw.name, t * god_map.qp_controller.sample_period, + errors.append(self.current_traj_point(self.joint.yaw.name, t * god_map.qp_controller.mpc_dt, Derivatives.velocity)) if t == 0 and not self.track_only_velocity: errors[-1] += self.rot_error_at(t) @@ -168,7 +168,7 @@ def add_trans_constraints(self): self.add_debug_expr(f'map_P_current.x', map_P_current.x) self.add_debug_expr('time', god_map.to_expr(identifier.time)) for t in range(god_map.qp_controller.prediction_horizon - 2): - trajectory_time_in_s = t * god_map.qp_controller.sample_period + trajectory_time_in_s = t * god_map.qp_controller.mpc_dt map_P_goal = self.make_map_T_base_footprint_goal(trajectory_time_in_s).to_position() map_V_error = (map_P_goal - map_P_current) self.add_debug_expr(f'map_P_goal.x/{t}', map_P_goal.x) @@ -197,7 +197,7 @@ def add_trans_constraints(self): # yaw1_goal_position = self.current_traj_point(self.joint.yaw1_vel.name, trajectory_time_in_s, # Derivatives.position) forward = self.current_traj_point(self.joint.forward_vel.name, - t * god_map.qp_controller.sample_period, + t * god_map.qp_controller.mpc_dt, Derivatives.velocity) * 1.1 lb_forward.append(forward) weight_vel = WEIGHT_ABOVE_CA diff --git a/giskardpy/goals/collision_avoidance.py b/giskardpy/goals/collision_avoidance.py index fade363f67..343cdfdfbc 100644 --- a/giskardpy/goals/collision_avoidance.py +++ b/giskardpy/goals/collision_avoidance.py @@ -48,7 +48,7 @@ def __init__(self, a_P_pa = self.get_closest_point_on_a_in_a() map_V_n = self.map_V_n_symbol() actual_distance = self.get_actual_distance() - sample_period = god_map.qp_controller.sample_period + sample_period = god_map.qp_controller.mpc_dt number_of_external_collisions = self.get_number_of_external_collisions() map_T_a = god_map.world.compose_fk_expression(self.root, self.link_name) @@ -175,7 +175,7 @@ def __init__(self, hard_threshold = cas.min(self.hard_threshold, self.soft_threshold / 2) actual_distance = self.get_actual_distance() number_of_self_collisions = self.get_number_of_self_collisions() - sample_period = god_map.qp_controller.sample_period + sample_period = god_map.qp_controller.mpc_dt # b_T_a2 = god_map.get_world().compose_fk_evaluated_expression(self.link_b, self.link_a) b_T_a = god_map.world.compose_fk_expression(self.link_b, self.link_a) diff --git a/giskardpy/model/joints.py b/giskardpy/model/joints.py index 544518eacb..5db101db6d 100644 --- a/giskardpy/model/joints.py +++ b/giskardpy/model/joints.py @@ -351,7 +351,7 @@ def __init__(self, self.translation_limits = { Derivatives.velocity: 0.5, Derivatives.acceleration: np.inf, - Derivatives.jerk: 5 + Derivatives.jerk: None } else: self.translation_limits = translation_limits @@ -360,7 +360,7 @@ def __init__(self, self.rotation_limits = { Derivatives.velocity: 0.6, Derivatives.acceleration: np.inf, - Derivatives.jerk: 10 + Derivatives.jerk: None } else: self.rotation_limits = rotation_limits @@ -383,8 +383,8 @@ def create_parent_T_child(self) -> None: self.parent_T_child = odom_T_bf.dot(bf_T_bf_vel).dot(bf_vel_T_bf) def create_free_variables(self) -> None: - translation_lower_limits = {derivative: -limit for derivative, limit in self.translation_limits.items()} - rotation_lower_limits = {derivative: -limit for derivative, limit in self.rotation_limits.items()} + translation_lower_limits = {derivative: -limit if limit is not None else None for derivative, limit in self.translation_limits.items()} + rotation_lower_limits = {derivative: -limit if limit is not None else None for derivative, limit in self.rotation_limits.items()} self.x = god_map.world.add_virtual_free_variable(name=self.x_name) self.y = god_map.world.add_virtual_free_variable(name=self.y_name) @@ -452,8 +452,8 @@ def __init__(self, if translation_limits is None: self.translation_limits = { Derivatives.velocity: 0.5, - Derivatives.acceleration: 1000, - Derivatives.jerk: 5 + Derivatives.acceleration: np.inf, + Derivatives.jerk: None } else: self.translation_limits = translation_limits @@ -461,8 +461,8 @@ def __init__(self, if rotation_limits is None: self.rotation_limits = { Derivatives.velocity: 0.6, - Derivatives.acceleration: 1000, - Derivatives.jerk: 10 + Derivatives.acceleration: np.inf, + Derivatives.jerk: None } else: self.rotation_limits = rotation_limits @@ -543,8 +543,8 @@ def __init__(self, if translation_limits is None: self.translation_limits = { Derivatives.velocity: 0.5, - Derivatives.acceleration: 1000, - Derivatives.jerk: 5 + Derivatives.acceleration: np.inf, + Derivatives.jerk: None } else: self.translation_limits = translation_limits @@ -552,8 +552,8 @@ def __init__(self, if rotation_limits is None: self.rotation_limits = { Derivatives.velocity: 0.6, - Derivatives.acceleration: 1000, - Derivatives.jerk: 10 + Derivatives.acceleration: np.inf, + Derivatives.jerk: None } else: self.rotation_limits = rotation_limits @@ -569,13 +569,13 @@ def create_free_variables(self) -> None: rotation_lower_limits = {derivative: -limit for derivative, limit in self.rotation_limits.items()} caster_upper_limits = { Derivatives.velocity: 100, - Derivatives.acceleration: 1000, - Derivatives.jerk: 100, + Derivatives.acceleration: np.inf, + Derivatives.jerk: None, } caster_lower_limits = { Derivatives.velocity: -100, - Derivatives.acceleration: -1000, - Derivatives.jerk: -100, + Derivatives.acceleration: -np.inf, + Derivatives.jerk: None, } self.x = god_map.world.add_virtual_free_variable(name=PrefixName('x', self.name)) diff --git a/giskardpy/model/trajectory.py b/giskardpy/model/trajectory.py index 386f8611f7..247122e8be 100644 --- a/giskardpy/model/trajectory.py +++ b/giskardpy/model/trajectory.py @@ -66,7 +66,7 @@ def values(self): @property def length_in_seconds(self) -> float: - return len(self) * god_map.qp_controller.sample_period + return len(self) * god_map.qp_controller.mpc_dt def to_dict(self, normalize_position: bool = False, filter_0_vel: bool = True) -> Dict[ Derivatives, Dict[PrefixName, np.ndarray]]: diff --git a/giskardpy/model/world.py b/giskardpy/model/world.py index f12fed1064..10986cc3f5 100644 --- a/giskardpy/model/world.py +++ b/giskardpy/model/world.py @@ -210,7 +210,7 @@ def update_default_limits(self, new_limits: Dict[Derivatives, float]): assert len(self._default_limits) == max(self._default_limits) for v in self.free_variables.values(): for d, new_limit in new_limits.items(): - v.set_lower_limit(d, -new_limit) + v.set_lower_limit(d, -new_limit if new_limit is not None else None) v.set_upper_limit(d, new_limit) def update_default_weights(self, new_weights: Dict[Derivatives, float]): @@ -666,7 +666,7 @@ def recursive_parser(urdf: up.Robot, parent_link: Link): joint = Joint.from_urdf(urdf_joint, group_name) if not isinstance(joint, FixedJoint): for derivative, limit in self.default_limits.items(): - joint.free_variable.set_lower_limit(derivative, -limit) + joint.free_variable.set_lower_limit(derivative, -limit if limit is not None else None) joint.free_variable.set_upper_limit(derivative, limit) self.add_joint(joint) diff --git a/giskardpy/model/world_config.py b/giskardpy/model/world_config.py index a10a32d06d..ddb6a10bf6 100644 --- a/giskardpy/model/world_config.py +++ b/giskardpy/model/world_config.py @@ -19,7 +19,7 @@ class WorldConfig(ABC): _default_limits = { Derivatives.velocity: 1, Derivatives.acceleration: np.inf, - Derivatives.jerk: 711 + Derivatives.jerk: None } def __init__(self, register_on_god_map: bool = True): @@ -72,7 +72,7 @@ def set_joint_limits(self, limit_map: derivative_map, joint_name: my_string, gro raise ValueError(f'Can\'t change limits because {joint_name} is not of type {str(OneDofJoint)}.') free_variable = self.world.free_variables[joint.free_variable.name] for derivative, limit in limit_map.items(): - free_variable.set_lower_limit(derivative, -limit) + free_variable.set_lower_limit(derivative, -limit if limit is not None else None) free_variable.set_upper_limit(derivative, limit) def set_default_color(self, r: float, g: float, b: float, a: float): @@ -207,7 +207,7 @@ def setup(self): self._default_limits = { Derivatives.velocity: 1, Derivatives.acceleration: np.inf, - Derivatives.jerk: 711 + Derivatives.jerk: None } self.set_default_limits(self._default_limits) self.add_empty_link(PrefixName('map')) @@ -244,7 +244,7 @@ def __init__(self, def setup(self, robot_name: Optional[str] = None): self.set_default_limits({Derivatives.velocity: 1, Derivatives.acceleration: np.inf, - Derivatives.jerk: 711}) + Derivatives.jerk: None}) self.add_empty_link(PrefixName(self.map_name)) self.add_empty_link(PrefixName(self.odom_link_name)) self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, @@ -257,12 +257,12 @@ def setup(self, robot_name: Optional[str] = None): translation_limits={ Derivatives.velocity: 0.2, Derivatives.acceleration: np.inf, - Derivatives.jerk: 142, + Derivatives.jerk: None, }, rotation_limits={ Derivatives.velocity: 0.2, Derivatives.acceleration: np.inf, - Derivatives.jerk: 142 + Derivatives.jerk: None }, robot_group_name=self.robot_group_name) @@ -289,7 +289,7 @@ def __init__(self, def setup(self): self.set_default_limits({Derivatives.velocity: 1, Derivatives.acceleration: np.inf, - Derivatives.jerk: 711}) + Derivatives.jerk: None}) self.add_empty_link(PrefixName(self.map_name)) self.add_empty_link(PrefixName(self.odom_link_name)) self.add_6dof_joint(parent_link=self.map_name, child_link=self.odom_link_name, @@ -302,11 +302,11 @@ def setup(self): translation_limits={ Derivatives.velocity: 0.2, Derivatives.acceleration: np.inf, - Derivatives.jerk: 142, + Derivatives.jerk: None, }, rotation_limits={ Derivatives.velocity: 0.2, Derivatives.acceleration: np.inf, - Derivatives.jerk: 142 + Derivatives.jerk: None }, robot_group_name=self.robot_group_name) diff --git a/giskardpy/qp/free_variable.py b/giskardpy/qp/free_variable.py index 1e80484555..3cbb319c2e 100644 --- a/giskardpy/qp/free_variable.py +++ b/giskardpy/qp/free_variable.py @@ -137,6 +137,8 @@ def get_lower_limit(self, derivative: Derivatives, default: bool = False, evalua else: raise KeyError(f'Free variable {self} doesn\'t have lower limit for derivative of order {derivative}') if evaluated: + if expr is None: + return None return float(symbol_manager.evaluate_expr(expr)) return expr @@ -158,6 +160,8 @@ def get_upper_limit(self, derivative: Derivatives, default: bool = False, evalua else: raise KeyError(f'Free variable {self} doesn\'t have upper limit for derivative of order {derivative}') if evaluated: + if expr is None: + return None return symbol_manager.evaluate_expr(expr) return expr @@ -185,8 +189,11 @@ def has_position_limits(self) -> bool: @profile def normalized_weight(self, t: int, derivative: Derivatives, prediction_horizon: int, alpha: float = 0.1, evaluated: bool = False) -> Union[Union[cas.Symbol, float], float]: + limit = self.get_upper_limit(derivative) + if limit is None: + return 0.0 weight = symbol_manager.evaluate_expr(self.quadratic_weights[derivative]) - limit = symbol_manager.evaluate_expr(self.get_upper_limit(derivative)) + limit = symbol_manager.evaluate_expr(limit) # weight = my_cycloid(t, weight, prediction_horizon, alpha) # -0.6975960014626146 # weight = my_cycloid2(t, weight, prediction_horizon, alpha, q=2) # -0.7023704675764478 diff --git a/giskardpy/qp/qp_controller.py b/giskardpy/qp/qp_controller.py index 1679d8a360..b07cda36db 100644 --- a/giskardpy/qp/qp_controller.py +++ b/giskardpy/qp/qp_controller.py @@ -206,29 +206,6 @@ def _remove_columns_columns_where_variables_are_zero(self, free_variable_model: free_variable_model.remove([], column_ids) return free_variable_model - @memoize - def find_best_jerk_limit(self, target_vel_limit: float, jerk_limit: float, - max_derivative: Derivatives, eps: float = 0.001) -> float: - upper_bound = jerk_limit - lower_bound = 0 - for i in range(100): - vel_limit = giskard_math.max_velocity_from_horizon_and_jerk_qp(prediction_horizon=self.prediction_horizon, - vel_limit=100, - acc_limit=np.inf, - jerk_limit=jerk_limit, - dt=self.dt, - max_derivative=max_derivative)[0] - if abs(vel_limit - target_vel_limit) < eps: - break - if vel_limit > target_vel_limit: - upper_bound = jerk_limit - jerk_limit = int((jerk_limit + lower_bound) / 2) - else: - lower_bound = jerk_limit - jerk_limit = int((jerk_limit + upper_bound) / 2) - print(f'best velocity limit: {vel_limit} with jerk limit: {jerk_limit} after {i} iterations') - return jerk_limit - @profile def velocity_limit(self, v: FreeVariable, max_derivative: Derivatives) -> Tuple[cas.Expression, cas.Expression]: current_position = v.get_symbol(Derivatives.position) @@ -239,41 +216,17 @@ def velocity_limit(self, v: FreeVariable, max_derivative: Derivatives) -> Tuple[ current_vel = v.get_symbol(Derivatives.velocity) current_acc = v.get_symbol(Derivatives.acceleration) - # lower_jerk_limit = v.get_lower_limit(Derivatives.jerk, evaluated=True) - # upper_jerk_limit = v.get_upper_limit(Derivatives.jerk, evaluated=True) + lower_jerk_limit = v.get_lower_limit(Derivatives.jerk, evaluated=True) + upper_jerk_limit = v.get_upper_limit(Derivatives.jerk, evaluated=True) if self.prediction_horizon == 1: return cas.Expression([lower_velocity_limit]), cas.Expression([upper_velocity_limit]) - # max_reachable_vel = giskard_math.max_velocity_from_horizon_and_jerk(prediction_horizon=self.prediction_horizon, - # vel_limit=upper_velocity_limit, - # acc_limit=upper_acc_limit, - # jerk_limit=upper_jerk_limit, - # sample_period=self.dt, - # max_derivative=max_derivative) - upper_jerk_limit = self.find_best_jerk_limit(upper_velocity_limit, (4 * upper_velocity_limit) / self.dt ** 2, max_derivative) - lower_jerk_limit = -upper_jerk_limit - - print( - f'''max vel qp {giskard_math.max_velocity_from_horizon_and_jerk_qp(prediction_horizon=self.prediction_horizon, - vel_limit=100, - acc_limit=upper_acc_limit, - jerk_limit=upper_jerk_limit, - dt=self.dt, - max_derivative=max_derivative)[0]}''') - # if max_reachable_vel < upper_velocity_limit: - # error_msg = f'Free variable "{v.name}" can\'t reach velocity limit of "{upper_velocity_limit}". ' \ - # f'Maximum reachable with prediction horizon = "{self.prediction_horizon}", ' \ - # f'jerk limit = "{upper_jerk_limit}" and dt = "{self.dt}" is "{max_reachable_vel}".' - # get_middleware().logerr(error_msg) - # raise VelocityLimitUnreachableException(error_msg) + if upper_jerk_limit is None: + upper_jerk_limit = giskard_math.find_best_jerk_limit(self.prediction_horizon, self.dt, + upper_velocity_limit) + lower_jerk_limit = -upper_jerk_limit if not v.has_position_limits(): - # lb = cas.Expression([lower_velocity_limit] * self.prediction_horizon - # + [lower_acc_limit] * self.prediction_horizon - # + [lower_jerk_limit] * self.prediction_horizon) - # ub = cas.Expression([upper_velocity_limit] * self.prediction_horizon - # + [upper_acc_limit] * self.prediction_horizon - # + [upper_jerk_limit] * self.prediction_horizon) lower_limit = upper_limit = None else: lower_limit = v.get_lower_limit(Derivatives.position, evaluated=True) @@ -290,13 +243,13 @@ def velocity_limit(self, v: FreeVariable, max_derivative: Derivatives) -> Tuple[ dt=self.dt, ph=self.prediction_horizon) except InfeasibleException as e: - max_reachable_vel = giskard_math.max_velocity_from_horizon_and_jerk( + max_reachable_vel = giskard_math.max_velocity_from_horizon_and_jerk_qp( prediction_horizon=self.prediction_horizon, - vel_limit=upper_velocity_limit, + vel_limit=100, acc_limit=upper_acc_limit, jerk_limit=upper_jerk_limit, - sample_period=self.dt, - max_derivative=self.max_derivative) + dt=self.dt, + max_derivative=max_derivative)[0] if max_reachable_vel < upper_velocity_limit: error_msg = f'Free variable "{v.name}" can\'t reach velocity limit of "{upper_velocity_limit}". ' \ f'Maximum reachable with prediction horizon = "{self.prediction_horizon}", ' \ @@ -437,6 +390,18 @@ def eq_derivative_weight_expressions(self) -> List[Dict[str, cas.Expression]]: params.append(derivative_constr_weights) return params + def eq_derivative_weight_expressions(self) -> List[Dict[str, cas.Expression]]: + params = [] + for d in Derivatives.range(Derivatives.velocity, self.max_derivative): + derivative_constr_weights = {} + for t in range(self.prediction_horizon): + d = Derivatives(d) + for c in self.get_eq_derivative_constraints(d): + if t < self.control_horizon: + derivative_constr_weights[f't{t:03}/{c.name}'] = c.normalized_weight(t) + params.append(derivative_constr_weights) + return params + def equality_weight_expressions(self) -> dict: error_slack_weights = {f'{c.name}/error': c.normalized_weight(self.control_horizon) for c in self.equality_constraints} @@ -579,6 +544,17 @@ def eq_derivative_slack_limits(self, derivative: Derivatives) \ upper_slack[f't{t:03}/{c.name}'] = c.upper_slack_limit[t] return lower_slack, upper_slack + def eq_derivative_slack_limits(self, derivative: Derivatives) \ + -> Tuple[Dict[str, cas.Expression], Dict[str, cas.Expression]]: + lower_slack = {} + upper_slack = {} + for t in range(self.prediction_horizon): + for c in self.get_eq_derivative_constraints(derivative): + if t < self.control_horizon: + lower_slack[f't{t:03}/{c.name}'] = c.lower_slack_limit[t] + upper_slack[f't{t:03}/{c.name}'] = c.upper_slack_limit[t] + return lower_slack, upper_slack + def equality_constraint_slack_lower_bound(self): return {f'{c.name}/error': c.lower_slack_limit for c in self.equality_constraints} @@ -1589,9 +1565,9 @@ class QPController: @profile def __init__(self, - sample_period: float = 0.05, + mpc_dt: float, + prediction_horizon: int, control_dt: Optional[float] = None, - prediction_horizon: int = 9, max_derivative: Derivatives = Derivatives.jerk, solver_id: Optional[SupportedQPSolver] = None, retries_with_relaxed_constraints: int = 0, @@ -1601,11 +1577,11 @@ def __init__(self, alpha: float = 0.1, verbose: bool = True): if control_dt is None: - control_dt = sample_period + control_dt = mpc_dt self.control_dt = control_dt self.alpha = alpha self.qp_formulation = qp_formulation - self.sample_period = sample_period + self.mpc_dt = mpc_dt self.max_derivative = max_derivative self.prediction_horizon = prediction_horizon self.retries_with_relaxed_constraints = retries_with_relaxed_constraints @@ -1619,7 +1595,7 @@ def __init__(self, if self.verbose: get_middleware().loginfo(f'Initialized QP Controller:\n' - f'sample period: "{self.sample_period}"s\n' + f'sample period: "{self.mpc_dt}"s\n' f'max derivative: "{self.max_derivative.name}"\n' f'prediction horizon: "{self.prediction_horizon}"\n' f'QP solver: "{self.qp_solver_class.solver_id.name}"') @@ -1740,7 +1716,7 @@ def compile(self, default_limits: bool = False) -> None: 'inequality_constraints': self.inequality_constraints, 'derivative_constraints': self.derivative_constraints, 'eq_derivative_constraints': self.eq_derivative_constraints, - 'sample_period': self.sample_period, + 'sample_period': self.mpc_dt, 'prediction_horizon': self.prediction_horizon, 'max_derivative': self.order, 'alpha': self.alpha, @@ -1803,13 +1779,13 @@ def get_cmd(self, substitutions: np.ndarray) -> NextCommands: # self._create_debug_pandas(self.qp_solver) if self.qp_formulation.is_implicit(): return NextCommands.from_xdot_implicit(self.free_variables, self.xdot_full, self.order, - self.prediction_horizon, god_map.world, self.sample_period) + self.prediction_horizon, god_map.world, self.mpc_dt) elif self.qp_formulation.is_explicit() or self.qp_formulation.is_no_mpc(): return NextCommands.from_xdot(self.free_variables, self.xdot_full, self.order, self.prediction_horizon) else: return NextCommands.from_xdot_explicit_no_acc(self.free_variables, self.xdot_full, self.order, self.prediction_horizon, god_map.world, - self.sample_period) + self.mpc_dt) except InfeasibleException as e_original: self.xdot_full = None self._create_debug_pandas(self.qp_solver) @@ -1856,14 +1832,14 @@ def pad(a, desired_length, pad_value): except KeyError: get_middleware().loginfo('start position not found in state') start_pos = 0 - ts = np.array([(i + 1) * self.sample_period for i in range(self.prediction_horizon)]) + ts = np.array([(i + 1) * self.mpc_dt for i in range(self.prediction_horizon)]) filtered_x = self.p_xdot.filter(like=f'/{joint_name}/', axis=0) vel_end = self.prediction_horizon - self.order + 1 acc_end = vel_end + self.prediction_horizon - self.order + 2 velocities = filtered_x[:vel_end].values positions = [start_pos] for x_ in velocities: - positions.append(positions[-1] + x_ * self.sample_period) + positions.append(positions[-1] + x_ * self.mpc_dt) positions = np.array(positions[1:]) positions = pad(positions.T[0], len(ts), pad_value=positions[-1]) @@ -1899,7 +1875,7 @@ def pad(a, desired_length, pad_value): if not np.isinf(lower_limit): ax.axhline(y=lower_limit, color='k', linestyle='--') # Example: Set x-ticks for each subplot - tick_labels = [f'{x}/{x * self.sample_period:.3f}' for x in range(self.prediction_horizon)] + tick_labels = [f'{x}/{x * self.mpc_dt:.3f}' for x in range(self.prediction_horizon)] axs[-1].set_xticks(ts) # Set tick locations axs[-1].set_xticklabels(tick_labels) # Set custom tick labels @@ -1931,21 +1907,21 @@ def _create_debug_pandas(self, qp_solver: QPSolver): if len(bE) > 0: self.p_bE_raw = pd.DataFrame(bE, self.equality_constr_names, ['data'], dtype=float) self.p_bE = deepcopy(self.p_bE_raw) - self.p_bE[len(self.equality_bounds.names_derivative_links):] /= self.sample_period + self.p_bE[len(self.equality_bounds.names_derivative_links):] /= self.mpc_dt else: self.p_bE = pd.DataFrame() if len(lbA) > 0: self.p_lbA_raw = pd.DataFrame(lbA, self.inequality_constr_names, ['data'], dtype=float) self.p_lbA = deepcopy(self.p_lbA_raw) - self.p_lbA /= self.sample_period + self.p_lbA /= self.mpc_dt self.p_ubA_raw = pd.DataFrame(ubA, self.inequality_constr_names, ['data'], dtype=float) self.p_ubA = deepcopy(self.p_ubA_raw) - self.p_ubA /= self.sample_period + self.p_ubA /= self.mpc_dt self.p_bA_raw = pd.DataFrame({'lbA': lbA, 'ubA': ubA}, self.inequality_constr_names, dtype=float) self.p_bA = deepcopy(self.p_bA_raw) - self.p_bA /= self.sample_period + self.p_bA /= self.mpc_dt else: self.p_lbA = pd.DataFrame() self.p_ubA = pd.DataFrame() diff --git a/giskardpy/qp/qp_controller_config.py b/giskardpy/qp/qp_controller_config.py index 85cac3f1e2..eeada8ceb0 100644 --- a/giskardpy/qp/qp_controller_config.py +++ b/giskardpy/qp/qp_controller_config.py @@ -14,7 +14,8 @@ class QPControllerConfig: def __init__(self, qp_solver: Optional[SupportedQPSolver] = None, prediction_horizon: int = 7, - sample_period: float = 0.0125, + mpc_dt: float = 0.0125, + control_dt: Optional[float] = None, max_trajectory_length: Optional[float] = 30, retries_with_relaxed_constraints: int = 5, added_slack: float = 100, @@ -22,7 +23,7 @@ def __init__(self, """ :param qp_solver: if not set, Giskard will search for the fasted installed solver. :param prediction_horizon: Giskard uses MPC and this is the length of the horizon. You usually don't need to change this. - :param sample_period: time (s) difference between commands in the MPC horizon. + :param mpc_dt: time (s) difference between commands in the MPC horizon. :param max_trajectory_length: Giskard will stop planning/controlling the robot until this amount of s has passed. This is disabled if set to None. :param retries_with_relaxed_constraints: don't change, only for the pros. @@ -33,7 +34,8 @@ def __init__(self, if prediction_horizon < 4: raise ValueError('prediction horizon must be >= 4.') self.__prediction_horizon = prediction_horizon - self.__sample_period = sample_period + self.__mpc_dt = mpc_dt + self.control_dt = control_dt self.__max_trajectory_length = max_trajectory_length self.__retries_with_relaxed_constraints = retries_with_relaxed_constraints self.__added_slack = added_slack @@ -42,7 +44,8 @@ def __init__(self, self.set_defaults() def set_defaults(self): - god_map.qp_controller = QPController(sample_period=self.__sample_period, + god_map.qp_controller = QPController(mpc_dt=self.__mpc_dt, + control_dt=self.control_dt, prediction_horizon=self.__prediction_horizon, solver_id=self.__qp_solver, max_derivative=self.__max_derivative, diff --git a/giskardpy/utils/math.py b/giskardpy/utils/math.py index 371a09c00c..1d9f084bb7 100644 --- a/giskardpy/utils/math.py +++ b/giskardpy/utils/math.py @@ -212,23 +212,24 @@ def max_velocity_from_horizon_and_jerk_qp(prediction_horizon: int, (-acc_limit,) * prediction_horizon, (-jerk_limit,) * prediction_horizon) return mpc(upper_limits=upper_limits, lower_limits=lower_limits, - current_values=(0, 0), dt=dt, ph=prediction_horizon, q_weight=(0,0,0), lin_weight=(-1,0,0), + current_values=(0, 0), dt=dt, ph=prediction_horizon, q_weight=(0, 0, 0), lin_weight=(-1, 0, 0), solver_class=None, link_to_current_vel=False) + def max_velocity_from_horizon_and_jerk_qp2(prediction_horizon: int, - vel_limit: float, - acc_limit: float, - jerk_limit: float, - dt: float, - max_derivative: Derivatives): + vel_limit: float, + acc_limit: float, + jerk_limit: float, + dt: float, + max_derivative: Derivatives): upper_limits = ((vel_limit,) * prediction_horizon, - tuple([0] + [acc_limit] * (prediction_horizon-1)), + tuple([0] + [acc_limit] * (prediction_horizon - 1)), (np.inf,) * prediction_horizon) lower_limits = ((-vel_limit,) * prediction_horizon, - tuple([0] + [-acc_limit] * (prediction_horizon-1)), + tuple([0] + [-acc_limit] * (prediction_horizon - 1)), (-np.inf,) * prediction_horizon) return mpc(upper_limits=upper_limits, lower_limits=lower_limits, - current_values=(vel_limit, 0), dt=dt, ph=prediction_horizon, q_weight=(0,0,1), lin_weight=(0,0,0), + current_values=(vel_limit, 0), dt=dt, ph=prediction_horizon, q_weight=(0, 0, 1), lin_weight=(0, 0, 0), solver_class=None, link_to_current_vel=True) @@ -492,3 +493,33 @@ def quaternion_slerp(q1, q2, t): if 0.001 > abs(sin_half_theta): return 0.5 * q1 + 0.5 * q2 return ratio_a * q1 + ratio_b * q2 + + +@memoize +def find_best_jerk_limit(prediction_horizon: int, dt: float, target_vel_limit: float, eps: float = 0.0001) -> float: + jerk_limit = (4 * target_vel_limit) / dt ** 2 + upper_bound = jerk_limit + lower_bound = 0 + best_vel_limit = 0 + best_jerk_limit = 0 + for i in range(100): + vel_limit = max_velocity_from_horizon_and_jerk_qp(prediction_horizon=prediction_horizon, + vel_limit=1000, + acc_limit=np.inf, + jerk_limit=jerk_limit, + dt=dt, + max_derivative=Derivatives.jerk)[0] + if abs(vel_limit - target_vel_limit) < abs(best_vel_limit - target_vel_limit): + best_vel_limit = vel_limit + best_jerk_limit = jerk_limit + if abs(vel_limit - target_vel_limit) < eps: + break + if vel_limit > target_vel_limit: + upper_bound = jerk_limit + jerk_limit = round((jerk_limit + lower_bound) / 2, 4) + else: + lower_bound = jerk_limit + jerk_limit = round((jerk_limit + upper_bound) / 2, 4) + print( + f'best velocity limit: {best_vel_limit} (target = {target_vel_limit}) with jerk limit: {best_jerk_limit} after {i + 1} iterations') + return best_jerk_limit diff --git a/test/test_giskard_library.py b/test/test_giskard_library.py index e82639791b..8dd0646f45 100644 --- a/test/test_giskard_library.py +++ b/test/test_giskard_library.py @@ -195,7 +195,7 @@ def test_joint_goal(self, box_world_prismatic: WorldTree): god_map.motion_goal_manager.init_task_state() eq, neq, neqd, lin_weight, quad_weight = god_map.motion_goal_manager.get_constraints_from_goals() - controller = QPController(sample_period=dt) + controller = QPController(mpc_dt=dt) controller.init(free_variables=list(box_world_prismatic.free_variables.values()), equality_constraints=eq) controller.compile() @@ -229,7 +229,7 @@ def test_cart_goal(self, box_world: WorldTree): god_map.motion_goal_manager.init_task_state() eq, neq, neqd, lin_weight, quad_weight = god_map.motion_goal_manager.get_constraints_from_goals() - controller = QPController(sample_period=dt) + controller = QPController(mpc_dt=dt) controller.init(free_variables=list(box_world.free_variables.values()), equality_constraints=eq) controller.compile() @@ -278,7 +278,7 @@ def test_cart_goal_abs_sequence(self, box_world: WorldTree): god_map.motion_goal_manager.init_task_state() eq, neq, neqd, lin_weight, quad_weight = god_map.motion_goal_manager.get_constraints_from_goals() - controller = QPController(sample_period=dt) + controller = QPController(mpc_dt=dt) controller.init(free_variables=list(box_world.free_variables.values()), equality_constraints=eq) controller.compile() @@ -295,7 +295,7 @@ def test_cart_goal_abs_sequence(self, box_world: WorldTree): god_map.monitor_manager.evaluate_monitors() traj.append(box_world.state[joint_name].position) - god_map.time += controller.sample_period + god_map.time += controller.mpc_dt god_map.control_cycle_counter += 1 fk = box_world.compute_fk_point(root_link=box_world.root_link_name, tip_link=box_name).to_np() np.testing.assert_almost_equal(fk[0], goal2.to_position().to_np()[0], decimal=3) @@ -332,7 +332,7 @@ def test_cart_goal_rel_sequence(self, box_world: WorldTree): god_map.motion_goal_manager.init_task_state() eq, neq, neqd, lin_weight, quad_weight = god_map.motion_goal_manager.get_constraints_from_goals() - controller = QPController(sample_period=dt) + controller = QPController(mpc_dt=dt) controller.init(free_variables=list(box_world.free_variables.values()), equality_constraints=eq) controller.compile() @@ -348,7 +348,7 @@ def test_cart_goal_rel_sequence(self, box_world: WorldTree): god_map.monitor_manager.evaluate_monitors() traj.append((box_world.state[box_world.joints[joint_name].x_name].position, box_world.state[box_world.joints[joint_name].y_name].position)) - god_map.time += controller.sample_period + god_map.time += controller.mpc_dt god_map.control_cycle_counter += 1 fk = box_world.compute_fk_point(root_link=box_world.root_link_name, tip_link=box_name).to_np() np.testing.assert_almost_equal(fk[0], goal1.to_position().to_np()[0], decimal=2) diff --git a/test/test_math.py b/test/test_math.py index 378e1597b2..2d11b1c049 100644 --- a/test/test_math.py +++ b/test/test_math.py @@ -9,7 +9,7 @@ def test_velocity_integral_jerk(self): limits = { Derivatives.velocity: 1, Derivatives.acceleration: np.inf, - Derivatives.jerk: 21.1 + Derivatives.jerk: None } actual = giskard_math.mpc_velocity_integral(limits, 0.05, 9) expected = giskard_math.mpc_velocity_integral3(limits, 0.05, 9) From 99759b33ea512550fdcb591302bced83bb47ec31 Mon Sep 17 00:00:00 2001 From: Simon Stelter Date: Fri, 29 Nov 2024 14:36:59 +0100 Subject: [PATCH 2/2] fixed diff drive when jerk limits are none --- giskardpy/model/joints.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/giskardpy/model/joints.py b/giskardpy/model/joints.py index 5db101db6d..2ff830b72f 100644 --- a/giskardpy/model/joints.py +++ b/giskardpy/model/joints.py @@ -478,8 +478,8 @@ def create_parent_T_child(self) -> None: self.parent_T_child = cas.dot(odom_T_bf, bf_T_bf_vel) def create_free_variables(self) -> None: - translation_lower_limits = {derivative: -limit for derivative, limit in self.translation_limits.items()} - rotation_lower_limits = {derivative: -limit for derivative, limit in self.rotation_limits.items()} + translation_lower_limits = {derivative: -limit if limit is not None else limit for derivative, limit in self.translation_limits.items()} + rotation_lower_limits = {derivative: -limit if limit is not None else limit for derivative, limit in self.rotation_limits.items()} self.x = god_map.world.add_virtual_free_variable(name=PrefixName('x', self.name)) self.y = god_map.world.add_virtual_free_variable(name=PrefixName('y', self.name))