AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-07 08:10:08 +00:00
Code Issues Releases Wiki Activity
e386b55543
bullet3/data/quadruped/vision/vision.py
Erwin Coumans ef9570c315 add yapf style and apply yapf to format all Python files 
This recreates pull request #2192
2019-04-27 07:31:15 -07:00

342 lines
14 KiB
Python
Raw Blame History

import pybullet as p
import time
import math
pi = 3.14159264
limitVal = 2 * pi
legpos = 3. / 4. * pi
legposS = 0
legposSright = 0 #-0.3
legposSleft = 0 #0.3
defaultERP = 0.4
maxMotorForce = 5000
maxGearForce = 10000
jointFriction = 0.1
p.connect(p.GUI)
amplitudeId = p.addUserDebugParameter("amplitude", 0, 3.14, 0.5)
timeScaleId = p.addUserDebugParameter("timeScale", 0, 10, 1)
jointTypeNames = {}
jointTypeNames[p.JOINT_REVOLUTE] = "JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED] = "JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
jointNamesToIndex = {}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
vision = p.loadURDF("vision60.urdf", [0, 0, 0.4], useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision, j)
jointInfoName = jointInfo[1].decode("utf-8")
print("joint ", j, " = ", jointInfoName, "type=", jointTypeNames[jointInfo[2]])
jointNamesToIndex[jointInfoName] = j
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision, j, p.VELOCITY_CONTROL, targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
motor_back_rightS_joint = jointNamesToIndex['motor_back_rightS_joint']
hip_back_rightR_joint = jointNamesToIndex['hip_back_rightR_joint']
knee_back_rightR_joint = jointNamesToIndex['knee_back_rightR_joint']
motor_back_rightL_joint = jointNamesToIndex['motor_back_rightL_joint']
chassis_left_center = jointNamesToIndex['chassis_left_center']
motor_front_leftL_joint = jointNamesToIndex['motor_front_leftL_joint']
motor_front_leftS_joint = jointNamesToIndex['motor_front_leftS_joint']
hip_front_leftL_joint = jointNamesToIndex['hip_front_leftL_joint']
knee_front_leftL_joint = jointNamesToIndex['knee_front_leftL_joint']
motor_front_leftR_joint = jointNamesToIndex['motor_front_leftR_joint']
motor_back_leftL_joint = jointNamesToIndex['motor_back_leftL_joint']
hip_back_leftL_joint = jointNamesToIndex['hip_back_leftL_joint']
knee_back_leftL_joint = jointNamesToIndex['knee_back_leftL_joint']
motor_back_leftR_joint = jointNamesToIndex['motor_back_leftR_joint']
motor_back_leftS_joint = jointNamesToIndex['motor_back_leftS_joint']
motA_rf_Id = p.addUserDebugParameter("motA_rf", -limitVal, limitVal, legpos)
motB_rf_Id = p.addUserDebugParameter("motB_rf", -limitVal, limitVal, legpos)
motC_rf_Id = p.addUserDebugParameter("motC_rf", -limitVal, limitVal, legposSright)
erp_rf_Id = p.addUserDebugParameter("erp_rf", 0, 1, defaultERP)
relPosTarget_rf_Id = p.addUserDebugParameter("relPosTarget_rf", -limitVal, limitVal, 0)
motA_lf_Id = p.addUserDebugParameter("motA_lf", -limitVal, limitVal, -legpos)
motB_lf_Id = p.addUserDebugParameter("motB_lf", -limitVal, limitVal, -legpos)
motC_lf_Id = p.addUserDebugParameter("motC_lf", -limitVal, limitVal, legposSleft)
erp_lf_Id = p.addUserDebugParameter("erp_lf", 0, 1, defaultERP)
relPosTarget_lf_Id = p.addUserDebugParameter("relPosTarget_lf", -limitVal, limitVal, 0)
motA_rb_Id = p.addUserDebugParameter("motA_rb", -limitVal, limitVal, legpos)
motB_rb_Id = p.addUserDebugParameter("motB_rb", -limitVal, limitVal, legpos)
motC_rb_Id = p.addUserDebugParameter("motC_rb", -limitVal, limitVal, legposSright)
erp_rb_Id = p.addUserDebugParameter("erp_rb", 0, 1, defaultERP)
relPosTarget_rb_Id = p.addUserDebugParameter("relPosTarget_rb", -limitVal, limitVal, 0)
motA_lb_Id = p.addUserDebugParameter("motA_lb", -limitVal, limitVal, -legpos)
motB_lb_Id = p.addUserDebugParameter("motB_lb", -limitVal, limitVal, -legpos)
motC_lb_Id = p.addUserDebugParameter("motC_lb", -limitVal, limitVal, legposSleft)
erp_lb_Id = p.addUserDebugParameter("erp_lb", 0, 1, defaultERP)
relPosTarget_lb_Id = p.addUserDebugParameter("relPosTarget_lb", -limitVal, limitVal, 0)
camTargetPos = [0.25, 0.62, -0.15]
camDist = 2
camYaw = -2
camPitch = -16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,
knee_front_rightR_joint,
vision,
motor_front_rightL_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_rf, gearRatio=-1, gearAuxLink=motor_front_rightR_joint, maxForce=maxGearForce)
c_lf = p.createConstraint(vision,
knee_front_leftL_joint,
vision,
motor_front_leftR_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_lf, gearRatio=-1, gearAuxLink=motor_front_leftL_joint, maxForce=maxGearForce)
c_rb = p.createConstraint(vision,
knee_back_rightR_joint,
vision,
motor_back_rightL_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_rb, gearRatio=-1, gearAuxLink=motor_back_rightR_joint, maxForce=maxGearForce)
c_lb = p.createConstraint(vision,
knee_back_leftL_joint,
vision,
motor_back_leftR_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_lb, gearRatio=-1, gearAuxLink=motor_back_leftL_joint, maxForce=maxGearForce)
p.setRealTimeSimulation(1)
for i in range(1):
#while (1):
motA_rf = p.readUserDebugParameter(motA_rf_Id)
motB_rf = p.readUserDebugParameter(motB_rf_Id)
motC_rf = p.readUserDebugParameter(motC_rf_Id)
erp_rf = p.readUserDebugParameter(erp_rf_Id)
relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
#motC_rf
p.setJointMotorControl2(vision,
motor_front_rightR_joint,
p.POSITION_CONTROL,
targetPosition=motA_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rf,
force=maxMotorForce)
p.changeConstraint(c_rf,
gearRatio=-1,
gearAuxLink=motor_front_rightR_joint,
erp=erp_rf,
relativePositionTarget=relPosTarget_rf,
maxForce=maxGearForce)
motA_lf = p.readUserDebugParameter(motA_lf_Id)
motB_lf = p.readUserDebugParameter(motB_lf_Id)
motC_lf = p.readUserDebugParameter(motC_lf_Id)
erp_lf = p.readUserDebugParameter(erp_lf_Id)
relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
p.setJointMotorControl2(vision,
motor_front_leftL_joint,
p.POSITION_CONTROL,
targetPosition=motA_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lf,
force=maxMotorForce)
p.changeConstraint(c_lf,
gearRatio=-1,
gearAuxLink=motor_front_leftL_joint,
erp=erp_lf,
relativePositionTarget=relPosTarget_lf,
maxForce=maxGearForce)
motA_rb = p.readUserDebugParameter(motA_rb_Id)
motB_rb = p.readUserDebugParameter(motB_rb_Id)
motC_rb = p.readUserDebugParameter(motC_rb_Id)
erp_rb = p.readUserDebugParameter(erp_rb_Id)
relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id)
p.setJointMotorControl2(vision,
motor_back_rightR_joint,
p.POSITION_CONTROL,
targetPosition=motA_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rb,
force=maxMotorForce)
p.changeConstraint(c_rb,
gearRatio=-1,
gearAuxLink=motor_back_rightR_joint,
erp=erp_rb,
relativePositionTarget=relPosTarget_rb,
maxForce=maxGearForce)
motA_lb = p.readUserDebugParameter(motA_lb_Id)
motB_lb = p.readUserDebugParameter(motB_lb_Id)
motC_lb = p.readUserDebugParameter(motC_lb_Id)
erp_lb = p.readUserDebugParameter(erp_lb_Id)
relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
p.setJointMotorControl2(vision,
motor_back_leftL_joint,
p.POSITION_CONTROL,
targetPosition=motA_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lb,
force=maxMotorForce)
p.changeConstraint(c_lb,
gearRatio=-1,
gearAuxLink=motor_back_leftL_joint,
erp=erp_lb,
relativePositionTarget=relPosTarget_lb,
maxForce=maxGearForce)
p.setGravity(0, 0, -10)
time.sleep(1. / 240.)
t = 0
prevTime = time.time()
while (1):
timeScale = p.readUserDebugParameter(timeScaleId)
amplitude = p.readUserDebugParameter(amplitudeId)
newTime = time.time()
dt = (newTime - prevTime) * timeScale
t = t + dt
prevTime = newTime
amp = amplitude
motA_rf = math.sin(t) * amp + legpos
motA_rb = math.sin(t) * amp + legpos
motA_lf = -(math.sin(t) * amp + legpos)
motA_lb = -(math.sin(t) * amp + legpos)
motB_rf = math.sin(t) * amp + legpos
motB_rb = math.sin(t) * amp + legpos
motB_lf = -(math.sin(t) * amp + legpos)
motB_lb = -(math.sin(t) * amp + legpos)
p.setJointMotorControl2(vision,
motor_front_rightR_joint,
p.POSITION_CONTROL,
targetPosition=motA_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftL_joint,
p.POSITION_CONTROL,
targetPosition=motA_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightR_joint,
p.POSITION_CONTROL,
targetPosition=motA_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftL_joint,
p.POSITION_CONTROL,
targetPosition=motA_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lb,
force=maxMotorForce)
p.setGravity(0, 0, -10)
time.sleep(1. / 240.)
Reference in New Issue View Git Blame Copy Permalink