update minitaur.py to use minitaur.urdf (instead of quadruped.urdf), also sort the legs in the same order as real hardware
added test urdf files for minitaur with all fixed joints, or fixed knees. added some stiffness/damping to minitaur legs (testing) tiny_obj_loader, don't crash on invalid texture coordinates btMultiBodyConstraintSolver: sweep back and forward to reduce asymmetry
This commit is contained in:
Erwin Coumans
@@ -17,10 +17,10 @@ class Minitaur:
|
||||
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_front_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftR_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'])
|
||||
@@ -28,21 +28,17 @@ class Minitaur:
|
||||
|
||||
|
||||
def reset(self):
|
||||
self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3)
|
||||
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
|
||||
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.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)
|
||||
@@ -51,42 +47,55 @@ class Minitaur:
|
||||
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
|
||||
|
||||
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)
|
||||
kneeFrictionForce = 0
|
||||
halfpi = 1.57079632679
|
||||
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
|
||||
|
||||
#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)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
|
||||
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.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
|
||||
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
#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)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
|
||||
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.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
|
||||
self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
#right front leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
|
||||
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.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
|
||||
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
|
||||
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.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
|
||||
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
|
||||
def getBasePosition(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
|
||||
@@ -58,7 +58,7 @@ evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAng
|
||||
|
||||
|
||||
|
||||
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=1000, sleepTime=0):
|
||||
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0):
|
||||
print('start evaluation')
|
||||
beforeTime = time.time()
|
||||
p.resetSimulation()
|
||||
|
||||
@@ -1,3 +1,7 @@
|
||||
import sys
|
||||
#some python interpreters need '.' added
|
||||
sys.path.append(".")
|
||||
|
||||
import pybullet as p
|
||||
from minitaur import Minitaur
|
||||
from minitaur_evaluate import *
|
||||
|
||||
21
examples/pybullet/mylittleminitaur.py
Normal file
21
examples/pybullet/mylittleminitaur.py
Normal file
@@ -0,0 +1,21 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
import sys
|
||||
sys.path.append(".")
|
||||
|
||||
from minitaur import Minitaur
|
||||
p.connect(p.GUI)
|
||||
p.setTimeOut(5)
|
||||
#p.setPhysicsEngineParameter(numSolverIterations=50)
|
||||
p.setGravity(0,0,-10)
|
||||
p.setTimeStep(0.01)
|
||||
|
||||
urdfRoot = ''
|
||||
p.loadURDF("%s/plane.urdf" % urdfRoot)
|
||||
minitaur = Minitaur(urdfRoot)
|
||||
|
||||
while (True):
|
||||
p.stepSimulation()
|
||||
time.sleep(0.01)
|
||||
|
||||
@@ -3,28 +3,31 @@ import time
|
||||
import math
|
||||
|
||||
useRealTime = 0
|
||||
fixedTimeStep = 0.001
|
||||
fixedTimeStep = 0.01
|
||||
speed = 10
|
||||
amplitude = 0.8
|
||||
jump_amp = 0.5
|
||||
maxForce = 3.5
|
||||
kneeFrictionForce = 0.00
|
||||
kp = .05
|
||||
kd = .5
|
||||
kp = 1
|
||||
kd = .1
|
||||
|
||||
|
||||
physId = p.connect(p.SHARED_MEMORY)
|
||||
if (physId<0):
|
||||
p.connect(p.GUI)
|
||||
#p.resetSimulation()
|
||||
p.loadURDF("plane.urdf",0,0,0)
|
||||
p.setPhysicsEngineParameter(numSolverIterations=50)
|
||||
|
||||
p.setTimeOut(4)
|
||||
p.loadURDF("plane.urdf",0,0,0.1)
|
||||
#p.loadSDF("kitchens/1.sdf")
|
||||
|
||||
p.setGravity(0,0,0)
|
||||
p.setTimeStep(fixedTimeStep)
|
||||
|
||||
orn = p.getQuaternionFromEuler([0,0,0.4])
|
||||
p.setRealTimeSimulation(0)
|
||||
quadruped = p.loadURDF("quadruped/minitaur.urdf",[0,0,0.2],useFixedBase=False)
|
||||
quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False)
|
||||
nJoints = p.getNumJoints(quadruped)
|
||||
|
||||
jointNameToId = {}
|
||||
@@ -58,47 +61,68 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
|
||||
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
|
||||
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
|
||||
|
||||
motordir=[-1,-1,-1,-1,1,1,1,1]
|
||||
halfpi = 1.57079632679
|
||||
kneeangle = -2.1834
|
||||
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
|
||||
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
|
||||
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
|
||||
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
|
||||
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.changeConstraint(cid,maxForce=10000)
|
||||
|
||||
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
|
||||
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
|
||||
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
|
||||
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
|
||||
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.changeConstraint(cid,maxForce=10000)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
|
||||
#p.getNumJoints(1)
|
||||
p.resetJointState(quadruped,motor_front_rightR_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_front_rightR_link,-2.2)
|
||||
p.resetJointState(quadruped,motor_front_rightL_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_front_rightL_link,-2.2)
|
||||
p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
|
||||
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
|
||||
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
|
||||
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
|
||||
p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
|
||||
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.changeConstraint(cid,maxForce=10000)
|
||||
|
||||
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.resetJointState(quadruped,motor_front_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_front_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_front_leftL_link,2.2)
|
||||
p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
|
||||
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
|
||||
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
|
||||
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
|
||||
cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.changeConstraint(cid,maxForce=10000)
|
||||
|
||||
p.resetJointState(quadruped,motor_back_rightR_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_back_rightR_link,-2.2)
|
||||
p.resetJointState(quadruped,motor_back_rightL_joint,1.57)
|
||||
p.resetJointState(quadruped,knee_back_rightL_link,-2.2)
|
||||
p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
|
||||
p.resetJointState(quadruped,motor_back_leftR_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_back_leftR_link,2.2)
|
||||
p.resetJointState(quadruped,motor_back_leftL_joint,-1.57)
|
||||
p.resetJointState(quadruped,knee_back_leftL_link,2.2)
|
||||
p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
p.setJointMotorControl(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,-1.57,maxForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setJointMotorControl(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,-1.57,maxForce)
|
||||
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
|
||||
|
||||
motordir=[-1,-1,-1,-1,1,1,1,1]
|
||||
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
|
||||
legnumbering=[
|
||||
motor_front_leftL_joint,
|
||||
motor_front_leftR_joint,
|
||||
motor_back_leftL_joint,
|
||||
motor_back_leftR_joint,
|
||||
motor_front_rightL_joint,
|
||||
motor_front_rightR_joint,
|
||||
motor_back_rightL_joint,
|
||||
motor_back_rightR_joint]
|
||||
|
||||
for i in range (8):
|
||||
print (legnumbering[i])
|
||||
#use the Minitaur leg numbering
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
@@ -108,9 +132,12 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMo
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
|
||||
p.stepSimulation()
|
||||
#while (True):
|
||||
# time.sleep(0.01)
|
||||
#p.stepSimulation()
|
||||
|
||||
|
||||
print("quadruped Id = ")
|
||||
@@ -118,15 +145,14 @@ print(quadruped)
|
||||
p.saveWorld("quadru.py")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.txt",[quadruped])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
|
||||
#stand still
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
|
||||
|
||||
#jump
|
||||
t = 0.0
|
||||
t_end = t + 10
|
||||
t_end = t + 100
|
||||
i=0
|
||||
ref_time = time.time()
|
||||
|
||||
@@ -135,26 +161,19 @@ while t < t_end:
|
||||
t = time.time()-ref_time
|
||||
else:
|
||||
t = t+fixedTimeStep
|
||||
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_front_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_rightL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftR_joint, controlMode=p.POSITION_CONTROL, targetPosition=math.sin(t*speed)*jump_amp+1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=motor_back_leftL_joint, controlMode=p.POSITION_CONTROL, targetPosition=-math.sin(t*speed)*jump_amp-1.57, positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
target = math.sin(t*speed)*jump_amp+1.57;
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
if (True):
|
||||
|
||||
target = math.sin(t*speed)*jump_amp+1.57;
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
|
||||
|
||||
if (useRealTime==0):
|
||||
p.stepSimulation()
|
||||
time.sleep(fixedTimeStep)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user