Merge pull request #1179 from erwincoumans/master
add mimic constraint for differential gears
This commit is contained in:
erwincoumans
GitHub
BIN
data/differential/diff_arm.stl
Normal file
BIN
data/differential/diff_arm.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_carrier.stl
Normal file
BIN
data/differential/diff_carrier.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_carrier_cover.stl
Normal file
BIN
data/differential/diff_carrier_cover.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_leftshaft.stl
Normal file
BIN
data/differential/diff_leftshaft.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_motor_cover.stl
Normal file
BIN
data/differential/diff_motor_cover.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_pinion.stl
Normal file
BIN
data/differential/diff_pinion.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_rightshaft.stl
Normal file
BIN
data/differential/diff_rightshaft.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_ring.stl
Normal file
BIN
data/differential/diff_ring.stl
Normal file
Binary file not shown.
198
data/differential/diff_ring.urdf
Normal file
198
data/differential/diff_ring.urdf
Normal 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
BIN
data/differential/diff_side.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_spider.stl
Normal file
BIN
data/differential/diff_spider.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_spider_shaft.stl
Normal file
BIN
data/differential/diff_spider_shaft.stl
Normal file
Binary file not shown.
BIN
data/differential/diff_stand.stl
Normal file
BIN
data/differential/diff_stand.stl
Normal file
Binary file not shown.
2
data/differential/modelorigin.txt
Normal file
2
data/differential/modelorigin.txt
Normal 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
2583
data/terrain.obj
Normal file
File diff suppressed because it is too large
Load Diff
59
data/terrain.py
Normal file
59
data/terrain.py
Normal 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
29
data/terrain.urdf
Normal 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
42
data/wheel.urdf
Normal 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>
|
||||||
@@ -323,7 +323,9 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
|
|||||||
|
|
||||||
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
|
bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
|
||||||
{
|
{
|
||||||
btAssert(g);
|
// btAssert(g);
|
||||||
|
if (g==0)
|
||||||
|
return false;
|
||||||
|
|
||||||
TiXmlElement *shape = g->FirstChildElement();
|
TiXmlElement *shape = g->FirstChildElement();
|
||||||
if (!shape)
|
if (!shape)
|
||||||
|
|||||||
@@ -5,8 +5,6 @@ useMaximalCoordinates = 0
|
|||||||
|
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
|
p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
|
||||||
p.setPhysicsEngineParameter(numSolverIterations=10)
|
|
||||||
p.setPhysicsEngineParameter(fixedTimeStep=1./120.)
|
|
||||||
sphereRadius = 0.05
|
sphereRadius = 0.05
|
||||||
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
|
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
|
||||||
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
|
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
|
||||||
|
|||||||
@@ -4,13 +4,22 @@
|
|||||||
import pybullet as p
|
import pybullet as p
|
||||||
import time
|
import time
|
||||||
p.connect(p.GUI)
|
p.connect(p.GUI)
|
||||||
wheelA = p.loadURDF("wheel.urdf",[0,0,0])
|
p.loadURDF("plane.urdf",0,0,-2)
|
||||||
wheelB = p.loadURDF("wheel.urdf",[0,0,1])
|
wheelA = p.loadURDF("differential/diff_ring.urdf",[0,0,0])
|
||||||
p.setJointMotorControl2(wheelA,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
|
for i in range(p.getNumJoints(wheelA)):
|
||||||
p.setJointMotorControl2(wheelB,0,p.VELOCITY_CONTROL,targetVelocity=1,force=1)
|
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)
|
p.setRealTimeSimulation(1)
|
||||||
while(1):
|
while(1):
|
||||||
|
|||||||
@@ -20,7 +20,12 @@ car = p.loadURDF("racecar/racecar.urdf")
|
|||||||
for i in range (p.getNumJoints(car)):
|
for i in range (p.getNumJoints(car)):
|
||||||
print (p.getJointInfo(car,i))
|
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]
|
steering = [4,6]
|
||||||
|
|
||||||
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)
|
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)
|
||||||
|
|||||||
@@ -15,3 +15,10 @@ register(
|
|||||||
timestep_limit=1000,
|
timestep_limit=1000,
|
||||||
reward_threshold=5.0,
|
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
54
examples/pybullet/gym/envs/bullet/racecar.py
Normal 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
108
examples/pybullet/gym/envs/bullet/racecarGymEnv.py
Normal 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
14
examples/pybullet/gym/racecarGymEnvTest.py
Normal 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)
|
||||||
Reference in New Issue
Block a user