cyy/RoboGauge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v1.0.0

Browse Source
This commit is contained in:
wty-yy
2025-12-29 23:41:09 +08:00
parent c28bb64b0a
commit 587d014683
44 changed files with 196 additions and 714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
robogauge/tasks/gauge/goals/velocity_goals.py
View File

@@ -171,6 +171,7 @@ class TargetPosVelocityGoal(BaseVelocityGoal):
def __init__(self,
control_dt: float,
backward: bool,
target_pos: List[float],
lin_vel_x: float,
lin_vel_y: float,
@@ -186,6 +187,7 @@ class TargetPosVelocityGoal(BaseVelocityGoal):
self.lin_vel_y = lin_vel_y
self.ang_vel_yaw = ang_vel_yaw
self.reach_threshold = reach_threshold
self.backward = backward
kwargs.pop('enabled', None)
if kwargs:
@@ -205,8 +207,12 @@ class TargetPosVelocityGoal(BaseVelocityGoal):
self.update_runtime_count(sim_data)
delta_pos, _, delta_ang = self.get_delta_info(sim_data)
ang_vel_yaw = np.sign(delta_ang) * min(abs(delta_ang) * 2, self.ang_vel_yaw)
lin_vel_x = self.lin_vel_x if abs(delta_ang) < PI / 4 else 0.0
if self.backward:
ang_vel_yaw = -np.sign(delta_ang) * min((PI - abs(delta_ang)) * 2, self.ang_vel_yaw)
lin_vel_x = -self.lin_vel_x if abs(delta_ang) > PI * 3 / 4 else 0.0
self.current_goal = VelocityGoal(
lin_vel_x=self.lin_vel_x if abs(delta_ang) < PI / 4 else 0.0,
lin_vel_x=lin_vel_x,
lin_vel_y=np.sign(delta_pos[1]) * min(abs(delta_pos[1]), self.lin_vel_y) if abs(delta_ang) >= PI / 4 else 0.0,
ang_vel_yaw=ang_vel_yaw
)