Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-10-28 19:45:35 +00:00
Code Issues Packages Projects 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
data/differential/diff_arm.stl
BIN
View File
Binary file not shown.
data/differential/diff_carrier.stl
BIN
View File
Binary file not shown.
data/differential/diff_carrier_cover.stl
BIN
View File
Binary file not shown.
data/differential/diff_leftshaft.stl
BIN
View File
Binary file not shown.
data/differential/diff_motor_cover.stl
BIN
View File
Binary file not shown.
data/differential/diff_pinion.stl
BIN
View File
Binary file not shown.
data/differential/diff_rightshaft.stl
BIN
View File
Binary file not shown.
data/differential/diff_ring.stl
BIN
View File
Binary file not shown.
data/differential/diff_ring.urdf
+198
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>
data/differential/diff_side.stl
BIN
View File
Binary file not shown.
data/differential/diff_spider.stl
BIN
View File
Binary file not shown.
data/differential/diff_spider_shaft.stl
BIN
View File
Binary file not shown.
data/differential/diff_stand.stl
BIN
View File
Binary file not shown.
data/differential/modelorigin.txt
+2
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
data/terrain.obj
+2583
View File
File diff suppressed because it is too large Load Diff
data/terrain.py
+59
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(" ")
data/terrain.urdf
+29
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>
data/wheel.urdf
+42
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>
examples/Importers/ImportURDFDemo/UrdfParser.cpp
+4 -2
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)
{
examples/pybullet/examples/createSphereMultiBodies.py
-2
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])
examples/pybullet/examples/mimicJointConstraint.py
+15 -6
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):
examples/pybullet/examples/racecar.py
+6 -1
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)
examples/pybullet/gym/envs/__init__.py
+7
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,
)
examples/pybullet/gym/envs/bullet/racecar.py
+54
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)
examples/pybullet/gym/envs/bullet/racecarGymEnv.py
+108
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
examples/pybullet/gym/racecarGymEnvTest.py
+14
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)