From 8ff4fcfb4b13778b60467cbb5dabdb11d09fbe3c Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 22 Jan 2024 11:28:21 +0100 Subject: [PATCH 1/3] @whoenig sim - backend - pinocchio: cleanup --- .../backend/data/pinocchio/crazyflie2.urdf | 34 +++++ .../crazyflie_sim/backend/pinocchio.py | 117 ++++++++++++++++++ 2 files changed, 151 insertions(+) create mode 100644 crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf create mode 100644 crazyflie_sim/crazyflie_sim/backend/pinocchio.py diff --git a/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf b/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf new file mode 100644 index 000000000..ea09245e6 --- /dev/null +++ b/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/crazyflie_sim/crazyflie_sim/backend/pinocchio.py b/crazyflie_sim/crazyflie_sim/backend/pinocchio.py new file mode 100644 index 000000000..eb287e655 --- /dev/null +++ b/crazyflie_sim/crazyflie_sim/backend/pinocchio.py @@ -0,0 +1,117 @@ +from pathlib import Path +import pinocchio as pin +import numpy as np +from rclpy.node import Node +from rclpy.time import Time +from rosgraph_msgs.msg import Clock + +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 pinocchio_state2sim_state(q, v): + result = State() + result.pos = q[0:3] + result.quat = np.concatenate((q[6:7], q[3:6])) + result.vel = v[0:3] + result.omega = v[3:6] + return result + + +def sim_state2pinocchio_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 + q = result[0:7] # geometric states: x, y, z, qx, qy, qz, qw + v = result[7:] # velocities: vx, vy, vz, wx, wy, wz + return q, v + + +class Quadrotor: + """Basic rigid body quadrotor model (no drag) using numpy and rowan.""" + + def __init__(self, state): + arm_length = 0.046 # m + arm = 0.707106781 * arm_length + t2t = 0.006 # thrust-to-torque ratio + self.B0 = np.array([ + [1, 1, 1, 1], + [-arm, -arm, arm, arm], + [-arm, arm, arm, -arm], + [-t2t, t2t, -t2t, t2t] + ]) + self.uav, self.collision_model, self.visual_model = pin.buildModelsFromUrdf( + str((Path(__file__).parent / 'data/pinocchio/crazyflie2.urdf'))) + self.uavPinocchioData = self.uav.createData() + self.state = state + + def step(self, action, dt): + + # m: 0.034 + # max_f: 1.3 + force_in_newton = rpm_to_force(action.rpm) + eta = np.dot(self.B0, force_in_newton) + force_generalized = np.array([0., 0., eta[0], eta[1], eta[2], eta[3]]) + + q, v = sim_state2pinocchio_state(self.state) + a = pin.aba(self.uav, self.uavPinocchioData, q, v, force_generalized) + v_next = v + a*dt + q_next = pin.integrate(self.uav, q, v*dt) + self.state = pinocchio_state2sim_state(q_next, v_next) + + # 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 a6e308668b139aeabd320076f04ca1cff6012922 Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 22 Jan 2024 12:17:54 +0100 Subject: [PATCH 2/3] clean up fpr pinocchio script --- crazyflie_sim/crazyflie_sim/backend/pinocchio.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crazyflie_sim/crazyflie_sim/backend/pinocchio.py b/crazyflie_sim/crazyflie_sim/backend/pinocchio.py index eb287e655..f1bc09686 100644 --- a/crazyflie_sim/crazyflie_sim/backend/pinocchio.py +++ b/crazyflie_sim/crazyflie_sim/backend/pinocchio.py @@ -1,6 +1,7 @@ from pathlib import Path -import pinocchio as pin + import numpy as np +import pinocchio as pin from rclpy.node import Node from rclpy.time import Time from rosgraph_msgs.msg import Clock From 3ca8db2c053864466f8f77b9a2dae7260655fb0e Mon Sep 17 00:00:00 2001 From: Khaledwahba1994 Date: Mon, 22 Jan 2024 13:25:14 +0100 Subject: [PATCH 3/3] Resolved --- .../crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf | 7 ------- crazyflie_sim/crazyflie_sim/backend/pinocchio.py | 2 +- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf b/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf index ea09245e6..d6f589ec9 100644 --- a/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf +++ b/crazyflie_sim/crazyflie_sim/backend/data/pinocchio/crazyflie2.urdf @@ -23,12 +23,5 @@ - - - - - - - \ No newline at end of file diff --git a/crazyflie_sim/crazyflie_sim/backend/pinocchio.py b/crazyflie_sim/crazyflie_sim/backend/pinocchio.py index f1bc09686..77b066aa8 100644 --- a/crazyflie_sim/crazyflie_sim/backend/pinocchio.py +++ b/crazyflie_sim/crazyflie_sim/backend/pinocchio.py @@ -10,7 +10,7 @@ class Backend: - """Backend that uses newton-euler rigid-body dynamics implemented in numpy.""" + """Backend that uses pinocchio to simulate the rigid-body dynamics.""" def __init__(self, node: Node, names: list[str], states: list[State]): self.node = node