Spaces:

gpue
/

nova-sim

Paused

Georg commited on Jan 16

Commit

d5b839c

1 Parent(s): c1222b2

Enhance UR5 environment jogging functionality with internal fallback

- Introduced an internal jogging mechanism for cartesian and joint movements when Nova jogging is unavailable.
- Updated the start_jog method to handle both Nova and internal jogging commands, improving robustness.
- Enhanced stop_jog method to manage both Nova and internal jogging states.
- Modified the step function to apply internal jogging updates to target positions and joint targets, ensuring smooth operation.

Files changed (2) hide show

robots/ur5/model/scene_t_push.xml +2 -2
robots/ur5/ur5_env.py +115 -41

robots/ur5/model/scene_t_push.xml CHANGED Viewed

@@ -188,8 +188,8 @@
     <!-- Movable T-shaped object to push into target -->
     <body name="t_object" pos="0.45 0.2 0.43">
       <freejoint name="t_object_joint"/>
-      <geom name="t_object_stem" type="box" pos="0 -0.05 0" size="0.02 0.07 0.01" material="t_object_mat" mass="0.3" friction="1 0.005 0.005"/>
-      <geom name="t_object_cap" type="box" pos="0 0.03 0" size="0.08 0.02 0.01" material="t_object_mat" mass="0.15" friction="1 0.005 0.005"/>
     </body>
   </worldbody>

     <!-- Movable T-shaped object to push into target -->
     <body name="t_object" pos="0.45 0.2 0.43">
       <freejoint name="t_object_joint"/>
+      <geom name="t_object_stem" type="box" pos="0 -0.05 0" size="0.02 0.07 0.03" material="t_object_mat" mass="3.0" friction="0.3 0.005 0.005"/>
+      <geom name="t_object_cap" type="box" pos="0 0.03 0" size="0.08 0.02 0.03" material="t_object_mat" mass="2.0" friction="0.3 0.005 0.005"/>
     </body>
   </worldbody>

robots/ur5/ur5_env.py CHANGED Viewed

@@ -140,6 +140,7 @@ class UR5Env(gym.Env):
         self._use_nova_jogging = False
         self._last_nova_sequence = None
         self._latest_nova_joints = None
         # IK controller for end-effector control
         self.controller = IKController(
@@ -338,7 +339,7 @@ class UR5Env(gym.Env):
         return self._control_mode
     def start_jog(self, jog_type: str, **kwargs) -> bool:
-        """Start Nova jogging command.
         Args:
             jog_type: 'joint', 'cartesian_translation', or 'cartesian_rotation'
@@ -347,50 +348,102 @@ class UR5Env(gym.Env):
         Returns:
             True if command sent successfully
         """
-        if not self._use_nova_jogging or self._nova_jogger is None:
-            print("[Nova] Jogging not enabled or jogger not connected")
-            return False
-        try:
-            if jog_type == 'joint':
-                return self._nova_jogger.start_joint_jog(
-                    joint=kwargs.get('joint', 0),
-                    direction=kwargs.get('direction', '+'),
-                    velocity_rads_per_sec=kwargs.get('velocity', 0.5)
-                )
-            elif jog_type == 'cartesian_translation':
-                return self._nova_jogger.start_cartesian_translation(
-                    axis=kwargs.get('axis', 'x'),
-                    direction=kwargs.get('direction', '+'),
-                    velocity_mm_per_sec=kwargs.get('velocity', 50.0),
-                    tcp_id=kwargs.get('tcp_id', 'Flange'),
-                    coord_system_id=kwargs.get('coord_system_id', 'world')
-                )
-            elif jog_type == 'cartesian_rotation':
-                return self._nova_jogger.start_cartesian_rotation(
-                    axis=kwargs.get('axis', 'x'),
-                    direction=kwargs.get('direction', '+'),
-                    velocity_rads_per_sec=kwargs.get('velocity', 0.3),
-                    tcp_id=kwargs.get('tcp_id', 'Flange'),
-                    coord_system_id=kwargs.get('coord_system_id', 'world')
-                )
-            else:
-                print(f"[Nova] Unknown jog type: {jog_type}")
                 return False
-        except Exception as e:
-            print(f"[Nova] Error starting jog: {e}")
-            return False
     def stop_jog(self) -> bool:
-        """Stop any active Nova jogging movement."""
-        if not self._use_nova_jogging or self._nova_jogger is None:
-            return False
-        try:
-            return self._nova_jogger.stop()
-        except Exception as e:
-            print(f"[Nova] Error stopping jog: {e}")
-            return False
     def set_joint_positions(self, positions, update_ik_target=True):
         """Set target joint positions directly (bypasses IK).
@@ -575,6 +628,27 @@ class UR5Env(gym.Env):
             self.steps += 1
             return self._get_obs()
         if self._control_mode == 'ik':
             joint_targets = self._compute_joint_targets()
         else:

         self._use_nova_jogging = False
         self._last_nova_sequence = None
         self._latest_nova_joints = None
+        self._active_jog = None  # Internal jogging state
         # IK controller for end-effector control
         self.controller = IKController(
         return self._control_mode
     def start_jog(self, jog_type: str, **kwargs) -> bool:
+        """Start jogging command (Nova or internal IK fallback).
         Args:
             jog_type: 'joint', 'cartesian_translation', or 'cartesian_rotation'
         Returns:
             True if command sent successfully
         """
+        # Try Nova jogging first if available
+        if self._use_nova_jogging and self._nova_jogger is not None:
+            try:
+                if jog_type == 'joint':
+                    return self._nova_jogger.start_joint_jog(
+                        joint=kwargs.get('joint', 0),
+                        direction=kwargs.get('direction', '+'),
+                        velocity_rads_per_sec=kwargs.get('velocity', 0.5)
+                    )
+                elif jog_type == 'cartesian_translation':
+                    return self._nova_jogger.start_cartesian_translation(
+                        axis=kwargs.get('axis', 'x'),
+                        direction=kwargs.get('direction', '+'),
+                        velocity_mm_per_sec=kwargs.get('velocity', 50.0),
+                        tcp_id=kwargs.get('tcp_id', 'Flange'),
+                        coord_system_id=kwargs.get('coord_system_id', 'world')
+                    )
+                elif jog_type == 'cartesian_rotation':
+                    return self._nova_jogger.start_cartesian_rotation(
+                        axis=kwargs.get('axis', 'x'),
+                        direction=kwargs.get('direction', '+'),
+                        velocity_rads_per_sec=kwargs.get('velocity', 0.3),
+                        tcp_id=kwargs.get('tcp_id', 'Flange'),
+                        coord_system_id=kwargs.get('coord_system_id', 'world')
+                    )
+                else:
+                    print(f"[Nova] Unknown jog type: {jog_type}")
+                    return False
+            except Exception as e:
+                print(f"[Nova] Error starting jog: {e}")
                 return False
+        # Fallback to internal IK for cartesian movements
+        print(f"[Internal] Using internal IK for jogging: {jog_type}")
+        if jog_type in ('cartesian_translation', 'cartesian_rotation'):
+            axis = kwargs.get('axis', 'x')
+            direction = kwargs.get('direction', '+')
+            # Calculate step size (velocity * dt where dt ~= 0.002s per step)
+            # For translation: velocity in mm/s, convert to m/s and multiply by step time
+            # For rotation: velocity in rad/s, multiply by step time
+            if jog_type == 'cartesian_translation':
+                velocity_mm_per_sec = kwargs.get('velocity', 50.0)
+                step_size = (velocity_mm_per_sec / 1000.0) * 0.02  # m per update (10 Hz updates)
+            else:  # rotation
+                velocity_rad_per_sec = kwargs.get('velocity', 0.3)
+                step_size = velocity_rad_per_sec * 0.02  # radians per update (10 Hz updates)
+            # Apply direction
+            if direction == '-':
+                step_size = -step_size
+            self._active_jog = {
+                'type': jog_type,
+                'axis': axis,
+                'step_size': step_size
+            }
+            return True
+        # For joint jogging with internal control
+        elif jog_type == 'joint':
+            joint = kwargs.get('joint', 0)
+            direction = kwargs.get('direction', '+')
+            velocity_rad_per_sec = kwargs.get('velocity', 0.5)
+            step_size = velocity_rad_per_sec * 0.02  # radians per update (10 Hz updates)
+            if direction == '-':
+                step_size = -step_size
+            self._active_jog = {
+                'type': 'joint',
+                'joint': joint,
+                'step_size': step_size
+            }
+            return True
+        print(f"[Internal] Unknown jog type: {jog_type}")
+        return False
     def stop_jog(self) -> bool:
+        """Stop any active jogging movement (Nova or internal)."""
+        # Stop Nova jogging if active
+        if self._use_nova_jogging and self._nova_jogger is not None:
+            try:
+                return self._nova_jogger.stop()
+            except Exception as e:
+                print(f"[Nova] Error stopping jog: {e}")
+                return False
+        # Stop internal jogging
+        if hasattr(self, '_active_jog'):
+            self._active_jog = None
+            print("[Internal] Stopped internal jogging")
+            return True
+        return False
     def set_joint_positions(self, positions, update_ik_target=True):
         """Set target joint positions directly (bypasses IK).
             self.steps += 1
             return self._get_obs()
+        # Apply internal jogging if active
+        if hasattr(self, '_active_jog') and self._active_jog is not None:
+            jog = self._active_jog
+            if jog['type'] == 'cartesian_translation':
+                # Update target position along specified axis
+                axis_idx = {'x': 0, 'y': 1, 'z': 2}[jog['axis']]
+                self._target_pos[axis_idx] += jog['step_size']
+            elif jog['type'] == 'cartesian_rotation':
+                # Update target orientation around specified axis
+                axis_idx = {'x': 0, 'y': 1, 'z': 2}[jog['axis']]
+                self._target_euler[axis_idx] += jog['step_size']
+            elif jog['type'] == 'joint':
+                # Update target joint position directly
+                self._joint_targets[jog['joint']] += jog['step_size']
+                # Clamp to joint limits
+                self._joint_targets = np.clip(
+                    self._joint_targets,
+                    self.action_space.low[:6],
+                    self.action_space.high[:6]
+                )
         if self._control_mode == 'ik':
             joint_targets = self._compute_joint_targets()
         else: