From ee7a5a470f02328c4595525885e299ffe467aeb6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 16 Nov 2016 21:36:51 -0800 Subject: [PATCH] tweak quadruped script to make a few more moves --- data/plane.urdf | 3 ++ examples/pybullet/quadruped.py | 77 ++++++++++++++++++++++++++++++---- 2 files changed, 72 insertions(+), 8 deletions(-) diff --git a/data/plane.urdf b/data/plane.urdf index 900080e08..480b0752f 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,6 +1,9 @@ + + + diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py index d4d4838bd..6205191f1 100644 --- a/examples/pybullet/quadruped.py +++ b/examples/pybullet/quadruped.py @@ -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() +