From e28a9f24478d5a34a873b5deb7cd62a701581fb5 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Thu, 18 Jan 2024 14:37:58 +0100
Subject: [PATCH] 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)