add rotary cartpole env

This commit is contained in:
2026-03-08 22:58:32 +01:00
parent c8f28ffbcc
commit c753c369b4
15 changed files with 464 additions and 171 deletions

View File

@@ -0,0 +1,105 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="rotary_cartpole">
<!-- Fixed world frame -->
<link name="world"/>
<!-- Base: motor housing, fixed to world -->
<link name="base_link">
<inertial>
<origin xyz="-0.00011 0.00117 0.06055" rpy="0 0 0"/>
<mass value="0.921"/>
<inertia ixx="0.002385" iyy="0.002484" izz="0.000559"
ixy="0.0" iyz="-0.000149" ixz="6e-06"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="base_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<!-- Arm: horizontal rotating arm driven by motor.
Real mass ~10g (Fusion assumed dense material, exported 279g). -->
<link name="arm">
<inertial>
<origin xyz="0.00005 0.0065 0.00563" rpy="0 0 0"/>
<mass value="0.150"/>
<inertia ixx="4.05e-05" iyy="1.17e-05" izz="3.66e-05"
ixy="0.0" iyz="1.08e-07" ixz="0.0"/>
</inertial>
<visual>
<origin xyz="0.006947 -0.00395 -0.14796" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/arm_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin xyz="0.006947 -0.00395 -0.14796" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/arm_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<!-- Motor joint: base → arm, rotates around vertical z-axis -->
<joint name="motor_joint" type="revolute">
<origin xyz="-0.006947 0.00395 0.14796" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="arm"/>
<axis xyz="0 0 1"/>
<limit lower="-1.5708" upper="1.5708" effort="10.0" velocity="200.0"/>
<dynamics damping="0.001"/>
</joint>
<!-- Pendulum: swings freely at the end of the arm.
Real mass: 5g pendulum + 10g weight at the tip (70mm from bearing) = 15g total.
(Fusion assumed dense material, exported 57g for the pendulum alone.) -->
<link name="pendulum">
<inertial>
<!-- Combined CoM: 5g rod (CoM ~35mm) + 10g tip weight at 70mm from pivot.
Tip at (0.07, -0.07, 0) → 45° diagonal in +X/-Y.
CoM = (5×0.035+10×0.07)/15 = 0.0583 along both +X and -Y.
Inertia tensor rotated 45° to match diagonal rod axis. -->
<origin xyz="0.0583 -0.0583 0.0" rpy="0 0 0"/>
<mass value="0.015"/>
<inertia ixx="6.16e-06" iyy="6.16e-06" izz="1.23e-05"
ixy="6.10e-06" iyz="0.0" ixz="0.0"/>
</inertial>
<visual>
<origin xyz="0.006895 -0.023224 -0.162953" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/pendulum_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin xyz="0.006895 -0.023224 -0.162953" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/pendulum_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<!-- Pendulum joint: arm → pendulum, bearing axis along Y.
Joint origin corrected from mesh analysis (Fusion2URDF was 180mm off). -->
<joint name="pendulum_joint" type="continuous">
<origin xyz="0.000052 0.019274 0.014993" rpy="0 0 0"/>
<parent link="arm"/>
<child link="pendulum"/>
<axis xyz="0 -1 0"/>
<dynamics damping="0.0005"/>
</joint>
</robot>