From 4df8b2762669f7ad116c763fea417c1830e37521 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Wed, 8 Feb 2017 17:26:36 -0800 Subject: [PATCH 1/3] make the motorId corresponds to that of the real minitaur. change the mass of the quadruped.urdf, change the friction of plane.urdf. --- data/plane.urdf | 2 +- data/quadruped/quadruped.urdf | 2 +- examples/pybullet/minitaur.py | 21 ++++++++++++--------- examples/pybullet/minitaur_test.py | 2 +- 4 files changed, 15 insertions(+), 12 deletions(-) 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..5364d8124 100644 --- a/examples/pybullet/minitaur.py +++ b/examples/pybullet/minitaur.py @@ -1,4 +1,5 @@ import pybullet as p +import numpy as np class Minitaur: def __init__(self): @@ -8,7 +9,8 @@ class Minitaur: self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) self.kp = 1 self.kd = 0.1 - self.maxForce = 100 + self.maxForce = 3.5 + self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] nJoints = p.getNumJoints(self.quadruped) self.jointNameToId = {} for i in range(nJoints): @@ -73,11 +75,12 @@ 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) + self.setMotorAngleByName('motor_front_leftR_joint', motorCommandsWithDir[0]) + self.setMotorAngleByName('motor_front_leftL_joint', motorCommandsWithDir[1]) + self.setMotorAngleByName('motor_back_leftR_joint', motorCommandsWithDir[2]) + self.setMotorAngleByName('motor_back_leftL_joint', motorCommandsWithDir[3]) + self.setMotorAngleByName('motor_front_rightL_joint', motorCommandsWithDir[4]) + self.setMotorAngleByName('motor_front_rightR_joint', motorCommandsWithDir[5]) + self.setMotorAngleByName('motor_back_rightL_joint', motorCommandsWithDir[6]) + self.setMotorAngleByName('motor_back_rightR_joint', motorCommandsWithDir[7]) diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py index 8f4591086..ddc2ac136 100644 --- a/examples/pybullet/minitaur_test.py +++ b/examples/pybullet/minitaur_test.py @@ -21,7 +21,7 @@ def main(unused_args): 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] + joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2] minitaur.applyAction(joint_values) p.stepSimulation() From 509b77054af28124035080dbd963bb0ae2176101 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Thu, 9 Feb 2017 14:43:40 -0800 Subject: [PATCH 2/3] now minitaur class can output joint angles, velocities and torques. I also extract evaluate functions to a file --- examples/pybullet/minitaur.py | 72 ++++++++++++++++++++------ examples/pybullet/minitaur_evaluate.py | 59 +++++++++++++++++++++ examples/pybullet/minitaur_test.py | 35 ++++++++----- 3 files changed, 136 insertions(+), 30 deletions(-) create mode 100644 examples/pybullet/minitaur_evaluate.py diff --git a/examples/pybullet/minitaur.py b/examples/pybullet/minitaur.py index 5364d8124..58aab4a38 100644 --- a/examples/pybullet/minitaur.py +++ b/examples/pybullet/minitaur.py @@ -2,15 +2,11 @@ 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 = 3.5 - self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1] + def buildJointNameToIdDict(self): nJoints = p.getNumJoints(self.quadruped) self.jointNameToId = {} for i in range(nJoints): @@ -20,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 @@ -76,11 +98,29 @@ class Minitaur: def applyAction(self, motorCommands): motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) - self.setMotorAngleByName('motor_front_leftR_joint', motorCommandsWithDir[0]) - self.setMotorAngleByName('motor_front_leftL_joint', motorCommandsWithDir[1]) - self.setMotorAngleByName('motor_back_leftR_joint', motorCommandsWithDir[2]) - self.setMotorAngleByName('motor_back_leftL_joint', motorCommandsWithDir[3]) - self.setMotorAngleByName('motor_front_rightL_joint', motorCommandsWithDir[4]) - self.setMotorAngleByName('motor_front_rightR_joint', motorCommandsWithDir[5]) - self.setMotorAngleByName('motor_back_rightL_joint', motorCommandsWithDir[6]) - self.setMotorAngleByName('motor_back_rightR_joint', motorCommandsWithDir[7]) + 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..73212f125 --- /dev/null +++ b/examples/pybullet/minitaur_evaluate.py @@ -0,0 +1,59 @@ +from minitaur import Minitaur +import time +import numpy as np +import pybullet as p +import math +import sys + +minitaur = None + +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_params_hop(params, 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) + + amplitude = params[0] + speed = params[1] + + global minitaur + minitaur = Minitaur(urdfRoot) + start_position = current_position() + last_position = None # for tracing line + + for i in range(maxNumSteps): + 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] + minitaur.applyAction(joint_values) + p.stepSimulation() + if (is_fallen()): + break + + if i % 100 == 0: + sys.stdout.write('.') + sys.stdout.flush() + time.sleep(sleepTime) + + print(' ') + + final_distance = np.linalg.norm(start_position - current_position()) + elapsedTime = time.time() - beforeTime + print ("trial for amplitude", amplitude, "speed", speed, "final_distance", final_distance, "elapsed_time", elapsedTime) + return final_distance diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py index ddc2ac136..2b0726f6f 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 +import minitaur_evaluate import time import math import numpy as np @@ -10,24 +11,30 @@ 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, a2, 1.57, 1.57, a1, 1.57, a2] - minitaur.applyAction(joint_values) - p.stepSimulation() -# print(minitaur.getBasePosition()) - time.sleep(timeStep) - final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) + final_distance = minitaur_evaluate.evaluate_params_hop(params=[amplitude, speed], timeStep=timeStep, sleepTime=timeStep) print(final_distance) + # p.resetSimulation() + # p.setTimeStep(timeStep) + # p.loadURDF("plane.urdf") + # p.setGravity(0,0,-10) + + # minitaur = Minitaur() + + # 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, a2, 1.57, 1.57, a1, 1.57, a2] + # minitaur.applyAction(joint_values) + # torques = minitaur.getMotorTorques() + # print(torques) + # p.stepSimulation() + # time.sleep(timeStep) + # final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) + # print(final_distance) + main(0) From 0665a447af084600dc1808a7731d6f70b99fb603 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Thu, 16 Feb 2017 15:23:46 -0800 Subject: [PATCH 3/3] refactor to make minitaur example more general --- examples/pybullet/minitaur_evaluate.py | 64 +++++++++++++++++++++----- examples/pybullet/minitaur_test.py | 31 +++---------- 2 files changed, 59 insertions(+), 36 deletions(-) diff --git a/examples/pybullet/minitaur_evaluate.py b/examples/pybullet/minitaur_evaluate.py index 73212f125..3c3e65280 100644 --- a/examples/pybullet/minitaur_evaluate.py +++ b/examples/pybullet/minitaur_evaluate.py @@ -1,12 +1,15 @@ from minitaur import Minitaur -import time -import numpy as np import pybullet as p -import math +import numpy as np +import time import sys +import math minitaur = None +evaluate_func_map = dict() + + def current_position(): global minitaur position = minitaur.getBasePosition() @@ -19,8 +22,43 @@ def is_fallen(): 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_params_hop(params, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0): +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() @@ -29,18 +67,18 @@ def evaluate_params_hop(params, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sl p.loadURDF("%s/plane.urdf" % urdfRoot) p.setGravity(0,0,-10) - amplitude = params[0] - speed = params[1] - global minitaur minitaur = Minitaur(urdfRoot) start_position = current_position() last_position = None # for tracing line + total_energy = 0 for i in range(maxNumSteps): - 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] + 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()): @@ -53,7 +91,9 @@ def evaluate_params_hop(params, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sl 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 amplitude", amplitude, "speed", speed, "final_distance", final_distance, "elapsed_time", elapsedTime) - return final_distance + 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 2b0726f6f..e98cd8cde 100644 --- a/examples/pybullet/minitaur_test.py +++ b/examples/pybullet/minitaur_test.py @@ -1,7 +1,7 @@ import pybullet as p - from minitaur import Minitaur -import minitaur_evaluate +from minitaur_evaluate import * + import time import math import numpy as np @@ -12,29 +12,12 @@ def main(unused_args): if (c<0): c = p.connect(p.GUI) - amplitude = 0.24795664427 - speed = 0.2860877729434 + params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539] + evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase' + energy_weight = 0.01 - final_distance = minitaur_evaluate.evaluate_params_hop(params=[amplitude, speed], timeStep=timeStep, sleepTime=timeStep) - print(final_distance) + finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep) - # p.resetSimulation() - # p.setTimeStep(timeStep) - # p.loadURDF("plane.urdf") - # p.setGravity(0,0,-10) - - # minitaur = Minitaur() - - # 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, a2, 1.57, 1.57, a1, 1.57, a2] - # minitaur.applyAction(joint_values) - # torques = minitaur.getMotorTorques() - # print(torques) - # p.stepSimulation() - # time.sleep(timeStep) - # final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) - # print(final_distance) + print(finalReturn) main(0)