Import upstream snapshot 80e710700aac9573a2230f74f7ce9e094833a0bc

Upstream: https://github.com/Rhoban/onshape-to-robot Upstream-Commit: 80e710700aac9573a2230f74f7ce9e094833a0bc Upstream-Branch: master
2026-08-28 15:42:31 +08:00
commit bb87928bce
107 changed files with 16958 additions and 0 deletions
@@ -0,0 +1,71 @@
+def main():
+    import math
+    import sys
+    import os
+    import time
+    import argparse
+    import pybullet as p
+    from .simulation import Simulation
+
+    parser = argparse.ArgumentParser(prog="onshape-to-robot-bullet")
+    parser.add_argument("-f", "--fixed", action="store_true")
+    parser.add_argument("-n", "--no-self-collisions", action="store_true")
+    parser.add_argument("-x", "--x", type=float, default=0)
+    parser.add_argument("-y", "--y", type=float, default=0)
+    parser.add_argument("-z", "--z", type=float, default=0)
+    parser.add_argument("directory")
+    args = parser.parse_args()
+
+    robotPath = args.directory
+    if not robotPath.endswith(".urdf"):
+        robotPath += "/robot.urdf"
+
+    sim = Simulation(
+        robotPath,
+        gui=True,
+        panels=True,
+        fixed=args.fixed,
+        ignore_self_collisions=args.no_self_collisions,
+    )
+    pos, rpy = sim.getRobotPose()
+    _, orn = p.getBasePositionAndOrientation(sim.robot)
+    sim.setRobotPose([pos[0] + args.x, pos[1] + args.y, pos[2] + args.z], orn)
+
+    controls = {}
+    for name in sim.getJoints():
+        if name.endswith("_speed"):
+            controls[name] = p.addUserDebugParameter(name, -math.pi * 3, math.pi * 3, 0)
+        else:
+            infos = sim.getJointsInfos(name)
+            low = -math.pi
+            high = math.pi
+            if "lowerLimit" in infos:
+                low = infos["lowerLimit"]
+            if "upperLimit" in infos:
+                high = infos["upperLimit"]
+            controls[name] = p.addUserDebugParameter(name, low, high, 0)
+
+    lastPrint = 0
+    while True:
+        targets = {}
+        for name in controls.keys():
+            targets[name] = p.readUserDebugParameter(controls[name])
+        sim.setJoints(targets)
+
+        if time.time() - lastPrint > 0.05:
+            lastPrint = time.time()
+            os.system("clear")
+            frames = sim.getFrames()
+            for frame in frames:
+                print(frame)
+                print("- x=%f\ty=%f\tz=%f" % frames[frame][0])
+                print("- r=%f\tp=%f\ty=%f" % frames[frame][1])
+                print("")
+            print("Center of mass:")
+            print(sim.getCenterOfMassPosition())
+
+        sim.tick()
+
+
+if __name__ == "__main__":
+    main()