Add PyBullet robot

arm_1dof/bullet_arm_1dof.py

@@ -0,0 +1,49 @@
+from tokenize import String
+import pybullet 
+import robot_arm_1dof
+
+import os
+
+class BulletArm1Dof:
+    """Class to initialize pybullet server and load robot"""
+    SDF_MODELS_SUBDIR="arm_1dof"
+    SDF_SEARCH_PATH="/".join([os.environ.get("SDF_MODELS_DIR", "."), SDF_MODELS_SUBDIR])
+    SDF_MODEL_NAME="demo_human_col"
+    SDF_MODEL_FILENAME="/".join([SDF_MODEL_NAME, "skeleton.sdf"])
+    URDF_MODEL_FILENAME="/".join([SDF_MODEL_NAME, "skeleton.urdf"])
+
+    def __init__(self) -> None:
+        self._server_id = -1
+        self._skel_arm = None
+        self._skel_arm_id = -1
+
+    @property
+    def server_id(self):
+        """Pybullet server ID"""
+        return self._server_id
+
+    def InitPybullet(self, g=[0., 0., 0.], bullet_connect=pybullet.DIRECT, sdf_search_path=SDF_SEARCH_PATH) -> None:
+        """Start bullet server and set sdf search path"""
+        # Connect to pybullet
+        self._server_id = pybullet.connect(bullet_connect)
+
+        pybullet.setGravity(g[0], g[1], g[2], physicsClientId=self._server_id)
+        pybullet.setAdditionalSearchPath(path=sdf_search_path, physicsClientId=self._server_id)
+
+        self._timestep = pybullet.getPhysicsEngineParameters(physicsClientId=self._server_id)['fixedTimeStep']
+
+
+    def LoadRobot(self, robot_sdf_filename=URDF_MODEL_FILENAME) -> robot_arm_1dof.RobotArm1Dof:
+        """Load robot from sdf file"""
+        self._skel_arm_id = pybullet.loadURDF(fileName=robot_sdf_filename, useFixedBase=True, physicsClientId=self._server_id)
+
+        self._skel_arm = robot_arm_1dof.RobotArm1Dof(bullet_ctrl=self, bullet_body_id=self._skel_arm_id)
+
+        return self._skel_arm
+
+    def Simulate(self, sim_time) -> None:
+        t = 0
+        while t < sim_time:
+            pybullet.stepSimulation(physicsClientId=self._server_id)
+            t += self._timestep
+