Demo entry 6796409

py

   

Submitted by anonymous on May 14, 2019 at 15:38
Language: Python 3. Code size: 5.2 kB.

import pybullet as p
import pybullet_data
import os
import numpy as np
from Compute_control_with_Incomplete_model import control_compute  # Data_decomposition is a python file including a class named Every_joint
import time
import matplotlib.pyplot as plt

#import data document
inputpath = "walk.calc"

# read the data from document
posture_data = []
with open(inputpath, 'r') as f:
    for i in range(1050):
        next(f)
    for lines in f.readlines():
        posture_data.append(list(map(float, lines.split())))

# numFrames is the total array number of the data file
numFrames = len(posture_data)

# connect the GUI
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(numSolverIterations=100)

# switch the path and load the inner plane urdf file
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")

# import the model written by URDF
p.setAdditionalSearchPath(os.getcwd())
princess = p.loadURDF("princess.urdf", globalScaling=1.3)

# get the joint and link index of urdf model
for j in range(p.getNumJoints(princess)):
    i = p.getJointInfo(princess, j)
    print("joint[", j, "].name=", i[1])
    print("link [", j, "].name=", i[12])

for j in range(p.getNumJoints(princess)):
    # change every joints' dynamics parameter
    p.changeDynamics(princess, j, linearDamping=1, angularDamping=1)
    # enable joint force torque sensor
    p.enableJointForceTorqueSensor(princess,j,1)



# kp is the postion gain used for position control
kp = 8
# normalize
num_interval = 100
t = np.linspace(0, 1, num_interval - 1)


# def the main function, control the robot by position control and calculate the power
def main():
    #  def the camera parameters in Pybullet GUI
    cyaw = 30
    cpitch = -15
    cdist = 2.5
    rotation=[]
    while p.isConnected():
        for i in range(numFrames):
            print("i=",i)
            for k in range(len(t)):
                power_data_Torque=[]       #define a list to store torque data of every joint
                power_data_angularVelocity=[]    #define a list to store angular velocity data of every joint
                for j in range(p.getNumJoints(princess)-1):
                    interval = t[k]  # def a interpolation between the current frame and the next frame
                    if (j==0):    #for every time loop, call the "control_compute" class
                        base_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 control
                    Control.position_control(kp,j+1)  # position control for every joint
                    power_data_angularVelocity.append(list(posture_data[i][10*j+7:10*j+10]))  #read in the angular velocity data for every joint
                    power_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 simulating
                keys = p.getKeyboardEvents()
                if keys.get(100):  # D
                    cyaw += 1
                if keys.get(97):  # A
                    cyaw -= 1
                if keys.get(99):  # C
                    cpitch += 1
                if keys.get(102):  # F
                    cpitch -= 1
                if keys.get(122):  # Z
                    cdist += 1
                if keys.get(120):  # X
                    cdist -= 1
                cubePos, cubeOrn = p.getBasePositionAndOrientation(princess)
                p.resetDebugVisualizerCamera(cameraDistance=cdist, cameraYaw=cyaw, cameraPitch=cpitch,
                                                 cameraTargetPosition=cubePos)


if __name__ == "__main__":
    main()

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).