49 lines
1.8 KiB
Python
49 lines
1.8 KiB
Python
import arm_1dof.robot_arm_1dof
|
|
import pybullet
|
|
from tokenize import String
|
|
|
|
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
|
|
|