def __init__(self, robot, host="127.0.0.1", port=19997):
self.robot = robot
self.sim_joints = [0, 0, 0, 0, 0, 0]
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(host, port, True, True, 5000,
5) # Connect to CoppeliaSim
if self.clientID != -1:
print('Connected to remote API server')
else:
print("coppeliasim connection failed")
return
res, objs = sim.simxGetObjects(self.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)
self.jh = [0, 0, 0, 0, 0, 0]
for i in range(6):
res, self.jh[i] = sim.simxGetObjectHandle(
self.clientID, 'UR5_joint{}'.format(i + 1),
sim.simx_opmode_blocking)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
def start_Simulation(self):
print('Program started')
# Just in case, close all opened connections
sim.simxFinish(-1)
# Connect to CoppeliaSim, as continuous remote API Server
self.clientID = sim.simxStart('127.0.0.1', 19997, False, True, 5000, 5)
if self.clientID != -1:
print('Connected to remote API server')
#Start simulation
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# Now send some data to CoppeliaSim in a non-blocking fashion:
sim.simxAddStatusbarMessage(self.clientID, 'Connection Succeed!',
sim.simx_opmode_oneshot)
# Before closing the connection to CoppeliaSim, make sure that the last command sent
# out had time to arrive. You can guarantee this with (for example):
sim.simxGetPingTime(self.clientID)
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(self.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)
else:
print('Failed connecting to remote API server')
return self.clientID
def vrepInterface(port):
global angles_handler
angles_handler = np.zeros(12)
global angles_error
angles_error = np.zeros(12)
global clientID
clientID = 0
global inital_name
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
ID = sim.simxStart('127.0.0.1', port, True, True, 5000,
5) # Connect to V-REP
print(ID)
if ID != -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(ID, 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)
else:
print('DIDNOT CONNECT!!!')
intial_name = [
'ab3', 'bc3', 'cd3', 'ab4', 'bc4', 'cd4', 'ab1', 'bc1', 'cd1', 'ab2',
'bc2', 'cd2'
]
for i in range(angles_handler.shape[0]):
angles_error[i], angles_handler[i] = sim.simxGetObjectHandle(
clientID, intial_name[i], sim.simx_opmode_blocking)
return ID
def create_dataset(path):
try:
shutil.rmtree(path + "/mav0")
except:
print("Removed before")
os.makedirs(path + "/mav0/cam0/data")
os.makedirs(path + "/mav0/cam1/data")
print('Argument List:', str(sys.argv[1]))
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
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)
er, t_rightWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
sim.simx_opmode_blocking)
er, t_leftWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
sim.simx_opmode_blocking)
er, cam_handle_left = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_leftSensor',
sim.simx_opmode_blocking)
er, cam_handle_right = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_rightSensor',
sim.simx_opmode_blocking)
i = 0
fl = open(path + "/mav0/timestamps.txt", "w")
while (True):
err, resolution_left, colorCam_left = sim.simxGetVisionSensorImage(
clientID, cam_handle_left, 0, sim.simx_opmode_oneshot_wait)
err, resolution_right, colorCam_right = sim.simxGetVisionSensorImage(
clientID, cam_handle_right, 0, sim.simx_opmode_oneshot_wait)
img_left_1 = np.array(colorCam_left, dtype=np.uint8)
img_right_1 = np.array(colorCam_right, dtype=np.uint8)
img_left_1.resize([resolution_left[1], resolution_left[0], 3])
img_right_1.resize([resolution_right[1], resolution_right[0], 3])
img_left_2 = np.flipud(img_left_1)
img_right_2 = np.flipud(img_right_1)
img_left_3 = img_left_2[..., ::-1].copy()
img_right_3 = img_right_2[..., ::-1].copy()
milli_time = int(round(time.time() * 1000)) * 1000000000
st = str(milli_time) + '\n'
st_left = path + '/mav0/cam0/data/{}.png'.format(milli_time)
st_right = path + '/mav0/cam1/data/{}.png'.format(milli_time)
cv2.imwrite(st_left, img_left_3)
cv2.imwrite(st_right, img_right_3)
cv2.waitKey(1)
i = i + 1
fl.write(st)
sim.simxSetJointTargetVelocity(clientID, t_rightWheel, 1,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, t_leftWheel, 1,
sim.simx_opmode_streaming)
fl.close()
def main():
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))
print("Client ID: ", clientID)
else:
print("Remote API function call returned with error code: ", res)
sys.exit("Could not connect")
pioneerRob = Pioneer_p3dx(clientID)
pioneerRob.actuation_sample()
"""
err_code, l_motor_handle = sim.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_blocking)
err_code, r_motor_handle = sim.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_blocking)
err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, 1.0, sim.simx_opmode_streaming)
err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 1.0, sim.simx_opmode_streaming)
time.sleep(5)
err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, -1.0, sim.simx_opmode_streaming)
err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 1.0, sim.simx_opmode_streaming)
time.sleep(2)
err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, 0.0, sim.simx_opmode_streaming)
err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 0.0, sim.simx_opmode_streaming)
time.sleep(2)
err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, 1.0, sim.simx_opmode_streaming)
err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 1.0, sim.simx_opmode_streaming)
time.sleep(2)
"""
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
else:
print("Failed connecting to remote API server")
print("Program ended")
def run_coppelia(self):
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to CoppeliaSim
if self.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(self.clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Found %d objects in the scene.' % len(objs))
else:
print('Remote API function call returned with error code: ',
res)
time.sleep(2)
robot_name = 'UR5'
errorCodes, self.joints = [], []
for i in range(1, 7):
errorCode, joint = sim.simxGetObjectHandle(
self.clientID, robot_name + '_joint' + str(i),
sim.simx_opmode_oneshot_wait)
errorCodes += [errorCode]
self.joints += [joint]
if -1 in errorCodes:
print("Handles creation error")
return (self.clientID, self.joints)
else:
print('Failed connecting to remote API server')
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "sim.py"')
print('--------------------------------------------------------------')
print('')
import time
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)
# Now retrieve streaming data (i.e. in a non-blocking fashion):
startTime = time.time()
sim.simxGetIntegerParameter(
clientID, sim.sim_intparam_mouse_x,
sim.simx_opmode_streaming) # Initialize streaming
while time.time() - startTime < 5:
returnCode, data = sim.simxGetIntegerParameter(
clientID, sim.sim_intparam_mouse_x,
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)
import time
import keyboard
import numpy as np
import math
a = np.zeros(3, dtype=float)
rot = np.zeros(3, dtype=float)
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000,
5) # Connect to V-REP
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 = vrep.simxGetObjects(clientID, vrep.sim_handle_all,
vrep.simx_opmode_blocking)
if res == vrep.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)
number, objecthandle = vrep.simxGetObjectHandle(clientID,
'Quadricopter_target',
vrep.simx_opmode_blocking)
while (a[0] == 0 and a[1] == 0 and a[2] == 0):
es, a = vrep.simxGetObjectPosition(clientID, objecthandle, -1,
vrep.simx_opmode_oneshot)
es, rot = vrep.simxGetObjectOrientation(clientID, objecthandle, -1,
vrep.simx_opmode_oneshot)
