Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1179 from erwincoumans/master

Browse Source 
add mimic constraint for differential gears
This commit is contained in:
erwincoumans
2017-06-09 15:02:59 -07:00
committed by GitHub
parent e5ce067cb9 b2a46a7edd
commit dc8c2e4d5e
26 changed files with 3121 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
data/differential/diff_arm.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_carrier.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_carrier_cover.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_leftshaft.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_motor_cover.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_pinion.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_rightshaft.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_ring.stl Normal file
View File

Binary file not shown.

198
data/differential/diff_ring.urdf Normal file
View File

@@ -0,0 +1,198 @@
<?xml version="1.0" ?>
<robot name="husky_robot" xmlns:xacro="http://ros.org/wiki/xacro">
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="world"/>
<link name="diff_ring">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_ring.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_carrier.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.025 0"/>
<geometry>
<cylinder length="0.01" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="diff_ring_world" type="continuous">
<parent link="world"/>
<child link="diff_ring"/>
<origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_spiderA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_spiderA_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_spiderA"/>
<origin rpy="0 0 1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_spiderB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_spiderB_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_spiderB"/>
<origin rpy="0 0 -1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_sideA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_sideA_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_sideA"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_sideB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="-1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="-1.570795 0 0" xyz="0 -0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_sideB_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_sideB"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
</robot>

BIN
data/differential/diff_side.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_spider.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_spider_shaft.stl Normal file
View File

Binary file not shown.

BIN
data/differential/diff_stand.stl Normal file
View File

Binary file not shown.

2
data/differential/modelorigin.txt Normal file
View File

@@ -0,0 +1,2 @@
stl files were copied from http://www.otvinta.com/download09.html
URDF file was manually created, along with mimicJointConstraint.py

2583
data/terrain.obj Normal file
View File

File diff suppressed because it is too large Load Diff

59
data/terrain.py Normal file
View File

@@ -0,0 +1,59 @@
import math
NUM_VERTS_X = 30
NUM_VERTS_Y = 30
totalVerts = NUM_VERTS_X*NUM_VERTS_Y
totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1)
offset = -50.0
TRIANGLE_SIZE = 1.
waveheight=0.1
gGroundVertices = [None] * totalVerts*3
gGroundIndices = [None] * totalTriangles*3
i=0
for i in range (NUM_VERTS_X):
for j in range (NUM_VERTS_Y):
gGroundVertices[(i+j*NUM_VERTS_X)*3+0] = (i-NUM_VERTS_X*0.5)*TRIANGLE_SIZE
gGroundVertices[(i+j*NUM_VERTS_X)*3+1] = (j-NUM_VERTS_Y*0.5)*TRIANGLE_SIZE
gGroundVertices[(i+j*NUM_VERTS_X)*3+2] = waveheight*math.sin(float(i))*math.cos(float(j)+offset)
index=0
for i in range (NUM_VERTS_X-1):
for j in range (NUM_VERTS_Y-1):
gGroundIndices[index] = 1+j*NUM_VERTS_X+i
index+=1
gGroundIndices[index] = 1+j*NUM_VERTS_X+i+1
index+=1
gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i+1
index+=1
gGroundIndices[index] = 1+j*NUM_VERTS_X+i
index+=1
gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i+1
index+=1
gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i
index+=1
#print(gGroundVertices)
#print(gGroundIndices)
print("o Terrain")
for i in range (totalVerts):
print("v "),
print(gGroundVertices[i*3+0]),
print(" "),
print(gGroundVertices[i*3+1]),
print(" "),
print(gGroundVertices[i*3+2])
for i in range (totalTriangles):
print("f "),
print(gGroundIndices[i*3+0]),
print(" "),
print(gGroundIndices[i*3+1]),
print(" "),
print(gGroundIndices[i*3+2]),
print(" ")

29
data/terrain.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="0.0" ?>
<robot name="terrain">
<link name="terrainLink">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="terrain.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision concave="yes">
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="terrain.obj" scale="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

42
data/wheel.urdf Normal file
View File

@@ -0,0 +1,42 @@
<?xml version="1.0" ?>
<robot name="husky_robot" xmlns:xacro="http://ros.org/wiki/xacro">
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="world"/>
<link name="front_left_wheel_link">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="husky/meshes/wheel.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1143" radius="0.17775"/>
</geometry>
</collision>
</link>
<joint name="front_left_wheel" type="continuous">
<parent link="world"/>
<child link="front_left_wheel_link"/>
<origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
</robot>

6
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -323,8 +323,10 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
{
btAssert(g);
// btAssert(g);
if (g==0)
return false;
TiXmlElement *shape = g->FirstChildElement();
if (!shape)
{

2
examples/pybullet/examples/createSphereMultiBodies.py
View File

@@ -5,8 +5,6 @@ useMaximalCoordinates = 0
p.connect(p.GUI)
p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(fixedTimeStep=1./120.)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])

21
examples/pybullet/examples/mimicJointConstraint.py
View File

@@ -4,13 +4,22 @@
import pybullet as p
import time
p.connect(p.GUI)
wheelA = p.loadURDF("wheel.urdf",[0,0,0])
wheelB = p.loadURDF("wheel.urdf",[0,0,1])
p.setJointMotorControl2(wheelA,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(wheelB,0,p.VELOCITY_CONTROL,targetVelocity=1,force=1)
p.loadURDF("plane.urdf",0,0,-2)
wheelA = p.loadURDF("differential/diff_ring.urdf",[0,0,0])
for i in range(p.getNumJoints(wheelA)):
print(p.getJointInfo(wheelA,i))
p.setJointMotorControl2(wheelA,i,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
c = p.createConstraint(wheelA,1,wheelA,3,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(wheelA,2,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(wheelA,1,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(wheelA,0,wheelB,0,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-0.1, maxForce=10000)
p.setRealTimeSimulation(1)
while(1):

7
examples/pybullet/examples/racecar.py
View File

@@ -20,7 +20,12 @@ car = p.loadURDF("racecar/racecar.urdf")
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
wheels = [2,3,5,7]
inactive_wheels = [3,5,7]
wheels = [2]
for wheel in inactive_wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
steering = [4,6]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)

7
examples/pybullet/gym/envs/__init__.py
View File

@@ -15,3 +15,10 @@ register(
timestep_limit=1000,
reward_threshold=5.0,
)
register(
id='RacecarBulletEnv-v0',
entry_point='envs.bullet:RacecarBulletEnv',
timestep_limit=1000,
reward_threshold=5.0,
)

54
examples/pybullet/gym/envs/bullet/racecar.py Normal file
View File

@@ -0,0 +1,54 @@
import pybullet as p
import numpy as np
import copy
import math
class Racecar:
def __init__(self, urdfRootPath='', timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self.reset()
def reset(self):
self.racecarUniqueId = p.loadURDF("racecar/racecar.urdf", [0,0,.2])
self.maxForce = 10
self.nMotors = 2
self.motorizedwheels=[2]
self.inactiveWheels = [3,5,7]
for wheel in self.inactiveWheels:
p.setJointMotorControl2(self.racecarUniqueId,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
self.motorizedWheels = [2]
self.steeringLinks=[4,6]
self.speedMultiplier = 10.
def getActionDimension(self):
return self.nMotors
def getObservationDimension(self):
return len(self.getObservation())
def getObservation(self):
observation = []
pos,orn=p.getBasePositionAndOrientation(self.racecarUniqueId)
observation.extend(list(pos))
return observation
def applyAction(self, motorCommands):
targetVelocity=motorCommands[0]*self.speedMultiplier
print("targetVelocity")
print(targetVelocity)
steeringAngle = motorCommands[1]
print("steeringAngle")
print(steeringAngle)
print("maxForce")
print(self.maxForce)
for motor in self.motorizedwheels:
p.setJointMotorControl2(self.racecarUniqueId,motor,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce)
for steer in self.steeringLinks:
p.setJointMotorControl2(self.racecarUniqueId,steer,p.POSITION_CONTROL,targetPosition=steeringAngle)

108
examples/pybullet/gym/envs/bullet/racecarGymEnv.py Normal file
View File

@@ -0,0 +1,108 @@
import math
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import time
import pybullet as p
from . import racecar
class RacecarGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
def __init__(self,
urdfRoot="",
actionRepeat=1,
isEnableSelfCollision=True,
render=True):
print("init")
self._timeStep = 0.01
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._observation = []
self._ballUniqueId = -1
self._envStepCounter = 0
self._render = render
self._p = p
if self._render:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
self._seed()
self.reset()
observationDim = self._racecar.getObservationDimension()
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
actionDim = 8
action_high = np.array([1] * actionDim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
#p.loadURDF("%splane.urdf" % self._urdfRoot)
p.loadSDF("%sstadium.sdf" % self._urdfRoot)
self._ballUniqueId = p.loadURDF("sphere2.urdf",[20,20,1])
p.setGravity(0,0,-10)
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
for i in range(100):
p.stepSimulation()
self._observation = self._racecar.getObservation()
return self._observation
def __del__(self):
p.disconnect()
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _step(self, action):
if (self._render):
basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
p.resetDebugVisualizerCamera(1, 30, -40, basePos)
if len(action) != self._racecar.getActionDimension():
raise ValueError("We expect {} continuous action not {}.".format(self._racecar.getActionDimension(), len(action)))
for i in range(len(action)):
if not -1.01 <= action[i] <= 1.01:
raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i]))
self._racecar.applyAction(action)
for i in range(self._actionRepeat):
p.stepSimulation()
if self._render:
time.sleep(self._timeStep)
self._observation = self._racecar.getObservation()
if self._termination():
break
self._envStepCounter += 1
reward = self._reward()
done = self._termination()
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
return
def _termination(self):
return False
def _reward(self):
closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
numPt = len(closestPoints)
reward=-1000
print(numPt)
if (numPt>0):
print("reward:")
reward = closestPoints[0][8]
print(reward)
return reward

14
examples/pybullet/gym/racecarGymEnvTest.py Normal file
View File

@@ -0,0 +1,14 @@
from envs.bullet.racecarGymEnv import RacecarGymEnv
print ("hello")
environment = RacecarGymEnv(render=True)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
action=[targetVelocity,steeringAngle]
state, reward, done, info = environment.step(action)