zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: pybullet script for walking robots

zr-v5.1
ashvath100 5 years ago committed by Randy Mackay
parent
485eb3fa88
commit
9ea390e24b
2 changed files with 625 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 375
      libraries/SITL/examples/JSON/pybullet/models/quadruped/quadruped.urdf
  2. 250
      libraries/SITL/examples/JSON/pybullet/walking_robot.py

375
libraries/SITL/examples/JSON/pybullet/models/quadruped/quadruped.urdf
Unescape View File

@ -0,0 +1,375 @@ @@ -0,0 +1,375 @@
<?xml version="1.0"?>
<robot name="materials">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="red">
<color rgba="0.8 0 0 1"/>
</material>
<link name="base_link">
<collision>
<geometry>
<box size="1.5 0.8 0.08"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0"/>
<mass value="100.843" />
<inertia ixx="0.002480" ixy="9.381E-19" ixz="-3.172E-18"
iyy="1.060E-02" iyz="1002.754E-09"
izz="1.243E-02" />
</inertial>
<visual>
<geometry>
<box size="1.5 0.8 0.08"/>
</geometry>
<material name="blue"/>
</visual>
</link>
#FRONT RIGHT
<link name="coxa_FR">
<visual>
<geometry>
<cylinder length="0.3" radius="0.1"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0.0 -0.15 0"/>
<material name="red"/>
</visual>
</link>
<link name="femur_FR">
<visual>
<geometry>
<cylinder length="0.85" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.425"/>
<material name="white"/>
</visual>
</link>
<link name="tibia_FR">
<visual>
<geometry>
<cylinder length="1.25" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.625"/>
<material name="blue"/>
</visual>
</link>
<link name="foot_FR">
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<mass value="200.843" />
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="coxaF_FR" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="base_link"/>
<child link="coxa_FR"/>
<origin rpy="0 0 0.785398" xyz="0.75 -0.4 0"/>
</joint>
<joint name="femurF_FR" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="coxa_FR"/>
<child link="femur_FR"/>
<origin rpy="4.71239 0 0" xyz="0 -0.3 0"/>
</joint>
<joint name="tibiaF_FR" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="femur_FR"/>
<child link="tibia_FR"/>
<origin rpy="1.5708 0 0" xyz="0 0 -0.85"/>
</joint>
<joint name="footF_FR" type="fixed">
<parent link="tibia_FR"/>
<child link="foot_FR"/>
<origin xyz="0 0 -1.25"/>
</joint>
#FRONT LEFT
<link name="coxa_FL">
<visual>
<geometry>
<cylinder length="0.3" radius="0.1"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0 -0.15 0"/>
<material name="red"/>
</visual>
</link>
<link name="femur_FL">
<visual>
<geometry>
<cylinder length="0.85" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.425"/>
<material name="white"/>
</visual>
</link>
<link name="tibia_FL">
<visual>
<geometry>
<cylinder length="1.25" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.625"/>
<material name="blue"/>
</visual>
</link>
<link name="foot_FL">
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<mass value="200.843" />
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="coxaF_FL" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="base_link"/>
<child link="coxa_FL"/>
<origin rpy="0 0 2.35619" xyz="0.74 0.4 0"/>
</joint>
<joint name="femurF_FL" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="coxa_FL"/>
<child link="femur_FL"/>
<origin rpy="4.71239 0 0" xyz="0 -0.3 0"/>
</joint>
<joint name="tibiaF_FL" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="femur_FL"/>
<child link="tibia_FL"/>
<origin rpy="1.5708 0 0" xyz="0 0 -0.85"/>
</joint>
<joint name="footF_FL" type="fixed">
<parent link="tibia_FL"/>
<child link="foot_FL"/>
<origin xyz="0 0 -1.25"/>
</joint>
#BACK RIGHT
<link name="coxa_BR">
<visual>
<geometry>
<cylinder length="0.3" radius="0.1"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0.0 -0.15 0"/>
<material name="red"/>
</visual>
</link>
<link name="femur_BR">
<visual>
<geometry>
<cylinder length="0.85" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.425"/>
<material name="white"/>
</visual>
</link>
<link name="tibia_BR">
<visual>
<geometry>
<cylinder length="1.25" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.625"/>
<material name="blue"/>
</visual>
</link>
<link name="foot_BR">
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<mass value="200.843" />
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="coxaF_BR" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="base_link"/>
<child link="coxa_BR"/>
<origin rpy="0 0 3.92699" xyz="-0.75 0.4 0"/>
</joint>
<joint name="femurF_BR" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="coxa_BR"/>
<child link="femur_BR"/>
<origin rpy="4.71239 0 0" xyz="0 -0.3 0"/>
</joint>
<joint name="tibiaF_BR" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="femur_BR"/>
<child link="tibia_BR"/>
<origin rpy="1.5708 0 0" xyz="0 0 -0.85"/>
</joint>
<joint name="footF_BR" type="fixed">
<parent link="tibia_BR"/>
<child link="foot_BR"/>
<origin xyz="0 0 -1.25"/>
</joint>
#BACK LEFT
<link name="coxa_BL">
<visual>
<geometry>
<cylinder length="0.3" radius="0.1"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0.0 -0.15 0"/>
<material name="red"/>
</visual>
</link>
<link name="femur_BL">
<visual>
<geometry>
<cylinder length="0.85" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.425"/>
<material name="white"/>
</visual>
</link>
<link name="tibia_BL">
<visual>
<geometry>
<cylinder length="1.25" radius="0.1"/>
</geometry>
<origin xyz="0 0 -0.625"/>
<material name="blue"/>
</visual>
</link>
<link name="foot_BL">
<collision>
<geometry>
<sphere radius="0.2"/>
</geometry>
</collision>
<mass value="200.843" />
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="coxaF_BL" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="base_link"/>
<child link="coxa_BL"/>
<origin rpy="0 0 5.48033" xyz="-0.75 -0.4 0"/>
</joint>
<joint name="femurF_BL" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="coxa_BL"/>
<child link="femur_BL"/>
<origin rpy="4.71239 0 0" xyz="0 -0.3 0"/>
</joint>
<joint name="tibiaF_BL" type="revolute">
<axis xyz="1 0 0"/>
<limit effort="1000.0" velocity="0.5"/>
<parent link="femur_BL"/>
<child link="tibia_BL"/>
<origin rpy="1.5708 0 0" xyz="0 0 -0.85"/>
</joint>
<joint name="footF_BL" type="fixed">
<parent link="tibia_BL"/>
<child link="foot_BL"/>
<origin xyz="0 0 -1.25"/>
</joint>
</robot>

250
libraries/SITL/examples/JSON/pybullet/walking_robot.py
Unescape View File

@ -0,0 +1,250 @@ @@ -0,0 +1,250 @@
#!/usr/bin/env python3
import os, inspect, sys
import socket
import struct
import json
import math
import pybullet_data
import pybullet as p
# use pymavlink for ArduPilot convention transformations
from pymavlink.rotmat import Vector3, Matrix3
from pymavlink.quaternion import Quaternion
import time
import argparse
from math import degrees, radians
parser = argparse.ArgumentParser(description="pybullet robot")
parser.add_argument("--fps", type=float, default=1000.0, help="physics frame rate")
args = parser.parse_args()
RATE_HZ = args.fps
TIME_STEP = 1.0 / RATE_HZ
GRAVITY_MSS = 9.80665
# Create simulator
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
FixedBase = False #if fixed no plane is imported
if (FixedBase == False):
p.loadURDF("plane.urdf")
p.changeDynamics(FixedBase,-1,lateralFriction=1.,
spinningFriction=0., rollingFriction=0., contactStiffness=-1, contactDamping=-1)
last_angles = [0.0] * 12
force = [500] * 12
servo_direction = [1,1,1,-1,-1,-1,-1,-1,1,1,1,-1]
def control_joints(pwm):
'''control a joint based bot'''
global last_angles
joint_speed = radians(250)
joints = [0,1,2,4,5,6,8,9,10,12,13,14]
pwm = pwm[0:len(joints)]
angles = [radians(((v-1500.0)*90)/1000) for v in pwm ]
for i in range(len(angles)):
angles[i] = angles[i] * servo_direction[i]
current = last_angles
max_change = joint_speed * TIME_STEP
for i in range(len(angles)):
angles[i] = constrain(angles[i], current[i]-max_change, current[i]+max_change)
p.setJointMotorControlArray(robot, joints, p.POSITION_CONTROL, angles,forces = force)
last_angles = angles
#spawn robot
position = [0, 0, 1.6]
robot = p.loadURDF("models/quadruped/quadruped.urdf",position, useFixedBase=FixedBase)
control_pwm = control_joints
p.setTimeStep(TIME_STEP)
time_now = 0
last_velocity = None
def quaternion_to_AP(quaternion):
'''convert pybullet quaternion to ArduPilot quaternion'''
return Quaternion([quaternion[3], quaternion[0], -quaternion[1], -quaternion[2]])
def vector_to_AP(vec):
'''convert pybullet vector tuple to ArduPilot Vector3'''
return Vector3(vec[0], -vec[1], -vec[2])
def quaternion_from_AP(q):
'''convert ArduPilot quaternion to pybullet quaternion'''
return [q.q[1], -q.q[2], -q.q[3], q.q[0]]
def to_tuple(vec3):
'''convert a Vector3 to a tuple'''
return (vec3.x, vec3.y, vec3.z)
def init():
global time_now
time_now = 0
position = [0,0,0]
orientation = [0,0,0,1]
p.reset_Base_Position_And_Orientations(robot,position,orientation)
def constrain(v,min_v,max_v):
'''constrain a value'''
if v < min_v:
v = min_v
if v > max_v:
v = max_v
return v
#robot.position = [ 0, 0, 2]
#robot.orientation = quaternion_from_AP(Quaternion([math.radians(0), math.radians(0), math.radians(50)]))
def step(sleep_dt=None):
# # call the step method for each body
# for body in self.bodies.values():
# if isinstance(body, Body):
# body.step()
# call simulation step
p.stepSimulation()
# sleep
if sleep_dt is not None:
time.sleep(sleep_dt)
def physics_step(pwm_in):
control_pwm(pwm_in)
step(sleep_dt=0)
# p.setRealTimeSimulation(1)
global time_now
time_now += TIME_STEP
# get the position orientation and velocity
pos,ori = p.getBasePositionAndOrientation(robot)
quaternion = quaternion_to_AP(ori)
roll, pitch, yaw = quaternion.euler
linear,angular = p.getBaseVelocity(robot)
velocity = vector_to_AP(linear)
position = vector_to_AP(pos)
# get ArduPilot DCM matrix (rotation matrix)
dcm = quaternion.dcm
# get gyro vector in body frame
gyro = dcm.transposed() * vector_to_AP(angular)
# calculate acceleration
global last_velocity
if last_velocity is None:
last_velocity = velocity
accel = (velocity - last_velocity) * (1.0 / TIME_STEP)
last_velocity = velocity
# add in gravity in earth frame
accel.z -= GRAVITY_MSS
# convert accel to body frame
accel = dcm.transposed() * accel
# convert to tuples
accel = to_tuple(accel)
gyro = to_tuple(gyro)
position = to_tuple(position)
velocity = to_tuple(velocity)
euler = (roll, pitch, yaw)
return time_now,gyro,accel,position,euler,velocity
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('', 9002))
sock.settimeout(0.1)
last_SITL_frame = -1
connected = False
frame_count = 0
frame_time = time.time()
print_frame_count = 1000
move_accel = 0.0
last_move = time.time()
while True:
py_time = time.time()
try:
data,address = sock.recvfrom(100)
except Exception as ex:
time.sleep(0.01)
continue
parse_format = 'HHI16H'
magic = 18458
if len(data) != struct.calcsize(parse_format):
print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
continue
decoded = struct.unpack(parse_format,data)
if magic != decoded[0]:
print("Incorrect protocol magic %u should be %u" % (decoded[0], magic))
continue
frame_rate_hz = decoded[1]
frame_count = decoded[2]
pwm = decoded[3:]
if frame_rate_hz != RATE_HZ:
print("New frame rate %u" % frame_rate_hz)
RATE_HZ = frame_rate_hz
TIME_STEP = 1.0 / RATE_HZ
p.setTimeStep(TIME_STEP)
# Check if the fame is in expected order
if frame_count < last_SITL_frame:
# Controller has reset, reset physics also
init()
print('Controller reset')
elif frame_count == last_SITL_frame:
# duplicate frame, skip
print('Duplicate input frame')
continue
elif frame_count != last_SITL_frame + 1 and connected:
print('Missed {0} input frames'.format(frame_count - last_SITL_frame - 1))
last_SITL_frame = frame_count
if not connected:
connected = True
print('Connected to {0}'.format(str(address)))
frame_count += 1
# physics time step
phys_time,gyro,accel,pos,euler,velo = physics_step(pwm)
# build JSON format
IMU_fmt = {
"gyro" : gyro,
"accel_body" : accel
}
JSON_fmt = {
"timestamp" : phys_time,
"imu" : IMU_fmt,
"position" : pos,
"attitude" : euler,
"velocity" : velo
}
JSON_string = "\n" + json.dumps(JSON_fmt,separators=(',', ':')) + "\n"
# Send to AP
sock.sendto(bytes(JSON_string,"ascii"), address)
# Track frame rate
if frame_count % print_frame_count == 0:
now = time.time()
total_time = now - frame_time
print("%.2f fps T=%.3f dt=%.3f" % (print_frame_count/total_time, phys_time, total_time))
frame_time = now
Write Preview
Loading…
Cancel
Save