Demo entry 6796409

importpybulletaspimportpybullet_dataimportosimportnumpyasnpfromCompute_control_with_Incomplete_modelimportcontrol_compute# Data_decomposition is a python file including a class named Every_jointimporttimeimportmatplotlib.pyplotasplt#import data documentinputpath="walk.calc"# read the data from documentposture_data=[]withopen(inputpath,'r')asf:foriinrange(1050):next(f)forlinesinf.readlines():posture_data.append(list(map(float,lines.split())))# numFrames is the total array number of the data filenumFrames=len(posture_data)# connect the GUIp.connect(p.GUI)p.setGravity(0,0,-9.8)p.setPhysicsEngineParameter(numSolverIterations=100)# switch the path and load the inner plane urdf filep.setAdditionalSearchPath(pybullet_data.getDataPath())planeId=p.loadURDF("plane.urdf")# import the model written by URDFp.setAdditionalSearchPath(os.getcwd())princess=p.loadURDF("princess.urdf",globalScaling=1.3)# get the joint and link index of urdf modelforjinrange(p.getNumJoints(princess)):i=p.getJointInfo(princess,j)print("joint[",j,"].name=",i[1])print("link [",j,"].name=",i[12])forjinrange(p.getNumJoints(princess)):# change every joints' dynamics parameterp.changeDynamics(princess,j,linearDamping=1,angularDamping=1)# enable joint force torque sensorp.enableJointForceTorqueSensor(princess,j,1)# kp is the postion gain used for position controlkp=8# normalizenum_interval=100t=np.linspace(0,1,num_interval-1)# def the main function, control the robot by position control and calculate the powerdefmain():# def the camera parameters in Pybullet GUIcyaw=30cpitch=-15cdist=2.5rotation=[]whilep.isConnected():foriinrange(numFrames):print("i=",i)forkinrange(len(t)):power_data_Torque=[]#define a list to store torque data of every jointpower_data_angularVelocity=[]#define a list to store angular velocity data of every jointforjinrange(p.getNumJoints(princess)-1):interval=t[k]# def a interpolation between the current frame and the next frameif(j==0):#for every time loop, call the "control_compute" classbase_data_current=posture_data[i][10*j+4:10*j+7]base_data_current.append(posture_data[i][10*j+3])base_data_next=posture_data[i+1][10*j+4:10*j+7]base_data_next.append(posture_data[i+1][10*j+3])Control=control_compute(princess,base_data_current,base_data_next,interval)Posture_data_current=posture_data[i][10*j+4:10*j+7]Posture_data_current.append(posture_data[i][10*j+3])#print(Posture_data_current)Posture_data_next=posture_data[i+1][10*j+4:10*j+7]Posture_data_next.append(posture_data[i+1][10*j+3])#print(Posture_data_next)Control.compute(Posture_data_current,Posture_data_next,interval)# get every joint's quaterion for next position controlControl.position_control(kp,j+1)# position control for every jointpower_data_angularVelocity.append(list(posture_data[i][10*j+7:10*j+10]))#read in the angular velocity data for every jointpower_data_Torque.append(list(p.getJointState(princess,j)[2][3:6]))#read the touque of every joint#print("power_data_angularVelocity=", power_data_angularVelocity)#print("powre_data_Torque=",power_data_Torque)Control.powerCalculation(power_data_angularVelocity,power_data_Torque)##call the powerCalculation function to calculate power. This function read in two parameters, which##are the data of the angular velocity and torque for every joint in a single moment respectively.p.stepSimulation()# adjust the camera scope along with the robot while simulatingkeys=p.getKeyboardEvents()ifkeys.get(100):# Dcyaw+=1ifkeys.get(97):# Acyaw-=1ifkeys.get(99):# Ccpitch+=1ifkeys.get(102):# Fcpitch-=1ifkeys.get(122):# Zcdist+=1ifkeys.get(120):# Xcdist-=1cubePos,cubeOrn=p.getBasePositionAndOrientation(princess)p.resetDebugVisualizerCamera(cameraDistance=cdist,cameraYaw=cyaw,cameraPitch=cpitch,cameraTargetPosition=cubePos)if__name__=="__main__":main()