def simulation(self):
sim.simxSynchronous(self.clientID, 1)
#同步模式
sim.simxSynchronousTrigger(self.clientID)
# Start simulation
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot_wait)
print("Simulation start")
def resetSim(self):
# print('[INFO] reseting sim...')
ret = sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
if ret == sim.simx_return_ok:
# print('[INFO] sim reset successfully.')
time.sleep(1)
else:
raise IOError('[ERROR] problem in reseting sim...')
sim.simxGetPingTime(self.clientID)
# print('[INFO] starting sim in synchronous mode...')
sim.simxSynchronous(self.clientID, True)
sim.simxGetPingTime(self.clientID)
ret = sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# if ret == sim.simx_return_ok:
# print('[INFO] sim started successfully.')
sim.simxGetPingTime(self.clientID)
sim.simxClearFloatSignal(self.clientID, 'actionX',
sim.simx_opmode_blocking)
sim.simxClearFloatSignal(self.clientID, 'actionY',
sim.simx_opmode_blocking)
sim.simxClearFloatSignal(self.clientID, 'actionZ',
sim.simx_opmode_blocking)
sim.simxGetPingTime(self.clientID)
self.stepCount = 0
self.reward = 0
def __init__(self): # n_actions:3 (target pos), n_states:6 (3pos+3force)
self.metadata = {'render.modes': ['human']}
super().__init__()
sim.simxFinish(-1)
for _ in range(5):
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000,
5)
if self.clientID != -1:
# print('[INFO] Connected to CoppeliaSim.')
break
if self.clientID == -1:
raise IOError('[ERROR] Could not connect to CoppeliaSim.')
sim.simxSynchronous(self.clientID, True)
# sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
sim.simxGetPingTime(self.clientID)
self.stepCount = 0
self.reward = 0
self.n_substeps = 10
# self.sim_timestep = 0.5 # set in coppeliaSim (not implemented)
self.n_actions = 3
self.n_states = 6
self.action_space = spaces.Box(-1.,
1,
shape=(self.n_actions, ),
dtype='float32')
self.observation_space = spaces.Box(-np.inf,
np.inf,
shape=(self.n_states, ),
dtype='float32')
self._getHandles()
sim.simxGetPingTime(self.clientID)
def __init__(self):
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if self.clientID != -1: #check if client connection successful
print('Connected to remote API server')
# enable the synchronous mode on the client:
sim.simxSynchronous(self.clientID, True)
else:
print('Connection not successful')
sys.exit('Could not connect')
self.Vmax = 0.05
self.Wmax = np.pi / 4
# Action Space
self.action_space = spaces.Discrete(3)
# Observation Space
self.observation_space = spaces.Box(
low=np.array([0, 0, 0., -np.pi / 4, 0, 0, 0, 0, 0, 0, 0, 0],
dtype=np.float32),
high=np.array(
[3, 2 * np.pi, 0.08, np.pi / 4, 1, 1, 1, 1, 1, 1, 1, 1],
dtype=np.float32),
dtype=np.float32)
# Objetcs in the Simulation Scene
errorCode, self.right_motor = sim.simxGetObjectHandle(
self.clientID, 'K4_Right_Motor', sim.simx_opmode_oneshot_wait)
errorCode, self.left_motor = sim.simxGetObjectHandle(
self.clientID, 'K4_Left_Motor', sim.simx_opmode_oneshot_wait)
errorCode, self.khepera = sim.simxGetObjectHandle(
self.clientID, 'Khepera_IV', sim.simx_opmode_oneshot_wait)
errorCode, self.target = sim.simxGetObjectHandle(
self.clientID, 'Target', sim.simx_opmode_oneshot_wait)
errorCode, self.camera = sim.simxGetObjectHandle(
self.clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait)
self.sensor = {}
for i in range(8):
handle = 'K4_Infrared_{}'.format(i + 1)
errorCode, self.sensor[i] = sim.simxGetObjectHandle(
self.clientID, handle, sim.simx_opmode_oneshot_wait)
self.viewer = None
self.seed()
self.steps = 0
self.radius = 0.8
self.MaxSteps = 800
self.problem = True
self.Velocities = [0, 0]
self.Movements = [[4.285, 0.515], [4.285, 4.285], [0.515, 4.285]]
self.xp, self.yp = self.getPositionTarget()
self.ResetSimulationScene()
self.Randomize = True
self.RobotOrientationRand = True
def __init__(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')
sim.simxSynchronous(self.clientID, True)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
def __init__(self, queue_dict, sync=True):
"""
this class loads coppeliasim and is responsible for communication between the sim and python...
api functions can be found https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm
"""
# open queues
self.queues = queue_dict
# close all connection that are remaining
sim.simxFinish(-1)
self.clientID = -1
attempt_num = 0
while self.clientID == -1:
print("attempting to connect to VREP...")
self.clientID = sim.simxStart('127.0.0.1', 19999,
True, True, 5000, 5)
attempt_num += 1
if attempt_num >= 3:
print("could not connect to vrep")
return
print("successful connection!")
self.sync = sync
if self.sync:
# set the simulation to synchronise with api
sim.simxSynchronous(self.clientID, True)
# get coppeliasim object handles
self.motor = {'left': self.getHandle("Pioneer_p3dx_leftMotor"),
'right': self.getHandle("Pioneer_p3dx_rightMotor")}
self.camHandle = self.getHandle("camera")
#self.gripperHandle = {"left": self.getHandle("left_joint"),
# "right": self.getHandle("right_joint")}
# init camera data stream with rgb and depth
sim.simxGetVisionSensorImage(self.clientID, self.camHandle, 0, sim.simx_opmode_streaming)
sim.simxGetVisionSensorDepthBuffer(self.clientID, self.camHandle, sim.simx_opmode_streaming)
# init position data stream with cartesian position and euler orientation
sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
# setting motor speed
self.speed = 1
self.lr = 0.3
self.images = None
# remote start simulation if not already
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# used for making keyboard switches
self.keyboard_key = Clicker("c", activated=True)
self.keyboard_controlled = self.keyboard_key.activated
self.gripper_key = Clicker("g", activated=False)
self.gripper_activated = self.gripper_key.activated
def reset(self):
# stop the simulation:
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
time.sleep(0.1)
sim.simxSynchronous(self.clientID, True)
# start the simulation:
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
self.steps = 0
self.ResetSimulationScene()
if self.Randomize:
ran_angle = random.random() * 360
self.xp, self.yp = self.change_target_angle(ran_angle)
else:
self.xp, self.yp = self.getPositionTarget()
#self.xp, self.yp = self.change_target_angle(self.angle)
d, Oc = self.get_obs()
observation = np.array([d, Oc])
return observation
def __init__(self):
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if self.clientID != -1: #check if client connection successful
print('Connected to remote API server')
# enable the synchronous mode on the client:
sim.simxSynchronous(self.clientID, True)
else:
print('Connection not successful')
sys.exit('Could not connect')
# Action Space
self.action_space = spaces.Discrete(3)
# Observation Space
self.observation_space = spaces.Box(low=np.array([0., -np.pi, -np.pi],
dtype=np.float32),
high=np.array([5, np.pi, np.pi],
dtype=np.float32),
dtype=np.float32)
# Objetcs in the Simulation Scene
errorCode, self.right_motor = sim.simxGetObjectHandle(
self.clientID, 'K4_Right_Motor', sim.simx_opmode_oneshot_wait)
errorCode, self.left_motor = sim.simxGetObjectHandle(
self.clientID, 'K4_Left_Motor', sim.simx_opmode_oneshot_wait)
errorCode, self.khepera = sim.simxGetObjectHandle(
self.clientID, 'Khepera_IV', sim.simx_opmode_oneshot_wait)
errorCode, self.target = sim.simxGetObjectHandle(
self.clientID, 'Target', sim.simx_opmode_oneshot_wait)
errorCode, self.camera = sim.simxGetObjectHandle(
self.clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait)
self.viewer = None
self.seed()
self.steps = 0
self.MaxSteps = 300
self.radius = 0.8
self.angle = 0
self.Randomize = False
self.xp, self.yp = self.getPositionTarget()
self.ResetSimulationScene()
def reset(self):
# stop the simulation:
self.problem = False
sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
sim.simxSynchronous(self.clientID, True)
# start the simulation:
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
self.ResetSimulationScene()
if self.Randomize:
self.xp, self.yp = self.change_target_position(radius=self.radius)
else:
self.xp, self.yp = self.getPositionTarget()
if self.RobotOrientationRand:
self.change_robot_orientation()
self.steps = 0
[d, Oc, alpha, v, w], Sensors = self.get_obs()
observation = np.append([d, Oc, v, w], Sensors)
return observation
def __init__(self,
velocity=20,
angular_velocity=100,
wheel_basis=0.212,
wheel_radius=0.03):
sim.simxFinish(-1)
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
self.reset = True
self.velocity = velocity
self.angular_velocity = angular_velocity
self.wheel_basis = wheel_basis
self.total_sensors = 13
self.wheel_radius = wheel_radius
self.position = [+6.5000e-01, -4.4900e+00, +8.0299e-02]
self.target_position = [+1.0900e+00, -3.5380e+00, +3.8000e-02
] #[+3.9470e+00,+3.8440e+00,+3.8000e-02]
self.sensor = np.zeros((self.total_sensors), dtype=np.int32)
self.flag = False
self.roomba_path = 'C:/Program Files/CoppeliaRobotics/CoppeliaSimEdu/models/selfmade/Roomba.ttm'
if self.clientID != -1:
print("Connected to API")
sim.simxSynchronous(self.clientID, True)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
else:
print("Unable to connect")
#Loading Model
_ = sim.simxLoadModel(self.clientID, self.roomba_path, 0,
sim.simx_opmode_blocking)
self._intial()
_ = sim.simxSetObjectPosition(self.clientID, self.bot, -1,
self.position, sim.simx_opmode_oneshot)
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1,
self.target_position,
sim.simx_opmode_oneshot)
returnCode = sim.simxSetJointTargetVelocity(clientID, rightWheel, 0,
sim.simx_opmode_oneshot)
# start simulation and initialize vision sensor
status = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
#^ 改为不断查询直至获得图像
returnCode = 1
while returnCode != sim.simx_return_ok:
returnCode, resolution, image = sim.simxGetVisionSensorImage(
clientID, visionSensorHandle, 1, sim.simx_opmode_streaming)
timeStart = time.time()
last_time = sim.simxGetLastCmdTime(clientID)
idx = 0
#^ 改为同步模式
sim.simxSynchronous(clientID, 1)
try:
while True:
returnCode, resolution, image = sim.simxGetVisionSensorImage(
clientID, visionSensorHandle, 1, sim.simx_opmode_buffer)
image = np.flipud(
np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0]]))
# if idx<=100:
# plt.imsave(f'full{idx}.jpg',np.stack([image]*3, axis=2))
# idx+=1
cropImage = crop(image, mainSizeX, mainSizeY)
#^ 大scale就是原图像下半部分
landscape = crop(image, bigX, bigY)
#^ 增加一个图像followLine用于控制直线速度
followLine = crop(image, followLineX, followLineY)
velocity = velUpperLimit
if (velocity < -velUpperLimit):
velocity = -velUpperLimit
return velocity
# Start streaming client.intSignalName integer signal, that signals when a step is finished:
sim.simxGetIntegerSignal(client.id, client.intSignalName,
sim.simx_opmode_streaming)
res, client.jointHandle = sim.simxGetObjectHandle(
client.id, 'joint', sim.simx_opmode_blocking)
sim.simxSetJointTargetVelocity(client.id, client.jointHandle,
360 * math.pi / 180,
sim.simx_opmode_oneshot)
sim.simxGetJointPosition(client.id, client.jointHandle,
sim.simx_opmode_streaming)
# enable the synchronous mode on the client:
sim.simxSynchronous(client.id, True)
sim.simxStartSimulation(client.id, sim.simx_opmode_oneshot)
moveToAngle(45 * math.pi / 180)
moveToAngle(90 * math.pi / 180)
moveToAngle(-89 * math.pi / 180) #no -90, to avoid passing below
moveToAngle(0 * math.pi / 180)
sim.simxStopSimulation(client.id, sim.simx_opmode_blocking)
def disable_synchronization(self):
"""Выключить режим синхронизации
"""
sim.simxSynchronous(self.client, False)
self.synchronous = False
def enable_synchronization(self):
"""Включить режим синхронизации
"""
sim.simxSynchronous(self.client, True)
self.synchronous = True
if clientID > -1:
break
else:
time.sleep(0.2)
print("Failed connecting to remote API server!")
print("Connection success!")
RAD2DEG = 180 / math.pi # 常数，弧度转度数
tstep = 0.01 # 定义仿真步长
# 设置仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(clientID,
vrep.sim_floatparam_simulation_time_step, tstep,
vrep.simx_opmode_oneshot)
# 然后打开同步模式
vrep.simxSynchronous(clientID, True)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
baseName = 'Body'
jointName = 'Tran'
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
_, jointHandle = vrep.simxGetObjectHandle(clientID, jointName,
vrep.simx_opmode_blocking)
print('Handles available!')
# _, base_pos = vrep.simxGetObjectPosition(clientID, baseHandle, -1, vrep.simx_opmode_streaming)
# _, jointConfig = vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming)
if client_ID > -1: # connected
print('Connect to remote API server.')
break
else:
print('Failed connecting to remote API server! Try it again ...')
# Pause the simulation
# res = vrep_sim.simxPauseSimulation(client_ID, vrep_sim.simx_opmode_blocking)
delta_t = 0.005 # simulation time step
# Set the simulation step size for VREP
vrep_sim.simxSetFloatingParameter(client_ID,
vrep_sim.sim_floatparam_simulation_time_step,
delta_t, vrep_sim.simx_opmode_oneshot)
# Open synchronous mode
vrep_sim.simxSynchronous(client_ID, True)
# Start simulation
vrep_sim.simxStartSimulation(client_ID, vrep_sim.simx_opmode_oneshot)
# ------------------------------- Initialize simulation model -------------------------------
UR5_sim_model = UR5SimModel()
UR5_sim_model.initializeSimModel(client_ID)
time.sleep(0.1)
t = np.linspace(0, 2 * np.pi, 800)
data_len = len(t)
reference_q = np.zeros((3, data_len))
reference_q[0, :] = np.sin(t)
reference_q[1, :] = np.sin(t)
reference_q[2, :] = np.sin(t)
def main(argv):
# https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm --> how to remote API
# Refer to https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStart
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
print('Failed connecting to remote API server')
else:
print('Connected to remote API client')
numberOfRuns = 10
numberOfMeasurements = 1
variationSize = 0.001
for i in range(numberOfRuns):
count = 0
dt = 0.0167
sim.simxSynchronous(clientID, True)
################################################### init variables ###################################################
ros_handle = simROS.SIMROS(argv)
targetAngle = 0
#targetAngles = list(np.random.randint(-25, 26, numberOfMeasurements))
targetAngles = list(np.zeros(numberOfMeasurements))
print(targetAngles)
randomVar = []
for i in range(18):
randomVar.append(
list(
np.random.uniform(-variationSize, variationSize,
numberOfMeasurements)))
if newFile:
with open(dataFilePath, 'w+') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
wr.writerow([
"Inclination", "LFx", "LFy", "LFz", "LMx", "LMy",
"LMz", "LHx", "LHy", "LHz", "RFx", "RFy", "RFz", "RMx",
"RMy", "RMz", "RHx", "RHy", "RHz"
])
counter = 0
Done = False
#Start the simulation
sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
################################################### Control Loop ###################################################
while not Done:
#code here
#print(sim.simxReadForceSensor(clientID, "3D_force0", sim.simx_opmode_blocking))
if counter == 150:
saveData(force, targetAngle)
print("Runs remaining: ", len(targetAngles))
if len(targetAngles) <= 0:
Done = True
else:
targetAngle = targetAngles.pop(0)
motor_positions = [
0, randomVar[1].pop() + 2.059, randomVar[2].pop(),
0, randomVar[4].pop() + 2.059, randomVar[5].pop(),
0, randomVar[7].pop() + 2.059, randomVar[8].pop(),
0, randomVar[10].pop() + 2.059,
randomVar[11].pop(), 0,
randomVar[13].pop() + 2.059, randomVar[14].pop(),
0, randomVar[16].pop() + 2.059,
randomVar[17].pop()
]
counter = 0
ros_handle.setSlopeAngle(targetAngle)
ros_handle.setLegMotorPosition(motor_positions)
counter = counter + 1
#Step the simulation
sim.simxSynchronousTrigger(clientID)
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxGetPingTime(clientID)
sim.simxFinish(clientID)
import time
import sim
import simConst
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
print("Connection success")
if clientID!=-1:
print ('Connected to remote API server')
res,left_motor = sim.simxGetObjectHandle(clientID, "left_motor", sim.simx_opmode_oneshot_wait)
sim.simxSynchronous(clientID,1)# -啟用同步模式（客戶端）。服務器端（即CoppeliaSim）也需要啟用。
sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot)# //開始仿真
sim.simxSetJointForce(clientID,left_motor,1.0f,sim.simx_opmode_oneshot）;#//設置聯合力/扭矩
sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置聯合目標速度
sim.simxSynchronousTrigger(clientID);#//觸發下一個模擬步驟。上面的命令將被應用
sim.simxSetJointForce(clientID,left_motor,0.5f,sim.simx_opmode_oneshot)# //設置聯合力/扭矩
sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置聯合目標速度
sim.simxSynchronousTrigger(clientID)# //下一個模擬步驟執行。上面的命令將被應用
print('"sim.py" could not be imported. This means very probably that')
print('either "sim.py" or the remoteApi library could not be found.')
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', 19997, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('Connected to remote API server')
sim.simxSynchronous(clientID, False)
ret = sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
time.sleep(2)
ret = sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
print('started simulation')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
ret, motor_hand = sim.simxGetObjectHandle(clientID, "motor",
sim.simx_opmode_blocking)
ret, cyl_hand = sim.simxGetObjectHandle(clientID, "Cylinder",
sim.simx_opmode_blocking)
ret, rob_hand = sim.simxGetObjectHandle(clientID, "Cuboid",
sim.simx_opmode_blocking)
# Now retrieve streaming data (i.e. in a non-blocking fashion):
startTime = time.time()
ret, [i, j, k] = sim.simxGetObjectOrientation(
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip,port,True,True,5000,5) # Connect to CoppeliaSim
scene_path = os.path.join('vrep_scene', 'hexapod_scene.ttt')
print('-'*5, 'Scene path:', scene_path, '-'*5)
sim.simxLoadScene(clientID, scene_path, 0xFF, sim.simx_opmode_blocking)
return clientID
if __name__ == "__main__":
args = parse_joint_args()
print('Program started')
clientID=conect_and_load(args.port, args.ip)
print('Starting the simulation')
sim.simxSynchronous(clientID, 1) # enable synchoronous mode
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxSynchronousTrigger(clientID)
time.sleep(1)
if clientID!=-1:
print('Connected to remote API server')
time.sleep(1)
env = HexapodEnv(clientID, args.faulty_joints)
print_info(f"Eps Decay Rate is {args.eps_decay}")
train_dir = get_train_dir(args.faulty_joints)
# save hyperparameters
def main():
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
sys.exit('Could not connect to remote API server')
sim.simxSynchronous(clientID, 1) #synchronous operation necessary for
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
jointHandles = [-1, -1, -1, -1, -1, -1]
move_helper.getJointHandles(clientID, jointHandles)
#handle of UR3
base_handle = sim.simxGetObjectHandle(clientID, "UR3",
sim.simx_opmode_blocking)[1]
#handle for end-effector
end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7",
sim.simx_opmode_blocking)[1]
#position of UR3 end effector wrt UR3 base frame
end_effector_wrt_base = sim.simxGetObjectPosition(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_blocking)[1]
#handle for the ball
ball_handle = sim.simxGetObjectHandle(clientID, 'Ball',
sim.simx_opmode_blocking)[1]
#handle for ball sensor/proximity sensor
sensorHandle = sim.simxGetObjectHandle(clientID, 'Ball_Sensor',
sim.simx_opmode_blocking)[1]
#position and orientation of proximity sensore wrt UR3 base frame
T_sensor_in_base = np.array( \
[[0, 0, -1, end_effector_wrt_base[0]],
[0, -1, 0, end_effector_wrt_base[1]], \
[-1, 0, 0, end_effector_wrt_base[2]],
[0, 0, 0, 1]])
#the actual end point of the ball based on path in the world frame
path = generate_path()
end_pt = path[path.shape[0] - 1]
detectedPoints = np.zeros((2, 3))
#the position and orientation of the UR3 base frame wrt world frame (T_wb)
T_base_in_world = np.array([[-1, 0, 0, -1.4001], [0, -1, 0, -0.000086],
[0, 0, 1, 0.043], [0, 0, 0, 1]])
#T_bw
T_world_in_base = np.linalg.inv(T_base_in_world)
#end pt coords in homog form
end_pt_homogenous_coords = np.array([[end_pt[0], end_pt[1], end_pt[2],
1]]).T
#T_bw * p_w = p_b (4,4) * (4, 1) -> (4, 1), left out final element -> (3, 1)
#the actual endpt of the ball in the base frame
end_pt_ball_in_base = np.matmul(T_world_in_base,
end_pt_homogenous_coords)[:3]
detection_thread = threading.Thread(target=utils.calc_ball_position,
args=(clientID, sensorHandle, path,
detectedPoints))
motion_thread = threading.Thread(target=utils.move_ball,
args=(clientID, ball_handle, path))
motion_thread.start()
detection_thread.start()
detection_thread.join()
#point_in_sensorX - homogeneous coords of detectedPoints in the frame of the proximity sensor
point_in_sensor1 = np.array([[detectedPoints[0][0]],
[detectedPoints[0][1]],
[detectedPoints[0][2]], [1]])
point_in_sensor2 = np.array([[detectedPoints[1][0]],
[detectedPoints[1][1]],
[detectedPoints[1][2]], [1]])
#T_bs * p_s = p_b
point1_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor1)[:3]
point2_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor2)[:3]
vector = (1.0 * point2_ball_in_base - point1_ball_in_base)
#convert to unit vector
vector /= np.linalg.norm(vector)
t = (end_effector_wrt_base[0] - point2_ball_in_base[0]) / vector[0]
final_x = end_effector_wrt_base[0]
final_y = point2_ball_in_base[1] + (t * vector[1])
final_z = point2_ball_in_base[2] + (t * vector[2])
predicted_point = np.array([[final_x], [final_y[0]], [final_z[0]]])
difference = predicted_point - end_pt_ball_in_base
print("Difference: " + str(difference))
#the predicted direction the ball is moving in, based off of predicted points 1 and 2
#if the difference between detected points is too small
if (vector[0] == 0):
vector[0] += 0.000000001
#find the estimated position of the ball when it is in the yz plane of the UR3
#the predicted point of the ball
print("Predicted Point: " + str(predicted_point))
print("End Point Ball in Base: " + str(end_pt_ball_in_base))
#compare prediction to actual end pt of ball
#our initial guess of what the UR3 joint angles should be to get to finalT
thetas = np.array([0.0, 0.0, 0.0])
#get a rough initial guess for theta of joint 2
predicted_y = predicted_point[1]
predicted_z = predicted_point[2]
position_joint2 = sim.simxGetObjectPosition(clientID, jointHandles[1],
base_handle,
sim.simx_opmode_blocking)[1]
print(position_joint2)
joint2_y = position_joint2[1]
joint2_z = position_joint2[2]
#distance from predicted point to joint2 in yz plane
d1 = np.sqrt((joint2_y - predicted_y)**2 + (joint2_z - predicted_z)**2)
z_length = predicted_z - joint2_z
y_length = predicted_y - joint2_y
#our thetas to be calculated
theta2_guess = 0.0
theta3_guess = 0.0
theta4_guess = 0.0
#define relevant link lengths
L1 = 0.244
L2 = 0.213
L3 = 0.083
#if it's in this range, it doesn't make sense to move others
if d1 < (L1 - L2):
theta2_guess = np.arctan2(y_length, z_length)
theta3_guess = 0.0
theta4_guess = 0.0
elif d1 <= (L1 + L2 - L3):
top2 = L2**2 - L1**2 - d1**2
bot2 = -2 * L1 * d1
theta2_guess = np.arctan2(y_length, z_length) - np.arccos(top2 / bot2)
top3 = d1**2 - L1**2 - L2**2
bot3 = -2 * L1 * L2
theta3_guess = np.pi - np.arccos(top3 / bot3)
#fold in on self
theta4_guess = np.pi / 2
else:
d2 = d1 - L1
theta2_guess = np.arctan2(y_length, z_length)
top3_2 = L3**2 - L2**2 - d2**2
bot3_2 = -2 * d2 * L2
print("top3_2/bot3_2: " + str(top3_2 / bot3_2))
theta3_guess = np.arccos(top3_2 / bot3_2)
top4 = d2**2 - L2**2 - L3**2
bot4 = -2 * L2 * L3
print("top4/bot4: " + str(top4 / bot4))
theta4_guess = np.arccos(top4 / bot4) - np.pi
thetas[0] = theta2_guess
thetas[1] = theta3_guess
thetas[2] = theta4_guess
print("thetas: " + str(thetas))
for i in range(1, 4):
sim.simxSetJointTargetPosition(clientID, jointHandles[i],
thetas[i - 1],
sim.simx_opmode_oneshot_wait)
motion_thread.join(
) #stop motion thread after robot has moved to defending position
print("Actual End Effector Position " + str(
sim.simxGetObjectPosition(clientID, end_effector_handle, base_handle,
sim.simx_opmode_blocking)[1]))
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxFinish(clientID)
return 1
sys.exit("Error: no se puede conectar") #Terminar este script
except:
print('Check if CoppeliaSim is open')
UR3_joints_IP=get_joint_handle(clientID) #Getting the reference
for i in UR3_joints_IP:
UR3_joints_IP[i].append(get_joint_orientation(clientID, UR3_joints_IP[i][0]))
#print(get_joint_orientation(clientID, UR3_joints_IP[i]))
#J1z,J2z,J3Z
#Practica 30 grados j1, 15 grados j2, 30 grados j3, 20 grados j4, 15 grados j5, 10 grados j6
new_angles=[60,25,10,30,15,50]
c=0
sim.simxSynchronous(clientID,True)
sim.simxSynchronousTrigger(clientID)
sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
for i in UR3_joints_IP:
set_joint_orientation(clientID, UR3_joints_IP[i][0],new_angles[c], mode=sim.simx_opmode_blocking) #Todas las juntas posicion inicial
c=c+1
time.sleep(5)
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxFinish(clientID)
def main():
obst_count = 68
targetCount = 5
obstaclePrefix = 'column'
targetPrefix = 'End'
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
err, QuadricopterT = sim.simxGetObjectHandle(clientID,
'Quadricopter_target',
sim.simx_opmode_blocking)
if err == -1:
print("No Quadricopter")
err, Quadricopter = sim.simxGetObjectHandle(clientID, 'Quadricopter',
sim.simx_opmode_blocking)
if err == -1:
print("No Quadricopter")
#sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)
# enable the synchronous mode on the client:
print("is connected!!!")
#retrieves the boxes from coppeliasim. This is slow (which is why it is commented out, we read from file instead)
#obstacle collection
#obst_list = []
bbox_list = []
if GENERATE_FILES:
for i in range(obst_count):
err, Obst = sim.simxGetObjectHandle(clientID,
obstaclePrefix + str(i),
sim.simx_opmode_blocking)
if err > 0:
print("could not retrieve column ", i)
obst_pose = flib.get_pos(clientID, Obst)
print("col ", i, "POSE: ", obst_pose)
obst_size = flib.get_size(clientID, Obst)
print("col ", i, "SIZE: ", obst_size)
obst_bbox = convert_bbox(obst_pose, obst_size)
print("col ", i, "BBOX: ", obst_bbox)
#obst = Obstacle(obst_pose, obst_size, obst_bbox)
bbox_list.append(obst_bbox)
#obst_list.append(obst)
#we write the boxes to a file to retrieve faster later.
write_boxes_file(bbox_list)
bbox_list = read_boxes_file()
#plot_boxes(bbox_list)
#target collection
deliveries = []
for i in range(targetCount):
err, targ = sim.simxGetObjectHandle(clientID,
targetPrefix + str(i + 1),
sim.simx_opmode_blocking)
tmp = flib.get_pos(clientID, targ)
print("Target ", i, "Location: ", tmp)
deliveries.append(np.array([tmp[0], tmp[1], tmp[2]]))
if GENERATE_FILES:
np.savetxt("targets.csv", deliveries, delimiter=",")
pose = flib.get_pos(clientID, Quadricopter)
if GENERATE_FILES:
np.savetxt("pose.csv", pose, delimiter=",")
print("Start position: ", pose)
print("Total distance: ", calc_total_distance(pose, deliveries))
#controller object
pathControl = quadsim_P2P(pose, bbox_list)
np.savetxt("pose.csv", pose, delimiter=",")
np.savetxt("targets.csv", np.array(deliveries), delimiter=",")
#plan route
before_rrt_t = datetime.datetime.now()
while not pathControl.plan(deliveries, clientID):
print("Retrying planning with: max iterations=",
pathControl.rrt.max_iter)
if pathControl.rrt.use_funnel:
print("search cone angle[Rad]=", pathControl.rrt.searchTheta)
print("the path is worthy! Calculation took: ",
(datetime.datetime.now() - before_rrt_t).total_seconds(),
" seconds.")
lastIter = -1
lastGoal = -1
#start simulation
pathControl.iterRun_start()
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
sim.simxSynchronous(clientID, 1)
lastTime = datetime.datetime.now()
startTime = now = datetime.datetime.now()
while pathControl.iterRunGo:
pos, ori = pathControl.iterRun_move()
#pathControl.display()
sim.simxSetObjectPosition(clientID, Quadricopter, -1, pos,
sim.simx_opmode_streaming)
sim.simxSetObjectOrientation(clientID, Quadricopter, -1, ori,
sim.simx_opmode_streaming)
if (pathControl.pathIter != lastIter):
sim.simxSetObjectPosition(
clientID, QuadricopterT, -1,
pathControl.path[pathControl.goalIter][pathControl.pathIter],
sim.simx_opmode_streaming)
now = datetime.datetime.now()
print("Time between goals: ", (now - lastTime).total_seconds(),
"[s]")
lastIter = pathControl.pathIter
lastTime = now
if pathControl.goalIter != lastGoal:
now = datetime.datetime.now()
print("Time of flight: ", (now - startTime).total_seconds(), "[s]")
lastGoal = pathControl.goalIter
startTime = now
pathControl.iterRun_stop()
sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)
sim.simxFinish(clientID)
