13 def __init__(self, dt=0.02, GUI=True, debug=False, urdf="robobee.urdf"):
14 print(f
"Hello from PyBullet ! dt={dt}, GUI={GUI} debug={debug}")
16 pprz_src = os.getenv(
"PAPARAZZI_SRC")
17 print(f
"pprz_src = {pprz_src}")
27 p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
30 p.setAdditionalSearchPath(pybullet_data.getDataPath())
38 vehicule_urdf = os.path.join(PYBULLET_CONF_PATH, urdf)
44 p.setRealTimeSimulation(0, physicsClientId=self.
physicsClient)
48 p.resetDebugVisualizerCamera( cameraDistance=3.5, cameraYaw=-80, cameraPitch=-40, cameraTargetPosition=[0.0, 0.0, 0.0])
50 self.
KF = 0.01 ; self.KM = 0.001
52 self.
vel = np.zeros(3)
64 self.
roll_Id = p.addUserDebugParameter(
"roll_cmd", -0.01, 0.01, 0)
65 self.
pitch_Id = p.addUserDebugParameter(
"pitch_cmd", -0.01, 0.01, 0)
66 self.
yaw_Id = p.addUserDebugParameter(
"yaw_cmd", -0.01, 0.01, 0)
69 ''' rpm = [ quadrotor prop rpms ]'''
70 forces = np.array(rpm**2)*self.
KF
71 torques = np.array(rpm**2)*self.KM
73 f_noise = np.random.normal(0, 0.01, len(rpm))
74 m_noise = np.random.normal(0, 0.001,len(rpm))
82 z_torque = (torques[0] - torques[1] + torques[2] - torques[3])
84 p.applyExternalForce(self.
vehicle,
86 forceObj=[f_noise[0], f_noise[1], forces[i]],
91 p.applyExternalTorque(self.
vehicle,
93 torqueObj=[m_noise[0], m_noise[1], z_torque],
117 roll_cmd = p.readUserDebugParameter(self.
roll_Id)
118 pitch_cmd = p.readUserDebugParameter(self.
pitch_Id)
119 yaw_cmd = p.readUserDebugParameter(self.
yaw_Id)
129 p.applyExternalTorque(self.
vehicle,
131 torqueObj=[roll_cmd, pitch_cmd, yaw_cmd],
141 v_pos, v_quat = p.getBasePositionAndOrientation(self.
vehicle)
142 v_rpy = p.getEulerFromQuaternion(v_quat)
143 v_vel, v_ang_v = p.getBaseVelocity(self.
vehicle)
145 self.
accel = (np.array(v_vel) - self.
vel)/self.
dt
146 self.
vel = np.array(v_vel)
148 self.
ang_vel = np.array(v_ang_v)
149 R = np.array(p.getMatrixFromQuaternion(v_quat)).reshape(3, 3)
150 body_ang_vel = R.T.dot(self.
ang_vel)
156 'vel':tuple(self.
vel),
157 'ang_v':tuple(body_ang_vel),
158 'accel':tuple(self.
accel),
159 'ang_accel':tuple(body_ang_accel)