diff --git a/data/plane.urdf b/data/plane.urdf index 480b0752f..ad10aeedf 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -2,7 +2,7 @@ - + diff --git a/data/quadruped/quadruped.urdf b/data/quadruped/quadruped.urdf index 32e9c131c..8a6da709e 100644 --- a/data/quadruped/quadruped.urdf +++ b/data/quadruped/quadruped.urdf @@ -15,7 +15,7 @@ - + diff --git a/examples/pybullet/minitaur.py b/examples/pybullet/minitaur.py index 39827071c..58aab4a38 100644 --- a/examples/pybullet/minitaur.py +++ b/examples/pybullet/minitaur.py @@ -1,14 +1,12 @@ import pybullet as p +import numpy as np class Minitaur: - def __init__(self): + def __init__(self, urdfRootPath=''): + self.urdfRootPath = urdfRootPath self.reset() - def reset(self): - self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) - self.kp = 1 - self.kd = 0.1 - self.maxForce = 100 + def buildJointNameToIdDict(self): nJoints = p.getNumJoints(self.quadruped) self.jointNameToId = {} for i in range(nJoints): @@ -18,13 +16,39 @@ class Minitaur: for i in range(100): p.stepSimulation() + def buildMotorIdList(self): + self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) + self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint']) + + + def reset(self): + self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3) + self.kp = 1 + self.kd = 0.1 + self.maxForce = 3.5 + self.nMotors = 8 + self.motorIdList = [] + self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] + self.buildJointNameToIdDict() + self.buildMotorIdList() + + def disableAllMotors(self): nJoints = p.getNumJoints(self.quadruped) for i in range(nJoints): p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0) + def setMotorAngleById(self, motorId, desiredAngle): + p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) + def setMotorAngleByName(self, motorName, desiredAngle): - p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=self.jointNameToId[motorName], controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) + self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) def resetPose(self): #right front leg @@ -73,11 +97,30 @@ class Minitaur: return orientation def applyAction(self, motorCommands): - self.setMotorAngleByName('motor_front_rightR_joint', motorCommands[0]) - self.setMotorAngleByName('motor_front_rightL_joint', motorCommands[1]) - self.setMotorAngleByName('motor_front_leftR_joint', motorCommands[2]) - self.setMotorAngleByName('motor_front_leftL_joint', motorCommands[3]) - self.setMotorAngleByName('motor_back_rightR_joint', motorCommands[4]) - self.setMotorAngleByName('motor_back_rightL_joint', motorCommands[5]) - self.setMotorAngleByName('motor_back_leftR_joint', motorCommands[6]) - self.setMotorAngleByName('motor_back_leftL_joint', motorCommands[7]) + motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) + for i in range(self.nMotors): + self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i]) + + def getMotorAngles(self): + motorAngles = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorAngles.append(jointState[0]) + motorAngles = np.multiply(motorAngles, self.motorDir) + return motorAngles + + def getMotorVelocities(self): + motorVelocities = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorVelocities.append(jointState[1]) + motorVelocities = np.multiply(motorVelocities, self.motorDir) + return motorVelocities + + def getMotorTorques(self): + motorTorques = [] + for i in range(self.nMotors): + jointState = p.getJointState(self.quadruped, self.motorIdList[i]) + motorTorques.append(jointState[3]) + motorTorques = np.multiply(motorTorques, self.motorDir) + return motorTorques diff --git a/examples/pybullet/minitaur_evaluate.py b/examples/pybullet/minitaur_evaluate.py new file mode 100644 index 000000000..3c3e65280 --- /dev/null +++ b/examples/pybullet/minitaur_evaluate.py @@ -0,0 +1,99 @@ +from minitaur import Minitaur +import pybullet as p +import numpy as np +import time +import sys +import math + +minitaur = None + +evaluate_func_map = dict() + + +def current_position(): + global minitaur + position = minitaur.getBasePosition() + return np.asarray(position) + +def is_fallen(): + global minitaur + orientation = minitaur.getBaseOrientation() + rotMat = p.getMatrixFromQuaterion(orientation) + localUp = rotMat[6:] + return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 + +def evaluate_desired_motorAngle_8Amplitude8Phase(i, params): + nMotors = 8 + speed = 0.35 + for jthMotor in range(nMotors): + joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57 + return joint_values + +def evaluate_desired_motorAngle_2Amplitude4Phase(i, params): + speed = 0.35 + phaseDiff = params[2] + a0 = math.sin(i * speed) * params[0] + 1.57 + a1 = math.sin(i * speed + phaseDiff) * params[1] + 1.57 + a2 = math.sin(i * speed + params[3]) * params[0] + 1.57 + a3 = math.sin(i * speed + params[3] + phaseDiff) * params[1] + 1.57 + a4 = math.sin(i * speed + params[4] + phaseDiff) * params[1] + 1.57 + a5 = math.sin(i * speed + params[4]) * params[0] + 1.57 + a6 = math.sin(i * speed + params[5] + phaseDiff) * params[1] + 1.57 + a7 = math.sin(i * speed + params[5]) * params[0] + 1.57 + joint_values = [a0, a1, a2, a3, a4, a5, a6, a7] + return joint_values + +def evaluate_desired_motorAngle_hop(i, params): + amplitude = params[0] + speed = params[1] + a1 = math.sin(i*speed)*amplitude+1.57 + a2 = math.sin(i*speed+3.14)*amplitude+1.57 + joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2] + return joint_values + + +evaluate_func_map['evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase +evaluate_func_map['evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase +evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop + + + +def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0): + print('start evaluation') + beforeTime = time.time() + p.resetSimulation() + + p.setTimeStep(timeStep) + p.loadURDF("%s/plane.urdf" % urdfRoot) + p.setGravity(0,0,-10) + + global minitaur + minitaur = Minitaur(urdfRoot) + start_position = current_position() + last_position = None # for tracing line + total_energy = 0 + + for i in range(maxNumSteps): + torques = minitaur.getMotorTorques() + velocities = minitaur.getMotorVelocities() + total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep + + joint_values = evaluate_func_map[evaluateFunc](i, params) + minitaur.applyAction(joint_values) + p.stepSimulation() + if (is_fallen()): + break + + if i % 100 == 0: + sys.stdout.write('.') + sys.stdout.flush() + time.sleep(sleepTime) + + print(' ') + + alpha = objectiveParams[0] + final_distance = np.linalg.norm(start_position - current_position()) + finalReturn = final_distance - alpha * total_energy + elapsedTime = time.time() - beforeTime + print ("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime) + return finalReturn diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py index 8f4591086..e98cd8cde 100644 --- a/examples/pybullet/minitaur_test.py +++ b/examples/pybullet/minitaur_test.py @@ -1,6 +1,7 @@ import pybullet as p - from minitaur import Minitaur +from minitaur_evaluate import * + import time import math import numpy as np @@ -10,24 +11,13 @@ def main(unused_args): c = p.connect(p.SHARED_MEMORY) if (c<0): c = p.connect(p.GUI) - p.resetSimulation() - p.setTimeStep(timeStep) - p.loadURDF("plane.urdf") - p.setGravity(0,0,-10) - minitaur = Minitaur() - amplitude = 0.24795664427 - speed = 0.2860877729434 - for i in range(1000): - a1 = math.sin(i*speed)*amplitude+1.57 - a2 = math.sin(i*speed+3.14)*amplitude+1.57 - joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57] - minitaur.applyAction(joint_values) + params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539] + evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase' + energy_weight = 0.01 - p.stepSimulation() -# print(minitaur.getBasePosition()) - time.sleep(timeStep) - final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) - print(final_distance) + finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep) + + print(finalReturn) main(0)