add preliminary vision60 urdf model (no real sizes, no real masses etc)
This commit is contained in:
Erwin Coumans
220
data/quadruped/vision/vision.py
Normal file
220
data/quadruped/vision/vision.py
Normal file
@@ -0,0 +1,220 @@
|
||||
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
Block a user