ashvath100
5 years ago
committed by
Randy Mackay
2 changed files with 625 additions and 0 deletions
@ -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> |
||||||
@ -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 |
||||||
Loading…
Reference in new issue