Georg commited on
Commit
dcf1b21
·
1 Parent(s): 4edf6f7

Add environment configuration and Nova API integration support

Browse files

- Introduced .env.example and .env.local files for Nova API configuration, allowing users to set up their instance details easily.
- Enhanced mujoco_server.py to load environment variables for Nova API integration, enabling state streaming and inverse kinematics.
- Updated UR5 environment to conditionally handle gripper presence and control based on configuration.
- Added example scripts for discovering Nova API configuration and testing connections, improving user onboarding.
- Enhanced README.md to document the new Nova API integration features and setup instructions, including troubleshooting tips.

.env.example ADDED
@@ -0,0 +1,27 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Nova API Configuration
2
+ # Copy this file to .env and fill in your Nova instance details
3
+
4
+ # Required: Nova instance URL
5
+ NOVA_INSTANCE_URL=https://your-instance.wandelbots.io
6
+
7
+ # Required: Nova API access token
8
+ NOVA_ACCESS_TOKEN=your_access_token_here
9
+
10
+ # Required: Cell ID
11
+ NOVA_CELL_ID=your_cell_id
12
+
13
+ # Required: Controller ID
14
+ NOVA_CONTROLLER_ID=your_controller_id
15
+
16
+ # Required: Motion Group ID
17
+ NOVA_MOTION_GROUP_ID=your_motion_group_id
18
+
19
+ # Optional: Motion group model (e.g., ur5e, ur10e)
20
+ # If not set, will be fetched from the API
21
+ NOVA_MOTION_GROUP_MODEL=ur5e
22
+
23
+ # Optional: TCP name (default: Flange)
24
+ NOVA_TCP_NAME=Flange
25
+
26
+ # Optional: Response rate in milliseconds (default: 200)
27
+ NOVA_RESPONSE_RATE_MS=200
README.md CHANGED
@@ -2,6 +2,8 @@
2
 
3
  A unified MuJoCo-based robot simulation platform with web interface for multiple robot types.
4
 
 
 
5
  ## Supported Robots
6
 
7
  ### Unitree G1 (Humanoid)
@@ -18,12 +20,13 @@ A unified MuJoCo-based robot simulation platform with web interface for multiple
18
 
19
  ### Universal Robots UR5e (Robot Arm)
20
  - 6 degrees of freedom (6-axis industrial arm)
21
- - Robotiq 2F-85 gripper
22
  - Two control modes:
23
  - **IK Mode**: Set target XYZ position and orientation (Roll/Pitch/Yaw), inverse kinematics computes joint angles using damped least squares with 6-DOF Jacobian
24
  - **Joint Mode**: Direct control of individual joint positions
25
  - End-effector position and orientation tracking
26
  - Full 6-DOF IK with orientation control (can be toggled on/off)
 
27
 
28
  ## Features
29
 
@@ -500,13 +503,22 @@ For robot arm (UR5):
500
  "joint_positions": [-1.57, -1.57, 1.57, -1.57, -1.57, 0.0],
501
  "control_mode": "ik",
502
  "use_orientation": true,
503
- "steps": 1234
 
 
 
 
 
504
  }
505
  }
506
  ```
507
  - `ee_orientation`: Current end-effector orientation as quaternion [w, x, y, z]
508
  - `target_orientation`: Target orientation as Euler angles (radians)
509
  - `use_orientation`: Whether 6-DOF orientation control is enabled
 
 
 
 
510
 
511
  ### HTTP Endpoints
512
 
@@ -520,6 +532,214 @@ For robot arm (UR5):
520
  <img src="http://localhost:3004/nova-sim/api/v1/video_feed" />
521
  ```
522
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
523
  ## License
524
 
525
  This project uses models from:
 
2
 
3
  A unified MuJoCo-based robot simulation platform with web interface for multiple robot types.
4
 
5
+ ![image](screenshot.png)
6
+
7
  ## Supported Robots
8
 
9
  ### Unitree G1 (Humanoid)
 
20
 
21
  ### Universal Robots UR5e (Robot Arm)
22
  - 6 degrees of freedom (6-axis industrial arm)
23
+ - Robotiq 2F-85 gripper (in regular scene only; T-push scene uses a push stick tool)
24
  - Two control modes:
25
  - **IK Mode**: Set target XYZ position and orientation (Roll/Pitch/Yaw), inverse kinematics computes joint angles using damped least squares with 6-DOF Jacobian
26
  - **Joint Mode**: Direct control of individual joint positions
27
  - End-effector position and orientation tracking
28
  - Full 6-DOF IK with orientation control (can be toggled on/off)
29
+ - **Optional [Wandelbots Nova API](#wandelbots-nova-api-integration) integration** for real robot state streaming and cloud-based IK
30
 
31
  ## Features
32
 
 
503
  "joint_positions": [-1.57, -1.57, 1.57, -1.57, -1.57, 0.0],
504
  "control_mode": "ik",
505
  "use_orientation": true,
506
+ "steps": 1234,
507
+ "nova_api": {
508
+ "connected": true,
509
+ "state_streaming": true,
510
+ "ik": false
511
+ }
512
  }
513
  }
514
  ```
515
  - `ee_orientation`: Current end-effector orientation as quaternion [w, x, y, z]
516
  - `target_orientation`: Target orientation as Euler angles (radians)
517
  - `use_orientation`: Whether 6-DOF orientation control is enabled
518
+ - `nova_api`: Nova API integration status (displays which controllers are active)
519
+ - `connected`: Whether Nova API client is connected
520
+ - `state_streaming`: Whether using Nova API for robot state streaming (vs. internal)
521
+ - `ik`: Whether using Nova API for inverse kinematics (vs. internal)
522
 
523
  ### HTTP Endpoints
524
 
 
532
  <img src="http://localhost:3004/nova-sim/api/v1/video_feed" />
533
  ```
534
 
535
+ ## Wandelbots Nova API Integration
536
+
537
+ The UR5 environment supports optional integration with the Wandelbots Nova API for real-time robot state streaming and cloud-based inverse kinematics.
538
+
539
+ ### Overview
540
+
541
+ Nova API integration provides two key features:
542
+ - **Robot State Streaming**: Real-time synchronization of joint positions from physical robots via WebSocket
543
+ - **Inverse Kinematics**: Cloud-based IK computation using Nova's kinematic solver
544
+
545
+ ### Quick Setup
546
+
547
+ 1. **Copy the environment template:**
548
+ ```bash
549
+ cp .env.example .env.local
550
+ ```
551
+
552
+ 2. **Configure your Nova credentials in `.env.local`:**
553
+ ```bash
554
+ NOVA_INSTANCE_URL=https://your-instance.wandelbots.io
555
+ NOVA_ACCESS_TOKEN=your_access_token
556
+ NOVA_CELL_ID=cell
557
+ NOVA_CONTROLLER_ID=your_controller_id
558
+ NOVA_MOTION_GROUP_ID=your_motion_group_id
559
+ NOVA_MOTION_GROUP_MODEL=ur5e # or your robot model
560
+ NOVA_TCP_NAME=Flange
561
+ NOVA_RESPONSE_RATE_MS=200
562
+ ```
563
+
564
+ 3. **Discover available robots in your Nova instance:**
565
+ ```bash
566
+ python3 scripts/discover_nova_simple.py
567
+ ```
568
+
569
+ 4. **Test your connection:**
570
+ ```bash
571
+ cd nova-sim
572
+ python3 scripts/test_nova_connection.py
573
+ ```
574
+
575
+ ### Usage Examples
576
+
577
+ #### Enable State Streaming Only
578
+ Sync robot state from Nova API (joint positions from real robot):
579
+
580
+ ```python
581
+ import os
582
+ from pathlib import Path
583
+ from nova_sim.robots.ur5.ur5_env import UR5Env
584
+
585
+ # Load environment variables
586
+ def load_env(filepath):
587
+ with open(filepath) as f:
588
+ for line in f:
589
+ if line.strip() and not line.startswith('#') and '=' in line:
590
+ key, val = line.split('=', 1)
591
+ os.environ[key.strip()] = val.strip()
592
+
593
+ load_env(Path("nova-sim/.env.local"))
594
+
595
+ # Create environment with state streaming enabled
596
+ env = UR5Env(
597
+ render_mode="human",
598
+ scene_name="scene",
599
+ nova_api_config={
600
+ "use_state_stream": True, # Enable state streaming
601
+ "use_ik": False # Use local IK
602
+ }
603
+ )
604
+
605
+ obs, info = env.reset()
606
+ for _ in range(1000):
607
+ obs = env.step_with_controller(dt=0.002)
608
+ env.render()
609
+ env.close()
610
+ ```
611
+
612
+ #### Enable Nova IK Only
613
+ Use Nova's IK solver while keeping local state:
614
+
615
+ ```python
616
+ env = UR5Env(
617
+ render_mode="human",
618
+ nova_api_config={
619
+ "use_state_stream": False,
620
+ "use_ik": True # Use Nova IK
621
+ }
622
+ )
623
+
624
+ # IK will be computed by Nova API
625
+ env.set_target(x=0.4, y=0.2, z=0.6)
626
+ ```
627
+
628
+ #### Full Integration (Both Features)
629
+ Enable both state streaming and Nova IK:
630
+
631
+ ```python
632
+ env = UR5Env(
633
+ render_mode="human",
634
+ nova_api_config={
635
+ "use_state_stream": True, # Sync robot state from Nova
636
+ "use_ik": True # Use Nova IK
637
+ }
638
+ )
639
+ ```
640
+
641
+ ### Environment Variables Reference
642
+
643
+ | Variable | Required | Default | Description |
644
+ |----------|----------|---------|-------------|
645
+ | `NOVA_INSTANCE_URL` | Yes | - | Nova instance URL (e.g., `https://nova.wandelbots.io`) |
646
+ | `NOVA_ACCESS_TOKEN` | Yes | - | Nova API access token |
647
+ | `NOVA_CELL_ID` | No | `cell` | Cell ID |
648
+ | `NOVA_CONTROLLER_ID` | Yes | - | Controller ID |
649
+ | `NOVA_MOTION_GROUP_ID` | Yes | - | Motion group ID |
650
+ | `NOVA_MOTION_GROUP_MODEL` | No | Auto-detected | Motion group model (e.g., `ur5e`, `KUKA_KR16_R2010_2`) |
651
+ | `NOVA_TCP_NAME` | No | `Flange` | Tool Center Point name |
652
+ | `NOVA_RESPONSE_RATE_MS` | No | `200` | State stream response rate in milliseconds |
653
+
654
+ **Note:** `NOVA_API` is also supported as an alias for `NOVA_INSTANCE_URL` for backward compatibility.
655
+
656
+ ### How It Works
657
+
658
+ #### State Streaming
659
+ When `use_state_stream` is enabled:
660
+ 1. A background thread connects to Nova's WebSocket state stream
661
+ 2. Robot state (joint positions) is continuously received at the configured rate
662
+ 3. Each time the environment queries robot state, the latest data from Nova is synced
663
+ 4. The simulation updates its joint positions to match the real robot
664
+ 5. MuJoCo forward kinematics computes the end-effector pose
665
+
666
+ **Note:** When state streaming is active, the simulation becomes a digital twin of the real robot.
667
+
668
+ #### Inverse Kinematics
669
+ When `use_ik` is enabled:
670
+ 1. Target poses are sent to Nova's IK API endpoint: `POST /api/v2/cells/{cell_id}/kinematic/inverse`
671
+ 2. Nova computes joint angles using the robot's kinematic model
672
+ 3. Joint targets are returned and applied to the simulation
673
+ 4. If Nova IK fails, the system automatically falls back to local IK computation
674
+
675
+ ### Getting Nova Credentials
676
+
677
+ To obtain your Nova instance credentials:
678
+
679
+ 1. Log in to the [Wandelbots Portal](https://portal.wandelbots.io/)
680
+ 2. Navigate to your Nova instance
681
+ 3. Generate an API access token in the settings
682
+ 4. Note your controller ID and motion group ID from the API documentation
683
+ 5. Use the discovery script to find available robots:
684
+ ```bash
685
+ python3 scripts/discover_nova_simple.py
686
+ ```
687
+
688
+ For more details, see the [Nova API Documentation](https://portal.wandelbots.io/docs/api/v2/ui/).
689
+
690
+ ### Troubleshooting
691
+
692
+ #### "Missing required environment variables"
693
+ Ensure all required variables are set in `.env.local`. Verify loading with:
694
+ ```python
695
+ import os
696
+ print("NOVA_INSTANCE_URL:", os.getenv("NOVA_INSTANCE_URL"))
697
+ ```
698
+
699
+ #### "websockets is required for Nova state streaming"
700
+ Install the websockets package:
701
+ ```bash
702
+ pip install websockets
703
+ ```
704
+
705
+ #### "Nova API error 401"
706
+ Your access token may be invalid or expired. Generate a new token from the Wandelbots Portal.
707
+
708
+ #### State Stream Errors
709
+ - Verify your instance URL is correct
710
+ - Check that controller and motion group IDs are valid
711
+ - Ensure your network can reach the Nova instance
712
+ - Confirm your access token has not expired
713
+
714
+ #### IK Falls Back to Local Solver
715
+ If you see "Nova IK failed, falling back to local IK":
716
+ - Check that the motion group model matches your robot
717
+ - Verify the TCP name matches a TCP defined in your Nova instance
718
+ - Ensure the target pose is reachable by the robot
719
+
720
+ ### Performance Considerations
721
+
722
+ - **State Stream Rate**: Default is 200ms (5Hz). Lower values provide more responsive streaming but increase bandwidth usage
723
+ - **IK Latency**: Nova IK requires a network round-trip (~50-200ms depending on connection quality)
724
+ - **Simulation Rate**: When using state streaming, simulation rate matches the Nova stream rate
725
+
726
+ ### Dependencies
727
+
728
+ - `websockets` (for state streaming): `pip install websockets`
729
+ - `python-dotenv` (optional, for easier `.env` loading): `pip install python-dotenv`
730
+
731
+ ### Helpful Scripts
732
+
733
+ Located in `scripts/`:
734
+ - **`discover_nova_simple.py`**: Discover available controllers and motion groups in your Nova instance
735
+ - **`test_nova_connection.py`**: Verify your Nova API configuration is working correctly
736
+
737
+ ### Implementation Details
738
+
739
+ The Nova API integration is implemented in:
740
+ - [robots/ur5/nova_api.py](robots/ur5/nova_api.py) - API client and configuration
741
+ - [robots/ur5/ur5_env.py](robots/ur5/ur5_env.py) - Environment integration (lines 123-203, 499-520)
742
+
743
  ## License
744
 
745
  This project uses models from:
examples/ur5_nova_api_example.py ADDED
@@ -0,0 +1,155 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #!/usr/bin/env python3
2
+ """Example script demonstrating Nova API integration with UR5 environment.
3
+
4
+ This script shows how to use the Wandelbots Nova API for robot state streaming
5
+ and inverse kinematics with the UR5 simulation environment.
6
+
7
+ Requirements:
8
+ - Nova instance credentials in .env file
9
+ - websockets: pip install websockets
10
+ - python-dotenv: pip install python-dotenv
11
+
12
+ Usage:
13
+ python examples/ur5_nova_api_example.py [--mode MODE]
14
+
15
+ Modes:
16
+ state_only: Enable state streaming only
17
+ ik_only: Enable Nova IK only
18
+ both: Enable both state streaming and Nova IK (default)
19
+ none: Disable Nova API (local simulation only)
20
+ """
21
+
22
+ import argparse
23
+ import sys
24
+ from pathlib import Path
25
+
26
+ # Add parent directory to path
27
+ sys.path.insert(0, str(Path(__file__).parent.parent.parent))
28
+
29
+ try:
30
+ from dotenv import load_dotenv
31
+ except ImportError:
32
+ print("Warning: python-dotenv not installed. Install with: pip install python-dotenv")
33
+ print("Continuing without .env file support...")
34
+ load_dotenv = None
35
+
36
+ from nova_sim.robots.ur5.ur5_env import UR5Env
37
+
38
+
39
+ def main():
40
+ parser = argparse.ArgumentParser(
41
+ description="UR5 Nova API Integration Example",
42
+ formatter_class=argparse.RawDescriptionHelpFormatter,
43
+ epilog=__doc__
44
+ )
45
+ parser.add_argument(
46
+ "--mode",
47
+ choices=["state_only", "ik_only", "both", "none"],
48
+ default="both",
49
+ help="Nova API integration mode"
50
+ )
51
+ parser.add_argument(
52
+ "--steps",
53
+ type=int,
54
+ default=1000,
55
+ help="Number of simulation steps to run"
56
+ )
57
+ parser.add_argument(
58
+ "--no-render",
59
+ action="store_true",
60
+ help="Disable rendering"
61
+ )
62
+ args = parser.parse_args()
63
+
64
+ # Load environment variables
65
+ if load_dotenv is not None:
66
+ env_path = Path(__file__).parent.parent / ".env"
67
+ if env_path.exists():
68
+ load_dotenv(env_path)
69
+ print(f"Loaded environment variables from {env_path}")
70
+ else:
71
+ print(f"Warning: .env file not found at {env_path}")
72
+ print("Copy .env.example to .env and fill in your Nova credentials")
73
+
74
+ # Configure Nova API based on mode
75
+ nova_config = None
76
+ if args.mode != "none":
77
+ use_state = args.mode in ("state_only", "both")
78
+ use_ik = args.mode in ("ik_only", "both")
79
+
80
+ nova_config = {
81
+ "use_state_stream": use_state,
82
+ "use_ik": use_ik
83
+ }
84
+
85
+ print(f"\nNova API Integration Mode: {args.mode}")
86
+ print(f" State Streaming: {'enabled' if use_state else 'disabled'}")
87
+ print(f" Nova IK: {'enabled' if use_ik else 'disabled'}")
88
+ else:
89
+ print("\nNova API Integration: disabled (local simulation only)")
90
+
91
+ # Create environment
92
+ print("\nCreating UR5 environment...")
93
+ try:
94
+ env = UR5Env(
95
+ render_mode="human" if not args.no_render else None,
96
+ scene_name="scene",
97
+ nova_api_config=nova_config
98
+ )
99
+ except Exception as e:
100
+ print(f"Error creating environment: {e}")
101
+ print("\nMake sure your .env file is configured correctly with Nova credentials.")
102
+ return 1
103
+
104
+ print("Environment created successfully!")
105
+
106
+ # Reset environment
107
+ print("\nResetting environment...")
108
+ obs, info = env.reset()
109
+ print(f"Initial observation shape: {obs.shape}")
110
+
111
+ # Get initial state
112
+ joint_pos = env.get_joint_positions()
113
+ ee_pos = env.get_end_effector_pos()
114
+ print(f"Initial joint positions: {joint_pos}")
115
+ print(f"Initial end-effector position: {ee_pos}")
116
+
117
+ # Set a target position
118
+ print("\nSetting target position...")
119
+ target_pos = [0.4, 0.2, 0.6]
120
+ env.set_target(*target_pos)
121
+ print(f"Target position: {target_pos}")
122
+
123
+ # Run simulation
124
+ print(f"\nRunning simulation for {args.steps} steps...")
125
+ print("Press Ctrl+C to stop early")
126
+
127
+ try:
128
+ for step in range(args.steps):
129
+ # Step with controller (uses IK to reach target)
130
+ obs = env.step_with_controller(dt=0.002)
131
+
132
+ # Render
133
+ if not args.no_render:
134
+ env.render()
135
+
136
+ # Print progress every 100 steps
137
+ if (step + 1) % 100 == 0:
138
+ ee_pos = env.get_end_effector_pos()
139
+ distance = ((ee_pos - target_pos) ** 2).sum() ** 0.5
140
+ print(f"Step {step + 1}/{args.steps} - Distance to target: {distance:.4f}m")
141
+
142
+ except KeyboardInterrupt:
143
+ print("\nSimulation interrupted by user")
144
+
145
+ finally:
146
+ # Clean up
147
+ print("\nClosing environment...")
148
+ env.close()
149
+ print("Done!")
150
+
151
+ return 0
152
+
153
+
154
+ if __name__ == "__main__":
155
+ sys.exit(main())
mujoco_server.py CHANGED
@@ -7,6 +7,7 @@ import base64
7
  import cv2
8
  import numpy as np
9
  import mujoco
 
10
  from flask import Flask, Response, render_template_string, request, jsonify
11
  from flask_sock import Sock
12
 
@@ -16,6 +17,21 @@ sys.path.insert(0, os.path.join(_nova_sim_dir, 'robots', 'g1'))
16
  from g1_env import G1Env
17
  sys.path.pop(0)
18
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
19
  # API prefix for all endpoints
20
  API_PREFIX = '/nova-sim/api/v1'
21
 
@@ -76,6 +92,48 @@ cam.lookat = np.array([0, 0, 0.8])
76
  camera_follow = True
77
 
78
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
79
  def init_g1():
80
  """Initialize G1 environment."""
81
  global env_g1
@@ -109,6 +167,15 @@ def init_ur5(scene_name="scene"):
109
  from ur5_env import UR5Env
110
  sys.path.pop(0)
111
 
 
 
 
 
 
 
 
 
 
112
  if scene_name == "scene_t_push":
113
  if env_ur5_t_push is None:
114
  env_ur5_t_push = UR5Env(
@@ -116,12 +183,18 @@ def init_ur5(scene_name="scene"):
116
  width=RENDER_WIDTH,
117
  height=RENDER_HEIGHT,
118
  scene_name="scene_t_push",
 
119
  )
120
  env_ur5_t_push.reset()
121
  return env_ur5_t_push
122
 
123
  if env_ur5 is None:
124
- env_ur5 = UR5Env(render_mode="rgb_array", width=RENDER_WIDTH, height=RENDER_HEIGHT)
 
 
 
 
 
125
  env_ur5.reset()
126
  return env_ur5
127
 
@@ -192,11 +265,17 @@ def broadcast_state():
192
  target = env.get_target()
193
  target_euler = env.get_target_orientation()
194
  gripper = env.get_gripper()
 
195
  joint_pos = env.get_joint_positions()
196
  joint_targets = env.get_joint_targets()
197
  control_mode = env.get_control_mode()
198
  use_orientation = env.get_use_orientation()
199
 
 
 
 
 
 
200
  state_msg = json.dumps({
201
  'type': 'state',
202
  'data': {
@@ -206,12 +285,18 @@ def broadcast_state():
206
  'target': {'x': float(target[0]), 'y': float(target[1]), 'z': float(target[2])},
207
  'target_orientation': {'roll': float(target_euler[0]), 'pitch': float(target_euler[1]), 'yaw': float(target_euler[2])},
208
  'gripper': float(gripper),
 
209
  'joint_positions': [float(j) for j in joint_pos],
210
  'joint_targets': [float(j) for j in joint_targets],
211
  'control_mode': control_mode,
212
  'use_orientation': use_orientation,
213
  'steps': int(steps),
214
- 'reward': env.get_task_reward()
 
 
 
 
 
215
  }
216
  })
217
  else:
@@ -864,9 +949,16 @@ def index():
864
  <strong>Arm State</strong><br>
865
  EE Pos: <span id="ee_pos">0.00, 0.00, 0.00</span><br>
866
  EE Ori: <span id="ee_ori">0.00, 0.00, 0.00</span><br>
867
- Gripper: <span id="gripper_val">50%</span><br>
868
  Reward: <span id="arm_reward">-</span><br>
869
- Mode: <span id="control_mode_display">IK</span> | Steps: <span id="arm_step_val">0</span>
 
 
 
 
 
 
 
870
  </div>
871
  </div>
872
 
@@ -944,17 +1036,17 @@ def index():
944
  <div class="target-sliders">
945
  <div class="slider-row">
946
  <label>X</label>
947
- <input type="range" id="target_x" min="0.1" max="0.7" step="0.01" value="0.4" oninput="updateTarget()">
948
  <span class="val-display" id="target_x_val">0.40</span>
949
  </div>
950
  <div class="slider-row">
951
  <label>Y</label>
952
- <input type="range" id="target_y" min="-0.5" max="0.5" step="0.01" value="0.0" oninput="updateTarget()">
953
  <span class="val-display" id="target_y_val">0.00</span>
954
  </div>
955
  <div class="slider-row">
956
  <label>Z</label>
957
- <input type="range" id="target_z" min="0.1" max="1.0" step="0.01" value="0.4" oninput="updateTarget()">
958
  <span class="val-display" id="target_z_val">0.40</span>
959
  </div>
960
  </div>
@@ -1025,7 +1117,7 @@ def index():
1025
  </div>
1026
  </div>
1027
 
1028
- <div class="control-group">
1029
  <label>Gripper</label>
1030
  <div class="gripper-btns">
1031
  <button class="rl-btn" onclick="setGripper('open')">Open</button>
@@ -1059,7 +1151,7 @@ def index():
1059
  'g1': '29 DOF humanoid with RL walking policy',
1060
  'spot': '12 DOF quadruped with trot gait controller',
1061
  'ur5': '6 DOF robot arm with Robotiq gripper',
1062
- 'ur5_t_push': 'UR5e T-push task with stick tool'
1063
  };
1064
 
1065
  const robotTitles = {
@@ -1128,9 +1220,20 @@ def index():
1128
  euler[0].toFixed(2) + ', ' + euler[1].toFixed(2) + ', ' + euler[2].toFixed(2);
1129
  }
1130
 
1131
- // Gripper: 0=open, 255=closed (Robotiq 2F-85)
1132
- document.getElementById('gripper_val').innerText =
1133
- ((255 - data.gripper) / 255 * 100).toFixed(0) + '% open';
 
 
 
 
 
 
 
 
 
 
 
1134
  document.getElementById('arm_step_val').innerText = data.steps;
1135
  const rewardEl = document.getElementById('arm_reward');
1136
  if (data.reward === null || data.reward === undefined) {
@@ -1195,6 +1298,37 @@ def index():
1195
  document.getElementById('orientation_sliders').style.opacity = data.use_orientation ? '1' : '0.4';
1196
  }
1197
  }
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1198
  } else {
1199
  // Locomotion state
1200
  heightVal.innerText = data.base_height.toFixed(2);
 
7
  import cv2
8
  import numpy as np
9
  import mujoco
10
+ from pathlib import Path
11
  from flask import Flask, Response, render_template_string, request, jsonify
12
  from flask_sock import Sock
13
 
 
17
  from g1_env import G1Env
18
  sys.path.pop(0)
19
 
20
+ # Load .env.local for Nova API configuration
21
+ def load_env_file(filepath):
22
+ """Load environment variables from .env file."""
23
+ if not filepath.exists():
24
+ return
25
+ with open(filepath, 'r') as f:
26
+ for line in f:
27
+ line = line.strip()
28
+ if line and not line.startswith('#') and '=' in line:
29
+ key, value = line.split('=', 1)
30
+ os.environ[key.strip()] = value.strip()
31
+
32
+ env_local_path = Path(_nova_sim_dir) / '.env.local'
33
+ load_env_file(env_local_path)
34
+
35
  # API prefix for all endpoints
36
  API_PREFIX = '/nova-sim/api/v1'
37
 
 
92
  camera_follow = True
93
 
94
 
95
+ def _parse_bool(value):
96
+ if value is None:
97
+ return False
98
+ return str(value).strip().lower() in ("1", "true", "yes", "on")
99
+
100
+
101
+ def _load_nova_ur5_config_from_env():
102
+ use_state = _parse_bool(os.environ.get("NOVA_UR5_USE_STATE_STREAM") or os.environ.get("NOVA_USE_STATE_STREAM"))
103
+ use_ik = _parse_bool(os.environ.get("NOVA_UR5_USE_IK") or os.environ.get("NOVA_USE_IK"))
104
+ if not (use_state or use_ik):
105
+ return None
106
+
107
+ instance_url = os.environ.get("NOVA_INSTANCE_URL")
108
+ access_token = os.environ.get("NOVA_ACCESS_TOKEN")
109
+ cell_id = os.environ.get("NOVA_CELL_ID")
110
+ controller_id = os.environ.get("NOVA_CONTROLLER_ID")
111
+ motion_group_id = os.environ.get("NOVA_MOTION_GROUP_ID")
112
+ missing = [key for key, value in {
113
+ "NOVA_INSTANCE_URL": instance_url,
114
+ "NOVA_ACCESS_TOKEN": access_token,
115
+ "NOVA_CELL_ID": cell_id,
116
+ "NOVA_CONTROLLER_ID": controller_id,
117
+ "NOVA_MOTION_GROUP_ID": motion_group_id,
118
+ }.items() if not value]
119
+ if missing:
120
+ print(f"Nova API integration disabled; missing env vars: {', '.join(missing)}")
121
+ return None
122
+
123
+ return {
124
+ "instance_url": instance_url,
125
+ "access_token": access_token,
126
+ "cell_id": cell_id,
127
+ "controller_id": controller_id,
128
+ "motion_group_id": motion_group_id,
129
+ "motion_group_model": os.environ.get("NOVA_MOTION_GROUP_MODEL"),
130
+ "tcp_name": os.environ.get("NOVA_TCP_NAME"),
131
+ "response_rate_ms": int(os.environ.get("NOVA_RESPONSE_RATE_MS", "200")),
132
+ "use_state_stream": use_state,
133
+ "use_ik": use_ik,
134
+ }
135
+
136
+
137
  def init_g1():
138
  """Initialize G1 environment."""
139
  global env_g1
 
167
  from ur5_env import UR5Env
168
  sys.path.pop(0)
169
 
170
+ # Check if Nova API is configured via environment variables
171
+ nova_config = None
172
+ if os.getenv('NOVA_INSTANCE_URL') and os.getenv('NOVA_ACCESS_TOKEN'):
173
+ print("Nova API configuration detected, enabling state streaming and IK")
174
+ nova_config = {
175
+ "use_state_stream": True,
176
+ "use_ik": True
177
+ }
178
+
179
  if scene_name == "scene_t_push":
180
  if env_ur5_t_push is None:
181
  env_ur5_t_push = UR5Env(
 
183
  width=RENDER_WIDTH,
184
  height=RENDER_HEIGHT,
185
  scene_name="scene_t_push",
186
+ nova_api_config=nova_config
187
  )
188
  env_ur5_t_push.reset()
189
  return env_ur5_t_push
190
 
191
  if env_ur5 is None:
192
+ env_ur5 = UR5Env(
193
+ render_mode="rgb_array",
194
+ width=RENDER_WIDTH,
195
+ height=RENDER_HEIGHT,
196
+ nova_api_config=nova_config
197
+ )
198
  env_ur5.reset()
199
  return env_ur5
200
 
 
265
  target = env.get_target()
266
  target_euler = env.get_target_orientation()
267
  gripper = env.get_gripper()
268
+ has_gripper = env.has_gripper
269
  joint_pos = env.get_joint_positions()
270
  joint_targets = env.get_joint_targets()
271
  control_mode = env.get_control_mode()
272
  use_orientation = env.get_use_orientation()
273
 
274
+ # Check Nova API status
275
+ nova_state_streaming = getattr(env, '_use_nova_state_stream', False)
276
+ nova_ik = getattr(env, '_use_nova_ik', False)
277
+ nova_connected = getattr(env, '_nova_client', None) is not None
278
+
279
  state_msg = json.dumps({
280
  'type': 'state',
281
  'data': {
 
285
  'target': {'x': float(target[0]), 'y': float(target[1]), 'z': float(target[2])},
286
  'target_orientation': {'roll': float(target_euler[0]), 'pitch': float(target_euler[1]), 'yaw': float(target_euler[2])},
287
  'gripper': float(gripper),
288
+ 'has_gripper': has_gripper,
289
  'joint_positions': [float(j) for j in joint_pos],
290
  'joint_targets': [float(j) for j in joint_targets],
291
  'control_mode': control_mode,
292
  'use_orientation': use_orientation,
293
  'steps': int(steps),
294
+ 'reward': env.get_task_reward(),
295
+ 'nova_api': {
296
+ 'connected': nova_connected,
297
+ 'state_streaming': nova_state_streaming,
298
+ 'ik': nova_ik
299
+ }
300
  }
301
  })
302
  else:
 
949
  <strong>Arm State</strong><br>
950
  EE Pos: <span id="ee_pos">0.00, 0.00, 0.00</span><br>
951
  EE Ori: <span id="ee_ori">0.00, 0.00, 0.00</span><br>
952
+ <span id="gripper_state_display">Gripper: <span id="gripper_val">50%</span><br></span>
953
  Reward: <span id="arm_reward">-</span><br>
954
+ Mode: <span id="control_mode_display">IK</span> | Steps: <span id="arm_step_val">0</span><br>
955
+ <div style="margin-top: 8px; padding-top: 8px; border-top: 1px solid rgba(188, 190, 236, 0.2);">
956
+ <strong style="font-size: 0.9em;">Controllers:</strong><br>
957
+ <span style="font-size: 0.85em;">
958
+ State: <span id="nova_state_controller" style="color: var(--wb-highlight);">Internal</span><br>
959
+ IK: <span id="nova_ik_controller" style="color: var(--wb-highlight);">Internal</span>
960
+ </span>
961
+ </div>
962
  </div>
963
  </div>
964
 
 
1036
  <div class="target-sliders">
1037
  <div class="slider-row">
1038
  <label>X</label>
1039
+ <input type="range" id="target_x" min="-0.4" max="1.0" step="0.01" value="0.4" oninput="updateTarget()">
1040
  <span class="val-display" id="target_x_val">0.40</span>
1041
  </div>
1042
  <div class="slider-row">
1043
  <label>Y</label>
1044
+ <input type="range" id="target_y" min="-0.8" max="0.8" step="0.01" value="0.0" oninput="updateTarget()">
1045
  <span class="val-display" id="target_y_val">0.00</span>
1046
  </div>
1047
  <div class="slider-row">
1048
  <label>Z</label>
1049
+ <input type="range" id="target_z" min="0.0" max="1.2" step="0.01" value="0.4" oninput="updateTarget()">
1050
  <span class="val-display" id="target_z_val">0.40</span>
1051
  </div>
1052
  </div>
 
1117
  </div>
1118
  </div>
1119
 
1120
+ <div class="control-group" id="gripper_controls">
1121
  <label>Gripper</label>
1122
  <div class="gripper-btns">
1123
  <button class="rl-btn" onclick="setGripper('open')">Open</button>
 
1151
  'g1': '29 DOF humanoid with RL walking policy',
1152
  'spot': '12 DOF quadruped with trot gait controller',
1153
  'ur5': '6 DOF robot arm with Robotiq gripper',
1154
+ 'ur5_t_push': 'UR5e T-push task with stick tool (no gripper)'
1155
  };
1156
 
1157
  const robotTitles = {
 
1220
  euler[0].toFixed(2) + ', ' + euler[1].toFixed(2) + ', ' + euler[2].toFixed(2);
1221
  }
1222
 
1223
+ // Show/hide gripper UI based on has_gripper
1224
+ const gripperStateDisplay = document.getElementById('gripper_state_display');
1225
+ const gripperControls = document.getElementById('gripper_controls');
1226
+ if (data.has_gripper) {
1227
+ gripperStateDisplay.style.display = '';
1228
+ gripperControls.style.display = '';
1229
+ // Gripper: 0=open, 255=closed (Robotiq 2F-85)
1230
+ document.getElementById('gripper_val').innerText =
1231
+ ((255 - data.gripper) / 255 * 100).toFixed(0) + '% open';
1232
+ } else {
1233
+ gripperStateDisplay.style.display = 'none';
1234
+ gripperControls.style.display = 'none';
1235
+ }
1236
+
1237
  document.getElementById('arm_step_val').innerText = data.steps;
1238
  const rewardEl = document.getElementById('arm_reward');
1239
  if (data.reward === null || data.reward === undefined) {
 
1298
  document.getElementById('orientation_sliders').style.opacity = data.use_orientation ? '1' : '0.4';
1299
  }
1300
  }
1301
+
1302
+ // Update Nova API controller status
1303
+ if (data.nova_api) {
1304
+ const stateController = document.getElementById('nova_state_controller');
1305
+ const ikController = document.getElementById('nova_ik_controller');
1306
+
1307
+ if (data.nova_api.connected) {
1308
+ // Nova API is connected
1309
+ if (data.nova_api.state_streaming) {
1310
+ stateController.innerText = 'Nova API';
1311
+ stateController.style.color = 'var(--wb-success)';
1312
+ } else {
1313
+ stateController.innerText = 'Internal';
1314
+ stateController.style.color = 'var(--wb-highlight)';
1315
+ }
1316
+
1317
+ if (data.nova_api.ik) {
1318
+ ikController.innerText = 'Nova API';
1319
+ ikController.style.color = 'var(--wb-success)';
1320
+ } else {
1321
+ ikController.innerText = 'Internal';
1322
+ ikController.style.color = 'var(--wb-highlight)';
1323
+ }
1324
+ } else {
1325
+ // Nova API not connected - using internal
1326
+ stateController.innerText = 'Internal';
1327
+ stateController.style.color = 'var(--wb-highlight)';
1328
+ ikController.innerText = 'Internal';
1329
+ ikController.style.color = 'var(--wb-highlight)';
1330
+ }
1331
+ }
1332
  } else {
1333
  // Locomotion state
1334
  heightVal.innerText = data.base_height.toFixed(2);
robots/ur5/model/scene_t_push.xml CHANGED
@@ -39,10 +39,6 @@
39
  <!-- Wandelbots purple accent instead of UR blue -->
40
  <material name="urblue" rgba="0.55 0.5 0.94 1" specular="0.6" shininess="0.35"/>
41
 
42
- <!-- Gripper materials -->
43
- <material name="metal" rgba="0.58 0.58 0.58 1"/>
44
- <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
45
- <material name="gray" rgba="0.4627 0.4627 0.4627 1"/>
46
 
47
  <!-- UR5e meshes -->
48
  <mesh file="base_0.obj"/>
@@ -66,15 +62,6 @@
66
  <mesh file="wrist2_2.obj"/>
67
  <mesh file="wrist3.obj"/>
68
 
69
- <!-- Gripper meshes -->
70
- <mesh name="base_mount" file="base_mount.stl" scale="0.001 0.001 0.001"/>
71
- <mesh name="base_g" file="base.stl" scale="0.001 0.001 0.001"/>
72
- <mesh name="driver" file="driver.stl" scale="0.001 0.001 0.001"/>
73
- <mesh name="coupler" file="coupler.stl" scale="0.001 0.001 0.001"/>
74
- <mesh name="follower" file="follower.stl" scale="0.001 0.001 0.001"/>
75
- <mesh name="pad" file="pad.stl" scale="0.001 0.001 0.001"/>
76
- <mesh name="silicone_pad" file="silicone_pad.stl" scale="0.001 0.001 0.001"/>
77
- <mesh name="spring_link" file="spring_link.stl" scale="0.001 0.001 0.001"/>
78
  </asset>
79
 
80
  <default>
@@ -102,36 +89,6 @@
102
  <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
103
  </default>
104
 
105
- <default class="gripper">
106
- <general biastype="affine"/>
107
- <joint axis="1 0 0"/>
108
- <default class="driver_j">
109
- <joint range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
110
- </default>
111
- <default class="follower_j">
112
- <joint range="-0.872664 0.872664" armature="0.001" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
113
- </default>
114
- <default class="spring_link_j">
115
- <joint range="-0.29670597283 0.8" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
116
- </default>
117
- <default class="coupler_j">
118
- <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
119
- </default>
120
- <default class="visual_g">
121
- <geom type="mesh" contype="0" conaffinity="0" group="2"/>
122
- </default>
123
- <default class="collision_g">
124
- <geom type="mesh" group="3"/>
125
- <default class="pad_box1">
126
- <geom mass="0" type="box" pos="0 -0.0026 0.028125" size="0.011 0.004 0.009375" friction="0.7"
127
- solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
128
- </default>
129
- <default class="pad_box2">
130
- <geom mass="0" type="box" pos="0 -0.0026 0.009375" size="0.011 0.004 0.009375" friction="0.6"
131
- solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
132
- </default>
133
- </default>
134
- </default>
135
  </default>
136
 
137
  <worldbody>
@@ -204,105 +161,16 @@
204
  <geom material="linkgray" mesh="wrist3" class="visual"/>
205
  <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
206
  <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
207
- <!-- Stick tool for T-push task (cylindrical, pointing -z from flange) -->
208
- <geom name="push_stick" type="cylinder" pos="0 0.1 -0.09" size="0.008 0.09"
209
- material="stick_mat" mass="0.02" friction="1 0.01 0.01"/>
210
 
211
- <!-- Gripper attached to wrist -->
212
- <body name="gripper_base_mount" pos="0 0.1 0" quat="-1 1 0 0" childclass="gripper">
213
- <body name="gripper_base_mount_inner" pos="0 0 0.007">
214
- <geom class="visual_g" mesh="base_mount" material="black"/>
215
- <geom class="collision_g" mesh="base_mount"/>
216
- <body name="gripper_base" pos="0 0 0.0038" quat="1 0 0 -1">
217
- <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0"
218
- diaginertia="0.000260285 0.000225381 0.000152708"/>
219
- <geom class="visual_g" mesh="base_g" material="black"/>
220
- <geom class="collision_g" mesh="base_g"/>
221
- <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
222
- <!-- End-effector site for IK -->
223
- <site name="ee_site" pos="0 0 0.16" type="sphere" size="0.01" rgba="1 0 0 0.5"/>
224
-
225
- <!-- Right-hand side 4-bar linkage -->
226
- <body name="right_driver" pos="0 0.0306011 0.054904">
227
- <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
228
- diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
229
- <joint name="right_driver_joint" class="driver_j"/>
230
- <geom class="visual_g" mesh="driver" material="gray"/>
231
- <geom class="collision_g" mesh="driver"/>
232
- <body name="right_coupler" pos="0 0.0315 -0.0041">
233
- <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
234
- diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
235
- <joint name="right_coupler_joint" class="coupler_j"/>
236
- <geom class="visual_g" mesh="coupler" material="black"/>
237
- <geom class="collision_g" mesh="coupler"/>
238
- </body>
239
- </body>
240
- <body name="right_spring_link" pos="0 0.0132 0.0609">
241
- <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
242
- diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
243
- <joint name="right_spring_link_joint" class="spring_link_j"/>
244
- <geom class="visual_g" mesh="spring_link" material="black"/>
245
- <geom class="collision_g" mesh="spring_link"/>
246
- <body name="right_follower" pos="0 0.055 0.0375">
247
- <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
248
- diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
249
- <joint name="right_follower_joint" class="follower_j"/>
250
- <geom class="visual_g" mesh="follower" material="black"/>
251
- <geom class="collision_g" mesh="follower"/>
252
- <body name="right_pad" pos="0 -0.0189 0.01352">
253
- <geom class="pad_box1" name="right_pad1"/>
254
- <geom class="pad_box2" name="right_pad2"/>
255
- <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107"
256
- diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
257
- <geom class="visual_g" mesh="pad"/>
258
- <body name="right_silicone_pad">
259
- <geom class="visual_g" mesh="silicone_pad" material="black"/>
260
- </body>
261
- </body>
262
- </body>
263
- </body>
264
- <!-- Left-hand side 4-bar linkage -->
265
- <body name="left_driver" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
266
- <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
267
- diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
268
- <joint name="left_driver_joint" class="driver_j"/>
269
- <geom class="visual_g" mesh="driver" material="gray"/>
270
- <geom class="collision_g" mesh="driver"/>
271
- <body name="left_coupler" pos="0 0.0315 -0.0041">
272
- <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
273
- diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
274
- <joint name="left_coupler_joint" class="coupler_j"/>
275
- <geom class="visual_g" mesh="coupler" material="black"/>
276
- <geom class="collision_g" mesh="coupler"/>
277
- </body>
278
- </body>
279
- <body name="left_spring_link" pos="0 -0.0132 0.0609" quat="0 0 0 1">
280
- <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
281
- diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
282
- <joint name="left_spring_link_joint" class="spring_link_j"/>
283
- <geom class="visual_g" mesh="spring_link" material="black"/>
284
- <geom class="collision_g" mesh="spring_link"/>
285
- <body name="left_follower" pos="0 0.055 0.0375">
286
- <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
287
- diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
288
- <joint name="left_follower_joint" class="follower_j"/>
289
- <geom class="visual_g" mesh="follower" material="black"/>
290
- <geom class="collision_g" mesh="follower"/>
291
- <body name="left_pad" pos="0 -0.0189 0.01352">
292
- <geom class="pad_box1" name="left_pad1"/>
293
- <geom class="pad_box2" name="left_pad2"/>
294
- <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1"
295
- diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
296
- <geom class="visual_g" mesh="pad"/>
297
- <body name="left_silicone_pad">
298
- <geom class="visual_g" mesh="silicone_pad" material="black"/>
299
- </body>
300
- </body>
301
- </body>
302
- </body>
303
- </body>
304
- </body>
305
  </body>
 
 
 
306
  </body>
307
  </body>
308
  </body>
@@ -325,28 +193,6 @@
325
  </body>
326
  </worldbody>
327
 
328
- <contact>
329
- <exclude body1="gripper_base" body2="left_driver"/>
330
- <exclude body1="gripper_base" body2="right_driver"/>
331
- <exclude body1="gripper_base" body2="left_spring_link"/>
332
- <exclude body1="gripper_base" body2="right_spring_link"/>
333
- <exclude body1="right_coupler" body2="right_follower"/>
334
- <exclude body1="left_coupler" body2="left_follower"/>
335
- </contact>
336
-
337
- <tendon>
338
- <fixed name="split">
339
- <joint joint="right_driver_joint" coef="0.5"/>
340
- <joint joint="left_driver_joint" coef="0.5"/>
341
- </fixed>
342
- </tendon>
343
-
344
- <equality>
345
- <connect anchor="0 0 0" body1="right_follower" body2="right_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
346
- <connect anchor="0 0 0" body1="left_follower" body2="left_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
347
- <joint joint1="right_driver_joint" joint2="left_driver_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref="0.005 1"/>
348
- </equality>
349
-
350
  <actuator>
351
  <!-- UR5e joint actuators -->
352
  <general class="size3" name="shoulder_pan" joint="shoulder_pan_joint"/>
@@ -355,13 +201,11 @@
355
  <general class="size1" name="wrist_1" joint="wrist_1_joint"/>
356
  <general class="size1" name="wrist_2" joint="wrist_2_joint"/>
357
  <general class="size1" name="wrist_3" joint="wrist_3_joint"/>
358
- <!-- Gripper actuator -->
359
- <general name="gripper" tendon="split" forcerange="-5 5" ctrlrange="0 255" gainprm="0.3137255 0 0" biasprm="0 -100 -10"/>
360
  </actuator>
361
 
362
  <keyframe>
363
- <!-- Official MuJoCo Menagerie UR5e home pose with gripper open (0=open) -->
364
- <key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0 0 0 0 0 0 0 0 0.45 0.2 0.43 1 0 0 0"
365
- ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0"/>
366
  </keyframe>
367
  </mujoco>
 
39
  <!-- Wandelbots purple accent instead of UR blue -->
40
  <material name="urblue" rgba="0.55 0.5 0.94 1" specular="0.6" shininess="0.35"/>
41
 
 
 
 
 
42
 
43
  <!-- UR5e meshes -->
44
  <mesh file="base_0.obj"/>
 
62
  <mesh file="wrist2_2.obj"/>
63
  <mesh file="wrist3.obj"/>
64
 
 
 
 
 
 
 
 
 
 
65
  </asset>
66
 
67
  <default>
 
89
  <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
90
  </default>
91
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
92
  </default>
93
 
94
  <worldbody>
 
161
  <geom material="linkgray" mesh="wrist3" class="visual"/>
162
  <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
163
  <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
 
 
 
164
 
165
+ <!-- Stick tool: attach as child body with flange orientation, stick extends along +Z in flange frame -->
166
+ <body name="push_stick_body" pos="0 0.1 0" quat="-1 1 0 0">
167
+ <!-- In flange frame: +Z points away from robot (downward when wrist is horizontal) -->
168
+ <geom name="push_stick" type="cylinder" fromto="0 0 0 0 0 0.18" size="0.008"
169
+ material="stick_mat" mass="0.02" friction="1 0.01 0.01"/>
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
170
  </body>
171
+
172
+ <!-- End-effector site for IK (at flange) -->
173
+ <site name="ee_site" pos="0 0.1 0" quat="-1 1 0 0" type="sphere" size="0.01" rgba="1 0 0 0.5"/>
174
  </body>
175
  </body>
176
  </body>
 
193
  </body>
194
  </worldbody>
195
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
196
  <actuator>
197
  <!-- UR5e joint actuators -->
198
  <general class="size3" name="shoulder_pan" joint="shoulder_pan_joint"/>
 
201
  <general class="size1" name="wrist_1" joint="wrist_1_joint"/>
202
  <general class="size1" name="wrist_2" joint="wrist_2_joint"/>
203
  <general class="size1" name="wrist_3" joint="wrist_3_joint"/>
 
 
204
  </actuator>
205
 
206
  <keyframe>
207
+ <!-- UR5e home pose (6 joints) + t_object freejoint (3 pos + 4 quat) -->
208
+ <key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0.45 0.2 0.43 1 0 0 0"
209
+ ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0"/>
210
  </keyframe>
211
  </mujoco>
robots/ur5/model/scene_with_gripper.xml ADDED
@@ -0,0 +1,192 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ <mujoco model="ur5e">
2
+ <compiler angle="radian" meshdir="assets" autolimits="true"/>
3
+
4
+ <option integrator="implicitfast" cone="elliptic" impratio="10"/>
5
+
6
+ <!-- Wandelbots Corporate Design Colors:
7
+ Primary Dark: #01040f (0.004, 0.016, 0.059)
8
+ Light/Secondary: #bcbeec (0.737, 0.745, 0.925)
9
+ Accent: #211c44 (0.129, 0.110, 0.267)
10
+ Highlight: #8b7fef (0.545, 0.498, 0.937)
11
+ -->
12
+ <visual>
13
+ <headlight diffuse="0.6 0.6 0.6" ambient="0.35 0.35 0.4" specular="0 0 0"/>
14
+ <rgba haze="0.02 0.04 0.12 1"/>
15
+ <global azimuth="120" elevation="-20"/>
16
+ </visual>
17
+
18
+ <asset>
19
+ <!-- Wandelbots gradient skybox - deep purple to near black -->
20
+ <texture type="skybox" builtin="gradient" rgb1="0.13 0.11 0.27" rgb2="0.004 0.016 0.059" width="512" height="3072"/>
21
+ <!-- Ground with Wandelbots purple accent -->
22
+ <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.08 0.07 0.15" rgb2="0.04 0.04 0.08"
23
+ markrgb="0.55 0.5 0.94" width="300" height="300"/>
24
+ <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.15"/>
25
+ <!-- Table with subtle purple tint -->
26
+ <material name="table" rgba="0.18 0.16 0.25 1" specular="0.4" shininess="0.4"/>
27
+ <!-- Target marker with Wandelbots highlight color -->
28
+ <material name="target_mat" rgba="0.55 0.5 0.94 0.6" specular="0.5" shininess="0.5"/>
29
+
30
+ <!-- UR5e materials - with Wandelbots accent colors -->
31
+ <material name="black" rgba="0.02 0.02 0.04 1" specular="0.5" shininess="0.25"/>
32
+ <material name="jointgray" rgba="0.22 0.22 0.26 1" specular="0.5" shininess="0.25"/>
33
+ <material name="linkgray" rgba="0.74 0.75 0.82 1" specular="0.5" shininess="0.25"/>
34
+ <!-- Wandelbots purple accent instead of UR blue -->
35
+ <material name="urblue" rgba="0.55 0.5 0.94 1" specular="0.6" shininess="0.35"/>
36
+
37
+
38
+ <!-- UR5e meshes -->
39
+ <mesh file="base_0.obj"/>
40
+ <mesh file="base_1.obj"/>
41
+ <mesh file="shoulder_0.obj"/>
42
+ <mesh file="shoulder_1.obj"/>
43
+ <mesh file="shoulder_2.obj"/>
44
+ <mesh file="upperarm_0.obj"/>
45
+ <mesh file="upperarm_1.obj"/>
46
+ <mesh file="upperarm_2.obj"/>
47
+ <mesh file="upperarm_3.obj"/>
48
+ <mesh file="forearm_0.obj"/>
49
+ <mesh file="forearm_1.obj"/>
50
+ <mesh file="forearm_2.obj"/>
51
+ <mesh file="forearm_3.obj"/>
52
+ <mesh file="wrist1_0.obj"/>
53
+ <mesh file="wrist1_1.obj"/>
54
+ <mesh file="wrist1_2.obj"/>
55
+ <mesh file="wrist2_0.obj"/>
56
+ <mesh file="wrist2_1.obj"/>
57
+ <mesh file="wrist2_2.obj"/>
58
+ <mesh file="wrist3.obj"/>
59
+
60
+ </asset>
61
+
62
+ <default>
63
+ <default class="ur5e">
64
+ <joint axis="0 1 0" range="-6.28319 6.28319" armature="0.1"/>
65
+ <general gaintype="fixed" biastype="affine" ctrlrange="-6.2831 6.2831" gainprm="2000" biasprm="0 -2000 -400" forcerange="-150 150"/>
66
+ <default class="size3">
67
+ <default class="size3_limited">
68
+ <joint range="-3.1415 3.1415"/>
69
+ <general ctrlrange="-3.1415 3.1415"/>
70
+ </default>
71
+ </default>
72
+ <default class="size1">
73
+ <general gainprm="500" biasprm="0 -500 -100" forcerange="-28 28"/>
74
+ </default>
75
+ <default class="visual">
76
+ <geom type="mesh" contype="0" conaffinity="0" group="2"/>
77
+ </default>
78
+ <default class="collision">
79
+ <geom type="capsule" group="3"/>
80
+ <default class="eef_collision">
81
+ <geom type="cylinder"/>
82
+ </default>
83
+ </default>
84
+ <site size="0.001" rgba="0.5 0.5 0.5 0.3" group="4"/>
85
+ </default>
86
+
87
+ </default>
88
+
89
+ <worldbody>
90
+ <light pos="0 0 3.5" dir="0 0 -1" directional="true"/>
91
+ <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
92
+
93
+ <!-- Table -->
94
+ <body name="table" pos="0 0 0">
95
+ <geom name="table_top" type="box" pos="0.5 0 0.4" size="0.4 0.6 0.02" material="table"/>
96
+ <geom name="table_leg1" type="box" pos="0.2 0.4 0.2" size="0.03 0.03 0.2" material="table"/>
97
+ <geom name="table_leg2" type="box" pos="0.2 -0.4 0.2" size="0.03 0.03 0.2" material="table"/>
98
+ <geom name="table_leg3" type="box" pos="0.8 0.4 0.2" size="0.03 0.03 0.2" material="table"/>
99
+ <geom name="table_leg4" type="box" pos="0.8 -0.4 0.2" size="0.03 0.03 0.2" material="table"/>
100
+ </body>
101
+
102
+ <!-- Target visualization sphere (for IK target) -->
103
+ <body name="target" pos="0.4 0.0 0.6" mocap="true">
104
+ <geom name="target_vis" type="sphere" size="0.03" material="target_mat" contype="0" conaffinity="0"/>
105
+ </body>
106
+
107
+ <!-- UR5e robot mounted on table edge -->
108
+ <body name="base" pos="0 0 0.42" quat="0 0 0 -1" childclass="ur5e">
109
+ <inertial mass="4.0" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
110
+ <geom mesh="base_0" material="black" class="visual"/>
111
+ <geom mesh="base_1" material="jointgray" class="visual"/>
112
+ <body name="shoulder_link" pos="0 0 0.163">
113
+ <inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666"/>
114
+ <joint name="shoulder_pan_joint" class="size3" axis="0 0 1"/>
115
+ <geom mesh="shoulder_0" material="urblue" class="visual"/>
116
+ <geom mesh="shoulder_1" material="black" class="visual"/>
117
+ <geom mesh="shoulder_2" material="jointgray" class="visual"/>
118
+ <geom class="collision" size="0.06 0.06" pos="0 0 -0.04"/>
119
+ <body name="upper_arm_link" pos="0 0.138 0" quat="1 0 1 0">
120
+ <inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074"/>
121
+ <joint name="shoulder_lift_joint" class="size3"/>
122
+ <geom mesh="upperarm_0" material="linkgray" class="visual"/>
123
+ <geom mesh="upperarm_1" material="black" class="visual"/>
124
+ <geom mesh="upperarm_2" material="jointgray" class="visual"/>
125
+ <geom mesh="upperarm_3" material="urblue" class="visual"/>
126
+ <geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06"/>
127
+ <geom class="collision" size="0.05 0.2" pos="0 0 0.2"/>
128
+ <body name="forearm_link" pos="0 -0.131 0.425">
129
+ <inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095"/>
130
+ <joint name="elbow_joint" class="size3_limited"/>
131
+ <geom mesh="forearm_0" material="urblue" class="visual"/>
132
+ <geom mesh="forearm_1" material="linkgray" class="visual"/>
133
+ <geom mesh="forearm_2" material="black" class="visual"/>
134
+ <geom mesh="forearm_3" material="jointgray" class="visual"/>
135
+ <geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06"/>
136
+ <geom class="collision" size="0.038 0.19" pos="0 0 0.2"/>
137
+ <body name="wrist_1_link" pos="0 0 0.392" quat="1 0 1 0">
138
+ <inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942"/>
139
+ <joint name="wrist_1_joint" class="size1"/>
140
+ <geom mesh="wrist1_0" material="black" class="visual"/>
141
+ <geom mesh="wrist1_1" material="urblue" class="visual"/>
142
+ <geom mesh="wrist1_2" material="jointgray" class="visual"/>
143
+ <geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07"/>
144
+ <body name="wrist_2_link" pos="0 0.127 0">
145
+ <inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942"/>
146
+ <joint name="wrist_2_joint" axis="0 0 1" class="size1"/>
147
+ <geom mesh="wrist2_0" material="black" class="visual"/>
148
+ <geom mesh="wrist2_1" material="urblue" class="visual"/>
149
+ <geom mesh="wrist2_2" material="jointgray" class="visual"/>
150
+ <geom class="collision" size="0.04 0.06" pos="0 0 0.04"/>
151
+ <geom class="collision" pos="0 0.02 0.1" quat="1 1 0 0" size="0.04 0.04"/>
152
+ <body name="wrist_3_link" pos="0 0 0.1">
153
+ <inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1"
154
+ diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
155
+ <joint name="wrist_3_joint" class="size1"/>
156
+ <geom material="linkgray" mesh="wrist3" class="visual"/>
157
+ <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
158
+ <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
159
+
160
+ <!-- End-effector site for IK (no gripper) -->
161
+ <site name="ee_site" pos="0 0.1 0" quat="-1 1 0 0" type="sphere" size="0.01" rgba="1 0 0 0.5"/>
162
+ </body>
163
+ </body>
164
+ </body>
165
+ </body>
166
+ </body>
167
+ </body>
168
+
169
+ <!-- Optional: object to manipulate - Wandelbots lavender accent -->
170
+ <body name="box" pos="0.5 0.2 0.45">
171
+ <freejoint name="box_joint"/>
172
+ <geom name="box_geom" type="box" size="0.025 0.025 0.025" rgba="0.74 0.75 0.93 1" mass="0.1"/>
173
+ </body>
174
+ </worldbody>
175
+
176
+ <actuator>
177
+ <!-- UR5e joint actuators -->
178
+ <general class="size3" name="shoulder_pan" joint="shoulder_pan_joint"/>
179
+ <general class="size3" name="shoulder_lift" joint="shoulder_lift_joint"/>
180
+ <general class="size3_limited" name="elbow" joint="elbow_joint"/>
181
+ <general class="size1" name="wrist_1" joint="wrist_1_joint"/>
182
+ <general class="size1" name="wrist_2" joint="wrist_2_joint"/>
183
+ <general class="size1" name="wrist_3" joint="wrist_3_joint"/>
184
+ <general name="gripper" tendon="split" forcerange="-5 5" ctrlrange="0 255" gainprm="0.3137255 0 0" biasprm="0 -100 -10"/>
185
+ </actuator>
186
+
187
+ <keyframe>
188
+ <!-- Official MuJoCo Menagerie UR5e home pose with gripper open (0=open) -->
189
+ <key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0 0 0 0 0 0 0 0 0.5 0.2 0.45 1 0 0 0"
190
+ ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0"/>
191
+ </keyframe>
192
+ </mujoco>
robots/ur5/nova_api.py CHANGED
@@ -1,5 +1,6 @@
1
  import json
2
  import math
 
3
  import threading
4
  import time
5
  import urllib.error
@@ -9,7 +10,9 @@ from typing import Any, Dict, Optional, Tuple
9
 
10
  try:
11
  from websockets.sync.client import connect
12
- except Exception: # pragma: no cover - optional dependency
 
 
13
  connect = None
14
 
15
 
@@ -24,6 +27,72 @@ class NovaApiConfig:
24
  tcp_name: Optional[str] = None
25
  response_rate_ms: int = 200
26
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
27
 
28
  class NovaApiClient:
29
  def __init__(self, config: NovaApiConfig):
@@ -124,8 +193,10 @@ class NovaApiClient:
124
  backoff = min(backoff * 2.0, 10.0)
125
 
126
  def _connect_state_stream(self):
 
 
127
  url = (
128
- f"{self.config.instance_url}/api/v2/cells/{self.config.cell_id}/controllers/"
129
  f"{self.config.controller_id}/motion-groups/{self.config.motion_group_id}/state-stream"
130
  f"?response_rate={int(self.config.response_rate_ms)}"
131
  )
 
1
  import json
2
  import math
3
+ import os
4
  import threading
5
  import time
6
  import urllib.error
 
10
 
11
  try:
12
  from websockets.sync.client import connect
13
+ except Exception as e: # pragma: no cover - optional dependency
14
+ print(f"[Nova] WARNING: Failed to import websockets: {type(e).__name__}: {e}")
15
+ print(f"[Nova] Install websockets with: pip install websockets")
16
  connect = None
17
 
18
 
 
27
  tcp_name: Optional[str] = None
28
  response_rate_ms: int = 200
29
 
30
+ @classmethod
31
+ def from_env(cls, override: Optional[Dict[str, Any]] = None) -> "NovaApiConfig":
32
+ """Create NovaApiConfig from environment variables.
33
+
34
+ Environment variables:
35
+ NOVA_INSTANCE_URL: Nova instance URL (e.g., https://nova.wandelbots.io)
36
+ NOVA_ACCESS_TOKEN: Nova API access token
37
+ NOVA_CELL_ID: Cell ID
38
+ NOVA_CONTROLLER_ID: Controller ID
39
+ NOVA_MOTION_GROUP_ID: Motion group ID
40
+ NOVA_MOTION_GROUP_MODEL: Motion group model (optional)
41
+ NOVA_TCP_NAME: TCP name (optional, default: Flange)
42
+ NOVA_RESPONSE_RATE_MS: Response rate in milliseconds (optional, default: 200)
43
+
44
+ Args:
45
+ override: Optional dict to override specific values from env vars
46
+
47
+ Returns:
48
+ NovaApiConfig instance
49
+
50
+ Raises:
51
+ ValueError: If required environment variables are not set
52
+ """
53
+ override = override or {}
54
+
55
+ # Required fields
56
+ # Support both NOVA_INSTANCE_URL and NOVA_API for backward compatibility
57
+ instance_url = override.get("instance_url") or os.getenv("NOVA_INSTANCE_URL") or os.getenv("NOVA_API")
58
+ access_token = override.get("access_token") or os.getenv("NOVA_ACCESS_TOKEN")
59
+ cell_id = override.get("cell_id") or os.getenv("NOVA_CELL_ID", "cell")
60
+ controller_id = override.get("controller_id") or os.getenv("NOVA_CONTROLLER_ID")
61
+ motion_group_id = override.get("motion_group_id") or os.getenv("NOVA_MOTION_GROUP_ID")
62
+
63
+ # Validate required fields
64
+ missing = []
65
+ if not instance_url:
66
+ missing.append("NOVA_INSTANCE_URL or NOVA_API")
67
+ if not access_token:
68
+ missing.append("NOVA_ACCESS_TOKEN")
69
+ if not controller_id:
70
+ missing.append("NOVA_CONTROLLER_ID")
71
+ if not motion_group_id:
72
+ missing.append("NOVA_MOTION_GROUP_ID")
73
+
74
+ if missing:
75
+ raise ValueError(
76
+ f"Missing required environment variables: {', '.join(missing)}. "
77
+ "Please set these environment variables or pass them in the override dict."
78
+ )
79
+
80
+ # Optional fields
81
+ motion_group_model = override.get("motion_group_model") or os.getenv("NOVA_MOTION_GROUP_MODEL")
82
+ tcp_name = override.get("tcp_name") or os.getenv("NOVA_TCP_NAME", "Flange")
83
+ response_rate_ms = override.get("response_rate_ms") or int(os.getenv("NOVA_RESPONSE_RATE_MS", "200"))
84
+
85
+ return cls(
86
+ instance_url=instance_url,
87
+ access_token=access_token,
88
+ cell_id=cell_id,
89
+ controller_id=controller_id,
90
+ motion_group_id=motion_group_id,
91
+ motion_group_model=motion_group_model,
92
+ tcp_name=tcp_name,
93
+ response_rate_ms=response_rate_ms,
94
+ )
95
+
96
 
97
  class NovaApiClient:
98
  def __init__(self, config: NovaApiConfig):
 
193
  backoff = min(backoff * 2.0, 10.0)
194
 
195
  def _connect_state_stream(self):
196
+ # Convert HTTP(S) URL to WebSocket URL
197
+ ws_url = self.config.instance_url.replace("https://", "wss://").replace("http://", "ws://")
198
  url = (
199
+ f"{ws_url}/api/v2/cells/{self.config.cell_id}/controllers/"
200
  f"{self.config.controller_id}/motion-groups/{self.config.motion_group_id}/state-stream"
201
  f"?response_rate={int(self.config.response_rate_ms)}"
202
  )
robots/ur5/ur5_env.py CHANGED
@@ -1,4 +1,4 @@
1
- """Gymnasium environment for Universal Robots UR5e with Robotiq 2F-85 gripper."""
2
  from typing import Optional
3
 
4
  import gymnasium as gym
@@ -24,8 +24,8 @@ except Exception:
24
 
25
  class UR5Env(gym.Env):
26
  """
27
- Gymnasium environment for Universal Robots UR5e with Robotiq 2F-85 gripper.
28
- UR5e has 6 actuated joints, gripper has 1 actuator controlling both fingers.
29
  """
30
  metadata = {"render_modes": ["human", "rgb_array"], "render_fps": 30}
31
 
@@ -79,9 +79,12 @@ class UR5Env(gym.Env):
79
 
80
  self.data = mujoco.MjData(self.model)
81
 
82
- # Number of actuators: 6 arm joints + 1 gripper
 
 
 
83
  self.num_arm_actuators = 6
84
- self.num_actuators = self.model.nu # 7 total
85
 
86
  # Find site IDs
87
  self.ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
@@ -89,22 +92,29 @@ class UR5Env(gym.Env):
89
  self.t_object_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "t_object")
90
  self.t_target_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "t_target")
91
 
92
- # Action space: 6 joint positions + 1 gripper (0-255)
93
- self.action_space = spaces.Box(
94
- low=np.array([-6.28, -6.28, -3.14, -6.28, -6.28, -6.28, 0], dtype=np.float32),
95
- high=np.array([6.28, 6.28, 3.14, 6.28, 6.28, 6.28, 255], dtype=np.float32),
96
- dtype=np.float32
97
- )
 
 
 
 
 
 
 
98
 
99
  # Observation space:
100
  # - Joint positions (6)
101
  # - Joint velocities (6)
102
  # - End-effector position (3)
103
  # - End-effector orientation (4, quaternion)
104
- # - Gripper state (1)
105
  # - Target position (3)
106
- # Total: 6 + 6 + 3 + 4 + 1 + 3 = 23
107
- obs_dim = 23
108
  self.observation_space = spaces.Box(
109
  low=-np.inf, high=np.inf,
110
  shape=(obs_dim,),
@@ -120,6 +130,12 @@ class UR5Env(gym.Env):
120
  self.steps = 0
121
  self.max_steps = 1000
122
 
 
 
 
 
 
 
123
  # IK controller for end-effector control
124
  self.controller = IKController(
125
  model=self.model,
@@ -142,50 +158,75 @@ class UR5Env(gym.Env):
142
 
143
  # Whether to use orientation in IK (user can toggle this)
144
  self._use_orientation = True
145
- # Gripper target: Robotiq 2F-85 uses 0=open, 255=closed
146
- self._gripper_target = 0.0 # Start open
 
 
147
 
148
  # Control mode: 'ik' (end-effector target) or 'joint' (direct joint positions)
149
  self._control_mode = 'ik'
150
  # Direct joint targets (used when control_mode is 'joint')
151
  self._joint_targets = self.DEFAULT_HOME_POSE.copy()
152
 
153
- self._nova_client = None
154
- self._use_nova_state_stream = False
155
- self._use_nova_ik = False
156
- self._last_nova_sequence = None
157
-
158
  if nova_api_config:
159
  self._configure_nova_api(nova_api_config)
160
 
161
  def _configure_nova_api(self, nova_api_config: dict) -> None:
 
 
162
  if nova_api is None:
163
- print("Nova API integration requested but nova_api module is unavailable.")
164
  return
165
 
166
  use_state = bool(nova_api_config.get("use_state_stream"))
167
  use_ik = bool(nova_api_config.get("use_ik"))
 
 
168
  if not (use_state or use_ik):
 
169
  return
170
 
171
  try:
172
- config = nova_api.NovaApiConfig(
173
- instance_url=nova_api_config["instance_url"],
174
- access_token=nova_api_config["access_token"],
175
- cell_id=nova_api_config["cell_id"],
176
- controller_id=nova_api_config["controller_id"],
177
- motion_group_id=nova_api_config["motion_group_id"],
178
- motion_group_model=nova_api_config.get("motion_group_model"),
179
- tcp_name=nova_api_config.get("tcp_name"),
180
- response_rate_ms=int(nova_api_config.get("response_rate_ms", 200)),
181
- )
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
182
  self._nova_client = nova_api.NovaApiClient(config)
183
  self._use_nova_state_stream = use_state
184
  self._use_nova_ik = use_ik
 
 
185
  if self._use_nova_state_stream:
 
186
  self._nova_client.start_state_stream()
 
187
  except Exception as exc:
188
- print(f"Failed to initialize Nova API integration: {exc}")
 
 
189
  self._nova_client = None
190
  self._use_nova_state_stream = False
191
  self._use_nova_ik = False
@@ -244,12 +285,15 @@ class UR5Env(gym.Env):
244
  return self._use_orientation
245
 
246
  def set_gripper(self, value: float):
247
- """Set gripper target. Robotiq 2F-85: 0=open, 255=closed."""
248
- self._gripper_target = np.clip(value, 0, 255)
 
249
 
250
  def get_gripper(self):
251
- """Get gripper target."""
252
- return self._gripper_target
 
 
253
 
254
  def set_control_mode(self, mode: str):
255
  """Set control mode: 'ik' or 'joint'."""
@@ -331,15 +375,19 @@ class UR5Env(gym.Env):
331
  # End-effector orientation
332
  ee_quat = self.get_end_effector_quat()
333
 
334
- # Gripper state (read from actuator ctrl, normalized)
335
- gripper_state = np.array([self.data.ctrl[6] / 255.0])
336
-
337
  # Target position
338
  target = self._target_pos.copy()
339
 
340
- return np.concatenate([
341
- joint_pos, joint_vel, ee_pos, ee_quat, gripper_state, target
342
- ]).astype(np.float32)
 
 
 
 
 
 
 
343
 
344
  def set_command(self, vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
345
  """Compatibility method - for arm robots, this does nothing.
@@ -367,7 +415,8 @@ class UR5Env(gym.Env):
367
  # Set to home pose
368
  self.data.qpos[:6] = self.DEFAULT_HOME_POSE.copy()
369
  self.data.ctrl[:6] = self.DEFAULT_HOME_POSE.copy()
370
- self.data.ctrl[6] = 0 # Gripper open (Robotiq: 0=open, 255=closed)
 
371
 
372
  # Reset task objects if present
373
  self._reset_freejoint("box_joint", [0.5, 0.2, 0.45], [1, 0, 0, 0])
@@ -386,7 +435,8 @@ class UR5Env(gym.Env):
386
  self._target_euler = np.array([roll, pitch, yaw], dtype=np.float32)
387
 
388
  self.data.mocap_pos[0] = self._target_pos
389
- self._gripper_target = 0.0 # Open gripper
 
390
  self._joint_targets = self.DEFAULT_HOME_POSE.copy()
391
 
392
  self.steps = 0
@@ -435,8 +485,9 @@ class UR5Env(gym.Env):
435
  # Apply joint targets
436
  self.data.ctrl[:6] = joint_targets
437
 
438
- # Apply gripper target
439
- self.data.ctrl[6] = self._gripper_target
 
440
 
441
  # Step simulation
442
  mujoco.mj_step(self.model, self.data)
 
1
+ """Gymnasium environment for Universal Robots UR5e (with optional gripper)."""
2
  from typing import Optional
3
 
4
  import gymnasium as gym
 
24
 
25
  class UR5Env(gym.Env):
26
  """
27
+ Gymnasium environment for Universal Robots UR5e.
28
+ UR5e has 6 actuated joints. Optional Robotiq 2F-85 gripper (detected automatically).
29
  """
30
  metadata = {"render_modes": ["human", "rgb_array"], "render_fps": 30}
31
 
 
79
 
80
  self.data = mujoco.MjData(self.model)
81
 
82
+ # Detect if gripper is present (scene.xml has 7 actuators, scene_t_push.xml has 6)
83
+ self.has_gripper = self.model.nu > 6
84
+
85
+ # Number of actuators: 6 arm joints + optional gripper
86
  self.num_arm_actuators = 6
87
+ self.num_actuators = self.model.nu
88
 
89
  # Find site IDs
90
  self.ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
 
92
  self.t_object_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "t_object")
93
  self.t_target_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "t_target")
94
 
95
+ # Action space: 6 joint positions + optional gripper (0-255)
96
+ if self.has_gripper:
97
+ self.action_space = spaces.Box(
98
+ low=np.array([-6.28, -6.28, -3.14, -6.28, -6.28, -6.28, 0], dtype=np.float32),
99
+ high=np.array([6.28, 6.28, 3.14, 6.28, 6.28, 6.28, 255], dtype=np.float32),
100
+ dtype=np.float32
101
+ )
102
+ else:
103
+ self.action_space = spaces.Box(
104
+ low=np.array([-6.28, -6.28, -3.14, -6.28, -6.28, -6.28], dtype=np.float32),
105
+ high=np.array([6.28, 6.28, 3.14, 6.28, 6.28, 6.28], dtype=np.float32),
106
+ dtype=np.float32
107
+ )
108
 
109
  # Observation space:
110
  # - Joint positions (6)
111
  # - Joint velocities (6)
112
  # - End-effector position (3)
113
  # - End-effector orientation (4, quaternion)
114
+ # - Gripper state (1, optional)
115
  # - Target position (3)
116
+ # Total: 6 + 6 + 3 + 4 + [0 or 1] + 3 = 22 or 23
117
+ obs_dim = 23 if self.has_gripper else 22
118
  self.observation_space = spaces.Box(
119
  low=-np.inf, high=np.inf,
120
  shape=(obs_dim,),
 
130
  self.steps = 0
131
  self.max_steps = 1000
132
 
133
+ # Initialize Nova API attributes FIRST (before any methods that might call _sync_nova_state)
134
+ self._nova_client = None
135
+ self._use_nova_state_stream = False
136
+ self._use_nova_ik = False
137
+ self._last_nova_sequence = None
138
+
139
  # IK controller for end-effector control
140
  self.controller = IKController(
141
  model=self.model,
 
158
 
159
  # Whether to use orientation in IK (user can toggle this)
160
  self._use_orientation = True
161
+
162
+ # Gripper target (only if gripper exists)
163
+ if self.has_gripper:
164
+ self._gripper_target = 0.0 # Start open
165
 
166
  # Control mode: 'ik' (end-effector target) or 'joint' (direct joint positions)
167
  self._control_mode = 'ik'
168
  # Direct joint targets (used when control_mode is 'joint')
169
  self._joint_targets = self.DEFAULT_HOME_POSE.copy()
170
 
171
+ # Configure Nova API if requested (must be after Nova attributes are initialized)
 
 
 
 
172
  if nova_api_config:
173
  self._configure_nova_api(nova_api_config)
174
 
175
  def _configure_nova_api(self, nova_api_config: dict) -> None:
176
+ print(f"[Nova] Configuring Nova API with config: {nova_api_config}")
177
+
178
  if nova_api is None:
179
+ print("[Nova] ERROR: Nova API integration requested but nova_api module is unavailable.")
180
  return
181
 
182
  use_state = bool(nova_api_config.get("use_state_stream"))
183
  use_ik = bool(nova_api_config.get("use_ik"))
184
+ print(f"[Nova] use_state_stream={use_state}, use_ik={use_ik}")
185
+
186
  if not (use_state or use_ik):
187
+ print("[Nova] Neither state streaming nor IK requested, skipping configuration")
188
  return
189
 
190
  try:
191
+ # Try to create config from environment variables first, with override from nova_api_config
192
+ # This allows env vars to be the primary source with optional dict overrides
193
+ print("[Nova] Attempting to load config from environment variables...")
194
+ try:
195
+ config = nova_api.NovaApiConfig.from_env(override=nova_api_config)
196
+ print(f"[Nova] Config loaded from env: {config.instance_url}, controller={config.controller_id}, motion_group={config.motion_group_id}")
197
+ except ValueError as env_err:
198
+ # If env vars are not set, fall back to using the dict directly
199
+ # (backward compatibility)
200
+ if all(k in nova_api_config for k in ["instance_url", "access_token", "cell_id",
201
+ "controller_id", "motion_group_id"]):
202
+ config = nova_api.NovaApiConfig(
203
+ instance_url=nova_api_config["instance_url"],
204
+ access_token=nova_api_config["access_token"],
205
+ cell_id=nova_api_config["cell_id"],
206
+ controller_id=nova_api_config["controller_id"],
207
+ motion_group_id=nova_api_config["motion_group_id"],
208
+ motion_group_model=nova_api_config.get("motion_group_model"),
209
+ tcp_name=nova_api_config.get("tcp_name", "Flange"),
210
+ response_rate_ms=int(nova_api_config.get("response_rate_ms", 200)),
211
+ )
212
+ else:
213
+ print(f"[Nova] ERROR: Environment variables not complete, and dict config incomplete: {env_err}")
214
+ raise env_err
215
+
216
+ print(f"[Nova] Creating NovaApiClient...")
217
  self._nova_client = nova_api.NovaApiClient(config)
218
  self._use_nova_state_stream = use_state
219
  self._use_nova_ik = use_ik
220
+ print(f"[Nova] Client created successfully. State streaming: {use_state}, IK: {use_ik}")
221
+
222
  if self._use_nova_state_stream:
223
+ print("[Nova] Starting state stream...")
224
  self._nova_client.start_state_stream()
225
+ print("[Nova] State stream started successfully")
226
  except Exception as exc:
227
+ print(f"[Nova] ERROR: Failed to initialize Nova API integration: {exc}")
228
+ import traceback
229
+ traceback.print_exc()
230
  self._nova_client = None
231
  self._use_nova_state_stream = False
232
  self._use_nova_ik = False
 
285
  return self._use_orientation
286
 
287
  def set_gripper(self, value: float):
288
+ """Set gripper target. Robotiq 2F-85: 0=open, 255=closed. Only works if gripper exists."""
289
+ if self.has_gripper:
290
+ self._gripper_target = np.clip(value, 0, 255)
291
 
292
  def get_gripper(self):
293
+ """Get gripper target. Returns 0 if no gripper."""
294
+ if self.has_gripper:
295
+ return self._gripper_target
296
+ return 0
297
 
298
  def set_control_mode(self, mode: str):
299
  """Set control mode: 'ik' or 'joint'."""
 
375
  # End-effector orientation
376
  ee_quat = self.get_end_effector_quat()
377
 
 
 
 
378
  # Target position
379
  target = self._target_pos.copy()
380
 
381
+ # Build observation (conditionally include gripper state)
382
+ if self.has_gripper:
383
+ gripper_state = np.array([self.data.ctrl[6] / 255.0])
384
+ return np.concatenate([
385
+ joint_pos, joint_vel, ee_pos, ee_quat, gripper_state, target
386
+ ]).astype(np.float32)
387
+ else:
388
+ return np.concatenate([
389
+ joint_pos, joint_vel, ee_pos, ee_quat, target
390
+ ]).astype(np.float32)
391
 
392
  def set_command(self, vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
393
  """Compatibility method - for arm robots, this does nothing.
 
415
  # Set to home pose
416
  self.data.qpos[:6] = self.DEFAULT_HOME_POSE.copy()
417
  self.data.ctrl[:6] = self.DEFAULT_HOME_POSE.copy()
418
+ if self.has_gripper:
419
+ self.data.ctrl[6] = 0 # Gripper open (Robotiq: 0=open, 255=closed)
420
 
421
  # Reset task objects if present
422
  self._reset_freejoint("box_joint", [0.5, 0.2, 0.45], [1, 0, 0, 0])
 
435
  self._target_euler = np.array([roll, pitch, yaw], dtype=np.float32)
436
 
437
  self.data.mocap_pos[0] = self._target_pos
438
+ if self.has_gripper:
439
+ self._gripper_target = 0.0 # Open gripper
440
  self._joint_targets = self.DEFAULT_HOME_POSE.copy()
441
 
442
  self.steps = 0
 
485
  # Apply joint targets
486
  self.data.ctrl[:6] = joint_targets
487
 
488
+ # Apply gripper target if gripper exists
489
+ if self.has_gripper:
490
+ self.data.ctrl[6] = self._gripper_target
491
 
492
  # Step simulation
493
  mujoco.mj_step(self.model, self.data)
screenshot.png ADDED

Git LFS Details

  • SHA256: d7078b72bc8cafd808d69023dd1041a51e4337e9990486f029d2dcce6862d726
  • Pointer size: 132 Bytes
  • Size of remote file: 3.04 MB
scripts/discover_nova_config.py ADDED
@@ -0,0 +1,160 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #!/usr/bin/env python3
2
+ """Discover Nova API configuration from your instance.
3
+
4
+ This script connects to your Nova instance and discovers available
5
+ controllers and motion groups to help you configure the environment variables.
6
+
7
+ Requirements:
8
+ - python-dotenv: pip install python-dotenv
9
+ """
10
+
11
+ import json
12
+ import os
13
+ import sys
14
+ import urllib.request
15
+ from pathlib import Path
16
+
17
+ try:
18
+ from dotenv import load_dotenv
19
+ except ImportError:
20
+ print("Error: python-dotenv is required")
21
+ print("Install with: pip install python-dotenv")
22
+ sys.exit(1)
23
+
24
+
25
+ def request_json(url, access_token):
26
+ """Make a GET request to the Nova API."""
27
+ request = urllib.request.Request(url)
28
+ request.add_header("Authorization", f"Bearer {access_token}")
29
+ request.add_header("Content-Type", "application/json")
30
+ try:
31
+ with urllib.request.urlopen(request, timeout=10) as response:
32
+ body = response.read().decode("utf-8")
33
+ return json.loads(body) if body else {}
34
+ except urllib.error.HTTPError as exc:
35
+ body = exc.read().decode("utf-8") if exc.fp else ""
36
+ print(f"Error: Nova API returned status {exc.code}")
37
+ print(f"Response: {body}")
38
+ return None
39
+
40
+
41
+ def main():
42
+ # Try to load from .env.local first, then .env
43
+ env_local = Path("nova-sim/.env.local")
44
+ env_file = Path("nova-sim/.env")
45
+
46
+ if env_local.exists():
47
+ load_dotenv(env_local)
48
+ print(f"Loaded configuration from {env_local}")
49
+ elif env_file.exists():
50
+ load_dotenv(env_file)
51
+ print(f"Loaded configuration from {env_file}")
52
+ else:
53
+ print("Error: No .env or .env.local file found")
54
+ print("Create one with at least NOVA_INSTANCE_URL and NOVA_ACCESS_TOKEN")
55
+ return 1
56
+
57
+ # Get configuration from environment
58
+ instance_url = os.getenv("NOVA_INSTANCE_URL") or os.getenv("NOVA_API")
59
+ access_token = os.getenv("NOVA_ACCESS_TOKEN")
60
+ cell_id = os.getenv("NOVA_CELL_ID", "cell")
61
+
62
+ if not instance_url:
63
+ print("Error: NOVA_INSTANCE_URL (or NOVA_API) not set")
64
+ return 1
65
+
66
+ if not access_token:
67
+ print("Error: NOVA_ACCESS_TOKEN not set")
68
+ return 1
69
+
70
+ print(f"\nConnecting to Nova instance: {instance_url}")
71
+ print(f"Cell ID: {cell_id}")
72
+ print()
73
+
74
+ # Fetch controllers
75
+ print("Fetching controllers...")
76
+ controllers_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers"
77
+ controllers_data = request_json(controllers_url, access_token)
78
+
79
+ if not controllers_data:
80
+ return 1
81
+
82
+ controllers = controllers_data.get("instances", [])
83
+ if not controllers:
84
+ print("No controllers found")
85
+ return 1
86
+
87
+ print(f"Found {len(controllers)} controller(s):")
88
+ print()
89
+
90
+ for controller in controllers:
91
+ controller_id = controller.get("controller", "unknown")
92
+ print(f"Controller ID: {controller_id}")
93
+
94
+ # Fetch motion groups for this controller
95
+ motion_groups_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers/{controller_id}/motion-groups"
96
+ motion_groups_data = request_json(motion_groups_url, access_token)
97
+
98
+ if not motion_groups_data:
99
+ continue
100
+
101
+ motion_groups = motion_groups_data.get("motion_groups", [])
102
+ print(f" Motion groups: {len(motion_groups)}")
103
+
104
+ for mg in motion_groups:
105
+ mg_id = mg.get("motion_group", "unknown")
106
+ model = mg.get("model_from_controller", "unknown")
107
+ name = mg.get("name_from_controller", "")
108
+
109
+ print(f" - ID: {mg_id}")
110
+ print(f" Model: {model}")
111
+ if name:
112
+ print(f" Name: {name}")
113
+
114
+ # Fetch motion group description
115
+ desc_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers/{controller_id}/motion-groups/{mg_id}/description"
116
+ desc_data = request_json(desc_url, access_token)
117
+
118
+ if desc_data:
119
+ tcps = desc_data.get("tcps", {})
120
+ if tcps:
121
+ print(f" Available TCPs: {', '.join(tcps.keys())}")
122
+ print()
123
+
124
+ # Generate recommended .env configuration
125
+ print("\n" + "=" * 60)
126
+ print("RECOMMENDED CONFIGURATION")
127
+ print("=" * 60)
128
+
129
+ if controllers:
130
+ controller = controllers[0]
131
+ controller_id = controller.get("controller", "unknown")
132
+
133
+ # Get motion groups for first controller
134
+ motion_groups_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers/{controller_id}/motion-groups"
135
+ motion_groups_data = request_json(motion_groups_url, access_token)
136
+
137
+ if motion_groups_data:
138
+ motion_groups = motion_groups_data.get("motion_groups", [])
139
+ if motion_groups:
140
+ mg = motion_groups[0]
141
+ mg_id = mg.get("motion_group", "unknown")
142
+ model = mg.get("model_from_controller", "unknown")
143
+
144
+ print("\nAdd these to your .env.local file:")
145
+ print(f"""
146
+ NOVA_INSTANCE_URL={instance_url}
147
+ NOVA_ACCESS_TOKEN={access_token[:20]}... # (your full token)
148
+ NOVA_CELL_ID={cell_id}
149
+ NOVA_CONTROLLER_ID={controller_id}
150
+ NOVA_MOTION_GROUP_ID={mg_id}
151
+ NOVA_MOTION_GROUP_MODEL={model}
152
+ NOVA_TCP_NAME=Flange
153
+ NOVA_RESPONSE_RATE_MS=200
154
+ """)
155
+
156
+ return 0
157
+
158
+
159
+ if __name__ == "__main__":
160
+ sys.exit(main())
scripts/discover_nova_simple.py ADDED
@@ -0,0 +1,184 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #!/usr/bin/env python3
2
+ """Discover Nova API configuration - simple version without dependencies."""
3
+
4
+ import json
5
+ import os
6
+ import sys
7
+ import urllib.request
8
+ from pathlib import Path
9
+
10
+
11
+ def load_env_file(filepath):
12
+ """Simple .env file loader."""
13
+ env_vars = {}
14
+ if not filepath.exists():
15
+ return env_vars
16
+
17
+ with open(filepath, 'r') as f:
18
+ for line in f:
19
+ line = line.strip()
20
+ if line and not line.startswith('#') and '=' in line:
21
+ key, value = line.split('=', 1)
22
+ env_vars[key.strip()] = value.strip()
23
+ return env_vars
24
+
25
+
26
+ def request_json(url, access_token):
27
+ """Make a GET request to the Nova API."""
28
+ request = urllib.request.Request(url)
29
+ request.add_header("Authorization", f"Bearer {access_token}")
30
+ request.add_header("Content-Type", "application/json")
31
+ try:
32
+ with urllib.request.urlopen(request, timeout=10) as response:
33
+ body = response.read().decode("utf-8")
34
+ return json.loads(body) if body else {}
35
+ except urllib.error.HTTPError as exc:
36
+ body = exc.read().decode("utf-8") if exc.fp else ""
37
+ print(f"Error: Nova API returned status {exc.code}")
38
+ print(f"Response: {body}")
39
+ return None
40
+ except Exception as exc:
41
+ print(f"Error: {exc}")
42
+ return None
43
+
44
+
45
+ def main():
46
+ # Try to load from .env.local first, then .env
47
+ env_local = Path("nova-sim/.env.local")
48
+ env_file = Path("nova-sim/.env")
49
+
50
+ env_vars = {}
51
+ if env_local.exists():
52
+ env_vars = load_env_file(env_local)
53
+ print(f"Loaded configuration from {env_local}")
54
+ elif env_file.exists():
55
+ env_vars = load_env_file(env_file)
56
+ print(f"Loaded configuration from {env_file}")
57
+ else:
58
+ print("Error: No .env or .env.local file found")
59
+ print("Create one with at least NOVA_INSTANCE_URL and NOVA_ACCESS_TOKEN")
60
+ return 1
61
+
62
+ # Get configuration
63
+ instance_url = env_vars.get("NOVA_INSTANCE_URL") or env_vars.get("NOVA_API")
64
+ access_token = env_vars.get("NOVA_ACCESS_TOKEN")
65
+ cell_id = env_vars.get("NOVA_CELL_ID", "cell")
66
+
67
+ if not instance_url:
68
+ print("Error: NOVA_INSTANCE_URL (or NOVA_API) not set in .env file")
69
+ return 1
70
+
71
+ if not access_token:
72
+ print("Error: NOVA_ACCESS_TOKEN not set in .env file")
73
+ return 1
74
+
75
+ print(f"\nConnecting to Nova instance: {instance_url}")
76
+ print(f"Cell ID: {cell_id}")
77
+ print()
78
+
79
+ # Fetch controllers
80
+ print("Fetching controllers...")
81
+ controllers_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers"
82
+ controllers_data = request_json(controllers_url, access_token)
83
+
84
+ if not controllers_data:
85
+ return 1
86
+
87
+ # Handle both list and dict responses
88
+ if isinstance(controllers_data, list):
89
+ controllers = controllers_data
90
+ else:
91
+ controllers = controllers_data.get("instances", [])
92
+
93
+ if not controllers:
94
+ print("No controllers found")
95
+ return 1
96
+
97
+ print(f"Found {len(controllers)} controller(s):")
98
+ print()
99
+
100
+ for controller in controllers:
101
+ # Handle both string and dict controller responses
102
+ if isinstance(controller, str):
103
+ controller_id = controller
104
+ else:
105
+ controller_id = controller.get("controller", "unknown")
106
+ print(f"Controller ID: {controller_id}")
107
+
108
+ # Fetch motion groups - try cell-level endpoint first
109
+ motion_groups_url = f"{instance_url}/api/v2/cells/{cell_id}/motion-groups"
110
+ motion_groups_data = request_json(motion_groups_url, access_token)
111
+
112
+ if not motion_groups_data:
113
+ # Try controller-specific endpoint
114
+ motion_groups_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers/{controller_id}/motion-groups"
115
+ motion_groups_data = request_json(motion_groups_url, access_token)
116
+
117
+ if not motion_groups_data:
118
+ print(" Motion groups: Could not fetch")
119
+ continue
120
+
121
+ motion_groups = motion_groups_data.get("motion_groups", [])
122
+ print(f" Motion groups: {len(motion_groups)}")
123
+
124
+ for mg in motion_groups:
125
+ mg_id = mg.get("motion_group", "unknown")
126
+ model = mg.get("model_from_controller", "unknown")
127
+ name = mg.get("name_from_controller", "")
128
+
129
+ print(f" - ID: {mg_id}")
130
+ print(f" Model: {model}")
131
+ if name:
132
+ print(f" Name: {name}")
133
+
134
+ # Fetch motion group description
135
+ desc_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers/{controller_id}/motion-groups/{mg_id}/description"
136
+ desc_data = request_json(desc_url, access_token)
137
+
138
+ if desc_data:
139
+ tcps = desc_data.get("tcps", {})
140
+ if tcps:
141
+ print(f" Available TCPs: {', '.join(tcps.keys())}")
142
+ print()
143
+
144
+ # Generate recommended .env configuration
145
+ print("\n" + "=" * 60)
146
+ print("RECOMMENDED CONFIGURATION")
147
+ print("=" * 60)
148
+
149
+ if controllers:
150
+ # Handle both string and dict controller responses
151
+ controller = controllers[0]
152
+ if isinstance(controller, str):
153
+ controller_id = controller
154
+ else:
155
+ controller_id = controller.get("controller", "unknown")
156
+
157
+ # Get motion groups for first controller
158
+ motion_groups_url = f"{instance_url}/api/v2/cells/{cell_id}/controllers/{controller_id}/motion-groups"
159
+ motion_groups_data = request_json(motion_groups_url, access_token)
160
+
161
+ if motion_groups_data:
162
+ motion_groups = motion_groups_data.get("motion_groups", [])
163
+ if motion_groups:
164
+ mg = motion_groups[0]
165
+ mg_id = mg.get("motion_group", "unknown")
166
+ model = mg.get("model_from_controller", "unknown")
167
+
168
+ print("\nAdd/update these in your .env.local file:")
169
+ print(f"""
170
+ NOVA_INSTANCE_URL={instance_url}
171
+ NOVA_ACCESS_TOKEN={access_token}
172
+ NOVA_CELL_ID={cell_id}
173
+ NOVA_CONTROLLER_ID={controller_id}
174
+ NOVA_MOTION_GROUP_ID={mg_id}
175
+ NOVA_MOTION_GROUP_MODEL={model}
176
+ NOVA_TCP_NAME=Flange
177
+ NOVA_RESPONSE_RATE_MS=200
178
+ """)
179
+
180
+ return 0
181
+
182
+
183
+ if __name__ == "__main__":
184
+ sys.exit(main())
scripts/test_nova_connection.py ADDED
@@ -0,0 +1,108 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #!/usr/bin/env python3
2
+ """Test Nova API connection and configuration."""
3
+
4
+ import os
5
+ import sys
6
+ from pathlib import Path
7
+
8
+ # Add parent directory to path
9
+ sys.path.insert(0, str(Path(__file__).parent.parent))
10
+
11
+ # Simple .env loader
12
+ def load_env_file(filepath):
13
+ """Simple .env file loader."""
14
+ if not filepath.exists():
15
+ return {}
16
+ env_vars = {}
17
+ with open(filepath, 'r') as f:
18
+ for line in f:
19
+ line = line.strip()
20
+ if line and not line.startswith('#') and '=' in line:
21
+ key, value = line.split('=', 1)
22
+ env_vars[key.strip()] = value.strip()
23
+ os.environ[key.strip()] = value.strip()
24
+ return env_vars
25
+
26
+
27
+ def main():
28
+ # Load .env.local
29
+ env_path = Path(__file__).parent.parent / ".env.local"
30
+ if not env_path.exists():
31
+ print(f"Error: {env_path} not found")
32
+ return 1
33
+
34
+ print(f"Loading environment from {env_path}")
35
+ env_vars = load_env_file(env_path)
36
+
37
+ # Import after loading env vars
38
+ try:
39
+ from robots.ur5 import nova_api
40
+ except ImportError:
41
+ print("Error: Could not import nova_api module")
42
+ print("Make sure you're in the nova-sim directory")
43
+ return 1
44
+
45
+ # Try to create config
46
+ print("\nTesting Nova API configuration...")
47
+ try:
48
+ config = nova_api.NovaApiConfig.from_env()
49
+ print("✓ Configuration created successfully!")
50
+ print(f"\n Instance URL: {config.instance_url}")
51
+ print(f" Cell ID: {config.cell_id}")
52
+ print(f" Controller ID: {config.controller_id}")
53
+ print(f" Motion Group ID: {config.motion_group_id}")
54
+ print(f" Motion Group Model: {config.motion_group_model}")
55
+ print(f" TCP Name: {config.tcp_name}")
56
+ print(f" Response Rate: {config.response_rate_ms}ms")
57
+ except ValueError as e:
58
+ print(f"✗ Configuration error: {e}")
59
+ return 1
60
+
61
+ # Try to create client
62
+ print("\nCreating Nova API client...")
63
+ try:
64
+ client = nova_api.NovaApiClient(config)
65
+ print("✓ Client created successfully!")
66
+ except Exception as e:
67
+ print(f"✗ Client creation error: {e}")
68
+ return 1
69
+
70
+ # Try to fetch motion group description
71
+ print("\nFetching motion group description...")
72
+ try:
73
+ description = client._ensure_motion_group_description()
74
+ if description:
75
+ print("✓ Successfully fetched motion group description!")
76
+ print(f"\n Motion Group Model: {description.get('motion_group_model', 'N/A')}")
77
+ tcps = description.get('tcps', {})
78
+ if tcps:
79
+ print(f" Available TCPs: {', '.join(tcps.keys())}")
80
+ else:
81
+ print("✗ No description returned")
82
+ return 1
83
+ except Exception as e:
84
+ print(f"✗ Error fetching description: {e}")
85
+ return 1
86
+
87
+ print("\n" + "=" * 60)
88
+ print("SUCCESS! Your Nova API configuration is working correctly.")
89
+ print("=" * 60)
90
+ print("\nYou can now use Nova API integration with the UR5 environment:")
91
+ print("""
92
+ from robots.ur5.ur5_env import UR5Env
93
+
94
+ # Enable both state streaming and Nova IK
95
+ env = UR5Env(
96
+ render_mode="human",
97
+ nova_api_config={
98
+ "use_state_stream": True,
99
+ "use_ik": True
100
+ }
101
+ )
102
+ """)
103
+
104
+ return 0
105
+
106
+
107
+ if __name__ == "__main__":
108
+ sys.exit(main())