Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Giskard library #11

Merged
merged 2 commits into from
Nov 29, 2024
Merged

Giskard library #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
990aba6
fixed bug, where control/mpc dt couldn't be properly set
ichumuh Nov 29, 2024
99759b3
fixed diff drive when jerk limits are none
ichumuh Nov 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion giskardpy/debug_expression_manager.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 = {}
Expand Down
16 changes: 8 additions & 8 deletions giskardpy/goals/base_traj_follower.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions giskardpy/goals/collision_avoidance.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
36 changes: 18 additions & 18 deletions giskardpy/model/joints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -452,17 +452,17 @@ 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

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
Expand All @@ -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))
Expand Down Expand Up @@ -543,17 +543,17 @@ 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

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
Expand All @@ -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))
Expand Down
2 changes: 1 addition & 1 deletion giskardpy/model/trajectory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]]:
Expand Down
4 changes: 2 additions & 2 deletions giskardpy/model/world.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]):
Expand Down Expand Up @@ -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)

Expand Down
18 changes: 9 additions & 9 deletions giskardpy/model/world_config.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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'))
Expand Down Expand Up @@ -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,
Expand All @@ -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)

Expand All @@ -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,
Expand All @@ -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)
9 changes: 8 additions & 1 deletion giskardpy/qp/free_variable.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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

Expand Down Expand Up @@ -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
Expand Down
Loading
Loading