From 7be73c4a064547bf86609fa5725719e5c6597cdf Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 18 Jan 2024 14:28:52 +0100 Subject: [PATCH 1/2] add initial draft of dynobench backend --- .../crazyflie_sim/backend/dynobench.py | 108 ++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 crazyflie_sim/crazyflie_sim/backend/dynobench.py diff --git a/crazyflie_sim/crazyflie_sim/backend/dynobench.py b/crazyflie_sim/crazyflie_sim/backend/dynobench.py new file mode 100644 index 000000000..a6dbd379e --- /dev/null +++ b/crazyflie_sim/crazyflie_sim/backend/dynobench.py @@ -0,0 +1,108 @@ +from __future__ import annotations + +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( + '/home/whoenig/projects/dynobench/models/quad3d_v0.yaml', [], []) + 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) + # print(sim_state2dynobench_state(self.state)) + print(normalized_force) + 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] From e28a9f24478d5a34a873b5deb7cd62a701581fb5 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 18 Jan 2024 14:37:58 +0100 Subject: [PATCH 2/2] sim - backend - dynobench: cleanup --- .../backend/data/dynobench/crazyflie2.yaml | 15 +++++++++++++++ crazyflie_sim/crazyflie_sim/backend/dynobench.py | 6 ++---- 2 files changed, 17 insertions(+), 4 deletions(-) create mode 100644 crazyflie_sim/crazyflie_sim/backend/data/dynobench/crazyflie2.yaml diff --git a/crazyflie_sim/crazyflie_sim/backend/data/dynobench/crazyflie2.yaml b/crazyflie_sim/crazyflie_sim/backend/data/dynobench/crazyflie2.yaml new file mode 100644 index 000000000..34ffef628 --- /dev/null +++ b/crazyflie_sim/crazyflie_sim/backend/data/dynobench/crazyflie2.yaml @@ -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 diff --git a/crazyflie_sim/crazyflie_sim/backend/dynobench.py b/crazyflie_sim/crazyflie_sim/backend/dynobench.py index a6dbd379e..0c092918c 100644 --- a/crazyflie_sim/crazyflie_sim/backend/dynobench.py +++ b/crazyflie_sim/crazyflie_sim/backend/dynobench.py @@ -1,4 +1,4 @@ -from __future__ import annotations +from pathlib import Path import numpy as np from rclpy.node import Node @@ -85,7 +85,7 @@ class Quadrotor: def __init__(self, state): self.uav = robot_python.robot_factory( - '/home/whoenig/projects/dynobench/models/quad3d_v0.yaml', [], []) + str((Path(__file__).parent / 'data/dynobench/crazyflie2.yaml').resolve()), [], []) self.state = state def step(self, action, dt): @@ -96,8 +96,6 @@ def step(self, action, dt): normalized_force = force_in_newton / (1.3 * (0.034 / 4) * 9.81) xnext = np.zeros(13) - # print(sim_state2dynobench_state(self.state)) - print(normalized_force) self.uav.step(xnext, sim_state2dynobench_state(self.state), normalized_force, dt) self.state = dynobench_state2sim_state(xnext)