Maastricht_University_Projects/bullet_muscle_sim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add muscle-controlled robot

Browse Source
This commit is contained in:
Michael Zechmair
2022-06-14 16:08:34 +02:00
parent c49e54893c
commit f20f2564ef
2 changed files with 116 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
arm_1dof/bullet_arm_1dof.py
View File

@@ -1,6 +1,8 @@
import arm_1dof.robot_arm_1dof
import arm_1dof.robot_arm_muscle_1dof
import pybullet
from tokenize import String
from MuscleModelPython import MUSCLE_INDEX, JOINT_INDEX
import os
@@ -16,6 +18,7 @@ class BulletArm1Dof:
self._server_id = -1
self._skel_arm = None
self._skel_arm_id = -1
self._skel_muscles = None
@property
def server_id(self):
@@ -41,9 +44,33 @@ class BulletArm1Dof:
return self._skel_arm
def LoadMuscleRobot(self, robot_sdf_filename=URDF_MODEL_FILENAME) -> arm_1dof.robot_arm_muscle_1dof.RobotArmMuscle1Dof:
"""Load robot from sdf file, add muscles"""
self._skel_arm_id = pybullet.loadURDF(fileName=robot_sdf_filename, useFixedBase=True, physicsClientId=self._server_id)
self._skel_arm = arm_1dof.robot_arm_muscle_1dof.RobotArmMuscle1Dof(bullet_ctrl=self, bullet_body_id=self._skel_arm_id)
self._skel_muscles = self._skel_arm.muscles
return self._skel_arm
def Simulate(self, sim_time) -> None:
t = 0
while t < sim_time:
pybullet.stepSimulation(physicsClientId=self._server_id)
if self._skel_muscles:
# Update muscle joint states
self._skel_arm.UpdateStats()
self._skel_muscles.SetJointPos(JOINT_INDEX(0), self._skel_arm.JointPos(0))
self._skel_muscles.SetJointPos(JOINT_INDEX(1), self._skel_arm.JointPos(1))
self._skel_muscles.SetJointVel(JOINT_INDEX(0), self._skel_arm.JointVel(0))
self._skel_muscles.SetJointVel(JOINT_INDEX(1), self._skel_arm.JointVel(1))
self._skel_muscles.Integrate(self._timestep)
# Apply muscle torques to robot
torques = [ self._skel_muscles.GetTorque(JOINT_INDEX(0)), self._skel_muscles.GetTorque(JOINT_INDEX(1))]
self._skel_arm.SetJointTorques(joint_ids=[0,1], torques=torques)
t += self._timestep