def getVelocityAtHandle(self, targetHandle, ignoreError = False, initialize = False):
if initialize:
sim.simxGetObjectVelocity(self.clientID, targetHandle, sim.simx_opmode_streaming)
res, l_vel, r_vel = sim.simxGetObjectVelocity(self.clientID, targetHandle, sim.simx_opmode_buffer)
if res!=sim.simx_return_ok and not ignoreError:
print('Failed to get velocity')
print(res)
return np.array(l_vel), np.array(r_vel)
def __init__(self, clientID):
self.clientID = clientID
self.pioneer3DX_array = [None] * 19
self.position_coordX = [None] * 3
self.position_coordXc = [None] * 3
self.orientation = None
self.velocity = [None]* 2
self.ultrasonic = np.zeros((16, 3))
self.name = "Pioneer_p3dx"
self.left_motor = 'Pioneer_p3dx_leftMotor'
self.right_motor = 'Pioneer_p3dx_rightMotor'
self.ultrasonic_sensors = "Pioneer_p3dx_ultrasonicSensor"
### Load Mobile Robot Pioneer parameters
# self.pioneer3DX_array[0] represents the entire mobile robot block
error,self.pioneer3DX_array[0] = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_blocking)
# self.pioneer3DX_array[1] represents only the left motor
error,self.pioneer3DX_array[1] = sim.simxGetObjectHandle(self.clientID,self.left_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[2] represents only the right motor
error,self.pioneer3DX_array[2] = sim.simxGetObjectHandle(self.clientID,self.right_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[3:18] represents the 16 ultrasonic sensors
num = 1
while num < 17:
error,self.pioneer3DX_array[2+num] = sim.simxGetObjectHandle(self.clientID,self.ultrasonic_sensors+str(num),sim.simx_opmode_blocking)
error, DetectionState, Points ,detectedObjectHandle, Vector = sim.simxReadProximitySensor(self.clientID,self.pioneer3DX_array[2+num],sim.simx_opmode_streaming)
num+=1
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_streaming)
error, position = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_streaming)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_streaming)
print("Pioneer 3DX loaded")
def get_velocity(self, relative_object=-1):
# Get velocity relative to an object, -1 for global frame
if relative_object != -1:
relative_object = self._get_handler(relative_object)
res, velocity, omega = sim.simxGetObjectVelocity(
self.client_id, self.frame, sim.simx_opmode_oneshot)
return np.array(velocity), np.array(omega)
def get_velocity(self) -> (Vec3, EulerAngles):
"""
Retrieves the linear and angular velocity.
@rtype (Vec3, EulerAngles)
"""
code, lin_vel, ang_vel = sim.simxGetObjectVelocity(self._id, self._handle, self._def_op_mode)
linear_velocity = Vec3(lin_vel[0], lin_vel[1], lin_vel[2])
angular_velocity = EulerAngles(ang_vel[0], ang_vel[1], ang_vel[2])
return linear_velocity, angular_velocity
def get_PositionData(self):
## Pioneer Velocity
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_buffer)
## Pioneer Position
error, self.position_coordXc = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_buffer)
## Pioneer Orientation (alpha, beta e gama)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_buffer)
self.orientation = angle[2]
# Linear Transform to find the Control Point
A = np.array([np.cos(angle[2]), np.sin(angle[2]), 0])
self.position_coordX = self.position_coordXc + 0.15*A
def _intial(self):
#handles
#bot_handle
_, self.bot = sim.simxGetObjectHandle(self.clientID, "Bot_collider",
sim.simx_opmode_blocking)
_, _ = sim.simxGetObjectPosition(self.clientID, self.bot, -1,
sim.simx_opmode_streaming)
#wheels
_, self.left_wheel = sim.simxGetObjectHandle(self.clientID,
"Left_wheel_joint",
sim.simx_opmode_blocking)
_, self.right_wheel = sim.simxGetObjectHandle(self.clientID,
"Right_wheel_joint",
sim.simx_opmode_blocking)
#Wall_collection
_, self.wall = sim.simxGetCollectionHandle(self.clientID, "wall",
sim.simx_opmode_blocking)
#Target
_, self.target = sim.simxGetObjectHandle(self.clientID, "target",
sim.simx_opmode_blocking)
_, _ = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
#Sensors
for i in range(self.total_sensors):
proxy = "proxy_" + str(i) + "_"
_, self.sensor[i] = sim.simxGetObjectHandle(
self.clientID, proxy, sim.simx_opmode_blocking)
_, _, _, _, _ = sim.simxReadProximitySensor(
self.clientID, self.sensor[i], sim.simx_opmode_streaming)
#Sensor velocity
_, _, _ = sim.simxGetObjectVelocity(self.clientID, self.bot,
sim.simx_opmode_streaming)
#Collision Data Stream
_, _ = sim.simxGetIntegerSignal(self.clientID, "collision_wall",
sim.simx_opmode_streaming)
_, _ = sim.simxGetIntegerSignal(self.clientID, "collision_target",
sim.simx_opmode_streaming)
self.reset = False
def main():
global hit_wall
global ball_coord
global ball_linear
global ball_hit
global detect_handle
global prox_handle
global stop_prog
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ', res)
time.sleep(2)
#Giving an initial velocity to the ball before starting the thread
detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere',
sim.simx_opmode_blocking)
prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor',
sim.simx_opmode_blocking)
racket_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor0',
sim.simx_opmode_blocking)
dummy_handle = sim.simxGetObjectHandle(clientID, 'Cylinder',
sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, detect_handle[1], -1,
[0.65, 0.47, 0.0463],
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, True)
sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3001, 1,
sim.simx_opmode_oneshot)
#sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3000, -0.01, sim.simx_opmode_oneshot)
sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3002, 1,
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
sim.simxReadProximitySensor(clientID, prox_handle[1],
sim.simx_opmode_streaming)
sim.simxGetObjectVelocity(clientID, detect_handle[1],
sim.simx_opmode_streaming)
sim.simxReadProximitySensor(clientID, racket_handle[1],
sim.simx_opmode_streaming)
sim.simxGetObjectPosition(clientID, dummy_handle[1], -1,
sim.simx_opmode_streaming)
ball_thread = threading.Thread(target=get_ball_info,
args=({
clientID: clientID
}))
try:
#getting joint handles and initializing the joints in the simulation
print("1. getting joint angles")
jointHandles = []
for i in range(6):
handle = sim.simxGetObjectHandle(
clientID, 'UR3_joint' + str(i + 1) + '#0',
sim.simx_opmode_blocking)
jointHandles.append(handle)
time.sleep(0.01)
for i in range(6):
print(jointHandles[i])
#hit_thread = threading.Thread(target=hit_ball, args=({clientID:clientID}))
ball_thread.daemon = True
#hit_thread.daemon = True
#hit_thread.start()
ball_thread.start()
# #Get handles for detecting object
# detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking)
# #Get handles for detecting the proximity sensor
# prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking)
#Create an instance to compute the ball trajectory
print("1. Initializing bouncing trajectory function")
b_ball = bouncing_ball.BouncingBall()
#Set-up some of the RML vectors:
vel = 60
accel = 10
jerk = 20
currentVel = [0, 0, 0, 0, 0, 0, 0]
currentAccel = [0, 0, 0, 0, 0, 0, 0]
maxVel = [
vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180,
vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180
]
maxAccel = [
accel * math.pi / 180, accel * math.pi / 180,
accel * math.pi / 180, accel * math.pi / 180,
accel * math.pi / 180, accel * math.pi / 180
]
maxJerk = [
jerk * math.pi / 180, jerk * math.pi / 180,
jerk * math.pi / 180, jerk * math.pi / 180,
jerk * math.pi / 180, jerk * math.pi / 180
]
targetVel = [0, 0, 0, 0, 0, 0]
sim.simxPauseCommunication(clientID, True)
for i in range(6):
sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
time.sleep(2)
#start the feedback loop
while True:
if not hit_wall:
#print("We haven't hit the wall yet")
continue
if hit_wall:
print("We hit wall ....")
#Predict the position of the ball from the wall and transform it to world coordinates
print("Predicting Trajectory ...")
pred_pos = b_ball.trajectory(ball_coord[0], ball_coord[1],
ball_coord[2], ball_linear)
T_prox = np.array([[-1, 0, 0, 0.025], [0, -1, 0, 2.85],
[0, 0, 1, -.05], [0, 0, 0, 1]])
prox_pos = np.array([[pred_pos[0]], [pred_pos[1]],
[pred_pos[2]], [1]])
ball_pos = T_prox @ prox_pos
print(ball_pos)
#set the left flag to true if on left side
left = 0
if ball_pos[0, :] < 0: left = 1
#convert to right-side coordinates for IK
ball_pos[0, :] = np.abs(ball_pos[0, :])
if ball_pos[0, :] > 0.9:
print("Ball too far away from robot. will not hit it ...")
raise ValueError
elif ball_pos[0, :] < 0.2:
print("Ball too close. cannot hit it...")
raise ValueError
twist_angle = (-92.85) * (ball_pos[0, :]) + 90
print(ball_pos, "left?: ", left)
targetPos0_ik = ik.findJointAngles(ball_pos[0, :],
ball_pos[1, :],
ball_pos[2, :] + 0.15)
#Invert joint angles if on left side of robot
for i in range(len(targetPos0_ik)):
targetPos0_ik[i] = ((-2 * left) + 1) * targetPos0_ik[i]
print("Applying the hitting motion")
#Apply the hitting motion using the new joint angles
end_pose = []
targetPos0_ik[0] = targetPos0_ik[0] + (
(-2 * left) + 1) * (0 * np.pi / 180) #To simulate a hit
targetPos0_ik[4] = targetPos0_ik[4] + (
(-2 * left) + 1) * (0 * np.pi / 180)
sim.simxPauseCommunication(clientID, True)
for i in range(6):
#print(jointHandles[i])
#print(targetPos0[i])
sim.simxSetJointTargetPosition(clientID,
jointHandles[i][1],
targetPos0_ik[i],
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID,
jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
#Now read from the proximity sensor to see if it detects any ball
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, racket_handle[1], sim.simx_opmode_buffer)
while (dS == 0):
_, end_pose = sim.simxGetObjectPosition(
clientID, dummy_handle[1], -1, sim.simx_opmode_buffer)
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, racket_handle[1], sim.simx_opmode_buffer)
print("Status of Ball is: ", dS)
#Now, attach a proximity sensor to the racquet and see if it detects a ball. If it does, let's start the
#hitting motion
#Actually, for now, let's just not worry about this. Let's make sure that the trajectory generation and the ball hitting
#works
print("Twist angle is: ", twist_angle)
targetPos0_ik[0] = targetPos0_ik[0] + ((-2 * left) + 1) * (
(1 * twist_angle) * np.pi / 180) #To simulate a hit
targetPos0_ik[4] = targetPos0_ik[4] + (
(-2 * left) + 1) * (-1 * twist_angle * np.pi / 180)
# sim.simxPauseCommunication(clientID, True)
# for i in range(6):
# #print(jointHandles[i])
# #print(targetPos0[i])
# sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_buffer)
# sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_buffer)
# sim.simxPauseCommunication(clientID, False)
# time.sleep(2)
ball_hit = True
hit_wall = False
sim.simxPauseCommunication(clientID, True)
for i in [0, 4]:
sim.simxSetJointTargetPosition(clientID,
jointHandles[i][1],
targetPos0_ik[i],
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID,
jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
time.sleep(2)
sim.simxPauseCommunication(clientID, True)
#Reset after hitting ball
for i in range(6):
#print(jointHandles[i])
#print(targetPos0[i])
sim.simxSetJointTargetPosition(clientID,
jointHandles[i][1], 0,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID,
jointHandles[i][1],
targetVel[i],
sim.simx_opmode_streaming)
sim.simxPauseCommunication(clientID, False)
##Inverse Kinematics experiment and analysis: CAN BE COMMENTED OUT
ball_pos[0, :] = (-2 * left + 1) * ball_pos[0, :]
ball_pos[2, :] = ball_pos[2, :] + 0.15
end_pose = np.array(end_pose).reshape((3, 1))
print("Desired position: \n", ball_pos[:3])
print("End-effector position: \n", end_pose[:3])
# print("Error (MMSE): ", np.linalg.norm(ball_pos[:3] - end_pose[:3]))
# x_err = np.abs((end_pose[0,:] - ball_pos[0,:])/(ball_pos[0,:])) * 100
# y_err = np.abs((end_pose[1,:] - ball_pos[1,:])/(ball_pos[1,:])) * 100
# z_err = np.abs((end_pose[2,:] - ball_pos[2,:])/(ball_pos[2,:])) * 100
# print("Error in X (%): ", x_err)
# print("Error in Y (%): ", y_err)
# print("Error in Z (%): ", z_err)
# print("Total Error (%): ", (x_err + y_err + z_err)/3)
except:
print(sys.exc_info())
#hit_thread.join()
stop_prog = True
print("Exception encountered, stopping program")
#ball_thread.join()
print("Ball thread stopped")
sim.simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
sim.simxGetPingTime(clientID)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
T = 0
newT = 0
#jump up from rest
rotate(-50, motor)
time.sleep(2) #let motor speed up for a bit
stop(
motor
) #coppeliasim motor is set to lock when velocity is equal to zero just like the real cubli
time.sleep(0.3) #wait a little before trying to control
while True:
#get states
err_code, position = sim.simxGetJointPosition(clientID, j_c,
sim.simx_opmode_blocking)
err_code, linear, angular_c = sim.simxGetObjectVelocity(
clientID, j_c, sim.simx_opmode_blocking)
err_code, linear, angular_m = sim.simxGetObjectVelocity(
clientID, motor, sim.simx_opmode_blocking)
x = np.array([[position + np.pi / 4, angular_c[0], angular_m[0]]])
#define LQR input
u = -1 * np.matmul(K.T, x)
#measure time since last loop
oldT = newT
newT = time.time()
T = newT - oldT
if (first_run == True):
T = 0
first_run = False
x = np.matmul(np.array(I + T * A), x.T) + T * B
def _get_velocity_reading_continue(self):
_, vel_reading, angular_reading = sim.simxGetObjectVelocity(
self.clientID, self.bot, sim.simx_opmode_streaming)
return np.round_(vel_reading[:2], 3), round(angular_reading[2], 3)
def initializeSimModel(self, client_ID):
try:
print('Connected to remote API server')
client_ID != -1
except:
print('Failed connecting to remote API server')
self.client_ID = client_ID
return_code, self.COM_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'COM', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object COM handle ok.')
return_code, self.left_hip_joint_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'left_hip', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object left hip joint handle ok.')
return_code, self.left_knee_joint_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'left_knee', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object left knee joint handle ok.')
return_code, self.right_hip_joint_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'right_hip', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object right hip joint handle ok.')
return_code, self.right_knee_joint_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'right_knee', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object right knee joint handle ok.')
return_code, self.right_knee_joint_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'right_knee', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object right knee joint handle ok.')
return_code, self.left_foot_force_sensor_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'left_foot_force_sensor', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object left foot force sensor handle ok.')
return_code, self.right_foot_force_sensor_handle = vrep_sim.simxGetObjectHandle(
client_ID, 'right_foot_force_sensor',
vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object right foot force sensor handle ok.')
return_code, pos = vrep_sim.simxGetObjectPosition(
self.client_ID, self.COM_handle, -1,
vrep_sim.simx_opmode_streaming)
return_code, linear_vel, angular_vel = vrep_sim.simxGetObjectVelocity(
self.client_ID, self.COM_handle, vrep_sim.simx_opmode_streaming)
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.left_hip_joint_handle,
vrep_sim.simx_opmode_streaming)
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.left_knee_joint_handle,
vrep_sim.simx_opmode_streaming)
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.right_hip_joint_handle,
vrep_sim.simx_opmode_streaming)
return_code, q = vrep_sim.simxGetJointPosition(
self.client_ID, self.right_knee_joint_handle,
vrep_sim.simx_opmode_streaming)
return_code, state, foot_pressure, torque = vrep_sim.simxReadForceSensor(
self.client_ID, self.left_foot_force_sensor_handle,
vrep_sim.simx_opmode_streaming)
return_code, state, foot_pressure, torque = vrep_sim.simxReadForceSensor(
self.client_ID, self.right_foot_force_sensor_handle,
vrep_sim.simx_opmode_streaming)
vrep_sim.simxSetJointTargetPosition(self.client_ID,
self.left_hip_joint_handle, 0,
vrep_sim.simx_opmode_streaming)
vrep_sim.simxSetJointTargetPosition(self.client_ID,
self.left_knee_joint_handle, 0,
vrep_sim.simx_opmode_streaming)
vrep_sim.simxSetJointTargetPosition(self.client_ID,
self.right_hip_joint_handle, 0,
vrep_sim.simx_opmode_streaming)
vrep_sim.simxSetJointTargetPosition(self.client_ID,
self.right_knee_joint_handle, 0,
vrep_sim.simx_opmode_streaming)
operationMode=sim.simx_opmode_buffer)
_, res, depth = sim.simxGetVisionSensorDepthBuffer(
clientID,
depthHandle,
operationMode=sim.simx_opmode_buffer)
img = np.array(img, dtype=np.uint8)
img.resize((res[1], res[0], 3))
img = np.flip(img, axis=0)
#print('depth:', depth)
#print('img is', img)
#plt.imshow(img)
#plt.show()
time.sleep(0.5)
# Read the velocity
_, linear_velocity, angular_v = sim.simxGetObjectVelocity(
clientID, robot_br_wheel, sim.simx_opmode_streaming)
linear_velocity = round2(linear_velocity[:-1])
angular_v = round2(angular_v)
agent_velocity = round2(
math.sqrt(linear_velocity[0]**2 + linear_velocity[1]**2))
# Read current position
_, agent_position = sim.simxGetObjectPosition(
clientID, agent_handle, -1, sim.simx_opmode_streaming)
_, agent_position = sim.simxGetObjectPosition(
clientID, agent_handle, -1, sim.simx_opmode_buffer)
agent_position = round2(agent_position[:-1])
_, agent_ori = sim.simxGetObjectOrientation(
clientID, agent_handle, -1, sim.simx_opmode_streaming)
_, agent_ori = sim.simxGetObjectOrientation(
lf_err_code, lf_motor_handle = vrep.simxGetObjectHandle(clientID, "WheelJoint_LF", vrep.simx_opmode_blocking)
lb_err_code, lb_motor_handle = vrep.simxGetObjectHandle(clientID, "WheelJoint_LB", vrep.simx_opmode_blocking)
rb_err_code, rb_motor_handle = vrep.simxGetObjectHandle(clientID, "WheelJoint_RB", vrep.simx_opmode_blocking)
rf_err_code, rf_motor_handle = vrep.simxGetObjectHandle(clientID, "WheelJoint_RF", vrep.simx_opmode_blocking)
# Turn on the drives
# err_code = vrep.simxSetJointTargetVelocity(clientID, lb_motor_handle, 1.0, vrep.simx_opmode_streaming)
# err_code = vrep.simxSetJointTargetVelocity(clientID, rb_motor_handle, 1.0, vrep.simx_opmode_streaming)
# err_code = vrep.simxSetJointTargetVelocity(clientID, lf_motor_handle, 1.0, vrep.simx_opmode_streaming)
# err_code = vrep.simxSetJointTargetVelocity(clientID, rf_motor_handle, 1.0, vrep.simx_opmode_streaming)
# get handle to the load
load_err_code, load_handle = vrep.simxGetObjectHandle(clientID, "Cuboid", vrep.simx_opmode_blocking)
# get first position of load
err_code, position = vrep.simxGetObjectPosition(clientID, load_handle, -1, vrep.simx_opmode_streaming)
# get positions of load
t = time.time()
while True: # read values for 15 seconds
err_code, position = vrep.simxGetObjectPosition(clientID, load_handle, -1, vrep.simx_opmode_buffer)
result = vrep.simxGetObjectVelocity(clientID, load_handle, vrep.simx_opmode_streaming)
obj_velocity = result[1][0]
if err_code == 0:
distance = position[0] - DESTINATION_POINT
control.input['distance'] = distance
control.compute()
speed = control.output['speed']
set_speed(speed)
print('Position =', position[0], '\t\tNew speed =', speed, '\t\tObject velocity =', obj_velocity)
# using proximity sensor to detect when the ball hits the wall (enters proximity range)
detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere',
sim.simx_opmode_blocking)
prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor',
sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, detect_handle[1], -1,
[0.5, 0.475, 0.0463], sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, True)
sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3001, 2,
sim.simx_opmode_oneshot)
sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3002, 2,
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
sim.simxReadProximitySensor(clientID, prox_handle[1],
sim.simx_opmode_streaming)
sim.simxGetObjectVelocity(clientID, detect_handle[1],
sim.simx_opmode_streaming)
y = 0
points = []
velocities = []
leave = 0
# enter while loop to read proximity sensor
while (y == 0):
# read prox sensor and get detected points
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, prox_handle[1], sim.simx_opmode_buffer)
ret, linear, angular = sim.simxGetObjectVelocity(
clientID, detect_handle[1], sim.simx_opmode_buffer)
# detecting
if (dS == 1):
def get_ball_info(clientID):
"""
stores the linear velocity and the position variable of the last time that the ball hit the wall
stores it in the global coordinates ball_coord and ball_linear
@param clientID: ID used to connect to CoppeliaSim's environment
@return None
"""
global ball_coord
global ball_linear
global hit_wall
global ball_hit
global detect_handle
global prox_handle
global stop_prog
print("Getting ball info: ")
# while 1:
# if cnt == 0:
# ret, ball_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking)
# ret, ball_coord = sim.simxGetObjectPosition(clientID, ball_handle, -1, sim.simx_opmode_streaming)
# ret, ball_linear, ball_angular = sim.simxGetObjectVelocity(clientID, ball_handle, sim.simx_opmode_streaming)
# cnt += 1
# else:
# ret, ball_coord = sim.simxGetObjectPosition(clientID, ball_handle, -1, sim.simx_opmode_buffer)
# ret, ball_linear_new, ball_angular = sim.simxGetObjectVelocity(clientID, ball_handle, sim.simx_opmode_buffer)
# if(ball_linear_new[1] < 0 and ball_linear[1] > 0):
# #if we come here, then the velocity direction has changed, meaning we hit the wall
# hit_wall = True
# detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking)
# print("Sphere Handle: ", detect_handle[1])
# prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking)
# print("Proximity Sensor Handle: ", prox_handle[1])
y = 0
points = []
velocities = []
leave = 0
while True:
if stop_prog:
break
if ball_hit:
#print("We hit the ball, we are now processing")
y = 0 #Represents if the ball is moving away from the wall or not
else:
#print("waiting for ball to be hit")
continue
while (y == 0): #While ball still coming towards wall
# read prox sensor and get detected points
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, prox_handle[1], sim.simx_opmode_buffer)
ret, linear, angular = sim.simxGetObjectVelocity(
clientID, detect_handle[1], sim.simx_opmode_buffer)
# detecting
if (dS == 1):
# store all the detected points
points.append(dP)
velocities.append(linear)
leave = 1
# not detecting and is heading away from wall
elif (dS == 0 and leave == 1):
y = 1
hit_wall = True #To show we have just hit the wall
ball_hit = False #To show that the ball has not been hit yet. Ideally, we don't start this loop
#until the ball has been hit
leave = 0
print("Storing left velocity and position")
if len(velocities) > 0:
ball_linear = velocities[-1]
if len(points) > 0:
ball_coord = points[-1]
velocities = []
points = []
#用于测试的计数
index = 0
# 开始仿真
while vrep.simxGetConnectionId(clientID) != -1:
currCmdTime = vrep.simxGetLastCmdTime(clientID) # 记录当前时间
dt = currCmdTime - lastCmdTime # 记录时间间隔，用于控制
# ***
_, base_pos = vrep.simxGetObjectPosition(clientID, baseHandle, -1,
vrep.simx_opmode_streaming)
_, jointConfig = vrep.simxGetJointPosition(clientID, jointHandle,
vrep.simx_opmode_streaming)
_, linear_velocity, angular_velocity = vrep.simxGetObjectVelocity(
clientID, baseHandle, vrep.simx_opmode_streaming)
#print(jointConfig)
#print(base_pos)
x_init[0] = base_pos[2]
x_init[1] = linear_velocity[2]
print(x_init)
controller.prob_describe()
controller.update(p_target, x_init, u_init)
sol = controller.sol
X = controller.X
U = controller.U
if clientID != -1:
break
start_sim = True
# simxGetPingTime(clientID)
_, drone = sim.simxGetObjectHandle(clientID, 'my_drone', sim.simx_opmode_blocking)
_, target = sim.simxGetObjectHandle(clientID, 'Quadcopter_target', sim.simx_opmode_blocking)
_, camera = sim.simxGetObjectHandle(clientID, 'Main_camera', sim.simx_opmode_oneshot_wait)
_, h_sensor = sim.simxGetObjectHandle(clientID, 'H_sensor', sim.simx_opmode_oneshot_wait)
_, gps = sim.simxGetStringSignal(clientID, 'gps', sim.simx_opmode_streaming)
_, gyro = sim.simxGetStringSignal(clientID, 'gyro', sim.simx_opmode_streaming)
_, accel = sim.simxGetStringSignal(clientID, 'nydavai', sim.simx_opmode_streaming)
_, velocity, _ = sim.simxGetObjectVelocity(clientID, drone, sim.simx_opmode_streaming)
_, euler = sim.simxGetObjectOrientation(clientID, drone, target, sim.simx_opmode_streaming)
_, h_detected, h_point, h_det_obj, h_norm = sim.simxReadProximitySensor(clientID, h_sensor, sim.simx_opmode_streaming)
_, orientation_target = sim.simxGetObjectOrientation(clientID, target, -1, sim.simx_opmode_streaming)
_, orientation_drone = sim.simxGetObjectOrientation(clientID, drone, -1, sim.simx_opmode_streaming)
_, location_drone = sim.simxGetObjectPosition(clientID, drone, -1, sim.simx_opmode_streaming)
_, location_target = sim.simxGetObjectPosition(clientID, target, -1, sim.simx_opmode_streaming)
_, resolution, image = sim.simxGetVisionSensorImage(clientID, camera, 0, sim.simx_opmode_streaming)
pid1 = PID_controll(2, 0, 0)
pid1.k_v = -2
pid2 = PID_controll(0.005, 0, 1)
pid3 = PID_controll(-0.005, 0, -1)
pid4 = PID_controll(0.1, 0, 2)
pid5 = PID_controll(0.25, 0, 2.1)
def getCOMVelocity(self):
return_code, linear_vel, angular_vel = vrep_sim.simxGetObjectVelocity(
self.client_ID, self.COM_handle, vrep_sim.simx_opmode_buffer)
return linear_vel, angular_vel
