add yapf style and apply yapf to format all Python files
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
@@ -3,51 +3,44 @@ import time
|
||||
import math
|
||||
|
||||
pi = 3.14159264
|
||||
limitVal = 2*pi
|
||||
legpos = 3./4.*pi
|
||||
limitVal = 2 * pi
|
||||
legpos = 3. / 4. * pi
|
||||
legposS = 0
|
||||
legposSright = 0#-0.3
|
||||
legposSleft = 0#0.3
|
||||
legposSright = 0 #-0.3
|
||||
legposSleft = 0 #0.3
|
||||
|
||||
defaultERP=0.4
|
||||
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)
|
||||
|
||||
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"
|
||||
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)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
|
||||
|
||||
jointNamesToIndex = {}
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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']
|
||||
@@ -72,149 +65,277 @@ 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_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)
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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]
|
||||
camTargetPos = [0.25, 0.62, -0.15]
|
||||
camDist = 2
|
||||
camYaw = -2
|
||||
camPitch=-16
|
||||
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_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)
|
||||
|
||||
|
||||
|
||||
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)
|
||||
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_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_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)
|
||||
|
||||
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.)
|
||||
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
|
||||
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)
|
||||
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)
|
||||
|
||||
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_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_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_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.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.)
|
||||
|
||||
p.setGravity(0, 0, -10)
|
||||
time.sleep(1. / 240.)
|
||||
|
||||
Reference in New Issue
Block a user