From 3b91a7d5ab651316f002259c5d00532451da0e54 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 28 Aug 2023 22:03:33 +0200 Subject: [PATCH] [sim] fix issue when using cmdFullState Fixes #225 --- crazyflie_sim/crazyflie_sim/crazyflie_sil.py | 25 ++++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py index 34b279160..9b5f2e774 100644 --- a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py +++ b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py @@ -165,11 +165,26 @@ def startTrajectory(self, trajectoryId: int, timescale: float = 1.0, reverse: bo def cmdFullState(self, pos, vel, acc, yaw, omega): self.mode = CrazyflieSIL.MODE_LOW_FULLSTATE - self.setState.pos = firm.mkvec(*pos) - self.setState.vel = firm.mkvec(*vel) - self.setState.acc = firm.mkvec(*acc) - self.setState.yaw = yaw - self.setState.omega = firm.mkvec(*omega) + self.setpoint.position.x = pos[0] + self.setpoint.position.y = pos[1] + self.setpoint.position.z = pos[2] + self.setpoint.velocity.x = vel[0] + self.setpoint.velocity.y = vel[1] + self.setpoint.velocity.z = vel[2] + self.setpoint.attitude.yaw = np.degrees(yaw) + self.setpoint.attitudeRate.roll = np.degrees(omega[0]) + self.setpoint.attitudeRate.pitch = np.degrees(omega[1]) + self.setpoint.attitudeRate.yaw = np.degrees(omega[2]) + self.setpoint.mode.x = firm.modeAbs + self.setpoint.mode.y = firm.modeAbs + self.setpoint.mode.z = firm.modeAbs + self.setpoint.mode.roll = firm.modeDisable + self.setpoint.mode.pitch = firm.modeDisable + self.setpoint.mode.yaw = firm.modeAbs + self.setpoint.mode.quat = firm.modeDisable + self.setpoint.acceleration.x = acc[0] + self.setpoint.acceleration.y = acc[1] + self.setpoint.acceleration.z = acc[2] # def cmdPosition(self, pos, yaw = 0): # self.mode = CrazyflieSIL.MODE_LOW_POSITION