Rosie

import mujoco
import numpy as np

model = mujoco.MjModel.from_xml_path("mjcf/rosie_1000.xml")
data = mujoco.MjData(model)
data.ctrl[:] = np.radians([30, 45, -10, 0, -35, 0])   # position servos on J1..J6
for _ in range(1500):                                  # 3 s at 2 ms steps
    mujoco.mj_step(model, data)
print(np.degrees(data.qpos).round(2))                  # joint angles (deg)
print(data.actuator_force.round(1))                    # joint torques (N m)
print(data.site("tool0").xpos.round(4))                # flange face (m)

import math
import pybullet as p

p.connect(p.DIRECT)                                    # p.GUI for a window
p.setGravity(0, 0, -9.81)
robot = p.loadURDF("urdf/rosie_1000.urdf", useFixedBase=True,
                   flags=p.URDF_USE_INERTIA_FROM_FILE)
joints = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
arm = [j for j in joints if j[2] == p.JOINT_REVOLUTE]  # J1..J6
tool0 = next(j[0] for j in joints if j[12] == b"tool0")

target = [math.radians(a) for a in (30, 45, -10, 0, -35, 0)]
for j, q in zip(arm, target):                          # j[10] peak torque, j[11] max speed
    p.setJointMotorControl2(robot, j[0], p.POSITION_CONTROL, targetPosition=q,
                            force=j[10], maxVelocity=j[11])
for _ in range(3 * 240):
    p.stepSimulation()
print(p.getLinkState(robot, tool0, computeForwardKinematics=True)[4])

cp -r rosie_1000_description ~/ros2_ws/src/
cd ~/ros2_ws && colcon build --packages-select rosie_1000_description
source install/setup.bash
ros2 launch rosie_1000_description display.launch.py   # RViz with joint sliders

import json
import urllib.request

import numpy as np

URL = "https://advancedmetalresearch.com/assets/sim/1000/robot.json"
req = urllib.request.Request(URL, headers={"User-Agent": "rosie-sim-kit/1.0"})
robot = json.load(urllib.request.urlopen(req))


def rot(axis, q):
    x, y, z = axis
    c, s, t = np.cos(q), np.sin(q), 1 - np.cos(q)
    return np.array([[t * x * x + c, t * x * y - s * z, t * x * z + s * y],
                     [t * x * y + s * z, t * y * y + c, t * y * z - s * x],
                     [t * x * z - s * y, t * y * z + s * x, t * z * z + c]])


def fk(q):
    """Pose of tool0 (4x4) in base_link for joint angles q (rad)."""
    T = np.eye(4)
    for j, qi in zip(robot["joints"], q):
        A = np.eye(4)
        A[:3, 3], A[:3, :3] = j["origin_xyz"], rot(j["axis"], qi)
        T = T @ A
    return T @ np.diag([1, -1, -1, 1])                 # tool0: z out of the flange


print(fk(robot["poses"]["stretched"])[:3, 3])          # (m)