From 692f7680a2b8a7e6d8303d69c80edb44da4c3cc7 Mon Sep 17 00:00:00 2001 From: Jie Tan Date: Mon, 6 Feb 2017 16:27:54 -0800 Subject: [PATCH] add minitaur class and test to the pull request --- examples/pybullet/minitaur.py | 83 ++++++++++++++++++++++++++++++ examples/pybullet/minitaur_test.py | 33 ++++++++++++ 2 files changed, 116 insertions(+) create mode 100644 examples/pybullet/minitaur.py create mode 100644 examples/pybullet/minitaur_test.py diff --git a/examples/pybullet/minitaur.py b/examples/pybullet/minitaur.py new file mode 100644 index 000000000..39827071c --- /dev/null +++ b/examples/pybullet/minitaur.py @@ -0,0 +1,83 @@ +import pybullet as p + +class Minitaur: + def __init__(self): + 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 + nJoints = p.getNumJoints(self.quadruped) + self.jointNameToId = {} + for i in range(nJoints): + jointInfo = p.getJointInfo(self.quadruped, i) + self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + self.resetPose() + for i in range(100): + p.stepSimulation() + + 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 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) + + def resetPose(self): + #right front leg + self.disableAllMotors() + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_front_rightR_joint', 1.57) + self.setMotorAngleByName('motor_front_rightL_joint',-1.57) + + #left front leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_front_leftR_joint', 1.57) + self.setMotorAngleByName('motor_front_leftL_joint',-1.57) + + #right back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) + self.setMotorAngleByName('motor_back_rightR_joint', 1.57) + self.setMotorAngleByName('motor_back_rightL_joint',-1.57) + + #left back leg + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2) + p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57) + p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2) + p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) + self.setMotorAngleByName('motor_back_leftR_joint', 1.57) + self.setMotorAngleByName('motor_back_leftL_joint',-1.57) + + def getBasePosition(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + return position + + def getBaseOrientation(self): + position, orientation = p.getBasePositionAndOrientation(self.quadruped) + 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]) diff --git a/examples/pybullet/minitaur_test.py b/examples/pybullet/minitaur_test.py new file mode 100644 index 000000000..8f4591086 --- /dev/null +++ b/examples/pybullet/minitaur_test.py @@ -0,0 +1,33 @@ +import pybullet as p + +from minitaur import Minitaur +import time +import math +import numpy as np + +def main(unused_args): + timeStep = 0.01 + 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) + + p.stepSimulation() +# print(minitaur.getBasePosition()) + time.sleep(timeStep) + final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition())) + print(final_distance) + +main(0)