v0.1.11
This commit is contained in:
wty-yy
@@ -7,8 +7,9 @@
|
||||
@Blog : https://wty-yy.github.io/
|
||||
@Desc : Velocity Goals Implementation
|
||||
'''
|
||||
import numpy as np
|
||||
from copy import deepcopy
|
||||
from typing import Optional
|
||||
from typing import Optional, List
|
||||
|
||||
from robogauge.tasks.gauge.goals import BaseGoal
|
||||
from robogauge.tasks.robots import RobotConfig
|
||||
@@ -16,27 +17,48 @@ from robogauge.tasks.simulator.sim_data import SimData
|
||||
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal
|
||||
from robogauge.utils.helpers import class_to_dict
|
||||
from robogauge.utils.logger import logger
|
||||
from robogauge.utils.math_utils import quat_rotate_inverse
|
||||
|
||||
PI = np.pi
|
||||
|
||||
class BaseVelocityGoal(BaseGoal):
|
||||
name = "base_velocity_goal"
|
||||
|
||||
def __init__(self, control_dt: float, cmd_duration: float = 5, **kwargs):
|
||||
"""
|
||||
Args:
|
||||
control_dt (float): Control timestep.
|
||||
cmd_duration (float): Duration for each sub-task (e.g. different velocity commands).
|
||||
"""
|
||||
super().__init__()
|
||||
self.control_dt = control_dt
|
||||
self.cmd_duration = cmd_duration
|
||||
self.goal_runtime = 0.0
|
||||
self.first_goal_after_reset = True
|
||||
self.last_reset_time = 0.0
|
||||
self.last_reset_time = -1
|
||||
self.goals = []
|
||||
|
||||
def is_reset(self, sim_data: SimData) -> bool:
|
||||
if self.last_reset_time == -1:
|
||||
self.last_reset_time = sim_data.sim_time
|
||||
if sim_data.sim_time - self.last_reset_time >= self.cmd_duration:
|
||||
self.last_reset_time = sim_data.sim_time
|
||||
self.first_goal_after_reset = True
|
||||
return True
|
||||
return False
|
||||
|
||||
def pre_get_goal(self, sim_data: SimData) -> bool:
|
||||
""" Run before getting the goal
|
||||
Returns:
|
||||
bool: whether the goal sequence is done
|
||||
"""
|
||||
self.update_runtime_count(sim_data)
|
||||
return self.count >= self.total
|
||||
|
||||
def update_runtime_count(self, sim_data: SimData):
|
||||
"""
|
||||
Update the goal runtime and count based on the simulation time.
|
||||
"""
|
||||
if self.first_goal_after_reset:
|
||||
self.last_reset_time = sim_data.sim_time # update last reset time (stance after reset)
|
||||
self.first_goal_after_reset = False
|
||||
@@ -46,7 +68,7 @@ class BaseVelocityGoal(BaseGoal):
|
||||
class MaxVelocityGoal(BaseVelocityGoal):
|
||||
name = "max_velocity"
|
||||
|
||||
""" Goal class for maximizing velocity commands. """
|
||||
""" Goal class for maximizing velocity commands, maximum 6 sub-tasks. """
|
||||
def __init__(self,
|
||||
control_dt: float,
|
||||
max_velocity: RobotConfig.commands,
|
||||
@@ -82,9 +104,6 @@ class MaxVelocityGoal(BaseVelocityGoal):
|
||||
self.sub_name = str(self.goals[0])
|
||||
|
||||
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
|
||||
self.update_runtime_count(sim_data)
|
||||
if self.count >= self.total:
|
||||
return None
|
||||
self.current_goal = self.goals[self.count]
|
||||
if self.end_stance and self.goal_runtime - self.count * self.cmd_duration >= self.move_duration:
|
||||
self.current_goal = VelocityGoal() # zero velocity
|
||||
@@ -103,7 +122,7 @@ class DiagonalVelocityGoal(BaseVelocityGoal):
|
||||
cmd_duration: float = 6,
|
||||
**kwargs
|
||||
):
|
||||
""" Goal class for diagonal velocity changes.
|
||||
""" Goal class for diagonal velocity changes, maximum 8 sub-tasks.
|
||||
Args:
|
||||
control_dt (float): Control timestep.
|
||||
max_velocity (RobotConfig.commands): Maximum velocity commands.
|
||||
@@ -114,9 +133,6 @@ class DiagonalVelocityGoal(BaseVelocityGoal):
|
||||
if kwargs:
|
||||
logger.warning(f"Unused kwargs in DiagonalVelocityGoal: {kwargs}")
|
||||
|
||||
self.cmd_duration = cmd_duration
|
||||
self.last_reset_time = 0.0
|
||||
|
||||
self.goals = []
|
||||
lin_vel_x_vals = max_velocity.lin_vel_x + [0]
|
||||
lin_vel_y_vals = max_velocity.lin_vel_y + [0]
|
||||
@@ -136,9 +152,6 @@ class DiagonalVelocityGoal(BaseVelocityGoal):
|
||||
self.sub_name = str(self.goals[0])
|
||||
|
||||
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
|
||||
self.update_runtime_count(sim_data)
|
||||
if self.count >= self.total:
|
||||
return None
|
||||
self.current_goal = self.goals[self.count]
|
||||
if self.goal_runtime - self.count * self.cmd_duration >= self.cmd_duration / 2:
|
||||
self.current_goal = self.current_goal.invert()
|
||||
@@ -147,3 +160,78 @@ class DiagonalVelocityGoal(BaseVelocityGoal):
|
||||
goal_type='velocity',
|
||||
velocity_goal=self.current_goal
|
||||
)
|
||||
|
||||
class TargetPosVelocityGoal(BaseVelocityGoal):
|
||||
name = "target_pos_velocity"
|
||||
|
||||
def __init__(self,
|
||||
control_dt: float,
|
||||
target_pos: List[float],
|
||||
lin_vel_x: float,
|
||||
lin_vel_y: float,
|
||||
ang_vel_yaw: float,
|
||||
max_cmd_duration: float,
|
||||
reach_threshold: float,
|
||||
**kwargs
|
||||
):
|
||||
super().__init__(control_dt=control_dt, cmd_duration=max_cmd_duration)
|
||||
|
||||
self.target_pos = target_pos
|
||||
self.lin_vel_x = lin_vel_x
|
||||
self.lin_vel_y = lin_vel_y
|
||||
self.ang_vel_yaw = ang_vel_yaw
|
||||
self.reach_threshold = reach_threshold
|
||||
|
||||
kwargs.pop('enabled', None)
|
||||
if kwargs:
|
||||
logger.warning(f"Unused kwargs in TargetPosVelocity: {kwargs}")
|
||||
self.sub_name = None
|
||||
self.count = 0
|
||||
self.total = 1 # only one task
|
||||
self.done = False
|
||||
self.success = False
|
||||
|
||||
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
|
||||
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)
|
||||
self.current_goal = VelocityGoal(
|
||||
lin_vel_x=self.lin_vel_x if abs(delta_ang) < PI / 4 else 0.0,
|
||||
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
|
||||
)
|
||||
return GoalData(
|
||||
goal_type='velocity',
|
||||
velocity_goal=self.current_goal,
|
||||
visualization_pos=self.target_pos
|
||||
)
|
||||
|
||||
def is_reset(self, sim_data: SimData) -> bool:
|
||||
_, norm, _ = self.get_delta_info(sim_data)
|
||||
time_out = super().is_reset(sim_data)
|
||||
reached = norm < self.reach_threshold
|
||||
if time_out or reached:
|
||||
self.done = True
|
||||
if reached:
|
||||
self.success = True
|
||||
return True
|
||||
return False
|
||||
|
||||
def pre_get_goal(self, sim_data: SimData) -> bool:
|
||||
""" Run before getting the goal
|
||||
Returns:
|
||||
bool: whether the goal is done
|
||||
"""
|
||||
return self.count >= self.total or self.done
|
||||
|
||||
def get_delta_info(self, sim_data: SimData) -> List[float]:
|
||||
current_pos = sim_data.proprio.base.pos
|
||||
delta_pos = [
|
||||
self.target_pos[0] - current_pos[0],
|
||||
self.target_pos[1] - current_pos[1],
|
||||
self.target_pos[2] - current_pos[2],
|
||||
]
|
||||
delta_pos = quat_rotate_inverse(sim_data.proprio.base.quat, delta_pos)
|
||||
norm = np.linalg.norm(delta_pos[:2])
|
||||
delta_ang = np.arctan2(delta_pos[1], delta_pos[0])
|
||||
return delta_pos, norm, delta_ang
|
||||
|
||||
Reference in New Issue
Block a user