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.)