def control_propeller_thrust(self, num_p) -> None:
""" set thrust for the particular propeller, num_p is the number of the propeller(1 to 4) """
particleVelocity = self.velocities[num_p - 1]
s = sim.simGetObjectSizeFactor(
self.force_sensors[num_p - 1]) # current size factor
if (s != self.pre_v): #When the size of the propeller changes
self.particleSize = self.particleSize * 0.005 * s
self.pre_v = s
self.ts = sim.simGetSimulationTimeStep()
m = sim.simGetObjectMatrix(self.force_sensors[num_p - 1], -1)
requiredParticleCnt = self.particleCountPerSecond * self.ts + self.notFullParticles
self.notFullParticles = requiredParticleCnt % 1
requiredParticleCnt = np.floor(requiredParticleCnt)
totalExertedForce = requiredParticleCnt * self.particleDensity * particleVelocity * np.pi * self.particleSize * self.particleSize * self.particleSize / (
6 * self.ts)
force = [0, 0, totalExertedForce]
m[3] = 0
m[7] = 0
m[11] = 0
force = sim.simMultiplyVector(m, force)
torque = [0, 0, pow(-1, num_p) * 0.002 * particleVelocity]
torque = sim.simMultiplyVector(m, torque)
sim.simAddForceAndTorque(self.respondables[num_p - 1], force, torque)
def simple_controller(self, target_loc):
'''simple PID controller'''
sp = [0.0, 0.0, 0.0]
euler = [0.0, 0.0, 0.0]
l = []
angu = []
targetPos = target_loc
#print("target position is: ",target_pos)
pos = self.get_3d_pose()[:3]
l, angu = sim.simGetVelocity(self._handle)
e = (targetPos[2] - pos[2])
self.cumul = self.cumul + e
pv = self.pParam * e
thrust = 5.335 + pv + self.iParam * self.cumul + self.dParam * (
e - self.lastE) + l[2] * self.vParam
self.lastE = e
for i in range(3):
sp[i] = targetPos[i] - pos[i]
m = sim.simGetObjectMatrix(self._handle, -1)
vx = [1, 0, 0]
vx = sim.simMultiplyVector(m, vx)
vy = [0, 1, 0]
vy = sim.simMultiplyVector(m, vy)
alphaE = (vy[2] - m[11])
alphaCorr = 0.25 * alphaE + 2.1 * (alphaE - self.pAlphaE)
betaE = (vx[2] - m[11])
betaCorr = -0.25 * betaE - 2.1 * (betaE - self.pBetaE)
self.pAlphaE = alphaE
self.pBetaE = betaE
alphaCorr = alphaCorr + sp[1] * 0.005 + 1 * (sp[1] - self.psp2)
betaCorr = betaCorr - sp[0] * 0.005 - 1 * (sp[0] - self.psp1)
self.psp2 = sp[1]
self.psp1 = sp[0]
eulert1 = self.get_3d_pose()[3:]
eulert2 = [0.0, 0.0, 0.0]
for i in range(3):
euler[i] = eulert1[i] - eulert2[i]
rotCorr = euler[2] * 0.1 + 2 * (euler[2] - self.prevEuler)
self.prevEuler = euler[2]
self.particlesTargetVelocities[0] = thrust * (1 - alphaCorr +
betaCorr + rotCorr)
self.particlesTargetVelocities[1] = thrust * (1 - alphaCorr -
betaCorr - rotCorr)
self.particlesTargetVelocities[2] = thrust * (1 + alphaCorr -
betaCorr + rotCorr)
self.particlesTargetVelocities[3] = thrust * (1 + alphaCorr +
betaCorr - rotCorr)
return self.particlesTargetVelocities
def set_thrust_and_torque(self, forces, torques=None, force_zero=None):
'''Sets force (thrust) and torque to a respondable shape.\
Must be configured specifically according to each robot's dynamics.
Args:
forces: Force vector applied at each propeller.
torques: Torque vector applied at each propeller.
'''
# torques = 0.00002*forces
count = 1 # set torque's clockwise (even) and counter-clockwise (odd)
t = sim.simGetSimulationTime()
# for propeller, joint, pwm, torque in zip(self.propellers,self.joints, forces, torques):
for k, (propeller, joint,
pwm) in enumerate(zip(self.propellers, self.joints, forces)):
# for propeller, force, torque in zip(self.force_sensors, forces, torques):
force = 1.5618e-4 * pwm * pwm + 1.0395e-2 * pwm + 0.13894
if force_zero:
force = 0
rot_matrix = (sim.simGetObjectMatrix(self.force_sensor_handles[k],
-1))
rot_matrix[3] = 0
rot_matrix[7] = 0
rot_matrix[11] = 0
z_force = np.array([0.0, 0.0, force])
applied_force = list(
self._my_simTransformVector(rot_matrix, z_force))
if count % 2:
z_torque = np.array([0.0, 0.0, -0.001 * pwm])
else:
z_torque = np.array([0.0, 0.0,
0.001 * pwm]) ## DEIXANDO IGUAL AO V-REP
applied_torque = list(
self._my_simTransformVector(rot_matrix, (z_torque)))
count += 1
self._simAddForceAndTorque(propeller._handle, applied_force,
applied_torque)
joint.set_joint_position(
position=t * 10, allow_force_mode=False
) ## allow_force_mode = True has a built-in step inside
def get_matrix(self, relative_to=None) -> np.ndarray:
"""Retrieves the transformation matrix of this object.
:param relative_to: Indicates relative to which reference frame we want
the matrix. Specify None to retrieve the absolute transformation
matrix, or an Object relative to whose reference frame we want the
transformation matrix.
:return: A 4x4 transformation matrix.
"""
relto = -1 if relative_to is None else relative_to.get_handle()
m = sim.simGetObjectMatrix(self._handle, relto)
m_np = np.array(m).reshape((3, 4))
return np.concatenate([m_np, [np.array([0, 0, 0, 1])]])
def rotate(self, rotation: List[float]) -> None:
"""Rotates a transformation matrix.
:param rotation: The x, y, z rotation to perform (in radians).
"""
m = sim.simGetObjectMatrix(self._handle, -1)
x_axis = [m[0], m[4], m[8]]
y_axis = [m[1], m[5], m[9]]
z_axis = [m[2], m[6], m[10]]
axis_pos = sim.simGetObjectPosition(self._handle, -1)
m = sim.simRotateAroundAxis(m, z_axis, axis_pos, rotation[2])
m = sim.simRotateAroundAxis(m, y_axis, axis_pos, rotation[1])
m = sim.simRotateAroundAxis(m, x_axis, axis_pos, rotation[0])
sim.simSetObjectMatrix(self._handle, -1, m)
def get_matrix(self, relative_to=None) -> List[float]:
"""Retrieves the transformation matrix of this object.
:param relative_to: Indicates relative to which reference frame we want
the matrix. Specify None to retrieve the absolute transformation
matrix, or an Object relative to whose reference frame we want the
transformation matrix.
:return: A list of 12 float values (the last row of the 4x4 matrix (
0,0,0,1) is not needed).
The x-axis of the orientation component is (m[0], m[4], m[8])
The y-axis of the orientation component is (m[1], m[5], m[9])
The z-axis of the orientation component is (m[2], m[6], m[10])
The translation component is (m[3], m[7], m[11])
"""
relto = -1 if relative_to is None else relative_to.get_handle()
return sim.simGetObjectMatrix(self._handle, relto)