def __init__(self, name):
super().__init__(name)
self._ref = '%s_reference' % (self.get_name())
self._last_time = sim.simGetSimulationTime()
self._old_transformation_matrix = self.get_matrix()[:3, :4].reshape(
(12, )).tolist()
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 read(self):
"""Reads the angular velocities applied to gyroscope.
:return: A list containing applied angular velocities along
the sensor's x, y and z-axes.
"""
current_time = sim.simGetSimulationTime()
dt = current_time - self._last_time
inv_old_matrix = sim.simInvertMatrix(self._old_transformation_matrix)
transformation_matrix = self.get_matrix()
mat = sim.simMultiplyMatrices(inv_old_matrix, transformation_matrix)
euler_angles = sim.simGetEulerAnglesFromMatrix(mat)
self._last_time = current_time
self._old_transformation_matrix = transformation_matrix
if dt != 0:
return [euler_angle / dt for euler_angle in euler_angles]
return [0.0, 0.0, 0.0]
cur_step = 0
while not done and cur_step <= 2800:
cur_pose = agent.get_2d_pose()
done = path_base.step(path)
# agent.set_velocity(10,10,10,10)
pr.step()
vel,avel = agent.get_velocity()
fx = np.append(fx,(cur_pose[0]+2.1)*100)
fy = np.append(fy,(cur_pose[1]+2.1)*100)
fyaw = np.append(fyaw,cur_pose[2])
fvelocity = np.append(fvelocity,vel)
fa_velocity = np.append(fa_velocity,avel)
ftime = np.append(ftime,sim.simGetSimulationTime())
cur_step += 1
path.clear_visualization()
for block in blocks:
block.remove()
del block
print('Reached target %d!' % i_wide)
plt.imshow(heightmap)
plt.plot(starty,startx,'.g')
plt.plot(goaly,goalx,'.b')
plt.plot(rx,ry,'-r')
plt.plot(fx,fy,'-k')
plt.gca().invert_yaxis()
# plt.show()