@@ -187,6 +187,31 @@ def __init__(self, cfg: Any) -> None:
187187 self ._fixed_reference_yaw_quat : Float32Array | None = None
188188 self ._fixed_reference_pivot_pos_w : Float32Array | None = None
189189
190+ # ---- Startup ramp (matching GR00T JointSafetyMonitor pattern) ----
191+ ramp_dur = float (cfg_get (cfg , "startup_ramp_duration" , cfg_get (real_cfg , "startup_ramp_duration" , 2.0 )))
192+ self ._ramp_duration_steps : int = max (1 , int (ramp_dur * self .policy_hz ))
193+ self ._ramp_step : int = 0
194+ self ._ramp_start_positions : Float32Array | None = None
195+ self ._ramp_active : bool = False
196+
197+ # ---- Joint safety (inspired by GR00T JointSafetyMonitor) ----
198+ self ._joint_vel_limit : float = float (
199+ cfg_get (cfg , "joint_vel_limit" , cfg_get (real_cfg , "joint_vel_limit" , 10.0 ))
200+ )
201+ joint_pos_lower = cfg_get (real_cfg , "joint_pos_lower" , None )
202+ joint_pos_upper = cfg_get (real_cfg , "joint_pos_upper" , None )
203+ if joint_pos_lower is not None and joint_pos_upper is not None :
204+ self ._joint_pos_lower = np .asarray (joint_pos_lower , dtype = np .float32 )
205+ self ._joint_pos_upper = np .asarray (joint_pos_upper , dtype = np .float32 )
206+ else :
207+ self ._joint_pos_lower = None
208+ self ._joint_pos_upper = None
209+
210+ # ---- Control loop timing diagnostics ----
211+ self ._loop_count : int = 0
212+ self ._loop_timer_start : float = 0.0
213+ self ._timing_log_interval : int = int (5.0 * self .policy_hz ) # every 5 seconds
214+
190215 # ---- Mocap switch safety ----
191216 mocap_sw = cfg_get (cfg , "mocap_switch" , {})
192217 self ._check_frames : int = int (cfg_get (mocap_sw , "check_frames" , 10 ))
@@ -207,6 +232,8 @@ def run(self) -> None:
207232 "Control loop started | mode=IDLE | press Start to enter STANDING"
208233 )
209234 dt = 1.0 / self .policy_hz
235+ self ._loop_count = 0
236+ self ._loop_timer_start = time .monotonic ()
210237
211238 try :
212239 while True :
@@ -226,16 +253,38 @@ def run(self) -> None:
226253 self ._sleep_until (t0 , dt )
227254 continue
228255
229- # 3. Mode transitions
256+ # 3. Joint velocity safety check
257+ if self .mode in (RobotMode .STANDING , RobotMode .MOCAP ):
258+ if self ._check_joint_velocity_safety ():
259+ self ._sleep_until (t0 , dt )
260+ continue
261+
262+ # 4. Mode transitions
230263 self ._handle_transitions ()
231264
232- # 4 . Execute current mode
265+ # 5 . Execute current mode
233266 if self .mode == RobotMode .STANDING :
234267 self ._standing_step ()
235268 elif self .mode == RobotMode .MOCAP :
236269 self ._mocap_step ()
237270
238- # 5. Rate control
271+ # 6. Timing diagnostics
272+ self ._loop_count += 1
273+ elapsed = time .monotonic () - t0
274+ if elapsed > dt * 1.5 :
275+ logger .warning (
276+ "Control loop overrun: %.1fms (target %.1fms)" ,
277+ elapsed * 1000 , dt * 1000 ,
278+ )
279+ if self ._loop_count % self ._timing_log_interval == 0 :
280+ wall = time .monotonic () - self ._loop_timer_start
281+ actual_hz = self ._loop_count / wall if wall > 0 else 0
282+ logger .info (
283+ "Control loop: %.1f Hz (target %.0f Hz) | mode=%s" ,
284+ actual_hz , self .policy_hz , self .mode .value ,
285+ )
286+
287+ # 7. Rate control
239288 self ._sleep_until (t0 , dt )
240289
241290 except KeyboardInterrupt :
@@ -276,6 +325,13 @@ def _standing_step(self) -> None:
276325
277326 action = self .policy .compute_action (obs )
278327 target_dof_pos = self .policy .get_target_dof_pos (action )
328+
329+ # Apply startup ramp: smoothly blend from locked position to policy output
330+ target_dof_pos = self ._apply_startup_ramp (target_dof_pos )
331+
332+ # Clip to joint limits if configured
333+ target_dof_pos = self ._clip_to_joint_limits (target_dof_pos )
334+
279335 self .robot .send_positions (target_dof_pos )
280336
281337 self ._last_action = np .asarray (action , dtype = np .float32 ).reshape (- 1 )
@@ -385,6 +441,10 @@ def _mocap_step(self) -> None:
385441 action = self .policy .compute_action (obs )
386442 target_dof_pos = self .policy .get_target_dof_pos (action )
387443
444+ # Apply startup ramp and joint limits
445+ target_dof_pos = self ._apply_startup_ramp (target_dof_pos )
446+ target_dof_pos = self ._clip_to_joint_limits (target_dof_pos )
447+
388448 # Update position targets (250Hz thread handles publishing)
389449 self .robot .send_positions (target_dof_pos )
390450
@@ -440,6 +500,57 @@ def _compute_anchor_velocities(
440500
441501 return anchor_lin_vel_w , anchor_ang_vel_w
442502
503+ # ------------------------------------------------------------------
504+ # Startup ramp and safety
505+ # ------------------------------------------------------------------
506+
507+ def _apply_startup_ramp (self , target_dof_pos : Float32Array ) -> Float32Array :
508+ """Smoothly ramp from locked position to policy output (matching GR00T pattern).
509+
510+ During the first ``_ramp_duration_steps`` after entering STANDING,
511+ linearly interpolate between the initial joint positions and the
512+ policy-commanded targets. This prevents the step discontinuity
513+ that causes violent shaking on the NX board.
514+ """
515+ if not self ._ramp_active :
516+ return target_dof_pos
517+
518+ if self ._ramp_start_positions is None :
519+ # Should not happen, but be safe
520+ self ._ramp_active = False
521+ return target_dof_pos
522+
523+ ramp_factor = min (1.0 , self ._ramp_step / self ._ramp_duration_steps )
524+ ramped = self ._ramp_start_positions + ramp_factor * (
525+ target_dof_pos - self ._ramp_start_positions
526+ )
527+
528+ self ._ramp_step += 1
529+ if self ._ramp_step >= self ._ramp_duration_steps :
530+ self ._ramp_active = False
531+ logger .info ("Startup ramp complete (%d steps)" , self ._ramp_duration_steps )
532+
533+ return np .asarray (ramped , dtype = np .float32 )
534+
535+ def _clip_to_joint_limits (self , target_dof_pos : Float32Array ) -> Float32Array :
536+ """Clip target positions to configured joint limits."""
537+ if self ._joint_pos_lower is not None and self ._joint_pos_upper is not None :
538+ return np .clip (target_dof_pos , self ._joint_pos_lower , self ._joint_pos_upper )
539+ return target_dof_pos
540+
541+ def _check_joint_velocity_safety (self ) -> bool :
542+ """Check joint velocities against safety limit. Returns True if violation detected."""
543+ state = self .robot .get_state ()
544+ max_vel = np .max (np .abs (state .qvel ))
545+ if max_vel > self ._joint_vel_limit :
546+ logger .error (
547+ "SAFETY: joint velocity %.2f rad/s exceeds limit %.2f -- entering damping" ,
548+ max_vel , self ._joint_vel_limit ,
549+ )
550+ self ._enter_damping ()
551+ return True
552+ return False
553+
443554 # ------------------------------------------------------------------
444555 # State machine transitions
445556 # ------------------------------------------------------------------
@@ -510,6 +621,15 @@ def _enter_standing(self) -> None:
510621 self ._last_retarget_qpos = init_qpos
511622 self ._last_reference_qpos = None
512623
624+ # Activate startup ramp: smoothly blend from current to policy targets
625+ self ._ramp_start_positions = state .qpos .copy ().astype (np .float32 )
626+ self ._ramp_step = 0
627+ self ._ramp_active = True
628+ logger .info (
629+ "Startup ramp armed: %d steps (%.1fs)" ,
630+ self ._ramp_duration_steps , self ._ramp_duration_steps / self .policy_hz ,
631+ )
632+
513633 if prev_mode == RobotMode .MOCAP :
514634 # Returning from MOCAP: soft reset to keep policy observation
515635 # continuity (history buffer, _last_action unchanged).
0 commit comments