def setMotorSpeeds(self, action):
L = 2.2
w = 1.44 # distance between left and right wheel
d = 0.63407
delta_ack = action[0]
vel = action[1]
wheel_speed = 2*vel/d
R = L/np.abs(delta_ack)
delta_o = np.arctan2(L,R+w/2)
delta_i = np.arctan2(L,R-w/2)
if delta_ack > 0:
deltaLeft = delta_i
deltaRight = delta_o
elif delta_ack < 0:
deltaLeft = -delta_o
deltaRight = -delta_i
else:
deltaLeft = 0
deltaRight = 0
vrep.simxSetJointTargetVelocity(clientID=self.vrep_client_id,
jointHandle=self.left_motor_handles,
targetVelocity=wheel_speed,
operationMode=vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID=self.vrep_client_id,
jointHandle=self.right_motor_handles,
targetVelocity=wheel_speed,
operationMode=vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID=self.vrep_client_id,
jointHandle=self.steeringLeft_handles,
targetPosition=deltaLeft,
operationMode=vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID=self.vrep_client_id,
jointHandle=self.steeringRight_handles,
targetPosition=deltaRight,
operationMode=vrep.simx_opmode_oneshot)
# Now step a few times:
for t in range(1, 500):
vrep.simxSynchronousTrigger(clientID)
_, lrf_bin = vrep.simxGetStringSignal(clientID, 'LiDAR',
opmode_blocking)
lrf = np.array(vrep.simxUnpackFloats(lrf_bin), dtype=float)
# control input
if np.min(lrf) > 0.1:
u1 = 1
u2 = 1
else:
u1 = 0
u2 = 0
# apply control input to the actuators
vrep.simxSetJointTargetVelocity(clientID, joint_handle[0], u1,
opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, joint_handle[1], u2,
opmode_blocking)
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
#joint handle,
#target position,
#operation mode
#)
vrep.simxSetJointTargetPosition(clientID, handle, pi / 4,
vrep.simx_opmode_blocking)
time.sleep(3)
#Set joint target velocity
#(
#client ID,
#joint handle,
#target velocity,
#operation mode
#)
vrep.simxSetJointTargetVelocity(clientID, handle, pi / 4,
vrep.simx_opmode_blocking)
time.sleep(3)
#Stop simulation(
#client ID,
#operation mode
#)
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
# Ackermann steering
R = L / np.abs(delta_ack)
delta_o = np.arctan2(L, R + w / 2)
delta_i = np.arctan2(L, R - w / 2)
if delta_ack > 0:
deltaLeft = delta_i
deltaRight = delta_o
elif delta_ack < 0:
deltaLeft = -delta_o
deltaRight = -delta_i
else:
deltaLeft = 0
deltaRight = 0
vrep.simxSetJointTargetPosition(clientID, joint_handle[0],
deltaLeft, opmode_blocking)
vrep.simxSetJointTargetPosition(clientID, joint_handle[1],
deltaRight, opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, joint_handle[2],
2 * vel / d, opmode_blocking)
vrep.simxSetJointTargetVelocity(clientID, joint_handle[3],
2 * vel / d, opmode_blocking)
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')