AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

tweak quadruped script to make a few more moves

Browse Source
This commit is contained in:
Erwin Coumans 2016-11-16 21:36:51 -08:00
parent 2329c00faa
commit ee7a5a470f
2 changed files with 72 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
data/plane.urdf
View File

@ -1,6 +1,9 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="planeLink">
<contact>
<lateral_friction value="1.5"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>

77
examples/pybullet/quadruped.py
View File

@ -1,5 +1,6 @@
import pybullet as p
import time
import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
@ -13,10 +14,10 @@ p.resetJointState(quadruped,3,-1.57)
p.resetJointState(quadruped,5,2.2)
p.createConstraint(quadruped,2,quadruped,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
p.setJointMotorControl(quadruped,0,p.VELOCITY_CONTROL,1,10)
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,3,p.VELOCITY_CONTROL,-1,10)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0)
@ -27,10 +28,10 @@ p.resetJointState(quadruped,9,-1.57)
p.resetJointState(quadruped,11,2.2)
p.createConstraint(quadruped,8,quadruped,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
p.setJointMotorControl(quadruped,6,p.VELOCITY_CONTROL,1,10)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,7,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,8,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,9,p.VELOCITY_CONTROL,-1,10)
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,10,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,11,p.VELOCITY_CONTROL,0,0)
@ -42,10 +43,10 @@ p.resetJointState(quadruped,15,-1.57)
p.resetJointState(quadruped,17,2.2)
p.createConstraint(quadruped,14,quadruped,17,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
p.setJointMotorControl(quadruped,12,p.VELOCITY_CONTROL,6,10)
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,15,p.VELOCITY_CONTROL,-6,10)
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0)
@ -56,12 +57,72 @@ p.resetJointState(quadruped,21,-1.57)
p.resetJointState(quadruped,23,2.2)
p.createConstraint(quadruped,20,quadruped,23,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
p.setJointMotorControl(quadruped,18,p.VELOCITY_CONTROL,6,10)
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,21,p.VELOCITY_CONTROL,-6,10)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
p.setGravity(0,0,-10)
p_gain = 2
speed = 10
amplitude = 1.3
#stand still
t_end = time.time() + 5
while time.time() < t_end:
p.stepSimulation()
jump_amp = 0.5
#jump
t_end = time.time() + 10
i=0
while time.time() < t_end:
t = time.time()
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.stepSimulation()
#hop forward
t_end = time.time() + 30
i=0
while time.time() < t_end:
t = time.time()
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,p_gain)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,p_gain)
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain)
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,p_gain)
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,p_gain)
p.stepSimulation()
#walk
t_end = time.time() + 120
i=0
while time.time() < t_end:
t = time.time()
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*3)*.3+1.57,1)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*3+0.5*3.14)*.3+1.57,1)
p.setJointMotorControl(quadruped,9,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,math.sin(t*3+3.14)*.3+1.57,1)
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*3+1.5*3.14)*.3+1.57,1)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.stepSimulation()