v0.1.3

robogauge/tasks/gauge/base_gauge.py

@@ -9,6 +9,9 @@
 '''
 from robogauge.tasks.robots.base_robot_config import RobotConfig
 from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
+from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal, PositionGoal
+from robogauge.tasks.simulator.sim_data import SimData
+from robogauge.utils.logger import logger
 
 class BaseGauge:
     def __init__(self, cfg: BaseGaugeConfig):
@@ -20,10 +23,19 @@ class BaseGauge:
     def is_done(self) -> bool:
         return False
     
-    def get_goal(self) -> dict:
-        goal = {}
+    def get_goal(self) -> GoalData:
+        goal = GoalData(
+            goal_type='velocity',
+            velocity_goal=VelocityGoal(
+                lin_vel=[5.0, 0.0, 0.0],
+                ang_vel=[0.0, 0.0, 0.0]
+            )
+        )
         return goal
     
-    def update_metrics(self, sim_info: dict):
-        ...
+    def update_metrics(self, sim_data: SimData):
+        if sim_data.n_step % int(0.1 / sim_data.sim_dt) != 0:
+            return
+        for i in range(len(sim_data.proprio.joint.force)):
+            logger.log(sim_data.proprio.joint.force[i], f'dof/force_{i}', step=sim_data.n_step)
     

robogauge/tasks/gauge/goal_data.py

@@ -0,0 +1,22 @@
+from enum import Enum
+from dataclasses import dataclass
+from typing import List, Optional, Literal
+
+@dataclass
+class VelocityGoal:
+    lin_vel: List[float]  # x, y, z [m/s], z is ignored for ground robots
+    ang_vel: List[float]  # roll, pitch, yaw [rad/s], roll and pitch are ignored for ground robots
+
+@dataclass
+class PositionGoal:
+    # relative to robot's current position
+    target_pos: List[float]  # x, y, z [m], z is ignored for ground robots
+    # reach target orientation
+    target_quat: List[float]  # x, y, z, w quaternion
+    tolerance: float  # [m] position tolerance to consider goal reached
+
+@dataclass
+class GoalData:
+    goal_type: Literal['velocity', 'position']
+    velocity_goal: Optional[VelocityGoal] = None
+    position_goal: Optional[PositionGoal] = None