Skip to content

Commit

Permalink
Merge pull request #404 from IMRCLab/feature_sim_dynobench
Browse files Browse the repository at this point in the history
sim - backend - dynobench: cleanup
  • Loading branch information
Khaledwahba1994 authored Jan 18, 2024
2 parents 3a3a44d + e28a9f2 commit 97358ec
Show file tree
Hide file tree
Showing 2 changed files with 121 additions and 0 deletions.
15 changes: 15 additions & 0 deletions crazyflie_sim/crazyflie_sim/backend/data/dynobench/crazyflie2.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
# distance_weights: [1. , .5, .2, .2 ]
distance_weights: [1., .5, .1, .05]
max_vel: 4
max_angular_vel: 8
max_acc: 25 # used for bounds on time
max_angular_acc: 20 # used for bounds on time
motor_control: true # True = controls are forces in the motors (False is NOT implemented)
m: 0.034
max_f: 1.3
arm_length: 0.046
t2t: 0.006 # thrust-to-torque ratio
J_v: [16.571710e-6, 16.655602e-6, 29.261652e-6] # inertia matrix
dt: .01
size: [.25]
dynamics: quad3d
106 changes: 106 additions & 0 deletions crazyflie_sim/crazyflie_sim/backend/dynobench.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
from pathlib import Path

import numpy as np
from rclpy.node import Node
from rclpy.time import Time
import robot_python
from rosgraph_msgs.msg import Clock

# import sys
# sys.path.append("/home/whoenig/projects/dynobench/build")

from ..sim_data_types import Action, State


class Backend:
"""Backend that uses newton-euler rigid-body dynamics implemented in numpy."""

def __init__(self, node: Node, names: list[str], states: list[State]):
self.node = node
self.names = names
self.clock_publisher = node.create_publisher(Clock, 'clock', 10)
self.t = 0
self.dt = 0.0005

self.uavs = []
for state in states:
uav = Quadrotor(state)
self.uavs.append(uav)

def time(self) -> float:
return self.t

def step(self, states_desired: list[State], actions: list[Action]) -> list[State]:
# advance the time
self.t += self.dt

next_states = []

for uav, action in zip(self.uavs, actions):
uav.step(action, self.dt)
next_states.append(uav.state)

# print(states_desired, actions, next_states)
# publish the current clock
clock_message = Clock()
clock_message.clock = Time(seconds=self.time()).to_msg()
self.clock_publisher.publish(clock_message)

return next_states

def shutdown(self):
pass


# convert RPM -> Force
def rpm_to_force(rpm):
# polyfit using data and scripts from https://github.com/IMRCLab/crazyflie-system-id
p = [2.55077341e-08, -4.92422570e-05, -1.51910248e-01]
force_in_grams = np.polyval(p, rpm)
force_in_newton = force_in_grams * 9.81 / 1000.0
return np.maximum(force_in_newton, 0)


def sim_state2dynobench_state(state):
result = np.empty(13)
result[0:3] = state.pos
result[3:6] = state.quat[1:]
result[6] = state.quat[0]
result[7:10] = state.vel
result[10:13] = state.omega
return result


def dynobench_state2sim_state(state):
result = State()
result.pos = state[0:3]
result.quat = np.concatenate((state[6:7], state[3:6]))
result.vel = state[7:10]
result.omega = state[10:13]
return result


class Quadrotor:
"""Basic rigid body quadrotor model (no drag) using numpy and rowan."""

def __init__(self, state):
self.uav = robot_python.robot_factory(
str((Path(__file__).parent / 'data/dynobench/crazyflie2.yaml').resolve()), [], [])
self.state = state

def step(self, action, dt):

# m: 0.034
# max_f: 1.3
force_in_newton = rpm_to_force(action.rpm)
normalized_force = force_in_newton / (1.3 * (0.034 / 4) * 9.81)

xnext = np.zeros(13)
self.uav.step(xnext, sim_state2dynobench_state(self.state), normalized_force, dt)
self.state = dynobench_state2sim_state(xnext)

# if we fall below the ground, set velocities to 0
if self.state.pos[2] < 0:
self.state.pos[2] = 0
self.state.vel = [0, 0, 0]
self.state.omega = [0, 0, 0]

0 comments on commit 97358ec

Please sign in to comment.