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 +27 -0
- README.md +222 -2
- examples/ur5_nova_api_example.py +155 -0
- mujoco_server.py +146 -12
- robots/ur5/model/scene_t_push.xml +11 -167
- robots/ur5/model/scene_with_gripper.xml +192 -0
- robots/ur5/nova_api.py +73 -2
- robots/ur5/ur5_env.py +98 -47
- screenshot.png +3 -0
- scripts/discover_nova_config.py +160 -0
- scripts/discover_nova_simple.py +184 -0
- scripts/test_nova_connection.py +108 -0
.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 |
+

|
| 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(
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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.
|
| 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.
|
| 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.
|
| 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 |
-
//
|
| 1132 |
-
document.getElementById('
|
| 1133 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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 |
-
<!--
|
| 212 |
-
<body name="
|
| 213 |
-
<
|
| 214 |
-
|
| 215 |
-
|
| 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 |
-
<!--
|
| 364 |
-
<key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0
|
| 365 |
-
ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 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"{
|
| 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
|
| 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
|
| 28 |
-
UR5e has 6 actuated joints
|
| 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 |
-
#
|
|
|
|
|
|
|
|
|
|
| 83 |
self.num_arm_actuators = 6
|
| 84 |
-
self.num_actuators = self.model.nu
|
| 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 +
|
| 93 |
-
self.
|
| 94 |
-
|
| 95 |
-
|
| 96 |
-
|
| 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 |
-
|
| 146 |
-
|
|
|
|
|
|
|
| 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 |
-
|
| 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
|
| 173 |
-
|
| 174 |
-
|
| 175 |
-
|
| 176 |
-
|
| 177 |
-
|
| 178 |
-
|
| 179 |
-
|
| 180 |
-
|
| 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.
|
|
|
|
| 249 |
|
| 250 |
def get_gripper(self):
|
| 251 |
-
"""Get gripper target."""
|
| 252 |
-
|
|
|
|
|
|
|
| 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 |
-
|
| 341 |
-
|
| 342 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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.
|
|
|
|
| 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.
|
|
|
|
| 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 |
-
|
|
|
|
| 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
|
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())
|