mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
154 lines
6.0 KiB
Python
154 lines
6.0 KiB
Python
import os
|
|
import copy
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
|
|
class Racecar:
|
|
|
|
def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01):
|
|
self.urdfRootPath = urdfRootPath
|
|
self.timeStep = timeStep
|
|
self._p = bullet_client
|
|
self.reset()
|
|
|
|
def reset(self):
|
|
car = self._p.loadURDF(os.path.join(self.urdfRootPath, "racecar/racecar_differential.urdf"),
|
|
[0, 0, .2],
|
|
useFixedBase=False)
|
|
self.racecarUniqueId = car
|
|
#for i in range (self._p.getNumJoints(car)):
|
|
# print (self._p.getJointInfo(car,i))
|
|
for wheel in range(self._p.getNumJoints(car)):
|
|
self._p.setJointMotorControl2(car,
|
|
wheel,
|
|
self._p.VELOCITY_CONTROL,
|
|
targetVelocity=0,
|
|
force=0)
|
|
self._p.getJointInfo(car, wheel)
|
|
|
|
#self._p.setJointMotorControl2(car,10,self._p.VELOCITY_CONTROL,targetVelocity=1,force=10)
|
|
c = self._p.createConstraint(car,
|
|
9,
|
|
car,
|
|
11,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
c = self._p.createConstraint(car,
|
|
10,
|
|
car,
|
|
13,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = self._p.createConstraint(car,
|
|
9,
|
|
car,
|
|
13,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = self._p.createConstraint(car,
|
|
16,
|
|
car,
|
|
18,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=1, maxForce=10000)
|
|
|
|
c = self._p.createConstraint(car,
|
|
16,
|
|
car,
|
|
19,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = self._p.createConstraint(car,
|
|
17,
|
|
car,
|
|
19,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
|
|
|
|
c = self._p.createConstraint(car,
|
|
1,
|
|
car,
|
|
18,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
|
|
c = self._p.createConstraint(car,
|
|
3,
|
|
car,
|
|
19,
|
|
jointType=self._p.JOINT_GEAR,
|
|
jointAxis=[0, 1, 0],
|
|
parentFramePosition=[0, 0, 0],
|
|
childFramePosition=[0, 0, 0])
|
|
self._p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
|
|
|
|
self.steeringLinks = [0, 2]
|
|
self.maxForce = 20
|
|
self.nMotors = 2
|
|
self.motorizedwheels = [8, 15]
|
|
self.speedMultiplier = 20.
|
|
self.steeringMultiplier = 0.5
|
|
|
|
def getActionDimension(self):
|
|
return self.nMotors
|
|
|
|
def getObservationDimension(self):
|
|
return len(self.getObservation())
|
|
|
|
def getObservation(self):
|
|
observation = []
|
|
pos, orn = self._p.getBasePositionAndOrientation(self.racecarUniqueId)
|
|
|
|
observation.extend(list(pos))
|
|
observation.extend(list(orn))
|
|
|
|
return observation
|
|
|
|
def applyAction(self, motorCommands):
|
|
targetVelocity = motorCommands[0] * self.speedMultiplier
|
|
#print("targetVelocity")
|
|
#print(targetVelocity)
|
|
steeringAngle = motorCommands[1] * self.steeringMultiplier
|
|
#print("steeringAngle")
|
|
#print(steeringAngle)
|
|
#print("maxForce")
|
|
#print(self.maxForce)
|
|
|
|
for motor in self.motorizedwheels:
|
|
self._p.setJointMotorControl2(self.racecarUniqueId,
|
|
motor,
|
|
self._p.VELOCITY_CONTROL,
|
|
targetVelocity=targetVelocity,
|
|
force=self.maxForce)
|
|
for steer in self.steeringLinks:
|
|
self._p.setJointMotorControl2(self.racecarUniqueId,
|
|
steer,
|
|
self._p.POSITION_CONTROL,
|
|
targetPosition=steeringAngle)
|